aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: added support for more ADIS IMUs

Browse Source 
support 32 bit delta angles and velocities
This commit is contained in:
Andrew Tridgell 2021-12-14 13:44:21 +11:00
parent f637facea2
commit 662327f2ea
4 changed files with 534 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

354
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
View File

@ -20,14 +20,13 @@
#include "AP_InertialSensor_ADIS1647x.h" #include "AP_InertialSensor_ADIS1647x.h"
#define BACKEND_SAMPLE_RATE 2000
/* /*
device registers device registers
*/ */
#define REG_PROD_ID 0x72 #define REG_PROD_ID 0x72
#define PROD_ID_16470 0x4056 #define PROD_ID_16470 0x4056
#define PROD_ID_16477 0x405d #define PROD_ID_16477 0x405d
#define PROD_ID_16507 0x407b
#define REG_GLOB_CMD 0x68 #define REG_GLOB_CMD 0x68
#define GLOB_CMD_SW_RESET 0x80 #define GLOB_CMD_SW_RESET 0x80
@ -36,11 +35,28 @@
#define REG_DATA_CNTR 0x22 #define REG_DATA_CNTR 0x22
#define REG_MSC_CTRL 0x60
# define REG_MSC_CTRL_BURST32 0x200
# define REG_MSC_CTRL_BURSTSEL 0x100
# define REG_MSC_CTRL_GCOMP 0x080
# define REG_MSC_CTRL_PCOMP 0x040
# define REG_MSC_CTRL_SENSBW 0x010
# define REG_MSC_CTRL_DRPOL 0x001
#define REG_DEC_RATE 0x64
# define REG_DEC_RATE_2000Hz 0
# define REG_DEC_RATE_1000Hz 1
# define REG_DEC_RATE_666Hz 2
# define REG_DEC_RATE_500Hz 3
# define REG_DEC_RATE_400Hz 4
#define REG_FILT_CTRL 0x5c
/* /*
timings timings
*/ */
#define T_STALL_US 20U #define T_STALL_US 20U
#define T_RESET_MS 250U #define T_RESET_MS 500U
#define TIMING_DEBUG 0 #define TIMING_DEBUG 0
#if TIMING_DEBUG #if TIMING_DEBUG
@ -89,8 +105,8 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
void AP_InertialSensor_ADIS1647x::start() void AP_InertialSensor_ADIS1647x::start()
{ {
if (!_imu.register_accel(accel_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) || if (!_imu.register_accel(accel_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) ||
!_imu.register_gyro(gyro_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) { !_imu.register_gyro(gyro_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) {
return; return;
} }
@ -113,21 +129,25 @@ void AP_InertialSensor_ADIS1647x::start()
/* /*
check product ID check product ID
*/ */
bool AP_InertialSensor_ADIS1647x::check_product_id(void) bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id)
{ {
uint16_t prod_id = read_reg16(REG_PROD_ID); prod_id = read_reg16(REG_PROD_ID);
switch (prod_id) { switch (prod_id) {
case PROD_ID_16470: case PROD_ID_16470:
// can do up to 40G // can do up to 40G
opmode = OpMode::Basic;
accel_scale = 1.25 * GRAVITY_MSS * 0.001; accel_scale = 1.25 * GRAVITY_MSS * 0.001;
_clip_limit = 39.5f * GRAVITY_MSS; _clip_limit = 39.5f * GRAVITY_MSS;
gyro_scale = radians(0.1); gyro_scale = radians(0.1);
expected_sample_rate_hz = 2000;
return true; return true;
case PROD_ID_16477: case PROD_ID_16477: {
// can do up to 40G // can do up to 40G
opmode = OpMode::Basic;
accel_scale = 1.25 * GRAVITY_MSS * 0.001; accel_scale = 1.25 * GRAVITY_MSS * 0.001;
_clip_limit = 39.5f * GRAVITY_MSS; _clip_limit = 39.5f * GRAVITY_MSS;
expected_sample_rate_hz = 2000;
// RANG_MDL register used for gyro range // RANG_MDL register used for gyro range
uint16_t rang_mdl = read_reg16(REG_RANG_MDL); uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
switch ((rang_mdl >> 2) & 3) { switch ((rang_mdl >> 2) & 3) {
@ -145,6 +165,39 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
} }
return true; return true;
} }
case PROD_ID_16507: {
opmode = OpMode::Delta32;
expected_sample_rate_hz = 1200;
accel_scale = 392.0 / 2097152000.0;
dvel_scale = 400.0 / 0x7FFFFFFF;
_clip_limit = 39.5f * GRAVITY_MSS;
// RANG_MDL register used for gyro range
uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
switch ((rang_mdl >> 2) & 3) {
case 0:
gyro_scale = radians(125) / 0x4E200000;
dangle_scale = radians(360.0 / 0x7FFFFFFF);
break;
case 1:
gyro_scale = radians(500) / 0x4E200000;
dangle_scale = radians(720.0 / 0x7FFFFFFF);
break;
case 3:
gyro_scale = radians(2000) / 0x4E200000;
dangle_scale = radians(2160.0 / 0x7FFFFFFF);
break;
default:
return false;
}
if (opmode == OpMode::Basic) {
accel_scale *= 0x10000;
gyro_scale *= 0x10000;
}
return true;
}
}
return false; return false;
} }
@ -152,20 +205,52 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
bool AP_InertialSensor_ADIS1647x::init() bool AP_InertialSensor_ADIS1647x::init()
{ {
WITH_SEMAPHORE(dev->get_semaphore()); WITH_SEMAPHORE(dev->get_semaphore());
if (!check_product_id()) {
uint8_t tries = 10;
uint16_t prod_id = 0;
do {
// perform software reset
write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
hal.scheduler->delay(100);
} while (!check_product_id(prod_id) && --tries);
if (tries == 0) {
return false; return false;
} }
// perform software reset // bring rate down
write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET); if (expected_sample_rate_hz < 450) {
hal.scheduler->delay(T_RESET_MS); if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_400Hz, true)) {
return false;
// re-check after reset }
if (!check_product_id()) { } else if (expected_sample_rate_hz < 600) {
return false; if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_500Hz, true)) {
return false;
}
} else if (expected_sample_rate_hz < 700) {
if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_666Hz, true)) {
return false;
}
} else if (expected_sample_rate_hz < 1500) {
if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_1000Hz, true)) {
return false;
}
} }
// we leave all config registers at defaults if (!write_reg16(REG_FILT_CTRL, 0, true)) {
return false;
}
// choose burst type and compensation
uint16_t msc_ctrl = REG_MSC_CTRL_GCOMP | REG_MSC_CTRL_PCOMP | REG_MSC_CTRL_DRPOL;
if (opmode == OpMode::Delta32) {
msc_ctrl |= REG_MSC_CTRL_BURST32 | REG_MSC_CTRL_BURSTSEL;
} else if (opmode == OpMode::AG32) {
msc_ctrl |= REG_MSC_CTRL_BURST32;
}
if (!write_reg16(REG_MSC_CTRL, msc_ctrl, true)) {
return true;
}
#if TIMING_DEBUG #if TIMING_DEBUG
// useful for debugging scheduling of transfers // useful for debugging scheduling of transfers
@ -200,24 +285,32 @@ uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const
/* /*
write a 16 bit register value write a 16 bit register value
*/ */
void AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value) const bool AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value, bool confirm) const
{ {
uint8_t req[2]; const uint8_t retries = 16;
req[0] = (regnum | 0x80); for (uint8_t i=0; i<retries; i++) {
req[1] = value & 0xFF; uint8_t req[2];
dev->transfer(req, sizeof(req), nullptr, 0); req[0] = (regnum | 0x80);
hal.scheduler->delay_microseconds(T_STALL_US); req[1] = value & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0);
hal.scheduler->delay_microseconds(T_STALL_US);
req[0] = ((regnum+1) | 0x80); req[0] = ((regnum+1) | 0x80);
req[1] = (value>>8) & 0xFF; req[1] = (value>>8) & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0); dev->transfer(req, sizeof(req), nullptr, 0);
hal.scheduler->delay_microseconds(T_STALL_US); hal.scheduler->delay_microseconds(T_STALL_US);
if (!confirm || read_reg16(regnum) == value) {
return true;
}
}
return false;
} }
/* /*
loop to read the sensor read the sensor using 16 bit burst transfer of gyro/accel data
*/ */
void AP_InertialSensor_ADIS1647x::read_sensor(void) void AP_InertialSensor_ADIS1647x::read_sensor16(void)
{ {
struct adis_data { struct adis_data {
uint8_t cmd[2]; uint8_t cmd[2];
@ -234,7 +327,6 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
uint8_t checksum; uint8_t checksum;
} data {}; } data {};
uint64_t sample_start_us = AP_HAL::micros64();
do { do {
WITH_SEMAPHORE(dev->get_semaphore()); WITH_SEMAPHORE(dev->get_semaphore());
data.cmd[0] = REG_GLOB_CMD; data.cmd[0] = REG_GLOB_CMD;
@ -281,10 +373,196 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
gyro *= gyro_scale; gyro *= gyro_scale;
_rotate_and_correct_accel(accel_instance, accel); _rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel, sample_start_us); _notify_new_accel_raw_sample(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro); _rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro, sample_start_us); _notify_new_gyro_raw_sample(gyro_instance, gyro);
/*
publish average temperature at 20Hz
*/
temp_sum += float(int16_t(be16toh(data.temp))*0.1);
temp_count++;
if (temp_count == 100) {
_publish_temperature(accel_instance, temp_sum/temp_count);
temp_sum = 0;
temp_count = 0;
}
DEBUG_SET_PIN(1, 0);
}
/*
read the sensor using 32 bit burst transfer of accel/gyro
*/
void AP_InertialSensor_ADIS1647x::read_sensor32(void)
{
struct adis_data {
uint8_t cmd[2];
uint16_t diag_stat;
uint16_t gx_low;
uint16_t gx_high;
uint16_t gy_low;
uint16_t gy_high;
uint16_t gz_low;
uint16_t gz_high;
uint16_t ax_low;
uint16_t ax_high;
uint16_t ay_low;
uint16_t ay_high;
uint16_t az_low;
uint16_t az_high;
uint16_t temp;
uint16_t counter;
uint8_t pad;
uint8_t checksum;
} data {};
do {
WITH_SEMAPHORE(dev->get_semaphore());
data.cmd[0] = REG_GLOB_CMD;
DEBUG_SET_PIN(2, 1);
if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
break;
}
DEBUG_SET_PIN(2, 0);
} while (be16toh(data.counter) == last_counter);
DEBUG_SET_PIN(1, 1);
/*
check the 8 bit checksum of the packet
*/
uint8_t sum = 0;
const uint8_t *b = (const uint8_t *)&data.diag_stat;
for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
sum += b[i];
}
if (sum != data.checksum) {
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
// corrupt data
return;
}
/*
check if we have lost a sample
*/
uint16_t counter = be16toh(data.counter);
if (done_first_read && uint16_t(last_counter+1) != counter) {
DEBUG_TOGGLE_PIN(3);
}
done_first_read = true;
last_counter = counter;
Vector3f accel{float(accel_scale*int32_t(be16toh(data.ax_low) | (be16toh(data.ax_high)<<16))),
-float(accel_scale*int32_t(be16toh(data.ay_low) | (be16toh(data.ay_high)<<16))),
-float(accel_scale*int32_t(be16toh(data.az_low) | (be16toh(data.az_high)<<16)))};
Vector3f gyro{float(gyro_scale*int32_t(be16toh(data.gx_low) | (be16toh(data.gx_high)<<16))),
-float(gyro_scale*int32_t(be16toh(data.gy_low) | (be16toh(data.gy_high)<<16))),
-float(gyro_scale*int32_t(be16toh(data.gz_low) | (be16toh(data.gz_high)<<16)))};
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
/*
publish average temperature at 20Hz
*/
temp_sum += float(int16_t(be16toh(data.temp))*0.1);
temp_count++;
if (temp_count == 100) {
_publish_temperature(accel_instance, temp_sum/temp_count);
temp_sum = 0;
temp_count = 0;
}
DEBUG_SET_PIN(1, 0);
}
/*
read the sensor using 32 bit burst transfer of delta-angle/delta-velocity
*/
void AP_InertialSensor_ADIS1647x::read_sensor32_delta(void)
{
struct adis_data {
uint8_t cmd[2];
uint16_t diag_stat;
uint16_t dax_low;
uint16_t dax_high;
uint16_t day_low;
uint16_t day_high;
uint16_t daz_low;
uint16_t daz_high;
uint16_t dvx_low;
uint16_t dvx_high;
uint16_t dvy_low;
uint16_t dvy_high;
uint16_t dvz_low;
uint16_t dvz_high;
uint16_t temp;
uint16_t counter;
uint8_t pad;
uint8_t checksum;
} data {};
do {
WITH_SEMAPHORE(dev->get_semaphore());
data.cmd[0] = REG_GLOB_CMD;
DEBUG_SET_PIN(2, 1);
if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
break;
}
DEBUG_SET_PIN(2, 0);
} while (be16toh(data.counter) == last_counter);
DEBUG_SET_PIN(1, 1);
/*
check the 8 bit checksum of the packet
*/
uint8_t sum = 0;
const uint8_t *b = (const uint8_t *)&data.diag_stat;
for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
sum += b[i];
}
if (sum != data.checksum) {
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
DEBUG_TOGGLE_PIN(3);
// corrupt data
return;
}
/*
check if we have lost a sample
*/
uint16_t counter = be16toh(data.counter);
if (done_first_read && uint16_t(last_counter+1) != counter) {
DEBUG_TOGGLE_PIN(3);
}
done_first_read = true;
last_counter = counter;
Vector3f dvel{float(dvel_scale*int32_t(be16toh(data.dvx_low) | (be16toh(data.dvx_high)<<16))),
-float(dvel_scale*int32_t(be16toh(data.dvy_low) | (be16toh(data.dvy_high)<<16))),
-float(dvel_scale*int32_t(be16toh(data.dvz_low) | (be16toh(data.dvz_high)<<16)))};
Vector3f dangle{float(dangle_scale*int32_t(be16toh(data.dax_low) | (be16toh(data.dax_high)<<16))),
-float(dangle_scale*int32_t(be16toh(data.day_low) | (be16toh(data.day_high)<<16))),
-float(dangle_scale*int32_t(be16toh(data.daz_low) | (be16toh(data.daz_high)<<16)))};
// compensate for clock errors, see "DELTA ANGLES" in datasheet
dangle *= expected_sample_rate_hz / _gyro_raw_sample_rate(gyro_instance);
dvel *= expected_sample_rate_hz / _accel_raw_sample_rate(gyro_instance);
_notify_new_delta_velocity(accel_instance, dvel);
_notify_new_delta_angle(gyro_instance, dangle);
/* /*
publish average temperature at 20Hz publish average temperature at 20Hz
@ -309,15 +587,21 @@ void AP_InertialSensor_ADIS1647x::loop(void)
uint32_t tstart = AP_HAL::micros(); uint32_t tstart = AP_HAL::micros();
// we deliberately set the period a bit fast to ensure we // we deliberately set the period a bit fast to ensure we
// don't lose a sample // don't lose a sample
const uint32_t period_us = 480; const uint32_t period_us = (1000000UL / expected_sample_rate_hz) - 20U;
bool wait_ok = false; bool wait_ok = false;
if (drdy_pin != 0) { if (drdy_pin != 0) {
// when we have a DRDY pin then wait for it to go high // when we have a DRDY pin then wait for it to go high
DEBUG_SET_PIN(0, 1); DEBUG_SET_PIN(0, 1);
wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 1000); wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 2100);
DEBUG_SET_PIN(0, 0); DEBUG_SET_PIN(0, 0);
} }
read_sensor(); if (opmode == OpMode::Delta32) {
read_sensor32_delta();
} else if (opmode == OpMode::AG32) {
read_sensor32();
} else {
read_sensor16();
}
uint32_t dt = AP_HAL::micros() - tstart; uint32_t dt = AP_HAL::micros() - tstart;
if (dt < period_us) { if (dt < period_us) {
uint32_t wait_us = period_us - dt; uint32_t wait_us = period_us - dt;

17
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
View File

@ -46,18 +46,26 @@ private:
initialise driver initialise driver
*/ */
bool init(); bool init();
void read_sensor(void); void read_sensor16(void);
void read_sensor32(void);
void read_sensor32_delta(void);
void loop(void); void loop(void);
bool check_product_id(); bool check_product_id(uint16_t &id);
// read a 16 bit register // read a 16 bit register
uint16_t read_reg16(uint8_t regnum) const; uint16_t read_reg16(uint8_t regnum) const;
// write a 16 bit register // write a 16 bit register
void write_reg16(uint8_t regnum, uint16_t value) const; bool write_reg16(uint8_t regnum, uint16_t value, bool confirm=false) const;
AP_HAL::OwnPtr<AP_HAL::Device> dev; AP_HAL::OwnPtr<AP_HAL::Device> dev;
enum class OpMode : uint8_t {
Basic =1,
AG32 =2,
Delta32 =3
} opmode;
uint8_t accel_instance; uint8_t accel_instance;
uint8_t gyro_instance; uint8_t gyro_instance;
enum Rotation rotation; enum Rotation rotation;
@ -67,7 +75,10 @@ private:
bool done_first_read; bool done_first_read;
float temp_sum; float temp_sum;
uint8_t temp_count; uint8_t temp_count;
float expected_sample_rate_hz;
float accel_scale; float accel_scale;
float gyro_scale; float gyro_scale;
double dangle_scale;
double dvel_scale;
}; };

196
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -7,8 +7,8 @@
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#if AP_MODULE_SUPPORTED #if AP_MODULE_SUPPORTED
#include <AP_Module/AP_Module.h> #include <AP_Module/AP_Module.h>
#include <stdio.h>
#endif #endif
#include <stdio.h>
#define SENSOR_RATE_DEBUG 0 #define SENSOR_RATE_DEBUG 0
@ -307,6 +307,127 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
} }
} }
/*
handle a delta-angle sample from the backend. This assumes FIFO
style sampling and the sample should not be rotated or corrected for
offsets.
This function should be used when the sensor driver can directly
provide delta-angle values from the sensor.
*/
void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const Vector3f &dangle)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
return;
}
float dt;
_update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
_imu._gyro_raw_sample_rates[instance]);
uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
// don't accept below 40Hz
if (_imu._gyro_raw_sample_rates[instance] < 40) {
return;
}
dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
_imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
uint64_t sample_us = _imu._gyro_last_sample_us[instance];
Vector3f gyro = dangle / dt;
_rotate_and_correct_gyro(instance, gyro);
#if AP_MODULE_SUPPORTED
// call gyro_sample hook if any
AP_Module::call_hook_gyro_sample(instance, dt, gyro);
#endif
// push gyros if optical flow present
if (hal.opticalflow) {
hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
}
// compute delta angle, including corrections
Vector3f delta_angle = gyro * dt;
// compute coning correction
// see page 26 of:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// see also examples/coning.py
Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
_imu._last_delta_angle[instance] * (1.0f / 6.0f));
delta_coning = delta_coning % delta_angle;
delta_coning *= 0.5f;
{
WITH_SEMAPHORE(_sem);
uint64_t now = AP_HAL::micros64();
if (now - last_sample_us > 100000U) {
// zero accumulator if sensor was unhealthy for 0.1s
_imu._delta_angle_acc[instance].zero();
_imu._delta_angle_acc_dt[instance] = 0;
dt = 0;
delta_angle.zero();
}
// integrate delta angle accumulator
// the angles and coning corrections are accumulated separately in the
// referenced paper, but in simulation little difference was found between
// integrating together and integrating separately (see examples/coning.py)
_imu._delta_angle_acc[instance] += delta_angle + delta_coning;
_imu._delta_angle_acc_dt[instance] += dt;
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
#if HAL_WITH_DSP
// capture gyro window for FFT analysis
if (_imu._gyro_window_size > 0) {
const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
_imu._gyro_window[instance][0].push(scaled_gyro.x);
_imu._gyro_window[instance][1].push(scaled_gyro.y);
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
Vector3f gyro_filtered = gyro;
// apply the notch filter
if (_gyro_notch_enabled()) {
gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
}
// apply the harmonic notch filter
if (gyro_harmonic_notch_enabled()) {
gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
}
// apply the low pass filter last to attentuate any notch induced noise
gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
// if the filtering failed in any way then reset the filters and keep the old value
if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
_imu._gyro_filter[instance].reset();
_imu._gyro_notch_filter[instance].reset();
_imu._gyro_harmonic_notch_filter[instance].reset();
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
}
_imu._new_gyro_data[instance] = true;
}
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_gyro_raw(instance, sample_us, gyro);
}
else {
log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
}
}
void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro)
{ {
AP_Logger *logger = AP_Logger::get_singleton(); AP_Logger *logger = AP_Logger::get_singleton();
@ -430,6 +551,79 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
} }
} }
/*
handle a delta-velocity sample from the backend. This assumes FIFO style sampling and
the sample should not be rotated or corrected for offsets
This function should be used when the sensor driver can directly
provide delta-velocity values from the sensor.
*/
void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, const Vector3f &dvel)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
return;
}
float dt;
_update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
_imu._accel_raw_sample_rates[instance]);
uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
// don't accept below 40Hz
if (_imu._accel_raw_sample_rates[instance] < 40) {
return;
}
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
_imu._accel_last_sample_us[instance] = AP_HAL::micros64();
uint64_t sample_us = _imu._accel_last_sample_us[instance];
Vector3f accel = dvel / dt;
_rotate_and_correct_accel(instance, accel);
#if AP_MODULE_SUPPORTED
// call accel_sample hook if any
AP_Module::call_hook_accel_sample(instance, dt, accel, false);
#endif
_imu.calc_vibration_and_clipping(instance, accel, dt);
{
WITH_SEMAPHORE(_sem);
uint64_t now = AP_HAL::micros64();
if (now - last_sample_us > 100000U) {
// zero accumulator if sensor was unhealthy for 0.1s
_imu._delta_velocity_acc[instance].zero();
_imu._delta_velocity_acc_dt[instance] = 0;
dt = 0;
}
// delta velocity including corrections
_imu._delta_velocity_acc[instance] += accel * dt;
_imu._delta_velocity_acc_dt[instance] += dt;
_imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
_imu._accel_filter[instance].reset();
}
_imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = true;
}
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_accel_raw(instance, sample_us, accel);
} else {
log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
}
}
void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
{ {
if (!_imu.batchsampler.doing_sensor_rate_logging()) { if (!_imu.batchsampler.doing_sensor_rate_logging()) {

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -149,6 +149,9 @@ protected:
// sensors, and should be set to zero for FIFO based sensors // sensors, and should be set to zero for FIFO based sensors
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0); void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
// alternative interface using delta-angles. Rotation and correction is handled inside this function
void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);
// rotate accel vector, scale, offset and publish // rotate accel vector, scale, offset and publish
void _publish_accel(uint8_t instance, const Vector3f &accel); void _publish_accel(uint8_t instance, const Vector3f &accel);
@ -160,6 +163,9 @@ protected:
// sensors, and should be set to zero for FIFO based sensors // sensors, and should be set to zero for FIFO based sensors
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false); void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false);
// alternative interface using delta-velocities. Rotation and correction is handled inside this function
void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
// set the amount of oversamping a accel is doing // set the amount of oversamping a accel is doing
void _set_accel_oversampling(uint8_t instance, uint8_t n); void _set_accel_oversampling(uint8_t instance, uint8_t n);