From 01002dbed3c50417fbe66d52f58c7c188fdff2fd Mon Sep 17 00:00:00 2001 From: oyvind1501 Date: Sat, 15 Feb 2020 14:11:06 +0100 Subject: [PATCH 01/11] Add callback function for IMU calibration --- src/stim300_driver_node.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 4fd98b0..0b63500 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -3,6 +3,26 @@ #include "ros/ros.h" #include "sensor_msgs/Imu.h" +#include "std_srvs/Trigger.h" +#include "std_srvs/Empty.h" + + +bool calibration_mode{false}; + + + +bool responseCalibrateIMU(std_srvs::Trigger::Request &calibration_request, std_srvs::Trigger::Response &calibration_response) +{ + + if (calibration_mode == false) + { + calibration_mode = true; + calibration_response.message = "IMU in calibration mode "; + calibration_response.success = true; + } + + return true; +} From cac5e7e1723a6a42f570f55e11168a1ec497b73d Mon Sep 17 00:00:00 2001 From: oyvind1501 Date: Sun, 16 Feb 2020 15:40:49 +0100 Subject: [PATCH 02/11] Add calibration node --- CMakeLists.txt | 3 +- package.xml | 3 ++ src/stim300_driver_node.cpp | 76 ++++++++++++++++++++++++++++++------- 3 files changed, 68 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 678a309..b4967ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,9 +20,10 @@ target_link_libraries(stim300_driver_lib) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs + std_srvs ) catkin_package( - CATKIN_DEPENDS sensor_msgs + CATKIN_DEPENDS sensor_msgs std_srvs ) add_executable(stim300_driver_node src/stim300_driver_node.cpp) diff --git a/package.xml b/package.xml index 271e7f7..5d256ea 100644 --- a/package.xml +++ b/package.xml @@ -23,10 +23,13 @@ catkin roscpp sensor_msgs + std_srvs roscpp sensor_msgs + std_srvs roscpp sensor_msgs + std_srvs diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 0b63500..8d45117 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -1,6 +1,6 @@ #include "driver_stim300.h" #include "serial_unix.h" - +#include "math.h" #include "ros/ros.h" #include "sensor_msgs/Imu.h" #include "std_srvs/Trigger.h" @@ -8,6 +8,7 @@ bool calibration_mode{false}; +constexpr int NUMBER_OF_CALIBRATION_SAMPLES{1000}; @@ -25,7 +26,6 @@ bool responseCalibrateIMU(std_srvs::Trigger::Request &calibration_request, std_s } - int main(int argc, char** argv) { ros::init(argc, argv, "stim300_driver_node"); @@ -39,7 +39,7 @@ int main(int argc, char** argv) double gravity{ 0 }; node.param("device_name", device_name, "/dev/ttyUSB0"); - node.param("variance_gyro", variance_gyro,0.0001); + node.param("variance_gyro", variance_gyro,0.0001*2*4.6*pow(10,-4)); node.param("variance_acc", variance_acc, 4.0); node.param("sample_rate", sample_rate, 125); node.param("gravity", gravity, 9.80665); @@ -61,6 +61,7 @@ int main(int argc, char** argv) ros::ServiceServer service = node.advertiseService("IMU_calibration",responseCalibrateIMU); + // New messages are sent from the sensor with sample_rate // As loop_rate determines how often we check for new data // on the serial buffer, theoretically loop_rate = sample_rate @@ -72,9 +73,28 @@ int main(int argc, char** argv) SerialUnix serial_driver(device_name, stim_const::BaudRate::BAUD_921600); DriverStim300 driver_stim300(serial_driver); - ROS_INFO("STIM300 IMU driver initialized successfully"); + int difference_in_dataGram{0}; + int count_messages{0}; + int number_of_samples{0}; + double inclination_x{0}; + double inclination_y{0}; + double inclination_z{0}; + + double average_calibration_roll{0}; + double average_calibration_pitch{0}; + double inclination_x_calibration_sum{0}; + double inclination_y_calibration_sum{0}; + double inclination_z_calibration_sum{0}; + double inclination_x_average{0}; + double inclination_y_average{0}; + double inclination_z_average{0}; + inclination_x = driver_stim300.getIncX(); + inclination_y = driver_stim300.getIncY(); + inclination_z = driver_stim300.getIncZ(); + + while (ros::ok()) { switch (driver_stim300.update()) @@ -84,15 +104,45 @@ int main(int argc, char** argv) case Stim300Status::OUTSIDE_OPERATING_CONDITIONS: ROS_DEBUG("Stim 300 outside operating conditions"); case Stim300Status::NEW_MEASURMENT: - stim300msg.header.stamp = ros::Time::now(); - stim300msg.linear_acceleration.x = driver_stim300.getAccX() * gravity; - stim300msg.linear_acceleration.y = driver_stim300.getAccY() * gravity; - stim300msg.linear_acceleration.z = driver_stim300.getAccZ() * gravity; - stim300msg.angular_velocity.x = driver_stim300.getGyroX(); - stim300msg.angular_velocity.y = driver_stim300.getGyroY(); - stim300msg.angular_velocity.z = driver_stim300.getGyroZ(); - imuSensorPublisher.publish(stim300msg); - break; + if (calibration_mode == true) + { + if(number_of_samples < NUMBER_OF_CALIBRATION_SAMPLES) + { + + inclination_x_calibration_sum += inclination_x; + inclination_y_calibration_sum += inclination_y; + inclination_z_calibration_sum += inclination_z; + + } + else + { + + inclination_x_average = inclination_x_calibration_sum/NUMBER_OF_CALIBRATION_SAMPLES; + inclination_y_average = inclination_y_calibration_sum/NUMBER_OF_CALIBRATION_SAMPLES; + inclination_z_average = inclination_z_calibration_sum/NUMBER_OF_CALIBRATION_SAMPLES; + + average_calibration_roll = atan2(inclination_y_average,inclination_z_average); + average_calibration_pitch = atan2(-inclination_x_average,sqrt(pow(inclination_y_average,2)+pow(inclination_z_average,2))); + + ROS_INFO("roll: %f", average_calibration_roll); + ROS_INFO("pitch: %f", average_calibration_pitch); + ROS_INFO("IMU Calibrated"); + calibration_mode = false; + } + break; + } + else + { + stim300msg.header.stamp = ros::Time::now(); + stim300msg.linear_acceleration.x = driver_stim300.getAccX() * gravity; + stim300msg.linear_acceleration.y = driver_stim300.getAccY() * gravity; + stim300msg.linear_acceleration.z = driver_stim300.getAccZ() * gravity; + stim300msg.angular_velocity.x = driver_stim300.getGyroX(); + stim300msg.angular_velocity.y = driver_stim300.getGyroY(); + stim300msg.angular_velocity.z = driver_stim300.getGyroZ(); + imuSensorPublisher.publish(stim300msg); + break; + } case Stim300Status::CONFIG_CHANGED: ROS_INFO("Updated Stim 300 imu config: "); ROS_INFO("%s", driver_stim300.printSensorConfig().c_str()); From f29ae45aeeef0d95fe2116f9946bc8a23a3e67d3 Mon Sep 17 00:00:00 2001 From: oyvind1501 Date: Sun, 16 Feb 2020 15:50:32 +0100 Subject: [PATCH 03/11] Debug add info --- src/stim300_driver_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 8d45117..0ce870e 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -5,7 +5,7 @@ #include "sensor_msgs/Imu.h" #include "std_srvs/Trigger.h" #include "std_srvs/Empty.h" - +#include "iostream" bool calibration_mode{false}; constexpr int NUMBER_OF_CALIBRATION_SAMPLES{1000}; @@ -123,7 +123,8 @@ int main(int argc, char** argv) average_calibration_roll = atan2(inclination_y_average,inclination_z_average); average_calibration_pitch = atan2(-inclination_x_average,sqrt(pow(inclination_y_average,2)+pow(inclination_z_average,2))); - + std::cout< Date: Sun, 16 Feb 2020 15:55:09 +0100 Subject: [PATCH 04/11] Debug add cout --- src/stim300_driver_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 0ce870e..cd3ac01 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -106,9 +106,10 @@ int main(int argc, char** argv) case Stim300Status::NEW_MEASURMENT: if (calibration_mode == true) { + std::cout<<"in calibration_mode"< Date: Sun, 16 Feb 2020 15:59:23 +0100 Subject: [PATCH 05/11] Debug add cout --- src/stim300_driver_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index cd3ac01..092ccf7 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -8,7 +8,7 @@ #include "iostream" bool calibration_mode{false}; -constexpr int NUMBER_OF_CALIBRATION_SAMPLES{1000}; +constexpr int NUMBER_OF_CALIBRATION_SAMPLES{20}; @@ -106,10 +106,10 @@ int main(int argc, char** argv) case Stim300Status::NEW_MEASURMENT: if (calibration_mode == true) { - std::cout<<"in calibration_mode"< Date: Sun, 16 Feb 2020 16:04:58 +0100 Subject: [PATCH 06/11] Debug add cout --- src/stim300_driver_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 092ccf7..b278290 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -8,7 +8,7 @@ #include "iostream" bool calibration_mode{false}; -constexpr int NUMBER_OF_CALIBRATION_SAMPLES{20}; +constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; @@ -110,6 +110,8 @@ int main(int argc, char** argv) if(number_of_samples < NUMBER_OF_CALIBRATION_SAMPLES) { //std::cout<<"in calibration_mode"< Date: Sun, 16 Feb 2020 16:09:42 +0100 Subject: [PATCH 07/11] Debug add cout --- src/stim300_driver_node.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index b278290..87851ae 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -90,9 +90,7 @@ int main(int argc, char** argv) double inclination_x_average{0}; double inclination_y_average{0}; double inclination_z_average{0}; - inclination_x = driver_stim300.getIncX(); - inclination_y = driver_stim300.getIncY(); - inclination_z = driver_stim300.getIncZ(); + while (ros::ok()) @@ -104,6 +102,9 @@ int main(int argc, char** argv) case Stim300Status::OUTSIDE_OPERATING_CONDITIONS: ROS_DEBUG("Stim 300 outside operating conditions"); case Stim300Status::NEW_MEASURMENT: + inclination_x = driver_stim300.getIncX(); + inclination_y = driver_stim300.getIncY(); + inclination_z = driver_stim300.getIncZ(); if (calibration_mode == true) { //std::cout<<"in calibration_mode"< Date: Sun, 16 Feb 2020 16:29:43 +0100 Subject: [PATCH 08/11] Add inclinometer --- src/stim300_driver_node.cpp | 38 ++++++++++++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 87851ae..32330b4 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -9,6 +9,33 @@ bool calibration_mode{false}; constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; +struct Quaternion +{ + double w, x, y, z; +}; + +struct EulerAngles { + double roll, pitch, yaw; +}; + +Quaternion FromRPYToQuaternion(EulerAngles angles) // yaw (Z), pitch (Y), roll (X) +{ + // Abbreviations for the various angular functions + double cy = cos(angles.yaw * 0.5); + double sy = sin(angles.yaw * 0.5); + double cp = cos(angles.pitch * 0.5); + double sp = sin(angles.pitch * 0.5); + double cr = cos(angles.roll * 0.5); + double sr = sin(angles.roll * 0.5); + + Quaternion q; + q.w = cy * cp * cr + sy * sp * sr; + q.x = cy * cp * sr - sy * sp * cr; + q.y = sy * cp * sr + cy * sp * cr; + q.z = sy * cp * cr - cy * sp * sr; + + return q; +} @@ -58,7 +85,7 @@ int main(int argc, char** argv) stim300msg.header.frame_id = "imu_0"; ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000); - + ros::Publisher orientationPublisher = node.advertise("imu/orientation", 1000); ros::ServiceServer service = node.advertiseService("IMU_calibration",responseCalibrateIMU); @@ -105,6 +132,8 @@ int main(int argc, char** argv) inclination_x = driver_stim300.getIncX(); inclination_y = driver_stim300.getIncY(); inclination_z = driver_stim300.getIncZ(); + Quaternion q; + EulerAngles RPY; if (calibration_mode == true) { //std::cout<<"in calibration_mode"< Date: Sat, 18 Apr 2020 17:45:25 +0200 Subject: [PATCH 09/11] Add acceleration and gyro wild point filter --- src/stim300_driver_node.cpp | 143 ++++++++++++++++++++++++++++++++++-- 1 file changed, 136 insertions(+), 7 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 32330b4..4593e45 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -9,6 +9,17 @@ bool calibration_mode{false}; constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100}; +constexpr double ACC_TOLERANCE{0.1}; +constexpr double MAX_DROPPED_ACC_X_MSG{5}; +constexpr double MAX_DROPPED_ACC_Y_MSG{5}; +constexpr double MAX_DROPPED_ACC_Z_MSG{5}; +constexpr double MAX_DROPPED_GYRO_X_MSG{5}; +constexpr double MAX_DROPPED_GYRO_Y_MSG{5}; +constexpr double MAX_DROPPED_GYRO_Z_MSG{5}; +constexpr double GYRO_X_PEAK_TO_PEAK_NOISE{0.0002}; +constexpr double GYRO_Y_PEAK_TO_PEAK_NOISE{0.0002}; +constexpr double GYRO_Z_PEAK_TO_PEAK_NOISE{0.0002}; + struct Quaternion { double w, x, y, z; @@ -85,7 +96,7 @@ int main(int argc, char** argv) stim300msg.header.frame_id = "imu_0"; ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000); - ros::Publisher orientationPublisher = node.advertise("imu/orientation", 1000); + //ros::Publisher orientationPublisher = node.advertise("imu/orientation", 1000); ros::ServiceServer service = node.advertiseService("IMU_calibration",responseCalibrateIMU); @@ -117,6 +128,23 @@ int main(int argc, char** argv) double inclination_x_average{0}; double inclination_y_average{0}; double inclination_z_average{0}; + + // Acc wild point filter + std::vector acceleration_buffer_x{}; + std::vector acceleration_buffer_y{}; + std::vector acceleration_buffer_z{}; + double dropped_acceleration_x_msg{0.0}; + double dropped_acceleration_y_msg{0.0}; + double dropped_acceleration_z_msg{0.0}; + + // Gyro wild point filter + std::vector gyro_buffer_x{}; + std::vector gyro_buffer_y{}; + std::vector gyro_buffer_z{}; + double dropped_gyro_x_msg{0.0}; + double dropped_gyro_y_msg{0.0}; + double dropped_gyro_z_msg{0.0}; + @@ -128,6 +156,7 @@ int main(int argc, char** argv) break; case Stim300Status::OUTSIDE_OPERATING_CONDITIONS: ROS_DEBUG("Stim 300 outside operating conditions"); + break; case Stim300Status::NEW_MEASURMENT: inclination_x = driver_stim300.getIncX(); inclination_y = driver_stim300.getIncY(); @@ -170,13 +199,113 @@ int main(int argc, char** argv) RPY.roll = atan2(inclination_y,inclination_z); RPY.pitch = atan2(-inclination_x,sqrt(pow(inclination_y,2)+pow(inclination_z,2))); q = FromRPYToQuaternion(RPY); + + // Acceleration wild point filter + + // Previous message + acceleration_buffer_x.push_back(driver_stim300.getAccX * gravity); + acceleration_buffer_y.push_back(driver_stim300.getAccY * gravity); + acceleration_buffer_z.push_back(driver_stim300.getAccZ * gravity); stim300msg.header.stamp = ros::Time::now(); - stim300msg.linear_acceleration.x = driver_stim300.getAccX() * gravity; - stim300msg.linear_acceleration.y = driver_stim300.getAccY() * gravity; - stim300msg.linear_acceleration.z = driver_stim300.getAccZ() * gravity; - stim300msg.angular_velocity.x = driver_stim300.getGyroX(); - stim300msg.angular_velocity.y = driver_stim300.getGyroY(); - stim300msg.angular_velocity.z = driver_stim300.getGyroZ(); + + if (acceleration_buffer_x.size() == 2 && acceleration_buffer_y.size() == 2 && acceleration_buffer_z.size() == 2) + { + if(std::abs(acceleration_buffer_x.back() - acceleration_buffer_x.front()) < ACC_TOLERANCE || dropped_acceleration_x_msg > MAX_DROPPED_ACC_X_MSG ) + { + stim300msg.linear_acceleration.x = acceleration_buffer_x.back(); + dropped_acceleration_x_msg = 0; + } + else + { + ROS_DEBUG("ACC_X_MSG wild point rejected"); + dropped_acceleration_x_msg +=1; + } + + if(std::abs(acceleration_buffer_y.back() - acceleration_buffer_y.front()) < ACC_TOLERANCE || dropped_acceleration_y_msg > MAX_DROPPED_ACC_Y_MSG ) + { + stim300msg.linear_acceleration.y = acceleration_buffer_y.back(); + dropped_acceleration_y_msg = 0; + } + else + { + ROS_DEBUG("ACC_Y_MSG wild point rejected"); + dropped_acceleration_y_msg +=1; + } + + if(std::abs(acceleration_buffer_z.back() - acceleration_buffer_z.front()) < ACC_TOLERANCE || dropped_acceleration_z_msg > MAX_DROPPED_ACC_Z_MSG ) + { + stim300msg.linear_acceleration.z = acceleration_buffer_z.back(); + dropped_acceleration_z_msg = 0; + } + else + { + ROS_DEBUG("ACC_Z_MSG wild point rejected"); + dropped_acceleration_z_msg +=1; + } + // Empty acceleration buffers + acceleration_buffer_x = std::vector{acceleration_buffer_x.back()}; + acceleration_buffer_y = std::vector{acceleration_buffer_y.back()}; + acceleration_buffer_z = std::vector{acceleration_buffer_z.back()}; + } + else + { + stim300msg.linear_acceleration.x = driver_stim300.getAccX() * gravity; + stim300msg.linear_acceleration.y = driver_stim300.getAccY() * gravity; + stim300msg.linear_acceleration.z = driver_stim300.getAccZ() * gravity; + } + + // Gyro wild point filter + gyro_buffer_x.push_back(driver_stim300.getGyroX()); + gyro_buffer_y.push_back(driver_stim300.getGyroY()); + gyro_buffer_z.push_back(driver_stim300.getGyroZ()); + + if(gyro_buffer_x.size() == 2 && gyro_buffer_y.size() == 2 && gyro_buffer_z.size() == 2) + { + + if(std::abs(gyro_buffer_x.back() - gyro_buffer_x.front()) < std::max(2*std::abs(gyro_buffer_x.front()),GYRO_X_PEAK_TO_PEAK_NOISE) || dropped_gyro_x_msg > MAX_DROPPED_GYRO_X_MSG) + { + stim300msg.angular_velocity.x = gyro_buffer_x.back(); + dropped_gyro_x_msg = 0; + } + else + { + ROS_DEBUG("GYRO_X_MSG wild point rejected"); + dropped_gyro_x_msg += 1; + } + + if(std::abs(gyro_buffer_y.back() - gyro_buffer_y.front()) < std::max(2*std::abs(gyro_buffer_y.front()),GYRO_Y_PEAK_TO_PEAK_NOISE) || dropped_gyro_y_msg > MAX_DROPPED_GYRO_Y_MSG) + { + stim300msg.angular_velocity.x = gyro_buffer_y.back(); + dropped_gyro_y_msg = 0; + } + else + { + ROS_DEBUG("GYRO_Y_MSG wild point rejected"); + dropped_gyro_y_msg += 1; + } + + if(std::abs(gyro_buffer_z.back() - gyro_buffer_z.front()) < std::max(2*std::abs(gyro_buffer_z.front()),GYRO_Z_PEAK_TO_PEAK_NOISE) || dropped_gyro_z_msg > MAX_DROPPED_GYRO_Z_MSG) + { + stim300msg.angular_velocity.z = gyro_buffer_z.back(); + dropped_gyro_z_msg = 0; + } + else + { + ROS_DEBUG("GYRO_Z_MSG wild point rejected"); + dropped_gyro_z_msg += 1; + } + + // Empty buffers + gyro_buffer_x = std::vector{gyro_buffer_x.back()}; + gyro_buffer_y = std::vector{gyro_buffer_y.back()}; + gyro_buffer_z = std::vector{gyro_buffer_z.back()}; + } + else + { + stim300msg.angular_velocity.x = driver_stim300.getGyroX(); + stim300msg.angular_velocity.y = driver_stim300.getGyroY(); + stim300msg.angular_velocity.z = driver_stim300.getGyroZ(); + } stim300msg.orientation.w = q.w; stim300msg.orientation.x = q.x; stim300msg.orientation.y = q.y; From b784d161509918aa7f4331fe3908afbfe75d63f8 Mon Sep 17 00:00:00 2001 From: Beluga Xavier Date: Sat, 12 Mar 2022 01:59:33 +0100 Subject: [PATCH 10/11] removed -1 in covariance matrix and changed acc covariance --- src/stim300_driver_node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 4593e45..0afea57 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -76,14 +76,14 @@ int main(int argc, char** argv) int sample_rate{ 0 }; double gravity{ 0 }; + node.param("device_name", device_name, "/dev/ttyUSB0"); node.param("variance_gyro", variance_gyro,0.0001*2*4.6*pow(10,-4)); - node.param("variance_acc", variance_acc, 4.0); + node.param("variance_acc", variance_acc, 0.000055); node.param("sample_rate", sample_rate, 125); node.param("gravity", gravity, 9.80665); sensor_msgs::Imu stim300msg{}; - stim300msg.orientation_covariance[0] = -1; stim300msg.angular_velocity_covariance[0] = variance_gyro; stim300msg.angular_velocity_covariance[4] = variance_gyro; stim300msg.angular_velocity_covariance[8] = variance_gyro; @@ -203,9 +203,9 @@ int main(int argc, char** argv) // Acceleration wild point filter // Previous message - acceleration_buffer_x.push_back(driver_stim300.getAccX * gravity); - acceleration_buffer_y.push_back(driver_stim300.getAccY * gravity); - acceleration_buffer_z.push_back(driver_stim300.getAccZ * gravity); + acceleration_buffer_x.push_back(driver_stim300.getAccX() * gravity); + acceleration_buffer_y.push_back(driver_stim300.getAccY() * gravity); + acceleration_buffer_z.push_back(driver_stim300.getAccZ() * gravity); stim300msg.header.stamp = ros::Time::now(); if (acceleration_buffer_x.size() == 2 && acceleration_buffer_y.size() == 2 && acceleration_buffer_z.size() == 2) From cd3d5966a1f6e3ce4834c2f70971f5877ae2f663 Mon Sep 17 00:00:00 2001 From: Beluga Xavier Date: Tue, 12 Apr 2022 15:27:10 +0200 Subject: [PATCH 11/11] changed covariance to calculated values from matlab --- src/stim300_driver_node.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 0afea57..d6e50fe 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -83,16 +83,17 @@ int main(int argc, char** argv) node.param("sample_rate", sample_rate, 125); node.param("gravity", gravity, 9.80665); + // These values have been estimated by having beluga in a pool for a couple of minutes, and then calculate the variance for each values sensor_msgs::Imu stim300msg{}; - stim300msg.angular_velocity_covariance[0] = variance_gyro; - stim300msg.angular_velocity_covariance[4] = variance_gyro; - stim300msg.angular_velocity_covariance[8] = variance_gyro; - stim300msg.linear_acceleration_covariance[0] = variance_acc; - stim300msg.linear_acceleration_covariance[4] = variance_acc; - stim300msg.linear_acceleration_covariance[8] = variance_acc; - stim300msg.orientation.x = 0; - stim300msg.orientation.y = 0; - stim300msg.orientation.z = 0; + stim300msg.angular_velocity_covariance[0] = 0.0000027474; + stim300msg.angular_velocity_covariance[4] = 0.0000027474; + stim300msg.angular_velocity_covariance[8] = 0.000007312; + stim300msg.linear_acceleration_covariance[0] = 0.00041915; + stim300msg.linear_acceleration_covariance[4] = 0.00041915; + stim300msg.linear_acceleration_covariance[8] = 0.000018995; + stim300msg.orientation.x = 0.00000024358; + stim300msg.orientation.y = 0.00000024358; + stim300msg.orientation.z = 0.00000024358; stim300msg.header.frame_id = "imu_0"; ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000);