diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3d85de018d..eb01e61dbf 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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), diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 4d28d492a3..529e1c2309 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index f3a8cf64a8..9cc56ccf06 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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 diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 5bfac98334..1b47db137d 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -98,6 +98,25 @@ void Matrix3::rotateXY(const Vector3 &g) (*this) += temp_matrix; } +// apply an additional inverse rotation to a rotation matrix but +// only use X, Y elements from rotation vector +template +void Matrix3::rotateXYinv(const Vector3 &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 Vector3 Matrix3::operator *(const Vector3 &v) const @@ -161,6 +180,7 @@ void Matrix3::zero(void) template void Matrix3::zero(void); template void Matrix3::rotate(const Vector3 &g); template void Matrix3::rotateXY(const Vector3 &g); +template void Matrix3::rotateXYinv(const Vector3 &g); template void Matrix3::from_euler(float roll, float pitch, float yaw); template void Matrix3::to_euler(float *roll, float *pitch, float *yaw); template Vector3 Matrix3::operator *(const Vector3 &v) const; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index a16487aee0..3e5c183be5 100644 --- a/libraries/AP_Math/matrix3.h +++ b/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 &g); + + // apply an additional inverse rotation to a rotation matrix but + // only use X, Y elements from rotation vector + void rotateXYinv(const Vector3 &g); }; typedef Matrix3 Matrix3i;