AP_AHRS: move trim-related functionality to front end

libraries/AP_AHRS/AP_AHRS.cpp

@@ -191,6 +191,11 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
     _ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF;
 #endif
     _dcm_matrix.identity();
+
+    // initialise the controller-to-autopilot-body trim state:
+    _last_trim = _trim.get();
+    _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
+    _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
 }
 
 // init sets up INS board orientation
@@ -222,6 +227,20 @@ void AP_AHRS::init()
 #endif
 }
 
+// updates matrices responsible for rotating vectors from vehicle body
+// frame to autopilot body frame from _trim variables
+void AP_AHRS::update_trim_rotation_matrices()
+{
+    if (_last_trim == _trim.get()) {
+        // nothing to do
+        return;
+    }
+
+    _last_trim = _trim.get();
+    _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
+    _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
+}
+
 // return the smoothed gyro vector corrected for drift
 const Vector3f &AP_AHRS::get_gyro(void) const
 {
@@ -281,6 +300,9 @@ void AP_AHRS::update(bool skip_ins_update)
     // calling delay_microseconds_boost()
     hal.scheduler->boost_end();
 
+    // update autopilot-body-to-vehicle-body from _trim parameters:
+    update_trim_rotation_matrices();
+
     update_DCM(skip_ins_update);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_AHRS/AP_AHRS.h

@@ -337,6 +337,23 @@ public:
     // return SSA
     float getSSA(void) const { return _SSA; }
 
+    /*
+     * trim-related functions
+     */
+
+    // get trim
+    const Vector3f &get_trim() const { return _trim.get(); }
+
+    // set trim
+    void set_trim(const Vector3f &new_trim);
+
+    // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
+    void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
+
+    // trim rotation matrices:
+    const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
+    const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
+
     /*
      * home-related functionality
      */
@@ -464,6 +481,23 @@ private:
     void update_external(void);
 #endif    
 
+    /*
+     * trim-related state and private methods:
+     */
+
+    // a vector to capture the difference between the controller and body frames
+    AP_Vector3f         _trim;
+
+    // cached trim rotations
+    Vector3f _last_trim;
+
+    Matrix3f _rotation_autopilot_body_to_vehicle_body;
+    Matrix3f _rotation_vehicle_body_to_autopilot_body;
+
+    // updates matrices responsible for rotating vectors from vehicle body
+    // frame to autopilot body frame from _trim variables
+    void update_trim_rotation_matrices();
+
 #if HAL_NMEA_OUTPUT_ENABLED
     class AP_NMEA_Output* _nmea_out;
 #endif

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -36,7 +36,7 @@ Vector3f AP_AHRS_Backend::get_gyro_latest(void) const
 }
 
 // set_trim
-void AP_AHRS_Backend::set_trim(const Vector3f &new_trim)
+void AP_AHRS::set_trim(const Vector3f &new_trim)
 {
     Vector3f trim;
     trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
@@ -45,7 +45,7 @@ void AP_AHRS_Backend::set_trim(const Vector3f &new_trim)
 }
 
 // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
-void AP_AHRS_Backend::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
+void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
 {
     Vector3f trim = _trim.get();
 
@@ -194,12 +194,6 @@ void AP_AHRS_Backend::calc_trig(const Matrix3f &rot,
 //      should be called after _dcm_matrix is updated
 void AP_AHRS_Backend::update_trig(void)
 {
-    if (_last_trim != _trim.get()) {
-        _last_trim = _trim.get();
-        _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
-        _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
-    }
-
     calc_trig(get_rotation_body_to_ned(),
               _cos_roll, _cos_pitch, _cos_yaw,
               _sin_roll, _sin_pitch, _sin_yaw);

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -50,10 +50,6 @@ public:
     AP_AHRS_Backend() {
         // enable centrifugal correction by default
         _flags.correct_centrifugal = true;
-
-        _last_trim = _trim.get();
-        _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
-        _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
     }
 
     // empty virtual destructor
@@ -199,9 +195,6 @@ public:
         quat.from_rotation_matrix(get_rotation_body_to_ned());
     }
 
-    const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
-    const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
-
     // get rotation matrix specifically from DCM backend (used for compass calibrator)
     virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
 
@@ -337,17 +330,6 @@ public:
         return _flags.correct_centrifugal;
     }
 
-    // get trim
-    const Vector3f &get_trim() const {
-        return _trim.get();
-    }
-
-    // set trim
-    void set_trim(const Vector3f &new_trim);
-
-    // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
-    void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
-
     // helper trig value accessors
     float cos_roll() const  {
         return _cos_roll;
@@ -574,16 +556,6 @@ protected:
     // update takeoff/touchdown flags
     void update_flags();
 
-    // pointer to airspeed object, if available
-
-    // a vector to capture the difference between the controller and body frames
-    AP_Vector3f         _trim;
-
-    // cached trim rotations
-    Vector3f _last_trim;
-    Matrix3f _rotation_autopilot_body_to_vehicle_body;
-    Matrix3f _rotation_vehicle_body_to_autopilot_body;
-
     // accelerometer values in the earth frame in m/s/s
     Vector3f        _accel_ef[INS_MAX_INSTANCES];
     Vector3f        _accel_ef_blended;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1012,7 +1012,7 @@ void AP_AHRS_DCM::estimate_wind(void)
 void
 AP_AHRS_DCM::euler_angles(void)
 {
-    _body_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
+    _body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
     _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
     update_cd_values();