2017-10-03 20:44:07 -03:00
|
|
|
#include "AP_InertialSensor.h"
|
|
|
|
#include <GCS_MAVLink/GCS.h>
|
2019-04-04 07:47:52 -03:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2017-10-03 20:44:07 -03:00
|
|
|
|
|
|
|
// 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),
|
|
|
|
|
2018-03-18 20:28:33 -03:00
|
|
|
// @Param: BAT_OPT
|
|
|
|
// @DisplayName: Batch Logging Options Mask
|
|
|
|
// @Description: Options for the BatchSampler
|
2019-05-17 12:57:43 -03:00
|
|
|
// @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering
|
2018-03-07 04:09:15 -04:00
|
|
|
// @User: Advanced
|
2018-03-18 20:28:33 -03:00
|
|
|
AP_GROUPINFO("BAT_OPT", 3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
|
|
|
|
|
|
|
|
// @Param: BAT_LGIN
|
|
|
|
// @DisplayName: logging interval
|
2019-01-18 00:23:42 -04:00
|
|
|
// @Description: Interval between pushing samples to the AP_Logger log
|
2018-03-18 20:28:33 -03:00
|
|
|
// @Units: ms
|
|
|
|
// @Increment: 10
|
|
|
|
AP_GROUPINFO("BAT_LGIN", 4, AP_InertialSensor::BatchSampler, push_interval_ms, 20),
|
|
|
|
|
|
|
|
// @Param: BAT_LGCT
|
|
|
|
// @DisplayName: logging count
|
|
|
|
// @Description: Number of samples to push to count every @PREFIX@BAT_LGIN
|
|
|
|
// @Increment: 1
|
|
|
|
AP_GROUPINFO("BAT_LGCT", 5, AP_InertialSensor::BatchSampler, samples_per_msg, 32),
|
|
|
|
|
2017-10-03 20:44:07 -03:00
|
|
|
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);
|
2019-07-26 23:08:05 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", (unsigned int)total_allocation, (unsigned int)hal.util->available_memory());
|
2017-10-03 20:44:07 -03:00
|
|
|
|
|
|
|
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;
|
2019-07-26 23:08:05 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for IMU batch sampling", (unsigned int)total_allocation);
|
2017-10-03 20:44:07 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
rotate_to_next_sensor();
|
|
|
|
|
|
|
|
initialised = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor::BatchSampler::periodic()
|
|
|
|
{
|
|
|
|
if (_sensor_mask == 0) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
push_data_to_log();
|
|
|
|
}
|
|
|
|
|
2018-03-18 20:28:33 -03:00
|
|
|
void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
|
|
|
|
{
|
2019-05-25 18:01:40 -03:00
|
|
|
// We can't do post-filter sensor rate logging
|
|
|
|
if ((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER) {
|
2019-05-17 12:57:43 -03:00
|
|
|
_doing_post_filter_logging = true;
|
2019-05-25 18:01:40 -03:00
|
|
|
_doing_sensor_rate_logging = false;
|
2019-05-17 12:57:43 -03:00
|
|
|
return;
|
|
|
|
}
|
2019-05-25 18:01:40 -03:00
|
|
|
_doing_post_filter_logging = false;
|
2018-03-18 20:28:33 -03:00
|
|
|
if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) {
|
|
|
|
_doing_sensor_rate_logging = false;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
const uint8_t bit = (1<<instance);
|
|
|
|
switch (type) {
|
|
|
|
case IMU_SENSOR_TYPE_GYRO:
|
|
|
|
_doing_sensor_rate_logging = _imu._gyro_sensor_rate_sampling_enabled & bit;
|
|
|
|
break;
|
|
|
|
case IMU_SENSOR_TYPE_ACCEL:
|
|
|
|
_doing_sensor_rate_logging = _imu._accel_sensor_rate_sampling_enabled & bit;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-10-03 20:44:07 -03:00
|
|
|
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;
|
2018-03-18 20:28:33 -03:00
|
|
|
multiplier = _imu._gyro_raw_sampling_multiplier[instance];
|
|
|
|
update_doing_sensor_rate_logging();
|
2017-10-03 20:44:07 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// log for accel sensor:
|
|
|
|
type = IMU_SENSOR_TYPE_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:
|
2018-03-18 20:28:33 -03:00
|
|
|
bool haveinstance = false;
|
2017-10-03 20:44:07 -03:00
|
|
|
for (uint8_t i=instance+1; i<_count; i++) {
|
|
|
|
if (_sensor_mask & (1U<<i)) {
|
|
|
|
instance = i;
|
2018-03-18 20:28:33 -03:00
|
|
|
haveinstance = true;
|
|
|
|
break;
|
2017-10-03 20:44:07 -03:00
|
|
|
}
|
|
|
|
}
|
2018-03-18 20:28:33 -03:00
|
|
|
if (!haveinstance) {
|
|
|
|
for (uint8_t i=0; i<=instance; i++) {
|
|
|
|
if (_sensor_mask & (1U<<i)) {
|
|
|
|
instance = i;
|
|
|
|
haveinstance = true;
|
|
|
|
break;
|
|
|
|
}
|
2017-10-03 20:44:07 -03:00
|
|
|
}
|
|
|
|
}
|
2018-03-18 20:28:33 -03:00
|
|
|
if (!haveinstance) {
|
|
|
|
// should not happen!
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
abort();
|
|
|
|
#endif
|
|
|
|
instance = 0;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
multiplier = _imu._accel_raw_sampling_multiplier[instance];
|
|
|
|
update_doing_sensor_rate_logging();
|
2017-10-03 20:44:07 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
2018-03-18 20:28:33 -03:00
|
|
|
if (AP_HAL::millis() - last_sent_ms < (uint16_t)push_interval_ms) {
|
2019-01-18 00:23:42 -04:00
|
|
|
// avoid flooding AP_Logger's buffer
|
2017-10-03 20:44:07 -03:00
|
|
|
return;
|
|
|
|
}
|
2019-02-11 04:32:47 -04:00
|
|
|
AP_Logger *logger = AP_Logger::get_singleton();
|
|
|
|
if (logger == nullptr) {
|
2017-10-03 20:44:07 -03:00
|
|
|
// 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:
|
2018-03-18 20:28:33 -03:00
|
|
|
sample_rate = _imu._gyro_raw_sample_rates[instance];
|
|
|
|
if (_doing_sensor_rate_logging) {
|
|
|
|
sample_rate *= _imu._gyro_over_sampling[instance];
|
|
|
|
}
|
2017-10-03 20:44:07 -03:00
|
|
|
break;
|
|
|
|
case IMU_SENSOR_TYPE_ACCEL:
|
2018-03-18 20:28:33 -03:00
|
|
|
sample_rate = _imu._accel_raw_sample_rates[instance];
|
|
|
|
if (_doing_sensor_rate_logging) {
|
|
|
|
sample_rate *= _imu._accel_over_sampling[instance];
|
|
|
|
}
|
2017-10-03 20:44:07 -03:00
|
|
|
break;
|
|
|
|
}
|
2019-02-11 04:32:47 -04:00
|
|
|
if (!logger->Write_ISBH(isb_seqnum,
|
2017-10-03 20:44:07 -03:00
|
|
|
type,
|
|
|
|
instance,
|
|
|
|
multiplier,
|
2017-10-10 20:35:41 -03:00
|
|
|
_required_count,
|
2017-10-03 20:44:07 -03:00
|
|
|
measurement_started_us,
|
|
|
|
sample_rate)) {
|
|
|
|
// buffer full?
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
isbh_sent = true;
|
|
|
|
}
|
|
|
|
// pack and send a data packet:
|
2019-02-11 04:32:47 -04:00
|
|
|
if (!logger->Write_ISBD(isb_seqnum,
|
2017-10-03 20:44:07 -03:00
|
|
|
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;
|
|
|
|
}
|
2019-02-11 04:32:47 -04:00
|
|
|
AP_Logger *logger = AP_Logger::get_singleton();
|
|
|
|
if (logger == nullptr) {
|
2018-03-18 20:28:33 -03:00
|
|
|
return false;
|
|
|
|
}
|
2017-10-03 20:44:07 -03:00
|
|
|
#define MASK_LOG_ANY 0xFFFF
|
2019-02-11 04:32:47 -04:00
|
|
|
if (!logger->should_log(MASK_LOG_ANY)) {
|
2017-10-03 20:44:07 -03:00
|
|
|
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
|
|
|
|
}
|