From be29b8a2935c91041dfee60cf3a06a5b24fa0bef Mon Sep 17 00:00:00 2001 From: Aaron Miller Date: Mon, 27 Mar 2017 22:08:17 +0000 Subject: [PATCH 1/4] Added variance to imu message --- include/iarc7_fc_comms/CommonConf.hpp | 2 ++ include/iarc7_fc_comms/CommonFcComms.hpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/include/iarc7_fc_comms/CommonConf.hpp b/include/iarc7_fc_comms/CommonConf.hpp index 1b39bef..e1afd52 100644 --- a/include/iarc7_fc_comms/CommonConf.hpp +++ b/include/iarc7_fc_comms/CommonConf.hpp @@ -43,6 +43,8 @@ struct CommonConf static constexpr const char* kTfParentName{"level_quad"}; static constexpr const char* kTfChildName{"quad"}; + // Variance of acceleration measurements (in m/2^2) + static constexpr const double kAccelerationVariance[3] = {1, 1, 1}; }; } // namespace FcComms diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp index 61ad462..814af31 100644 --- a/include/iarc7_fc_comms/CommonFcComms.hpp +++ b/include/iarc7_fc_comms/CommonFcComms.hpp @@ -539,6 +539,12 @@ void CommonFcComms::sendAccelerations(double (&accelerations)[3]) imu.linear_acceleration.y = accelerations[1]; imu.linear_acceleration.z = accelerations[2]; + imu.orientation_covariance[0] = -1; + imu.angular_velocity_covariance[0] = -1; + imu.linear_acceleration_covariance[0] = CommonConf::kAccelerationVariance[0]; + imu.linear_acceleration_covariance[3] = CommonConf::kAccelerationVariance[1]; + imu.linear_acceleration_covariance[6] = CommonConf::kAccelerationVariance[2]; + imu_publisher.publish(imu); } From 0a20571213ab701a69f35aa8f08d46b401fff375 Mon Sep 17 00:00:00 2001 From: Aaron Miller Date: Mon, 27 Mar 2017 22:08:27 +0000 Subject: [PATCH 2/4] Added frame to imu message --- include/iarc7_fc_comms/CommonFcComms.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp index 814af31..5e7869a 100644 --- a/include/iarc7_fc_comms/CommonFcComms.hpp +++ b/include/iarc7_fc_comms/CommonFcComms.hpp @@ -534,6 +534,7 @@ void CommonFcComms::sendAccelerations(double (&accelerations)[3]) sensor_msgs::Imu imu; imu.header.stamp = ros::Time::now(); + imu.header.frame_id = CommonConf::kTfChildName; imu.linear_acceleration.x = accelerations[0]; imu.linear_acceleration.y = accelerations[1]; From 2705197b58efd4de1953f7b39d2175ccc5c50d4e Mon Sep 17 00:00:00 2001 From: Aaron Miller Date: Mon, 27 Mar 2017 22:30:57 +0000 Subject: [PATCH 3/4] Rescaled accelerations to m/s^2 from g's --- include/iarc7_fc_comms/MspCommands.hpp | 4 ++-- include/iarc7_fc_comms/MspFcComms.hpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/iarc7_fc_comms/MspCommands.hpp b/include/iarc7_fc_comms/MspCommands.hpp index d9d3ccf..29edadf 100644 --- a/include/iarc7_fc_comms/MspCommands.hpp +++ b/include/iarc7_fc_comms/MspCommands.hpp @@ -216,7 +216,7 @@ namespace FcComms uint8_t response[FcCommsMspConf::kMspMaxDataLength]; - // Returns the IMU values + // Returns the IMU values in m/s^2 void getAcc(double (&acc_values)[3]) { // Jetson runs in little endian mode and the FC @@ -238,7 +238,7 @@ namespace FcComms // We are also using a hacked up version of cleanflight that does not // apply this scaling factor using a bitshift hack before sending so // we need to do it here. - acc_values[i] = static_cast(*temp)/(512.0 * 8.0); + acc_values[i] = 9.8 * static_cast(*temp)/(512.0 * 8.0); } } }; diff --git a/include/iarc7_fc_comms/MspFcComms.hpp b/include/iarc7_fc_comms/MspFcComms.hpp index e0d5a2d..18afc0e 100644 --- a/include/iarc7_fc_comms/MspFcComms.hpp +++ b/include/iarc7_fc_comms/MspFcComms.hpp @@ -72,6 +72,7 @@ namespace FcComms FcCommsReturns __attribute__((warn_unused_result)) printRawRC(); + // Get the acceleration in m/s^2 FcCommsReturns __attribute__((warn_unused_result)) getAccelerations(double (&accelerations)[3]); From d6b6fabf0d43bc0a214e4f82cc9ab5bdffc712b6 Mon Sep 17 00:00:00 2001 From: Aaron Miller Date: Thu, 30 Mar 2017 01:03:37 +0000 Subject: [PATCH 4/4] Change acceleration covariance --- include/iarc7_fc_comms/CommonConf.hpp | 2 +- include/iarc7_fc_comms/CommonFcComms.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/iarc7_fc_comms/CommonConf.hpp b/include/iarc7_fc_comms/CommonConf.hpp index e1afd52..4f928ed 100644 --- a/include/iarc7_fc_comms/CommonConf.hpp +++ b/include/iarc7_fc_comms/CommonConf.hpp @@ -44,7 +44,7 @@ struct CommonConf static constexpr const char* kTfChildName{"quad"}; // Variance of acceleration measurements (in m/2^2) - static constexpr const double kAccelerationVariance[3] = {1, 1, 1}; + static constexpr const double kAccelerationVariance[3] = {0.05, 0.05, 0.05}; }; } // namespace FcComms diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp index 5e7869a..35f75d6 100644 --- a/include/iarc7_fc_comms/CommonFcComms.hpp +++ b/include/iarc7_fc_comms/CommonFcComms.hpp @@ -543,8 +543,8 @@ void CommonFcComms::sendAccelerations(double (&accelerations)[3]) imu.orientation_covariance[0] = -1; imu.angular_velocity_covariance[0] = -1; imu.linear_acceleration_covariance[0] = CommonConf::kAccelerationVariance[0]; - imu.linear_acceleration_covariance[3] = CommonConf::kAccelerationVariance[1]; - imu.linear_acceleration_covariance[6] = CommonConf::kAccelerationVariance[2]; + imu.linear_acceleration_covariance[4] = CommonConf::kAccelerationVariance[1]; + imu.linear_acceleration_covariance[8] = CommonConf::kAccelerationVariance[2]; imu_publisher.publish(imu); }