AP_InertialSensor: Privatize Logging

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -281,6 +281,10 @@ public:
     // indicate which bit in LOG_BITMASK indicates raw logging enabled
     void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
 
+    // Logging Functions
+    void Write_IMU() const;
+    void Write_Vibration() const;
+
     // calculate vibration levels and check for accelerometer clipping (called by a backends)
     void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
 
@@ -403,6 +407,10 @@ public:
         bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
         void push_data_to_log();
 
+        // Logging functions
+        bool Write_ISBH(const float sample_rate_hz) const;
+        bool Write_ISBD() const;
+
         uint64_t measurement_started_us;
 
         bool initialised : 1;
@@ -460,6 +468,9 @@ private:
     // save gyro calibration values to eeprom
     void _save_gyro_calibration();
 
+    // Logging function
+    void Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const;
+    
     // backend objects
     AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
 

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -0,0 +1,98 @@
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+#include <AP_Logger/AP_Logger.h>
+
+// 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
+{
+    const Vector3f &gyro = get_gyro(imu_instance);
+    const Vector3f &accel = get_accel(imu_instance);
+    const struct log_IMU pkt{
+        LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
+        time_us : time_us,
+        instance: imu_instance,
+        gyro_x  : gyro.x,
+        gyro_y  : gyro.y,
+        gyro_z  : gyro.z,
+        accel_x : accel.x,
+        accel_y : accel.y,
+        accel_z : accel.z,
+        gyro_error  : get_gyro_error_count(imu_instance),
+        accel_error : get_accel_error_count(imu_instance),
+        temperature : get_temperature(imu_instance),
+        gyro_health : (uint8_t)get_gyro_health(imu_instance),
+        accel_health : (uint8_t)get_accel_health(imu_instance),
+        gyro_rate : get_gyro_rate_hz(imu_instance),
+        accel_rate : get_accel_rate_hz(imu_instance),
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write IMU data packet for all instances
+void AP_InertialSensor::Write_IMU() const
+{
+    const uint64_t time_us = AP_HAL::micros64();
+
+    uint8_t n = MAX(get_accel_count(), get_gyro_count());
+    for (uint8_t i=0; i<n; i++) {
+        Write_IMU_instance(time_us, i);
+    }
+}
+
+// Write VIBE data packet for all instances
+void AP_InertialSensor::Write_Vibration() const
+{
+    const uint64_t time_us = AP_HAL::micros64();
+    for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
+        if (!use_accel(i)) {
+            continue;
+        }
+
+        const Vector3f vibration = get_vibration_levels(i);
+        const struct log_Vibe pkt{
+            LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
+            time_us     : time_us,
+            imu         : i,
+            vibe_x      : vibration.x,
+            vibe_y      : vibration.y,
+            vibe_z      : vibration.z,
+            clipping    : get_accel_clip_count(i)
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+    }
+}
+
+// Write information about a series of IMU readings to log:
+bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
+{
+    const struct log_ISBH pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
+        time_us        : AP_HAL::micros64(),
+        seqno          : isb_seqnum,
+        sensor_type    : (uint8_t)type,
+        instance       : instance,
+        multiplier     : multiplier,
+        sample_count   : (uint16_t)_required_count,
+        sample_us      : measurement_started_us,
+        sample_rate_hz : sample_rate_hz,
+    };
+
+    return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
+}
+
+// Write a series of IMU readings to log:
+bool AP_InertialSensor::BatchSampler::Write_ISBD() const
+{
+    struct log_ISBD pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG),
+        time_us    : AP_HAL::micros64(),
+        isb_seqno  : isb_seqnum,
+        seqno      : (uint16_t) (data_read_offset/samples_per_msg)
+    };
+    memcpy(pkt.x, &data_x[data_read_offset], sizeof(pkt.x));
+    memcpy(pkt.y, &data_y[data_read_offset], sizeof(pkt.y));
+    memcpy(pkt.z, &data_z[data_read_offset], sizeof(pkt.z));
+
+    return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
+}

libraries/AP_InertialSensor/BatchSampler.cpp

@@ -209,27 +209,18 @@ void AP_InertialSensor::BatchSampler::push_data_to_log()
             }
             break;
         }
-        if (!logger->Write_ISBH(isb_seqnum,
-                                       type,
-                                       instance,
-                                       multiplier,
-                                       _required_count,
-                                       measurement_started_us,
-                                       sample_rate)) {
+        if (!Write_ISBH(sample_rate)) {
             // buffer full?
             return;
         }
         isbh_sent = true;
     }
-    // pack and send a data packet:
-    if (!logger->Write_ISBD(isb_seqnum,
-                                   data_read_offset/samples_per_msg,
-                                   &data_x[data_read_offset],
-                                   &data_y[data_read_offset],
-                                   &data_z[data_read_offset])) {
+    // pack a nd send a data packet:
+    if (!Write_ISBD()) {
         // maybe later?!
         return;
     }
+
     data_read_offset += samples_per_msg;
     last_sent_ms = AP_HAL::millis();
     if (data_read_offset >= _required_count) {

libraries/AP_InertialSensor/LogStructure.h

@@ -0,0 +1,88 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_INERTIALSENSOR \
+    LOG_IMU_MSG, \
+    LOG_ISBH_MSG, \
+    LOG_ISBD_MSG, \
+    LOG_VIBE_MSG
+
+// @LoggerMessage: IMU
+// @Description: Inertial Measurement Unit data
+// @Field: TimeUS: Time since system startup
+// @Field: I: IMU sensor instance number
+// @Field: GyrX: measured rotation rate about X axis
+// @Field: GyrY: measured rotation rate about Y axis
+// @Field: GyrZ: measured rotation rate about Z axis
+// @Field: AccX: acceleration along X axis
+// @Field: AccY: acceleration along Y axis
+// @Field: AccZ: acceleration along Z axis
+// @Field: EG: gyroscope error count
+// @Field: EA: accelerometer error count
+// @Field: T: IMU temperature
+// @Field: GH: gyroscope health
+// @Field: AH: accelerometer health
+// @Field: GHz: gyroscope measurement rate
+// @Field: AHz: accelerometer measurement rate
+struct PACKED log_IMU {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    float gyro_x, gyro_y, gyro_z;
+    float accel_x, accel_y, accel_z;
+    uint32_t gyro_error, accel_error;
+    float temperature;
+    uint8_t gyro_health, accel_health;
+    uint16_t gyro_rate, accel_rate;
+};
+
+struct PACKED log_ISBH {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint16_t seqno;
+    uint8_t sensor_type; // e.g. GYRO or ACCEL
+    uint8_t instance;
+    uint16_t multiplier;
+    uint16_t sample_count;
+    uint64_t sample_us;
+    float sample_rate_hz;
+};
+static_assert(sizeof(log_ISBH) < 256, "log_ISBH is over-size");
+
+struct PACKED log_ISBD {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint16_t isb_seqno;
+    uint16_t seqno; // seqno within isb_seqno
+    int16_t x[32];
+    int16_t y[32];
+    int16_t z[32];
+};
+static_assert(sizeof(log_ISBD) < 256, "log_ISBD is over-size");
+
+// @LoggerMessage: VIBE
+// @Description: Processed (acceleration) vibration information
+// @Field: TimeUS: Time since system startup
+// @Field: IMU: Vibration instance number
+// @Field: VibeX: Primary accelerometer filtered vibration, x-axis
+// @Field: VibeY: Primary accelerometer filtered vibration, y-axis
+// @Field: VibeZ: Primary accelerometer filtered vibration, z-axis
+// @Field: Clip: Number of clipping events on 1st accelerometer
+struct PACKED log_Vibe {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t imu;
+    float vibe_x, vibe_y, vibe_z;
+    uint32_t clipping;
+};
+
+#define LOG_STRUCTURE_FROM_INERTIALSENSOR        \
+    { 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), \
+      "VIBE", "QBfffI", "TimeUS,IMU,VibeX,VibeY,VibeZ,Clip", "s#----", "F-----" }, \
+    { LOG_ISBH_MSG, sizeof(log_ISBH), \
+      "ISBH", "QHBBHHQf", "TimeUS,N,type,instance,mul,smp_cnt,SampleUS,smp_rate", "s-----sz", "F-----F-" },  \
+    { LOG_ISBD_MSG, sizeof(log_ISBD), \
+      "ISBD", "QHHaaa", "TimeUS,N,seqno,x,y,z", "s--ooo", "F--???" },