AP_InertialSensor: move GYR & ACC logging functions to Logging.cpp

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -306,17 +306,7 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
         return;
     }
     if (should_log_imu_raw()) {
-        uint64_t now = AP_HAL::micros64();
+        Write_GYR(instance, sample_us, gyro);
-        const struct log_GYR pkt{
-            LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
-            time_us   : now,
-            instance  : instance,
-            sample_us : sample_us?sample_us:now,
-            GyrX      : gyro.x,
-            GyrY      : gyro.y,
-            GyrZ      : gyro.z
-        };
-        logger->WriteBlock(&pkt, sizeof(pkt));
     } else {
         if (!_imu.batchsampler.doing_sensor_rate_logging()) {
             _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro);
@@ -454,17 +444,7 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s
         return;
     }
     if (should_log_imu_raw()) {
-        uint64_t now = AP_HAL::micros64();
+        Write_ACC(instance, sample_us, accel);
-        const struct log_ACC pkt {
-            LOG_PACKET_HEADER_INIT(LOG_ACC_MSG),
-            time_us   : now,
-            instance  : instance,
-            sample_us : sample_us?sample_us:now,
-            AccX      : accel.x,
-            AccY      : accel.y,
-            AccZ      : accel.z
-        };
-        logger->WriteBlock(&pkt, sizeof(pkt));
     } else {
         if (!_imu.batchsampler.doing_sensor_rate_logging()) {
             _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -342,4 +342,8 @@ private:
     void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel);
     void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo);
 
+    // logging
+    void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const; // Write ACC data packet: raw accel data
+    void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const;  // Write GYR data packet: raw gyro data
+
 };

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -3,6 +3,38 @@
 
 #include <AP_Logger/AP_Logger.h>
 
+// Write ACC data packet: raw accel data
+void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const
+{
+        const uint64_t now = AP_HAL::micros64();
+        const struct log_ACC pkt {
+            LOG_PACKET_HEADER_INIT(LOG_ACC_MSG),
+            time_us   : now,
+            instance  : instance,
+            sample_us : sample_us?sample_us:now,
+            AccX      : accel.x,
+            AccY      : accel.y,
+            AccZ      : accel.z
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write GYR data packet: raw gyro data
+void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const
+{
+        const uint64_t now = AP_HAL::micros64();
+        const struct log_GYR pkt{
+            LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
+            time_us   : now,
+            instance  : instance,
+            sample_us : sample_us?sample_us:now,
+            GyrX      : gyro.x,
+            GyrY      : gyro.y,
+            GyrZ      : gyro.z
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write IMU data packet: raw accel/gyro data
 void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const
 {

libraries/AP_InertialSensor/LogStructure.h

@@ -3,11 +3,45 @@
 #include <AP_Logger/LogStructure.h>
 
 #define LOG_IDS_FROM_INERTIALSENSOR \
+    LOG_ACC_MSG, \
+    LOG_GYR_MSG, \
     LOG_IMU_MSG, \
     LOG_ISBH_MSG, \
     LOG_ISBD_MSG, \
     LOG_VIBE_MSG
 
+// @LoggerMessage: ACC
+// @Description: IMU accelerometer data
+// @Field: TimeUS: Time since system startup
+// @Field: I: accelerometer sensor instance number
+// @Field: SampleUS: time since system startup this sample was taken
+// @Field: AccX: acceleration along X axis
+// @Field: AccY: acceleration along Y axis
+// @Field: AccZ: acceleration along Z axis
+struct PACKED log_ACC {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    uint64_t sample_us;
+    float AccX, AccY, AccZ;
+};
+
+// @LoggerMessage: GYR
+// @Description: IMU gyroscope data
+// @Field: TimeUS: Time since system startup
+// @Field: I: gyroscope sensor instance number
+// @Field: SampleUS: time since system startup this sample was taken
+// @Field: GyrX: measured rotation rate about X axis
+// @Field: GyrY: measured rotation rate about Y axis
+// @Field: GyrZ: measured rotation rate about Z axis
+struct PACKED log_GYR {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    uint64_t sample_us;
+    float GyrX, GyrY, GyrZ;
+};
+
 // @LoggerMessage: IMU
 // @Description: Inertial Measurement Unit data
 // @Field: TimeUS: Time since system startup
@@ -78,6 +112,10 @@ struct PACKED log_Vibe {
 };
 
 #define LOG_STRUCTURE_FROM_INERTIALSENSOR        \
+    { LOG_ACC_MSG, sizeof(log_ACC), \
+      "ACC", "QBQfff",        "TimeUS,I,SampleUS,AccX,AccY,AccZ", "s#sooo", "F-F000" }, \
+    { LOG_GYR_MSG, sizeof(log_GYR), \
+      "GYR", "QBQfff",        "TimeUS,I,SampleUS,GyrX,GyrY,GyrZ", "s#sEEE", "F-F000" }, \
     { LOG_IMU_MSG, sizeof(log_IMU), \
       "IMU",  "QBffffffIIfBBHH", "TimeUS,I,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,EG,EA,T,GH,AH,GHz,AHz", "s#EEEooo--O--zz", "F-000000-----00" }, \
     { LOG_VIBE_MSG, sizeof(log_Vibe), \