AP_InertialSensor: added support for more ADIS IMUs
support 32 bit delta angles and velocities
This commit is contained in:
Andrew Tridgell
parent
f637facea2
commit
662327f2ea
@ -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 @@
|
||||
|
||||
#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,
|
||||
|
||||
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()
|
||||
/*
|
||||
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)
|
||||
}
|
||||
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)
|
||||
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);
|
||||
|
||||
// re-check after reset
|
||||
if (!check_product_id()) {
|
||||
return false;
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
// 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
|
||||
*/
|
||||
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);
|
||||
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);
|
||||
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)
|
||||
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)
|
||||
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, 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
|
||||
@ -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;
|
||||
|
||||
@ -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:
|
||||
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;
|
||||
};
|
||||
|
||||
@ -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,
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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,
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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()) {
|
||||
|
||||
@ -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:
|
||||
// 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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user