zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: added support for more ADIS IMUs 

support 32 bit delta angles and velocities
gps-1.3.1
Andrew Tridgell 3 years ago
parent
f637facea2
commit
662327f2ea
4 changed files with 534 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 352
      libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
  2. 17
      libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
  3. 196
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
  4. 6
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

352
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
Unescape View File

@ -20,14 +20,13 @@ @@ -20,14 +20,13 @@
#include "AP_InertialSensor_ADIS1647x.h"
#define BACKEND_SAMPLE_RATE 2000
/*
device registers
*/
#define REG_PROD_ID 0x72
#define PROD_ID_16470 0x4056
#define PROD_ID_16477 0x405d
#define PROD_ID_16507 0x407b
#define REG_GLOB_CMD 0x68
#define GLOB_CMD_SW_RESET 0x80
@ -36,11 +35,28 @@ @@ -36,11 +35,28 @@
#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
*/
#define T_STALL_US 20U
#define T_RESET_MS 250U
#define T_RESET_MS 500U
#define TIMING_DEBUG 0
#if TIMING_DEBUG
@ -89,8 +105,8 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu, @@ -89,8 +105,8 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
void AP_InertialSensor_ADIS1647x::start()
{
if (!_imu.register_accel(accel_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) ||
!_imu.register_gyro(gyro_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, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) {
return;
}
@ -113,21 +129,25 @@ void AP_InertialSensor_ADIS1647x::start() @@ -113,21 +129,25 @@ void AP_InertialSensor_ADIS1647x::start()
/*
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) {
case PROD_ID_16470:
// can do up to 40G
opmode = OpMode::Basic;
accel_scale = 1.25 * GRAVITY_MSS * 0.001;
_clip_limit = 39.5f * GRAVITY_MSS;
gyro_scale = radians(0.1);
expected_sample_rate_hz = 2000;
return true;
case PROD_ID_16477:
case PROD_ID_16477: {
// can do up to 40G
opmode = OpMode::Basic;
accel_scale = 1.25 * GRAVITY_MSS * 0.001;
_clip_limit = 39.5f * GRAVITY_MSS;
expected_sample_rate_hz = 2000;
// RANG_MDL register used for gyro range
uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
switch ((rang_mdl >> 2) & 3) {
@ -145,6 +165,39 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void) @@ -145,6 +165,39 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
}
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;
}
@ -152,20 +205,52 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void) @@ -152,20 +205,52 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(void)
bool AP_InertialSensor_ADIS1647x::init()
{
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;
}
// perform software reset
write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
hal.scheduler->delay(T_RESET_MS);
// bring rate down
if (expected_sample_rate_hz < 450) {
if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_400Hz, true)) {
return false;
}
} else if (expected_sample_rate_hz < 600) {
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;
}
}
// re-check after reset
if (!check_product_id()) {
if (!write_reg16(REG_FILT_CTRL, 0, true)) {
return false;
}
// we leave all config registers at defaults
// 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
// useful for debugging scheduling of transfers
@ -200,24 +285,32 @@ uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const @@ -200,24 +285,32 @@ uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const
/*
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];
req[0] = (regnum | 0x80);
req[1] = value & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0);
hal.scheduler->delay_microseconds(T_STALL_US);
req[0] = ((regnum+1) | 0x80);
req[1] = (value>>8) & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0);
hal.scheduler->delay_microseconds(T_STALL_US);
const uint8_t retries = 16;
for (uint8_t i=0; i<retries; i++) {
uint8_t req[2];
req[0] = (regnum | 0x80);
req[1] = value & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0);
hal.scheduler->delay_microseconds(T_STALL_US);
req[0] = ((regnum+1) | 0x80);
req[1] = (value>>8) & 0xFF;
dev->transfer(req, sizeof(req), nullptr, 0);
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 {
uint8_t cmd[2];
@ -234,7 +327,6 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void) @@ -234,7 +327,6 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
uint8_t checksum;
} data {};
uint64_t sample_start_us = AP_HAL::micros64();
do {
WITH_SEMAPHORE(dev->get_semaphore());
data.cmd[0] = REG_GLOB_CMD;
@ -281,10 +373,196 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void) @@ -281,10 +373,196 @@ void AP_InertialSensor_ADIS1647x::read_sensor(void)
gyro *= gyro_scale;
_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);
_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, 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 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
@ -309,15 +587,21 @@ void AP_InertialSensor_ADIS1647x::loop(void) @@ -309,15 +587,21 @@ void AP_InertialSensor_ADIS1647x::loop(void)
uint32_t tstart = AP_HAL::micros();
// we deliberately set the period a bit fast to ensure we
// 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;
if (drdy_pin != 0) {
// when we have a DRDY pin then wait for it to go high
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);
}
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;
if (dt < period_us) {
uint32_t wait_us = period_us - dt;

17
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
Unescape View File

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

196
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
Unescape View File

@ -7,8 +7,8 @@ @@ -7,8 +7,8 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#if AP_MODULE_SUPPORTED
#include <AP_Module/AP_Module.h>
#include <stdio.h>
#endif
#include <stdio.h>
#define SENSOR_RATE_DEBUG 0
@ -307,6 +307,127 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -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)
{
AP_Logger *logger = AP_Logger::get_singleton();
@ -430,6 +551,79 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, @@ -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)
{
if (!_imu.batchsampler.doing_sensor_rate_logging()) {

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Unescape View File

@ -149,6 +149,9 @@ protected: @@ -149,6 +149,9 @@ protected:
// 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);
// 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
void _publish_accel(uint8_t instance, const Vector3f &accel);
@ -160,6 +163,9 @@ protected: @@ -160,6 +163,9 @@ protected:
// 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);
// 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
void _set_accel_oversampling(uint8_t instance, uint8_t n);

Write Preview
Loading…
Cancel
Save