AP_InertialSensor: put some functions in fast ram

precisely split backend/frontend on Invensense for RAMFUNC allocation
allow building with no IMUs

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

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

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<<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);
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -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
 
 };

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -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;