From d5be669f7ea0b31bebafdebdeb3264f1bcb7bc5a Mon Sep 17 00:00:00 2001 From: Johannes Barthel Date: Fri, 5 Nov 2021 23:23:30 +0100 Subject: [PATCH 1/5] change scaling factor of acceleration 1000->100, include gravity in linear_acceleration output, change euler angles to radians in config --- src/bno055_driver.cpp | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/bno055_driver.cpp b/src/bno055_driver.cpp index 36bc0d9..25787a9 100644 --- a/src/bno055_driver.cpp +++ b/src/bno055_driver.cpp @@ -112,7 +112,7 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) // Set the units port_->write_command_.address = BNO055Register::UNIT_SEL; - port_->write_command_.data[0] = 0x02; + port_->write_command_.data[0] = 0b00000110; port_->write(); // Set power mode @@ -491,40 +491,40 @@ 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]) / 900.0; + imu_msg_->angular_velocity.y = bytes_to_short(&port_->read_response_.data[14]) / 900.0; + imu_msg_->angular_velocity.z = bytes_to_short(&port_->read_response_.data[16]) / 900.0; + imu_msg_->linear_acceleration.x = bytes_to_short(&port_->read_response_.data[0]) / 100.0; + imu_msg_->linear_acceleration.y = bytes_to_short(&port_->read_response_.data[2]) / 100.0; + imu_msg_->linear_acceleration.z = bytes_to_short(&port_->read_response_.data[4]) / 100.0; + imu_msg_->orientation.w = bytes_to_short(&port_->read_response_.data[24]) / 16384.0; + imu_msg_->orientation.x = bytes_to_short(&port_->read_response_.data[26]) / 16384.0; + imu_msg_->orientation.y = bytes_to_short(&port_->read_response_.data[28]) / 16384.0; + imu_msg_->orientation.z = bytes_to_short(&port_->read_response_.data[30]) / 16384.0; 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]) / 16000000.0; + mag_msg_->magnetic_field.y = bytes_to_short(&port_->read_response_.data[8]) / 16000000.0; + mag_msg_->magnetic_field.z = bytes_to_short(&port_->read_response_.data[10]) / 16000000.0; 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]; tmp_pub_->publish(*tmp_msg_); } From a2e6a224d2ff61b37fff13dd2e99d7efffccdaca Mon Sep 17 00:00:00 2001 From: Johannes Bier Date: Sat, 6 Nov 2021 00:09:45 +0100 Subject: [PATCH 2/5] Add unit selection with ros2 params --- include/bno055_driver/bno055_driver.hpp | 5 ++ include/bno055_driver/bno055_scales.hpp | 62 ++++++++++++++ src/bno055_driver.cpp | 104 ++++++++++++++++++++---- 3 files changed, 156 insertions(+), 15 deletions(-) create mode 100644 include/bno055_driver/bno055_scales.hpp 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..5a1e070 --- /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_to_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 25787a9..da2ff64 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,71 @@ 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)) { + 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)) { + 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)) { + 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)) { + 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] = 0b00000110; + port_->write_command_.data[0] = unit_config; port_->write(); // Set power mode @@ -499,32 +571,34 @@ void BNO055Driver::publish() try if (imu_pub_->is_activated()) { imu_msg_->header.stamp = stamp; - imu_msg_->angular_velocity.x = bytes_to_short(&port_->read_response_.data[12]) / 900.0; - imu_msg_->angular_velocity.y = bytes_to_short(&port_->read_response_.data[14]) / 900.0; - imu_msg_->angular_velocity.z = bytes_to_short(&port_->read_response_.data[16]) / 900.0; - imu_msg_->linear_acceleration.x = bytes_to_short(&port_->read_response_.data[0]) / 100.0; - imu_msg_->linear_acceleration.y = bytes_to_short(&port_->read_response_.data[2]) / 100.0; - imu_msg_->linear_acceleration.z = bytes_to_short(&port_->read_response_.data[4]) / 100.0; - imu_msg_->orientation.w = bytes_to_short(&port_->read_response_.data[24]) / 16384.0; - imu_msg_->orientation.x = bytes_to_short(&port_->read_response_.data[26]) / 16384.0; - imu_msg_->orientation.y = bytes_to_short(&port_->read_response_.data[28]) / 16384.0; - imu_msg_->orientation.z = bytes_to_short(&port_->read_response_.data[30]) / 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[6]) / 16000000.0; - mag_msg_->magnetic_field.y = bytes_to_short(&port_->read_response_.data[8]) / 16000000.0; - mag_msg_->magnetic_field.z = bytes_to_short(&port_->read_response_.data[10]) / 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[44]; + tmp_msg_->temperature = port_->read_response_.data[44] / temp_scale_; tmp_pub_->publish(*tmp_msg_); } From 9ef7eed41f5268e4759f1fedb6dc0abd861eae1a Mon Sep 17 00:00:00 2001 From: Johannes Bier Date: Sat, 6 Nov 2021 00:15:25 +0100 Subject: [PATCH 3/5] Add parameter type check --- include/bno055_driver/bno055_scales.hpp | 2 +- src/bno055_driver.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/bno055_driver/bno055_scales.hpp b/include/bno055_driver/bno055_scales.hpp index 5a1e070..6cc9e24 100644 --- a/include/bno055_driver/bno055_scales.hpp +++ b/include/bno055_driver/bno055_scales.hpp @@ -46,7 +46,7 @@ namespace bno055_driver * Table 3-31: Quaternion data representation * 1 Quaternion (unit less) = 2^14 LSB */ - constexpr double quat_to_quat = 16384; + constexpr double quat = 16384; /** * Table 3-37: Temperature data representation diff --git a/src/bno055_driver.cpp b/src/bno055_driver.cpp index da2ff64..743f539 100644 --- a/src/bno055_driver.cpp +++ b/src/bno055_driver.cpp @@ -122,7 +122,8 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) // Set the units uint8_t unit_config = 0b00000110; rclcpp::Parameter angular_velocity_param; - if (get_parameter("unit.angular_velocity", 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; @@ -137,7 +138,8 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) } rclcpp::Parameter linear_acceleration_param; - if (get_parameter("unit.linear_acceleration", 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; @@ -156,7 +158,8 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) } rclcpp::Parameter magnetic_field_param; - if (get_parameter("unit.magnetic_field", 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; @@ -169,7 +172,8 @@ BNO055Driver::on_configure(const rclcpp_lifecycle::State &) } rclcpp::Parameter temperature_param; - if (get_parameter("unit.temperature", 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; From e10f01159750b2ac658b96bf00f8166ffffbcfbd Mon Sep 17 00:00:00 2001 From: Johannes Bier Date: Sat, 6 Nov 2021 00:27:39 +0100 Subject: [PATCH 4/5] =?UTF-8?q?Fix=20'warning:=20=E2=80=98using=20Executor?= =?UTF-8?q?Args=20=3D=20struct=20rclcpp::ExecutorOptions=E2=80=99=20is=20d?= =?UTF-8?q?eprecated'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/unix_test_harness.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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()); From 5e1e14e303b333facb8e832fe7bd39a9fc132e20 Mon Sep 17 00:00:00 2001 From: Johannes Bier Date: Sat, 6 Nov 2021 00:39:55 +0100 Subject: [PATCH 5/5] Add readme for parameter desc and data sheet --- README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 README.md 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