Merge pull request #34 from Pitt-RAS/add-acceleration-frame-and-variance

Add acceleration frame and variance
Pitt-RAS · Mar 30, 2017 · 2e4d106 · 2e4d106
2 parents 33bf5be + d6b6fab
commit 2e4d106
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 2 deletions.
diff --git 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] = {0.05, 0.05, 0.05};
 };
 
 } // namespace FcComms

diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp
@@ -534,11 +534,18 @@ void CommonFcComms<T>::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];
   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[4] = CommonConf::kAccelerationVariance[1];
+  imu.linear_acceleration_covariance[8] = CommonConf::kAccelerationVariance[2];
+
   imu_publisher.publish(imu);
 }
 

diff --git 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<double>(*temp)/(512.0 * 8.0);
+                acc_values[i] = 9.8 * static_cast<double>(*temp)/(512.0 * 8.0);
             }
         }
     };

diff --git 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]);