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

AP_InertialSensor: correct loop rate on BMI055, clean up setting of loop rate on BMI055 and BMI088

This commit is contained in:
Andy Piper 2019-11-24 20:33:59 +00:00 committed by Randy Mackay
parent cce993e35b
commit 805d8ed776
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
View File

@ -58,6 +58,9 @@
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA 0x3F
#define ACCEL_BACKEND_SAMPLE_RATE 2000
#define GYRO_BACKEND_SAMPLE_RATE 2000
extern const AP_HAL::HAL& hal;
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
@ -98,17 +101,17 @@ AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu,
void AP_InertialSensor_BMI055::start()
{
accel_instance = _imu.register_accel(2000, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055));
gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055));
accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055));
gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055));
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);
set_accel_orientation(accel_instance, rotation);
// setup callbacks
dev_accel->register_periodic_callback(1000,
dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_accel, void));
dev_gyro->register_periodic_callback(1000,
dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_gyro, void));
}
@ -156,7 +159,6 @@ bool AP_InertialSensor_BMI055::accel_init()
goto failed;
}
hal.console->printf("BMI055: found accel\n");
dev_accel->get_semaphore()->give();

13
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -56,6 +56,9 @@
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA 0x3F
#define ACCEL_BACKEND_SAMPLE_RATE 1600
#define GYRO_BACKEND_SAMPLE_RATE 2000
extern const AP_HAL::HAL& hal;
AP_InertialSensor_BMI088::AP_InertialSensor_BMI088(AP_InertialSensor &imu,
@ -94,17 +97,17 @@ AP_InertialSensor_BMI088::probe(AP_InertialSensor &imu,
void AP_InertialSensor_BMI088::start()
{
accel_instance = _imu.register_accel(1600, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088));
gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088));
accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088));
gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088));
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);
set_accel_orientation(accel_instance, rotation);
// setup callbacks
dev_accel->register_periodic_callback(1000000UL / 1600,
dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void));
dev_gyro->register_periodic_callback(1000000UL / 2000,
dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void));
}