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

AP_InertialSensor: synchronize BMI70 on fifo reads

Browse Source
This commit is contained in:
Andy Piper 2022-08-24 21:24:58 +02:00 committed by Andrew Tridgell
parent 6358a3c962
commit fcbaa0969e
2 changed files with 10 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
View File

@ -143,6 +143,8 @@ const uint8_t AP_InertialSensor_BMI270::maximum_fifo_config_file[] = { BMI270_RE
#define BMI270_HARDWARE_INIT_MAX_TRIES 5 #define BMI270_HARDWARE_INIT_MAX_TRIES 5
const uint32_t BACKEND_PERIOD_US = 1000000UL / BMI270_BACKEND_SAMPLE_RATE;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_InertialSensor_BMI270::AP_InertialSensor_BMI270(AP_InertialSensor &imu, AP_InertialSensor_BMI270::AP_InertialSensor_BMI270(AP_InertialSensor &imu,
@ -219,9 +221,8 @@ void AP_InertialSensor_BMI270::start()
set_gyro_orientation(_gyro_instance, _rotation); set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation); set_accel_orientation(_accel_instance, _rotation);
/* Call _poll_data() at 1600Hz */ /* Call read_fifo() at 1600Hz */
_dev->register_periodic_callback(1000000UL / BMI270_BACKEND_SAMPLE_RATE, periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::poll_data, void));
} }
bool AP_InertialSensor_BMI270::update() bool AP_InertialSensor_BMI270::update()
@ -407,6 +408,10 @@ void AP_InertialSensor_BMI270::read_fifo(void)
return; return;
} }
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo() without updating the sensor data
_dev->adjust_periodic_callback(periodic_handle, BACKEND_PERIOD_US);
const uint8_t *p = &data[0]; const uint8_t *p = &data[0];
while (fifo_length >= 12) { while (fifo_length >= 12) {
/* /*
@ -501,11 +506,6 @@ void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
_notify_new_gyro_raw_sample(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro);
} }
void AP_InertialSensor_BMI270::poll_data()
{
read_fifo();
}
bool AP_InertialSensor_BMI270::hardware_init() bool AP_InertialSensor_BMI270::hardware_init()
{ {
bool init = false; bool init = false;

7
libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
View File

@ -103,11 +103,6 @@ private:
*/ */
void configure_fifo(); void configure_fifo();
/**
* Device periodic callback to read data from the sensors.
*/
void poll_data();
/** /**
* Read samples from fifo. * Read samples from fifo.
*/ */
@ -116,6 +111,8 @@ private:
void parse_gyro_frame(const uint8_t* d); void parse_gyro_frame(const uint8_t* d);
AP_HAL::OwnPtr<AP_HAL::Device> _dev; AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_HAL::Device::PeriodicHandle periodic_handle;
enum Rotation _rotation; enum Rotation _rotation;
uint8_t _accel_instance; uint8_t _accel_instance;