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

AP_InertialSensor: put some functions in fast ram 

precisely split backend/frontend on Invensense for RAMFUNC allocation
allow building with no IMUs
This commit is contained in:
Andy Piper 2021-09-14 21:28:20 +01:00 committed by Peter Hall
parent 2beb9cfc7d
commit c7a43f2a90
6 changed files with 52 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -986,9 +986,9 @@ AP_InertialSensor::detect_backends(void)
}
#endif
uint8_t probe_count = 0;
uint8_t enable_mask = uint8_t(_enable_mask.get());
uint8_t found_mask = 0;
uint8_t probe_count __attribute__((unused)) = 0;
uint8_t enable_mask __attribute__((unused)) = uint8_t(_enable_mask.get());
uint8_t found_mask __attribute__((unused)) = 0;
/*
use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -361,7 +361,7 @@ public:
};
void init();
void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample);
void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample) __RAMFUNC__;
// a function called by the main thread at the main loop rate:
void periodic();
@ -400,7 +400,7 @@ public:
void rotate_to_next_sensor();
void update_doing_sensor_rate_logging();
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type) __RAMFUNC__;
void push_data_to_log();
// Logging functions

17
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -165,7 +165,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
/*
rotate gyro vector and add the gyro offset
*/
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro)
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
return;
@ -447,7 +447,7 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
/*
rotate accel vector, scale and add the accel offset
*/
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
return;
@ -687,17 +687,10 @@ void AP_InertialSensor_Backend::_inc_gyro_error_count(uint8_t instance)
_imu._gyro_error_count[instance]++;
}
// return the requested loop rate at which samples will be made available in Hz
uint16_t AP_InertialSensor_Backend::get_loop_rate_hz(void) const
{
// enum can be directly cast to Hz
return (uint16_t)_imu._loop_rate;
}
/*
publish a temperature value for an instance
*/
void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature)
void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
return;
@ -718,7 +711,7 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem
/*
common gyro update function for all backends
*/
void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);
@ -771,7 +764,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
/*
common accel update function for all backends
*/
void AP_InertialSensor_Backend::update_accel(uint8_t instance)
void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);

56
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -47,7 +47,7 @@ public:
* accumulated sensor readings to the frontend structure via the
* _publish_gyro() and _publish_accel() functions
*/
virtual bool update() = 0;
virtual bool update() = 0; /* front end */
/*
* optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
@ -74,9 +74,6 @@ public:
//Returns the Clip Limit
float get_clip_limit() const { return _clip_limit; }
// notify of a fifo reset
void notify_fifo_reset(void);
// get a startup banner to output to the GCS
virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
@ -136,11 +133,11 @@ protected:
//Default Clip Limit
float _clip_limit = 15.5f * GRAVITY_MSS;
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel);
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro);
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
// rotate gyro vector, offset and publish
void _publish_gyro(uint8_t instance, const Vector3f &gyro);
void _publish_gyro(uint8_t instance, const Vector3f &gyro); /* front end */
// this should be called every time a new gyro raw sample is
// available - be it published or not the sample is raw in the
@ -148,13 +145,13 @@ protected:
// corrected (_rotate_and_correct_gyro)
// The sample_us value must be provided for non-FIFO based
// 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);
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0) __RAMFUNC__;
// 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);
void _publish_accel(uint8_t instance, const Vector3f &accel); /* front end */
// this should be called every time a new accel raw sample is available -
// be it published or not
@ -162,7 +159,7 @@ protected:
// be rotated and corrected (_rotate_and_correct_accel)
// The sample_us value must be provided for non-FIFO based
// 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);
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false) __RAMFUNC__;
// alternative interface using delta-velocities. Rotation and correction is handled inside this function
void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
@ -200,7 +197,7 @@ protected:
}
// update the sensor rate for FIFO sensors
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const;
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;
// return true if the sensors are still converging and sampling rates could change significantly
bool sensors_converging() const { return AP_HAL::millis() < 30000; }
@ -231,7 +228,7 @@ protected:
}
// publish a temperature value
void _publish_temperature(uint8_t instance, float temperature);
void _publish_temperature(uint8_t instance, float temperature); /* front end */
// set accelerometer error_count
void _set_accel_error_count(uint8_t instance, uint32_t error_count);
@ -240,10 +237,10 @@ protected:
void _set_gyro_error_count(uint8_t instance, uint32_t error_count);
// increment accelerometer error_count
void _inc_accel_error_count(uint8_t instance);
void _inc_accel_error_count(uint8_t instance) __RAMFUNC__;
// increment gyro error_count
void _inc_gyro_error_count(uint8_t instance);
void _inc_gyro_error_count(uint8_t instance) __RAMFUNC__;
// backend unique identifier or -1 if backend doesn't identify itself
int16_t _id = -1;
@ -254,8 +251,11 @@ protected:
// return the default filter frequency in Hz for the sample rate
uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
// return the requested sample rate in Hz
uint16_t get_loop_rate_hz(void) const;
// return the requested loop rate at which samples will be made available in Hz
uint16_t get_loop_rate_hz(void) const {
// enum can be directly cast to Hz
return (uint16_t)_imu._loop_rate;
}
// return the notch filter center in Hz for the sample rate
float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
@ -286,10 +286,10 @@ protected:
bool gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
// common gyro update function for all backends
void update_gyro(uint8_t instance);
void update_gyro(uint8_t instance); /* front end */
// common accel update function for all backends
void update_accel(uint8_t instance);
void update_accel(uint8_t instance); /* front end */
// support for updating filter at runtime
uint16_t _last_accel_filter_hz;
@ -329,17 +329,17 @@ protected:
// called by subclass when data is received from the sensor, thus
// at the 'sensor rate'
void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel);
void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro);
void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) __RAMFUNC__;
void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) __RAMFUNC__;
/*
notify of a FIFO reset so we don't use bad data to update observed sensor rate
*/
void notify_accel_fifo_reset(uint8_t instance);
void notify_gyro_fifo_reset(uint8_t instance);
void notify_accel_fifo_reset(uint8_t instance) __RAMFUNC__;
void notify_gyro_fifo_reset(uint8_t instance) __RAMFUNC__;
// log an unexpected change in a register for an IMU
void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg &reg);
void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg &reg) __RAMFUNC__;
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
@ -347,12 +347,12 @@ protected:
private:
bool should_log_imu_raw() const;
void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel);
void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo);
bool should_log_imu_raw() const ;
void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) __RAMFUNC__;
void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo) __RAMFUNC__;
// logging
void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const; // Write ACC data packet: raw accel data
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const; // Write GYR data packet: raw gyro data
void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const __RAMFUNC__; // Write GYR data packet: raw gyro data
};

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -427,7 +427,7 @@ bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banne
/*
publish any pending data
*/
bool AP_InertialSensor_Invensense::update()
bool AP_InertialSensor_Invensense::update() /* front end */
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);

27
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -32,7 +32,6 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
public:
virtual ~AP_InertialSensor_Invensense();
static AP_InertialSensor_Invensense &from(AP_InertialSensor_Backend &backend) {
return static_cast<AP_InertialSensor_Invensense&>(backend);
}
@ -46,8 +45,8 @@ public:
enum Rotation rotation);
/* update accel and gyro state */
bool update() override;
void accumulate() override;
bool update() override; /* front end */
void accumulate() override; /* front end */
/*
* Return an AuxiliaryBus if the bus driver allows it
@ -86,31 +85,31 @@ private:
bool _check_whoami();
void _set_filter_register(void);
void _fifo_reset(bool log_error);
void _fifo_reset(bool log_error) __RAMFUNC__;
bool _has_auxiliary_bus();
/* Read samples from FIFO (FIFO enabled) */
void _read_fifo();
void _read_fifo() __RAMFUNC__;
/* Check if there's data available by either reading DRDY pin or register */
bool _data_ready();
bool _data_ready() __RAMFUNC__;
/* Poll for new data (non-blocking) */
void _poll_data();
void _poll_data() __RAMFUNC__;
// debug function to watch for register changes
void _check_register_change(void);
void _check_register_change(void) __RAMFUNC__;
/* Read and write functions taking the differences between buses into
* account */
bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size) __RAMFUNC__;
uint8_t _register_read(uint8_t reg) __RAMFUNC__;
void _register_write(uint8_t reg, uint8_t val, bool checked=false) __RAMFUNC__;
bool _accumulate(uint8_t *samples, uint8_t n_samples);
bool _accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples);
bool _accumulate(uint8_t *samples, uint8_t n_samples) __RAMFUNC__;
bool _accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples) __RAMFUNC__;
bool _check_raw_temp(int16_t t2);
bool _check_raw_temp(int16_t t2) __RAMFUNC__;
int16_t _raw_temp;