Skip to content

Commit

Permalink
AP_InertialSensor: prearm check to validate backend read rates agains…
Browse files Browse the repository at this point in the history
…t scheduler loop rate
  • Loading branch information
bugobliterator committed Dec 11, 2024
1 parent 42ad2a7 commit 097095b
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 0 deletions.
19 changes: 19 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include "AP_InertialSensor_Invensensev3.h"
#include "AP_InertialSensor_NONE.h"
#include "AP_InertialSensor_SCHA63T.h"
#include <AP_Scheduler/AP_Scheduler.h>

/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
* Output is on the debug console. */
Expand Down Expand Up @@ -1492,6 +1493,24 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
return (get_gyro_count() > 0);
}

// Prearm check to verify that we have sane sched loop rates set based on Gyro backend rates
bool AP_InertialSensor::pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const
{
#if AP_SCHEDULER_ENABLED
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!_use(i) || _backends[i] == nullptr) {
continue;
}
if (_backends[i]->get_gyro_backend_rate_hz() < (2*_loop_rate)) {
hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d backend rate %dHz < sched loop ratex2 %dHz",
i, _backends[i]->get_gyro_backend_rate_hz(), 2 * _loop_rate);
return false;
}
}
#endif
return true;
}

// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_gyro(uint8_t instance) const
{
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,9 @@ class AP_InertialSensor : AP_AccelCal_Client
uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }

// validate backend sample rates
bool pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const;

// FFT support access
#if HAL_GYROFFT_ENABLED
const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; }
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,12 @@ class AP_InertialSensor_Backend
bool has_been_killed(uint8_t instance) const { return false; }
#endif

// get the backend update rate for the gyro in Hz
// if the backend polling rate is the same as the sample rate or higher, return raw sample rate
// override and return the backend rate in Hz if it is lower than the sample rate
virtual uint16_t get_gyro_backend_rate_hz() const {
return _gyro_raw_sample_rate(gyro_instance);
}

/*
device driver IDs. These are used to fill in the devtype field
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,11 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
// get a startup banner to output to the GCS
bool get_output_banner(char* banner, uint8_t banner_len) override;

// get the gyro backend rate in Hz at which the FIFO is being read
uint16_t get_gyro_backend_rate_hz() const override {
return _gyro_backend_rate_hz;
}

enum Invensense_Type {
Invensense_MPU6000=0,
Invensense_MPU6500,
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,11 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
// get a startup banner to output to the GCS
bool get_output_banner(char* banner, uint8_t banner_len) override;

// get the gyro backend rate in Hz at which the FIFO is being read
uint16_t get_gyro_backend_rate_hz() const override {
return _gyro_backend_rate_hz;
}

enum Invensensev2_Type {
Invensensev2_ICM20948 = 0,
Invensensev2_ICM20648,
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,11 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples);
bool accumulate_highres_samples(const struct FIFODataHighRes *data, uint8_t n_samples);

// get the gyro backend rate in Hz at which the FIFO is being read
uint16_t get_gyro_backend_rate_hz() const override {
return backend_rate_hz;
}

// reset FIFO configure1 register
uint8_t fifo_config1;

Expand Down

0 comments on commit 097095b

Please sign in to comment.