diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index c2c5db7cbb..ce5dd53fc6 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -359,15 +359,15 @@ void AP_AHRS::update_cd_values(void) } /* - create a rotated view of AP_AHRS + create a rotated view of AP_AHRS with pitch trim */ -AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation) +AP_AHRS_View *AP_AHRS::create_view_trim(enum Rotation rotation, float pitch_trim) { if (_view != nullptr) { // can only have one return nullptr; } - _view = new AP_AHRS_View(*this, rotation); + _view = new AP_AHRS_View(*this, rotation, pitch_trim); return _view; } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1d68f1a01b..c39b672312 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -546,8 +546,13 @@ public: } // create a view - AP_AHRS_View *create_view(enum Rotation rotation); - + AP_AHRS_View *create_view(enum Rotation rotation) { + return create_view_trim(rotation, 0.0f); + } + + // create a view with pitch trim + AP_AHRS_View *create_view_trim(enum Rotation rotation, float pitch_trim); + // return calculated AOA float getAOA(void); diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 5e5676c232..f5076799b0 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -21,24 +21,28 @@ #include "AP_AHRS_View.h" #include -AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) : +AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim) : rotation(_rotation), ahrs(_ahrs) { switch (rotation) { case ROTATION_NONE: - rot_view.identity(); break; case ROTATION_PITCH_90: - rot_view.from_euler(0, radians(90), 0); + y_angle = 90; break; case ROTATION_PITCH_270: - rot_view.from_euler(0, radians(270), 0); + y_angle = 270; break; default: AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation); } + // Add pitch trim + y_angle = wrap_360(y_angle + pitch_trim); + + rot_view.from_euler(radians(x_angle), radians(y_angle), radians(z_angle)); + // setup initial state update(); } @@ -49,7 +53,7 @@ void AP_AHRS_View::update(bool skip_ins_update) rot_body_to_ned = ahrs.get_rotation_body_to_ned(); gyro = ahrs.get_gyro(); - if (rotation != ROTATION_NONE) { + if (!is_zero(x_angle) || !is_zero(y_angle) || !is_zero(z_angle)) { Matrix3f &r = rot_body_to_ned; r.transpose(); r = rot_view * r; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 653dfb4790..8868feb0db 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -26,7 +26,7 @@ class AP_AHRS_View { public: // Constructor - AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation); + AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim); // update state void update(bool skip_ins_update=false); @@ -188,4 +188,8 @@ private: float sin_pitch; float sin_yaw; } trig; + + float x_angle; + float y_angle; + float z_angle; };