Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: move gyro and accel instance ids into AP_InertialSensor_Backend

This commit is contained in:
Andy Piper 2024-06-20 21:09:05 +01:00 committed by Andrew Tridgell
parent 5911b87ae3
commit a86f4fdc8f
24 changed files with 160 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
View File

@ -66,8 +66,6 @@ private:
Delta32 =3
} opmode;
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t drdy_pin;

2
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h
View File

@ -62,8 +62,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t temperature_counter;
};

2
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
View File

@ -79,8 +79,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
AP_HAL::Device::PeriodicHandle gyro_periodic_handle;
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t temperature_counter;
enum DevTypes _accel_devtype;

16
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -201,8 +201,8 @@ void AP_InertialSensor_BMI160::start()
_dev->get_semaphore()->give();
if (!_imu.register_accel(_accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) ||
!_imu.register_gyro(_gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) {
if (!_imu.register_accel(accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) ||
!_imu.register_gyro(gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) {
return;
}
@ -213,8 +213,8 @@ void AP_InertialSensor_BMI160::start()
bool AP_InertialSensor_BMI160::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}
@ -420,11 +420,11 @@ read_fifo_read_data:
accel *= _accel_scale;
gyro *= _gyro_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_notify_new_accel_raw_sample(accel_instance, accel);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
if (excess) {

3
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
View File

@ -120,10 +120,7 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _accel_instance;
float _accel_scale;
uint8_t _gyro_instance;
float _gyro_scale;
AP_HAL::DigitalSource *_int1_pin;

38
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
View File

@ -212,14 +212,14 @@ void AP_InertialSensor_BMI270::start()
_dev->get_semaphore()->give();
if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||
!_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {
if (!_imu.register_accel(accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||
!_imu.register_gyro(gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {
return;
}
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(gyro_instance, _rotation);
set_accel_orientation(accel_instance, _rotation);
/* Call read_fifo() at 1600Hz */
periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
@ -227,8 +227,8 @@ void AP_InertialSensor_BMI270::start()
bool AP_InertialSensor_BMI270::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}
@ -364,8 +364,8 @@ void AP_InertialSensor_BMI270::fifo_reset()
// flush and reset FIFO
write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH);
notify_accel_fifo_reset(_accel_instance);
notify_gyro_fifo_reset(_gyro_instance);
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
/*
@ -383,8 +383,8 @@ void AP_InertialSensor_BMI270::read_fifo(void)
uint8_t len[2];
if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) {
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(accel_instance);
_inc_gyro_error_count(gyro_instance);
return;
}
uint16_t fifo_length = len[0] + (len[1]<<8);
@ -403,8 +403,8 @@ void AP_InertialSensor_BMI270::read_fifo(void)
uint8_t data[fifo_length];
if (!read_registers(BMI270_REG_FIFO_DATA, data, fifo_length)) {
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(accel_instance);
_inc_gyro_error_count(gyro_instance);
return;
}
@ -462,14 +462,14 @@ void AP_InertialSensor_BMI270::read_fifo(void)
temperature_counter = 0;
uint8_t tbuf[2];
if (!read_registers(BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) {
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(accel_instance);
_inc_gyro_error_count(gyro_instance);
} else {
uint16_t tval = tbuf[0] | (tbuf[1] << 8);
if (tval != 0x8000) { // 0x8000 is invalid
int16_t klsb = static_cast<int16_t>(tval);
float temp_degc = klsb * 0.002f + 23.0f;
_publish_temperature(_accel_instance, temp_degc);
_publish_temperature(accel_instance, temp_degc);
}
}
}
@ -487,8 +487,8 @@ void AP_InertialSensor_BMI270::parse_accel_frame(const uint8_t* d)
accel *= scale;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel);
}
void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
@ -502,8 +502,8 @@ void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
Vector3f gyro(xyz[0], xyz[1], xyz[2]);
gyro *= scale;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
bool AP_InertialSensor_BMI270::hardware_init()

2
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
View File

@ -115,8 +115,6 @@ private:
enum Rotation _rotation;
uint8_t _accel_instance;
uint8_t _gyro_instance;
uint8_t temperature_counter;
static const uint8_t maximum_fifo_config_file[];

12
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -141,6 +141,10 @@ protected:
//Default Clip Limit
float _clip_limit = 15.5f * GRAVITY_MSS;
// instance numbers of accel and gyro data
uint8_t gyro_instance;
uint8_t accel_instance;
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
@ -281,6 +285,14 @@ protected:
_imu._accel_orientation[instance] = rotation;
}
uint8_t get_gyro_instance() const {
return gyro_instance;
}
uint8_t get_accel_instance() const {
return accel_instance;
}
// increment clipping counted. Used by drivers that do decimation before supplying
// samples to the frontend
void increment_clip_count(uint8_t instance) {

2
libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h
View File

@ -21,8 +21,6 @@ public:
bool get_output_banner(char* banner, uint8_t banner_len) override;
private:
uint8_t gyro_instance;
uint8_t accel_instance;
const uint8_t serial_port;
bool started;
};

78
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -202,8 +202,8 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
notify_accel_fifo_reset(_accel_instance);
notify_gyro_fifo_reset(_gyro_instance);
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
void AP_InertialSensor_Invensense::_fast_fifo_reset()
@ -211,8 +211,8 @@ void AP_InertialSensor_Invensense::_fast_fifo_reset()
fast_reset_count++;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl | BIT_USER_CTRL_FIFO_RESET);
notify_accel_fifo_reset(_accel_instance);
notify_gyro_fifo_reset(_gyro_instance);
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
@ -224,7 +224,7 @@ bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
void AP_InertialSensor_Invensense::start()
{
// pre-fetch instance numbers for checking fast sampling settings
if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) {
return;
}
@ -320,8 +320,8 @@ void AP_InertialSensor_Invensense::start()
break;
}
if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) {
if (!_imu.register_gyro(gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(accel_instance, 1000, _dev->get_bus_id_devtype(adev))) {
return;
}
@ -329,12 +329,12 @@ void AP_InertialSensor_Invensense::start()
_set_filter_register();
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
_set_accel_raw_sample_rate(accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
_set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
// set sample rate to 1000Hz and apply a software filter
// In this configuration, the gyro sample rate is 8kHz
@ -426,8 +426,8 @@ void AP_InertialSensor_Invensense::start()
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(gyro_instance, _rotation);
set_accel_orientation(accel_instance, _rotation);
// setup scale factors for fifo data after downsampling
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
@ -447,7 +447,7 @@ void AP_InertialSensor_Invensense::start()
bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) {
if (_fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
_gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);
gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);
return true;
}
return false;
@ -459,10 +459,10 @@ bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banne
*/
bool AP_InertialSensor_Invensense::update() /* front end */
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
update_accel(accel_instance);
update_gyro(gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
_publish_temperature(accel_instance, _temp_filtered);
if (fast_reset_count) {
// check if we have reported in the last 1 seconds or
@ -472,13 +472,13 @@ bool AP_InertialSensor_Invensense::update() /* front end */
if (now - last_fast_reset_count_report_ms > 5000U) {
last_fast_reset_count_report_ms = now;
char param_name[sizeof("IMUxx_RST")];
snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(_gyro_instance,9));
snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(gyro_instance,9));
gcs().send_named_float(param_name, fast_reset_count);
}
#endif
#if HAL_LOGGING_ENABLED
if (last_fast_reset_count != fast_reset_count) {
AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count);
AP::logger().Write_MessageF("IMU%u fast fifo reset %u", gyro_instance, fast_reset_count);
last_fast_reset_count = fast_reset_count;
}
#endif
@ -602,7 +602,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
return false;
} else {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2);
}
_fifo_reset(true);
return false;
@ -615,11 +615,11 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
-int16_val(data, 6));
gyro *= _gyro_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_notify_new_accel_raw_sample(accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
}
@ -652,7 +652,7 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
ret = false;
} else {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2);
}
_fifo_reset(true);
ret = false;
@ -674,14 +674,14 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_notify_new_accel_sensor_rate_sample(accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_rotate_and_correct_accel(accel_instance, _accum.accel);
_notify_new_accel_raw_sample(accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
@ -696,20 +696,20 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
-int16_val(data, 6));
Vector3f g2 = g * _gyro_scale;
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_notify_new_gyro_sensor_rate_sample(gyro_instance, g2);
_accum.gyro += g;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_rotate_and_correct_gyro(gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(gyro_instance, _accum.gyro);
_accum.gyro.zero();
}
}
if (clipped) {
increment_clip_count(_accel_instance);
increment_clip_count(accel_instance);
}
if (ret) {
@ -788,7 +788,7 @@ void AP_InertialSensor_Invensense::_read_fifo()
if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) {
if (!hal.scheduler->in_expected_delay() && !_enable_fast_fifo_reset) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
debug("IMU[%u] stop at %u of %u", accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
}
break;
}
@ -831,8 +831,8 @@ check_registers:
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(gyro_instance);
_inc_accel_error_count(accel_instance);
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
@ -890,7 +890,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
_gyro_to_accel_sample_ratio = 2;
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1000;
if (enable_fast_sampling(_accel_instance)) {
if (enable_fast_sampling(accel_instance)) {
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (_fast_sampling) {
// constrain the gyro rate to be at least the loop rate
@ -919,11 +919,11 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
}
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_oversampling(accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
_set_accel_sensor_rate_sampling_enabled(accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(gyro_instance, true);
/* set divider for internal sample rate to 0x1F when fast
sampling enabled. This reduces the impact of the slave

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -115,10 +115,6 @@ private:
int16_t _raw_temp;
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
uint8_t _accel_instance;
float temp_sensitivity = 1.0f/340; // degC/LSB
float temp_zero = 36.53f; // degC

70
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -155,8 +155,8 @@ void AP_InertialSensor_Invensensev2::_fifo_reset()
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
notify_accel_fifo_reset(_accel_instance);
notify_gyro_fifo_reset(_gyro_instance);
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus()
@ -167,7 +167,7 @@ bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus()
void AP_InertialSensor_Invensensev2::start()
{
// pre-fetch instance numbers for checking fast sampling settings
if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) {
return;
}
@ -207,8 +207,8 @@ void AP_InertialSensor_Invensensev2::start()
break;
}
if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) {
if (!_imu.register_gyro(gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(accel_instance, 1125, _dev->get_bus_id_devtype(adev))) {
return;
}
@ -219,12 +219,12 @@ void AP_InertialSensor_Invensensev2::start()
_register_write(INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true);
#endif
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
_set_accel_raw_sample_rate(accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
_set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
// set sample rate to 1.125KHz
_register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true);
@ -238,8 +238,8 @@ void AP_InertialSensor_Invensensev2::start()
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(gyro_instance, _rotation);
set_accel_orientation(accel_instance, _rotation);
// setup scale factors for fifo data after downsampling
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
@ -259,7 +259,7 @@ void AP_InertialSensor_Invensensev2::start()
bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t banner_len) {
if (_fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
_gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);
gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);
return true;
}
return false;
@ -270,10 +270,10 @@ bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t ban
*/
bool AP_InertialSensor_Invensensev2::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
update_accel(accel_instance);
update_gyro(gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
_publish_temperature(accel_instance, _temp_filtered);
return true;
}
@ -341,7 +341,7 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2);
}
_fifo_reset();
return false;
@ -353,11 +353,11 @@ bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_sam
-int16_val(data, 5));
gyro *= GYRO_SCALE;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_notify_new_accel_raw_sample(accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
}
@ -386,7 +386,7 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
debug("temp reset IMU[%u] %d %d", accel_instance, _raw_temp, t2);
}
_fifo_reset();
ret = false;
@ -406,14 +406,14 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_notify_new_accel_sensor_rate_sample(accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_rotate_and_correct_accel(accel_instance, _accum.accel);
_notify_new_accel_raw_sample(accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
@ -428,20 +428,20 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
-int16_val(data, 5));
Vector3f g2 = g * GYRO_SCALE;
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_notify_new_gyro_sensor_rate_sample(gyro_instance, g2);
_accum.gyro += g;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_rotate_and_correct_gyro(gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(gyro_instance, _accum.gyro);
_accum.gyro.zero();
}
}
if (clipped) {
increment_clip_count(_accel_instance);
increment_clip_count(accel_instance);
}
if (ret) {
@ -519,7 +519,7 @@ void AP_InertialSensor_Invensensev2::_read_fifo()
if (_fast_sampling) {
if (!_accumulate_sensor_rate_sampling(rx, n)) {
if (!hal.scheduler->in_expected_delay()) {
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE);
debug("IMU[%u] stop at %u of %u", accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE);
}
break;
}
@ -542,8 +542,8 @@ check_registers:
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(gyro_instance);
_inc_accel_error_count(accel_instance);
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
@ -608,7 +608,7 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
_gyro_fifo_downsample_rate = _accel_fifo_downsample_rate = 1;
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1125;
if (enable_fast_sampling(_accel_instance)) {
if (enable_fast_sampling(accel_instance)) {
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (_fast_sampling) {
// constrain the gyro rate to be at least the loop rate
@ -631,11 +631,11 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
_accel_backend_rate_hz *= MIN(fast_sampling_rate, 4);
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_oversampling(accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
_set_accel_sensor_rate_sampling_enabled(accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(gyro_instance, true);
/* set divider for internal sample rate to 0x1F when fast
sampling enabled. This reduces the impact of the slave

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
View File

@ -103,10 +103,6 @@ private:
int16_t _raw_temp;
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
uint8_t _accel_instance;
float temp_sensitivity = 1.0f/333.87f; // degC/LSB
float temp_zero = 21; // degC

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -74,10 +74,6 @@ private:
bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples);
bool accumulate_highres_samples(const struct FIFODataHighRes *data, uint8_t n_samples);
// instance numbers of accel and gyro data
uint8_t gyro_instance;
uint8_t accel_instance;
// reset FIFO configure1 register
uint8_t fifo_config1;

16
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -250,8 +250,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/
void AP_InertialSensor_L3G4200D::start(void)
{
if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) ||
!_imu.register_accel(_accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) {
if (!_imu.register_gyro(gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) ||
!_imu.register_accel(accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) {
return;
}
@ -276,8 +276,8 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
*/
bool AP_InertialSensor_L3G4200D::update(void)
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
update_gyro(gyro_instance);
update_accel(accel_instance);
return true;
}
@ -313,8 +313,8 @@ void AP_InertialSensor_L3G4200D::_accumulate_gyro (void)
// Adjust for chip scaling to get radians/sec
//hal.console->printf("gyro %f \r\n",gyro.x);
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
}
}
@ -341,8 +341,8 @@ void AP_InertialSensor_L3G4200D::_accumulate_accel (void)
Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
// Adjust for chip scaling to get m/s/s
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel);
}
}
}

4
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -52,9 +52,5 @@ private:
LowPassFilter2pVector3f _gyro_filter;
enum Rotation _rotation;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;
};
#endif // __AP_INERTIAL_SENSOR_L3G4200D2_H__

30
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -512,19 +512,19 @@ fail_whoami:
*/
void AP_InertialSensor_LSM9DS0::start(void)
{
if (!_imu.register_gyro(_gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) ||
!_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) {
if (!_imu.register_gyro(gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) ||
!_imu.register_accel(accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) {
return;
}
if (whoami_g == LSM9DS0_G_WHOAMI_H) {
set_gyro_orientation(_gyro_instance, _rotation_gH);
set_gyro_orientation(gyro_instance, _rotation_gH);
} else {
set_gyro_orientation(_gyro_instance, _rotation_g);
set_gyro_orientation(gyro_instance, _rotation_g);
}
set_accel_orientation(_accel_instance, _rotation_a);
set_accel_orientation(accel_instance, _rotation_a);
_set_accel_max_abs_offset(_accel_instance, 5.0f);
_set_accel_max_abs_offset(accel_instance, 5.0f);
/* start the timer process to read samples */
_dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void));
@ -700,11 +700,11 @@ void AP_InertialSensor_LSM9DS0::_poll_data()
AP_HAL::Device::checkreg reg;
if (!_dev_gyro->check_next_register(reg)) {
log_register_change(_dev_gyro->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
_inc_gyro_error_count(gyro_instance);
}
if (!_dev_accel->check_next_register(reg)) {
log_register_change(_dev_accel->get_bus_id(), reg);
_inc_accel_error_count(_accel_instance);
_inc_accel_error_count(accel_instance);
}
}
@ -740,8 +740,8 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z);
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64());
_rotate_and_correct_accel(accel_instance, accel_data);
_notify_new_accel_raw_sample(accel_instance, accel_data, AP_HAL::micros64());
// read temperature every 10th sample
if (_temp_counter++ >= 10) {
@ -770,15 +770,15 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data, AP_HAL::micros64());
_rotate_and_correct_gyro(gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(gyro_instance, gyro_data, AP_HAL::micros64());
}
bool AP_InertialSensor_LSM9DS0::update()
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
_publish_temperature(_accel_instance, _temperature);
update_gyro(gyro_instance);
update_accel(accel_instance);
_publish_temperature(accel_instance, _temperature);
return true;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -97,8 +97,6 @@ private:
float _accel_scale;
int _drdy_pin_num_a;
int _drdy_pin_num_g;
uint8_t _gyro_instance;
uint8_t _accel_instance;
float _temperature;
uint8_t _temp_counter;
LowPassFilter2pFloat _temp_filter;

28
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
View File

@ -321,8 +321,8 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset()
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
notify_accel_fifo_reset(_accel_instance);
notify_gyro_fifo_reset(_gyro_instance);
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
/*
@ -330,15 +330,15 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset()
*/
void AP_InertialSensor_LSM9DS1::start(void)
{
if (!_imu.register_gyro(_gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) ||
!_imu.register_accel(_accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) {
if (!_imu.register_gyro(gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) ||
!_imu.register_accel(accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) {
return;
}
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(accel_instance, _rotation);
set_gyro_orientation(gyro_instance, _rotation);
_set_accel_max_abs_offset(_accel_instance, 5.0f);
_set_accel_max_abs_offset(accel_instance, 5.0f);
/* start the timer process to read samples */
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS1::_poll_data, void));
@ -431,7 +431,7 @@ void AP_InertialSensor_LSM9DS1::_poll_data()
AP_HAL::Device::checkreg reg;
if (!_dev->check_next_register(reg)) {
log_register_change(_dev->get_bus_id(), reg);
_inc_accel_error_count(_accel_instance);
_inc_accel_error_count(accel_instance);
}
}
@ -465,8 +465,8 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_x(uint16_t samples)
Vector3f accel_data(raw_data.x, raw_data.y, -raw_data.z);
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data);
_rotate_and_correct_accel(accel_instance, accel_data);
_notify_new_accel_raw_sample(accel_instance, accel_data);
}
/*
@ -501,14 +501,14 @@ void AP_InertialSensor_LSM9DS1::_read_data_transaction_g(uint16_t samples)
Vector3f gyro_data(raw_data.x, raw_data.y, -raw_data.z);
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
_rotate_and_correct_gyro(gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(gyro_instance, gyro_data);
}
bool AP_InertialSensor_LSM9DS1::update()
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
update_gyro(gyro_instance);
update_accel(accel_instance);
return true;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h
View File

@ -68,7 +68,5 @@ private:
float _gyro_scale;
float _accel_scale;
int _drdy_pin_num_xg;
uint8_t _gyro_instance;
uint8_t _accel_instance;
enum Rotation _rotation;
};

20
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
View File

@ -336,13 +336,13 @@ bool AP_InertialSensor_RST::_init_sensor(void)
*/
void AP_InertialSensor_RST::start(void)
{
if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) ||
!_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) {
if (!_imu.register_gyro(gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) ||
!_imu.register_accel(accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) {
return;
}
set_gyro_orientation(_gyro_instance, _rotation_g);
set_accel_orientation(_accel_instance, _rotation_a);
set_gyro_orientation(gyro_instance, _rotation_g);
set_accel_orientation(accel_instance, _rotation_a);
// start the timer process to read samples
_dev_gyro->register_periodic_callback(1150, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::gyro_measure, void));
@ -354,8 +354,8 @@ void AP_InertialSensor_RST::start(void)
*/
bool AP_InertialSensor_RST::update(void)
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
update_gyro(gyro_instance);
update_accel(accel_instance);
return true;
}
@ -376,8 +376,8 @@ void AP_InertialSensor_RST::gyro_measure(void)
if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
gyro *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
}
@ -397,8 +397,8 @@ void AP_InertialSensor_RST::accel_measure(void)
if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
accel *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel);
}
}

3
libraries/AP_InertialSensor/AP_InertialSensor_RST.h
View File

@ -47,9 +47,6 @@ private:
float _gyro_scale;
float _accel_scale;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;
enum Rotation _rotation_g;
enum Rotation _rotation_a;
};

2
libraries/AP_InertialSensor/AP_InertialSensor_SCHA63T.h
View File

@ -84,7 +84,5 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev_uno;
AP_HAL::OwnPtr<AP_HAL::Device> dev_due;
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
};

2
libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
View File

@ -45,8 +45,6 @@ private:
const uint16_t gyro_sample_hz;
const uint16_t accel_sample_hz;
uint8_t gyro_instance;
uint8_t accel_instance;
uint64_t next_gyro_sample;
uint64_t next_accel_sample;
float gyro_time;