From 9566abb3a891d47c17ec75504aa471fa607005e6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Oct 2017 10:44:07 +1100 Subject: [PATCH] AP_InertialSensor: add IMU batch sampling --- .../AP_InertialSensor/AP_InertialSensor.cpp | 19 +- .../AP_InertialSensor/AP_InertialSensor.h | 73 ++++++ .../AP_InertialSensor_Backend.cpp | 4 + libraries/AP_InertialSensor/BatchSampler.cpp | 216 ++++++++++++++++++ 4 files changed, 311 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_InertialSensor/BatchSampler.cpp diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index c75d9623b8..090318759b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index fdfc8c8325..07b68f6712 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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 #include @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 10403eed9e..ca01d9db20 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/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); } } diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp new file mode 100644 index 0000000000..874c051ff1 --- /dev/null +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -0,0 +1,216 @@ +#include "AP_InertialSensor.h" +#include + +// 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< (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<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 +}