AP_AHRS & AP_Math: fixed bug in use of AHRS_TRIM parameters

libraries/AP_AHRS/AP_AHRS.cpp

@@ -62,23 +62,26 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 
     // @Param: TRIM_X
     // @DisplayName: AHRS Trim Roll
-    // @Description: Compensates for the roll angle difference between the control board and the frame
+    // @Description: Compensates for the roll angle difference between the control board and the frame. Positive values make the vehicle roll right.
     // @Units: Radians
-    // @Range: -10 10
-    // @User: Advanced
+    // @Range: -0.1745 +0.1745
+    // @Increment: 0.01
+    // @User: User
 
     // @Param: TRIM_Y
     // @DisplayName: AHRS Trim Pitch
-    // @Description: Compensates for the pitch angle difference between the control board and the frame
+    // @Description: Compensates for the pitch angle difference between the control board and the frame. Positive values make the vehicle pitch up/back.
     // @Units: Radians
-    // @Range: -10 10
-    // @User: Advanced
+    // @Range: -0.1745 +0.1745
+    // @Increment: 0.01
+    // @User: User
 
     // @Param: TRIM_Z
     // @DisplayName: AHRS Trim Yaw
     // @Description: Not Used
     // @Units: Radians
-    // @Range: -10 10
+    // @Range: -0.1745 +0.1745
+    // @Increment: 0.01
     // @User: Advanced
     AP_GROUPINFO("TRIM", 8, AP_AHRS, _trim, 0),
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -499,7 +499,6 @@ Vector3f AP_AHRS_DCM::ra_delayed(const Vector3f &ra)
 void
 AP_AHRS_DCM::drift_correction(float deltat)
 {
-    Matrix3f temp_dcm = _dcm_matrix;
     Vector3f velocity;
     uint32_t last_correction_time;
 
@@ -507,11 +506,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // vector
     drift_correction_yaw();
 
-    // apply trim
-    temp_dcm.rotateXY(_trim);
-
     // rotate accelerometer values into the earth frame
-    _accel_ef = temp_dcm * _ins.get_accel();
+    _accel_ef = _dcm_matrix * _ins.get_accel();
 
     // integrate the accel vector in the earth frame between GPS readings
     _ra_sum += _accel_ef * deltat;
@@ -798,12 +794,15 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
 
 
 
-// calculate the euler angles which will be used for high level
-// navigation control
+// calculate the euler angles and DCM matrix which will be used for high level
+// navigation control. Apply trim such that a positive trim value results in a 
+// positive vehicle rotation about that axis (ie a negative offset)
 void
 AP_AHRS_DCM::euler_angles(void)
 {
-    _dcm_matrix.to_euler(&roll, &pitch, &yaw);
+    _body_dcm_matrix = _dcm_matrix;
+    _body_dcm_matrix.rotateXYinv(_trim);
+    _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
     roll_sensor     = degrees(roll)  * 100;
     pitch_sensor    = degrees(pitch) * 100;

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -42,8 +42,10 @@ public:
     const Vector3f get_gyro(void) const {
         return _omega + _omega_P + _omega_yaw_P;
     }
+
+    // return rotation matrix representing rotaton from body to earth axes
     const Matrix3f &get_dcm_matrix(void) const {
-        return _dcm_matrix;
+        return _body_dcm_matrix;
     }
 
     // return the current drift correction integrator value
@@ -92,9 +94,12 @@ private:
     void            estimate_wind(Vector3f &velocity);
     bool            have_gps(void) const;
 
-    // primary representation of attitude
+    // primary representation of attitude of board used for all inertial calculations
     Matrix3f _dcm_matrix;
 
+    // primary representation of attitude of flight vehicle body
+    Matrix3f _body_dcm_matrix;
+
     Vector3f _omega_P;                          // accel Omega proportional correction
     Vector3f _omega_yaw_P;                      // proportional yaw correction
     Vector3f _omega_I;                          // Omega Integrator correction

libraries/AP_Math/matrix3.cpp

@@ -98,6 +98,25 @@ void Matrix3<T>::rotateXY(const Vector3<T> &g)
     (*this) += temp_matrix;
 }
 
+// apply an additional inverse rotation to a rotation matrix but 
+// only use X, Y elements from rotation vector
+template <typename T>
+void Matrix3<T>::rotateXYinv(const Vector3<T> &g)
+{
+    Matrix3f temp_matrix;
+    temp_matrix.a.x =   a.z * g.y;
+    temp_matrix.a.y = - a.z * g.x;
+    temp_matrix.a.z = - a.x * g.y + a.y * g.x;
+    temp_matrix.b.x =   b.z * g.y;
+    temp_matrix.b.y = - b.z * g.x;
+    temp_matrix.b.z = - b.x * g.y + b.y * g.x;
+    temp_matrix.c.x =   c.z * g.y;
+    temp_matrix.c.y = - c.z * g.x;
+    temp_matrix.c.z = - c.x * g.y + c.y * g.x;
+
+    (*this) += temp_matrix;
+}
+
 // multiplication by a vector
 template <typename T>
 Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
@@ -161,6 +180,7 @@ void Matrix3<T>::zero(void)
 template void Matrix3<float>::zero(void);
 template void Matrix3<float>::rotate(const Vector3<float> &g);
 template void Matrix3<float>::rotateXY(const Vector3<float> &g);
+template void Matrix3<float>::rotateXYinv(const Vector3<float> &g);
 template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
 template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
 template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;

libraries/AP_Math/matrix3.h

@@ -215,6 +215,10 @@ public:
     // apply an additional rotation from a body frame gyro vector
     // to a rotation matrix but only use X, Y elements from gyro vector
     void        rotateXY(const Vector3<T> &g);
+
+    // apply an additional inverse rotation to a rotation matrix but 
+    // only use X, Y elements from rotation vector
+    void        rotateXYinv(const Vector3<T> &g);
 };
 
 typedef Matrix3<int16_t>                Matrix3i;