AP_InertialSensor: add IMU batch sampling

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -429,7 +429,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @Description: Gyro notch filter
     // @User: Advanced
     AP_SUBGROUPINFO(_notch_filter, "NOTCH_",  37, AP_InertialSensor, NotchFilterVector3fParam),
-    
+
+    // @Param: LOG_
+    // @DisplayName: Log Settings
+    // @Description: Log Settings
+    // @User: Advanced
+    AP_SUBGROUPINFO(batchsampler, "LOG_",  39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -651,6 +657,9 @@ AP_InertialSensor::init(uint16_t sample_rate)
     _next_sample_usec = 0;
     _last_sample_usec = 0;
     _have_sample = false;
+
+    // initialise IMU batch logging
+    batchsampler.init();
 }
 
 bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
@@ -834,6 +843,14 @@ AP_InertialSensor::detect_backends(void)
     }
 }
 
+// Armed, Copter, PixHawk:
+// ins_periodic: 57500 events, 0 overruns, 208754us elapsed, 3us avg, min 1us max 218us 40.662us rms
+void AP_InertialSensor::periodic()
+{
+    batchsampler.periodic();
+}
+
+
 /*
   _calculate_trim - calculates the x and y trim angles. The
   accel_sample must be correctly scaled, offset and oriented for the

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -16,6 +16,8 @@
 #define INS_MAX_BACKENDS  6
 #define INS_VIBRATION_CHECK_INSTANCES 2
 
+#define DEFAULT_IMU_LOG_BAT_MASK 0
+
 #include <stdint.h>
 
 #include <AP_AccelCal/AP_AccelCal.h>
@@ -77,6 +79,9 @@ public:
     uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id);
     uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id);
 
+    // a function called by the main thread at the main loop rate:
+    void periodic();
+
     bool calibrate_trim(float &trim_roll, float &trim_pitch);
 
     /// calibrating - returns true if the gyros or accels are currently being calibrated
@@ -272,6 +277,74 @@ public:
     // return time in microseconds of last update() call
     uint32_t get_last_update_usec(void) const { return _last_update_usec; }
 
+    enum IMU_SENSOR_TYPE {
+        IMU_SENSOR_TYPE_ACCEL = 0,
+        IMU_SENSOR_TYPE_GYRO = 1,
+    };
+
+    class BatchSampler {
+    public:
+        BatchSampler(const AP_InertialSensor &imu) :
+            type(IMU_SENSOR_TYPE_ACCEL),
+            _imu(imu) {
+            AP_Param::setup_object_defaults(this, var_info);
+        };
+
+        void init();
+        void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample);
+
+        // a function called by the main thread at the main loop rate:
+        void periodic();
+
+        // class level parameters
+        static const struct AP_Param::GroupInfo var_info[];
+
+        // Parameters
+        AP_Int16 _required_count;
+        AP_Int8 _sensor_mask;
+        // end Parameters
+
+    private:
+
+        void rotate_to_next_sensor();
+
+        bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
+        void push_data_to_log();
+
+        uint64_t measurement_started_us;
+
+        bool initialised : 1;
+        bool isbh_sent : 1;
+        uint8_t instance : 3; // instance we are sending data for
+        AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
+        uint16_t isb_seqnum;
+        int16_t *data_x;
+        int16_t *data_y;
+        int16_t *data_z;
+        uint16_t data_write_offset; // units: samples
+        uint16_t data_read_offset; // units: samples
+        uint32_t last_sent_ms;
+
+        // all samples are multiplied by this
+        static const uint16_t multiplier_accel = INT16_MAX/radians(2000);
+        static const uint16_t multiplier_gyro = INT16_MAX/(16*GRAVITY_MSS);
+        uint16_t multiplier = multiplier_accel;
+
+        // push blocks to DataFlash at regular intervals.  each
+        // message is ~ 108 bytes in size, so we use about 1kB/s of
+        // logging bandwidth with a 100ms interval.  If we are taking
+        // 1024 samples then we need to send 32 packets, so it will
+        // take ~3 seconds to push a complete batch to the log.  If
+        // you are running a on an FMU with three IMUs then you
+        // will loop back around to the first sensor after about
+        // twenty seconds.
+        const uint8_t push_interval_ms = 100;
+        const uint16_t samples_per_msg = 32;
+
+        const AP_InertialSensor &_imu;
+    };
+    BatchSampler batchsampler{*this};
+
 private:
     AP_InertialSensor();
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -221,6 +221,8 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
             GyrZ      : gyro.z
         };
         dataflash->WriteBlock(&pkt, sizeof(pkt));
+    } else {
+        _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro);
     }
 }
 
@@ -329,6 +331,8 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s
             AccZ      : accel.z
         };
         dataflash->WriteBlock(&pkt, sizeof(pkt));
+    } else {
+        _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);
     }
 }
 

libraries/AP_InertialSensor/BatchSampler.cpp

@@ -0,0 +1,216 @@
+#include "AP_InertialSensor.h"
+#include <GCS_MAVLink/GCS.h>
+
+// Class level parameters
+const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
+    // @Param: BAT_CNT
+    // @DisplayName: sample count per batch
+    // @Description: Number of samples to take when logging streams of IMU sensor readings.  Will be rounded down to a multiple of 32.
+    // @User: Advanced
+    // @Increment: 32
+    AP_GROUPINFO("BAT_CNT",  1, AP_InertialSensor::BatchSampler, _required_count,   1024),
+
+    // @Param: BAT_MASK
+    // @DisplayName: Sensor Bitmask
+    // @Description: Bitmap of which IMUs to log batch data for
+    // @User: Advanced
+    // @Values: 0:None,1:First IMU,255:All
+    // @Bitmask: 0:IMU1,1:IMU2,2:IMU3
+    AP_GROUPINFO("BAT_MASK",  2, AP_InertialSensor::BatchSampler, _sensor_mask,   DEFAULT_IMU_LOG_BAT_MASK),
+
+    AP_GROUPEND
+};
+
+
+extern const AP_HAL::HAL& hal;
+void AP_InertialSensor::BatchSampler::init()
+{
+    if (_sensor_mask == 0) {
+        return;
+    }
+    if (_required_count <= 0) {
+        return;
+    }
+
+    _required_count -= _required_count % 32; // round down to nearest multiple of 32
+
+    const uint32_t total_allocation = 3*_required_count*sizeof(uint16_t);
+    gcs().send_text(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", total_allocation, hal.util->available_memory());
+
+    data_x = (int16_t*)calloc(_required_count, sizeof(int16_t));
+    data_y = (int16_t*)calloc(_required_count, sizeof(int16_t));
+    data_z = (int16_t*)calloc(_required_count, sizeof(int16_t));
+    if (data_x == nullptr || data_y == nullptr || data_z == nullptr) {
+        free(data_x);
+        free(data_y);
+        free(data_z);
+        data_x = nullptr;
+        data_y = nullptr;
+        data_z = nullptr;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for IMU batch sampling", total_allocation);
+        return;
+    }
+
+    rotate_to_next_sensor();
+
+    initialised = true;
+}
+
+void AP_InertialSensor::BatchSampler::periodic()
+{
+    if (_sensor_mask == 0) {
+        return;
+    }
+    push_data_to_log();
+}
+
+void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
+{
+    if (_sensor_mask == 0) {
+        // should not have been called
+        return;
+    }
+    if ((1U<<instance) > (uint8_t)_sensor_mask) {
+        // should only ever happen if user resets _sensor_mask
+        instance = 0;
+    }
+
+    if (type == IMU_SENSOR_TYPE_ACCEL) {
+        // we have logged accelerometers, now log gyros:
+        type = IMU_SENSOR_TYPE_GYRO;
+        multiplier = multiplier_gyro;
+        return;
+    }
+
+    // log for accel sensor:
+    type = IMU_SENSOR_TYPE_ACCEL;
+    multiplier = multiplier_accel;
+
+    // move to next IMU backend:
+
+    // we assume the number of gyros and accels is the same, taking
+    // this minimum stops us doing bad things if that isn't true:
+    const uint8_t _count = MIN(_imu._accel_count, _imu._gyro_count);
+
+    // find next backend instance to log:
+    for (uint8_t i=instance+1; i<_count; i++) {
+        if (_sensor_mask & (1U<<i)) {
+            instance = i;
+            return;
+        }
+    }
+    for (uint8_t i=0; i<instance; i++) {
+        if (_sensor_mask & (1U<<i)) {
+            instance = i;
+            return;
+        }
+    }
+}
+
+void AP_InertialSensor::BatchSampler::push_data_to_log()
+{
+    if (!initialised) {
+        return;
+    }
+    if (_sensor_mask == 0) {
+        return;
+    }
+    if (data_write_offset - data_read_offset < samples_per_msg) {
+        // insuffucient data to pack a packet
+        return;
+    }
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_sent_ms < push_interval_ms) {
+        // avoid flooding DataFlash's buffer
+        return;
+    }
+    DataFlash_Class *dataflash = DataFlash_Class::instance();
+    if (dataflash == nullptr) {
+        // should not have been called
+        return;
+    }
+
+    // possibly send isb header:
+    if (!isbh_sent && data_read_offset == 0) {
+        float sample_rate = 0; // avoid warning about uninitialised values
+        switch(type) {
+        case IMU_SENSOR_TYPE_GYRO:
+            sample_rate = _imu._gyro_raw_sample_rates[instance];
+            break;
+        case IMU_SENSOR_TYPE_ACCEL:
+            sample_rate = _imu._accel_raw_sample_rates[instance];
+            break;
+        }
+        if (!dataflash->Log_Write_ISBH(isb_seqnum,
+                                       type,
+                                       instance,
+                                       multiplier,
+                                       measurement_started_us,
+                                       sample_rate)) {
+            // buffer full?
+            return;
+        }
+        isbh_sent = true;
+    }
+    // pack and send a data packet:
+    if (!dataflash->Log_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])) {
+        // maybe later?!
+        return;
+    }
+    data_read_offset += samples_per_msg;
+    last_sent_ms = AP_HAL::millis();
+    if (data_read_offset >= _required_count) {
+        // that was the last one.  Clean up:
+        data_read_offset = 0;
+        isb_seqnum++;
+        isbh_sent = false;
+        // rotate to next instance:
+        rotate_to_next_sensor();
+        data_write_offset = 0; // unlocks writing process
+    }
+}
+
+bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_TYPE _type)
+{
+    if (_sensor_mask == 0) {
+        return false;
+    }
+    if (!initialised) {
+        return false;
+    }
+    if (_instance != instance) {
+        return false;
+    }
+    if (_type != type) {
+        return false;
+    }
+    if (data_write_offset >= _required_count) {
+        return false;
+    }
+    DataFlash_Class *dataflash = DataFlash_Class::instance();
+#define MASK_LOG_ANY                    0xFFFF
+    if (!dataflash->should_log(MASK_LOG_ANY)) {
+        return false;
+    }
+    return true;
+}
+
+void AP_InertialSensor::BatchSampler::sample(uint8_t _instance, AP_InertialSensor::IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &_sample)
+{
+    if (!should_log(_instance, _type)) {
+        return;
+    }
+    if (data_write_offset == 0) {
+        measurement_started_us = sample_us;
+    }
+
+    data_x[data_write_offset] = multiplier*_sample.x;
+    data_y[data_write_offset] = multiplier*_sample.y;
+    data_z[data_write_offset] = multiplier*_sample.z;
+
+    data_write_offset++; // may unblock the reading process
+}