From c7a43f2a905c36ee878b0a22881656050f829570 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 14 Sep 2021 21:28:20 +0100 Subject: [PATCH] AP_InertialSensor: put some functions in fast ram precisely split backend/frontend on Invensense for RAMFUNC allocation allow building with no IMUs --- .../AP_InertialSensor/AP_InertialSensor.cpp | 6 +- .../AP_InertialSensor/AP_InertialSensor.h | 4 +- .../AP_InertialSensor_Backend.cpp | 17 ++---- .../AP_InertialSensor_Backend.h | 56 +++++++++---------- .../AP_InertialSensor_Invensense.cpp | 2 +- .../AP_InertialSensor_Invensense.h | 27 +++++---- 6 files changed, 52 insertions(+), 60 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 9009acfb27..e983491134 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 73ce9b9d47..f4618664fc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 550cd37401..e0c8dc21ad 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -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<(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;