diff --git a/README.md b/README.md new file mode 100644 index 0000000..596a5e3 --- /dev/null +++ b/README.md @@ -0,0 +1,21 @@ +# ROS2 BNO055 Driver +Driver written in C++ + +## Params +### Units +It is highly adviced to use the default values to adher to [REP 103](https://www.ros.org/reps/rep-0103.html). + +| Param | Values | Description | +|:----- |:------ |:----------- | +|`unit.angular_velocity`|`"rps"`|radians per second| +||`"rps"`|radians per second| +|`unit.linear_acceleration`|`"mps2"`|meter per second squared| +||`"mg"`|milli force| +||`"g"`|force| +|`unit.magnetic_field`|`"T"`|tesla| +||`"uT"`|micro tesla| +|`unit.temperature`|`"C"`|celsius| +||`"F"`|fahrenheit| + +## Data sheet +- https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf \ No newline at end of file diff --git a/include/bno055_driver/bno055_driver.hpp b/include/bno055_driver/bno055_driver.hpp index 1286631..f432017 100644 --- a/include/bno055_driver/bno055_driver.hpp +++ b/include/bno055_driver/bno055_driver.hpp @@ -94,6 +94,11 @@ class BNO055Driver : public rclcpp_lifecycle::LifecycleNode lifecycle_msgs::srv::ChangeState::Request::SharedPtr change_state_request_; rclcpp::Client::SharedPtr change_state_client_; rclcpp::Client::SharedFuture change_state_future_; + + double vel_scale_; + double lacc_scale_; + double mag_scale_; + double temp_scale_; }; } // namespace bno055_driver diff --git a/include/bno055_driver/bno055_scales.hpp b/include/bno055_driver/bno055_scales.hpp new file mode 100644 index 0000000..6cc9e24 --- /dev/null +++ b/include/bno055_driver/bno055_scales.hpp @@ -0,0 +1,62 @@ +#ifndef BNO055_DRIVER__BNO055_SCALES_HPP_ +#define BNO055_DRIVER__BNO055_SCALES_HPP_ + +/** + * All the constant scale values are from the BNO055 Data sheet. + * + * All values are const double, because the ROS messages expect + * double values as well. + */ +namespace bno055_driver +{ + namespace scale { + /** + * > Table 3-22: Gyroscope unit settings + * > 1 Dps = 16 LSB Rps 1 Rps = 900 LSB + */ + constexpr double vel_to_dps = 16; + constexpr double vel_to_rps = 900; + + /** + * > Table 3-17: Accelerometer Unit settings + * > 1 m/s2 = 100 LSB and 1 mg = 1 LSB + * + * > Table 3-35: Gravity Vector data representation + * > 1 m/s2 = 100 LSB and 1 mg = 1 LSB + */ + constexpr double acc_to_mps2 = 100; + constexpr double acc_to_mg = 1; + constexpr double acc_to_g = 1000; + + /** + * > Table 3-19: Magnetometer Unit settings + * > 1 μT = 16 LSB + */ + constexpr double mag_to_ut = 16; + constexpr double mag_to_t = 16000000; + + /** + * > Table 3-29: Euler angle data representation + * > 1 degree = 16 LSB and 1 radian = 900 LSB + */ + constexpr double euler_to_deg = 16; + constexpr double euler_to_rad = 900; + + /** + * Table 3-31: Quaternion data representation + * 1 Quaternion (unit less) = 2^14 LSB + */ + constexpr double quat = 16384; + + /** + * Table 3-37: Temperature data representation + * 1°C = 1 LSB and 2°F = 1 LSB + */ + constexpr double temp_to_c = 1; + constexpr double temp_to_f = 2; + + } // namespace scale + +} // namespace bno055_driver + +#endif // BNO055_DRIVER__BNO055_SCALES_HPP_ \ No newline at end of file diff --git a/src/bno055_driver.cpp b/src/bno055_driver.cpp index 36bc0d9..743f539 100644 --- a/src/bno055_driver.cpp +++ b/src/bno055_driver.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "bno055_driver/bno055_driver.hpp" +#include "bno055_driver/bno055_scales.hpp" #include #include @@ -64,6 +65,14 @@ BNO055Driver::BNO055Driver(const std::string & node_name, const rclcpp::NodeOpti declare_parameter("calibration.accelerometer_radius"); declare_parameter("calibration.magnetometer_radius"); + /** + * Set SI by default after ROS REP 103 + */ + declare_parameter("unit.angular_velocity", "rps"); + declare_parameter("unit.linear_acceleration", "mps2"); + declare_parameter("unit.magnetic_field", "T"); + declare_parameter("unit.temperature", "C"); + if (get_parameter("self_manage").get_value()) { change_state_request_ = std::make_shared(); change_state_client_ = this->create_client( @@ -111,8 +120,75 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) port_->write(); // Set the units + uint8_t unit_config = 0b00000110; + rclcpp::Parameter angular_velocity_param; + if (get_parameter("unit.angular_velocity", angular_velocity_param) && + angular_velocity_param.get_type() == rclcpp::PARAMETER_STRING) { + if (angular_velocity_param.as_string() == "rps") { + RCLCPP_INFO(get_logger(), "Setting unit angular_velocity to rad/s"); + unit_config |= 0b00000010; + vel_scale_ = scale::vel_to_rps; + } else if (angular_velocity_param.as_string() == "dps") { + RCLCPP_INFO(get_logger(), "Setting unit angular_velocity to deg/s"); + unit_config &= ~0b00000010; + vel_scale_ = scale::vel_to_dps; + } else { + RCLCPP_ERROR(get_logger(), "Invalid value for unit angular_velocity"); + } + } + + rclcpp::Parameter linear_acceleration_param; + if (get_parameter("unit.linear_acceleration", linear_acceleration_param) && + linear_acceleration_param.get_type() == rclcpp::PARAMETER_STRING) { + if (linear_acceleration_param.as_string() == "mps2") { + RCLCPP_INFO(get_logger(), "Setting unit linear_acceleration to m/s2"); + unit_config &= ~0b00000001; + lacc_scale_ = scale::acc_to_mps2; + } else if (linear_acceleration_param.as_string() == "mg") { + RCLCPP_INFO(get_logger(), "Setting unit linear_acceleration to mg"); + unit_config |= 0b00000001; + lacc_scale_ = scale::acc_to_mg; + } else if (linear_acceleration_param.as_string() == "g") { + RCLCPP_INFO(get_logger(), "Setting unit linear_acceleration to g"); + unit_config |= 0b00000001; + lacc_scale_ = scale::acc_to_g; + } else { + RCLCPP_ERROR(get_logger(), "Invalid value for unit linear_acceleration"); + } + } + + rclcpp::Parameter magnetic_field_param; + if (get_parameter("unit.magnetic_field", magnetic_field_param) && + magnetic_field_param.get_type() == rclcpp::PARAMETER_STRING) { + if (magnetic_field_param.as_string() == "T") { + RCLCPP_INFO(get_logger(), "Setting unit magnetic_field to T"); + mag_scale_ = scale::mag_to_t; + } else if (magnetic_field_param.as_string() == "uT") { + RCLCPP_INFO(get_logger(), "Setting unit magnetic_field to uT"); + mag_scale_ = scale::mag_to_ut; + } else { + RCLCPP_ERROR(get_logger(), "Invalid value for unit magnetic_field"); + } + } + + rclcpp::Parameter temperature_param; + if (get_parameter("unit.temperature", temperature_param) && + temperature_param.get_type() == rclcpp::PARAMETER_STRING) { + if (temperature_param.as_string() == "C") { + RCLCPP_INFO(get_logger(), "Setting unit temperature to C"); + unit_config &= ~0b00010000; + temp_scale_ = scale::temp_to_c; + } else if (temperature_param.as_string() == "F") { + RCLCPP_INFO(get_logger(), "Setting unit temperature to F"); + unit_config |= 0b00010000; + temp_scale_ = scale::temp_to_f; + } else { + RCLCPP_ERROR(get_logger(), "Invalid value for unit temperature"); + } + } + port_->write_command_.address = BNO055Register::UNIT_SEL; - port_->write_command_.data[0] = 0x02; + port_->write_command_.data[0] = unit_config; port_->write(); // Set power mode @@ -491,40 +567,42 @@ void BNO055Driver::publish() try { RCLCPP_DEBUG(get_logger(), "Querying for current IMU data"); - port_->read_command_.address = BNO055Register::MAG_DATA_X_LSB; - port_->read_command_.length = 40; + port_->read_command_.address = BNO055Register::ACC_DATA_X_LSB; + port_->read_command_.length = 46; port_->read(); rclcpp::Time stamp = now(); if (imu_pub_->is_activated()) { imu_msg_->header.stamp = stamp; - imu_msg_->angular_velocity.x = bytes_to_short(&port_->read_response_.data[6]) / 900.0; - imu_msg_->angular_velocity.y = bytes_to_short(&port_->read_response_.data[8]) / 900.0; - imu_msg_->angular_velocity.z = bytes_to_short(&port_->read_response_.data[10]) / 900.0; - imu_msg_->linear_acceleration.x = bytes_to_short(&port_->read_response_.data[26]) / 1000.0; - imu_msg_->linear_acceleration.y = bytes_to_short(&port_->read_response_.data[28]) / 1000.0; - imu_msg_->linear_acceleration.z = bytes_to_short(&port_->read_response_.data[30]) / 1000.0; - imu_msg_->orientation.w = bytes_to_short(&port_->read_response_.data[18]) / 16384.0; - imu_msg_->orientation.x = bytes_to_short(&port_->read_response_.data[20]) / 16384.0; - imu_msg_->orientation.y = bytes_to_short(&port_->read_response_.data[22]) / 16384.0; - imu_msg_->orientation.z = bytes_to_short(&port_->read_response_.data[24]) / 16384.0; + imu_msg_->angular_velocity.x = bytes_to_short(&port_->read_response_.data[12]) / vel_scale_; + imu_msg_->angular_velocity.y = bytes_to_short(&port_->read_response_.data[14]) / vel_scale_; + imu_msg_->angular_velocity.z = bytes_to_short(&port_->read_response_.data[16]) / vel_scale_; + + imu_msg_->linear_acceleration.x = bytes_to_short(&port_->read_response_.data[0]) / lacc_scale_; + imu_msg_->linear_acceleration.y = bytes_to_short(&port_->read_response_.data[2]) / lacc_scale_; + imu_msg_->linear_acceleration.z = bytes_to_short(&port_->read_response_.data[4]) / lacc_scale_; + + imu_msg_->orientation.w = bytes_to_short(&port_->read_response_.data[24]) / scale::quat; + imu_msg_->orientation.x = bytes_to_short(&port_->read_response_.data[26]) / scale::quat; + imu_msg_->orientation.y = bytes_to_short(&port_->read_response_.data[28]) / scale::quat; + imu_msg_->orientation.z = bytes_to_short(&port_->read_response_.data[30]) / scale::quat; imu_pub_->publish(*imu_msg_); } if (mag_pub_->is_activated()) { mag_msg_->header.stamp = stamp; - mag_msg_->magnetic_field.x = bytes_to_short(&port_->read_response_.data[0]) / 16000000.0; - mag_msg_->magnetic_field.y = bytes_to_short(&port_->read_response_.data[2]) / 16000000.0; - mag_msg_->magnetic_field.z = bytes_to_short(&port_->read_response_.data[4]) / 16000000.0; + mag_msg_->magnetic_field.x = bytes_to_short(&port_->read_response_.data[6]) / mag_scale_; + mag_msg_->magnetic_field.y = bytes_to_short(&port_->read_response_.data[8]) / mag_scale_; + mag_msg_->magnetic_field.z = bytes_to_short(&port_->read_response_.data[10]) / mag_scale_; mag_pub_->publish(*mag_msg_); } if (tmp_pub_->is_activated()) { tmp_msg_->header.stamp = stamp; - tmp_msg_->temperature = port_->read_response_.data[38]; + tmp_msg_->temperature = port_->read_response_.data[44] / temp_scale_; tmp_pub_->publish(*tmp_msg_); } diff --git a/test/unix_test_harness.cpp b/test/unix_test_harness.cpp index 37eea04..1fd4701 100644 --- a/test/unix_test_harness.cpp +++ b/test/unix_test_harness.cpp @@ -40,7 +40,7 @@ UnixTestHarness::UnixTestHarness() driver = std::make_unique("test_driver", node_options); - rclcpp::executor::ExecutorArgs exe_args; + rclcpp::ExecutorOptions exe_args; exe_args.context = context; executor = std::make_shared(exe_args); executor->add_node(driver->get_node_base_interface());