From c62c64d27b91772cad2dc5bc12b16665ca069dd4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Feb 2017 19:15:34 +1100 Subject: [PATCH] AP_AHRS: added support for AP_AHRS_View --- libraries/AP_AHRS/AP_AHRS.cpp | 112 ++++++++++++++++----------- libraries/AP_AHRS/AP_AHRS.h | 16 ++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 6 ++ libraries/AP_AHRS/AP_AHRS_View.cpp | 24 ++++++ libraries/AP_AHRS/AP_AHRS_View.h | 26 +++---- 5 files changed, 126 insertions(+), 58 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 0d74567f03..33c3ec348f 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -15,7 +15,9 @@ along with this program. If not, see . */ #include "AP_AHRS.h" +#include "AP_AHRS_View.h" #include + extern const AP_HAL::HAL& hal; // table of user settable parameters @@ -229,6 +231,55 @@ Vector2f AP_AHRS::groundspeed_vector(void) return Vector2f(0.0f, 0.0f); } +/* + calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix + */ +void AP_AHRS::calc_trig(const Matrix3f &rot, + float &cr, float &cp, float &cy, + float &sr, float &sp, float &sy) const +{ + Vector2f yaw_vector; + + yaw_vector.x = rot.a.x; + yaw_vector.y = rot.b.x; + if (fabsf(yaw_vector.x) > 0 || + fabsf(yaw_vector.y) > 0) { + yaw_vector.normalize(); + } + sy = constrain_float(yaw_vector.y, -1.0, 1.0); + cy = constrain_float(yaw_vector.x, -1.0, 1.0); + + // sanity checks + if (yaw_vector.is_inf() || yaw_vector.is_nan()) { + sy = 0.0f; + cy = 1.0f; + } + + float cx2 = rot.c.x * rot.c.x; + if (cx2 >= 1.0f) { + cp = 0; + cr = 1.0f; + } else { + cp = safe_sqrt(1 - cx2); + cr = rot.c.z / cp; + } + cp = constrain_float(cp, 0, 1.0); + cr = constrain_float(cr, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing + + sp = -rot.c.x; + + if (!is_zero(cp)) { + sr = rot.c.y / cp; + } + + if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) { + float r, p, y; + rot.to_euler(&r, &p, &y); + cr = cosf(r); + sr = sinf(r); + } +} + // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude // should be called after _dcm_matrix is updated void AP_AHRS::update_trig(void) @@ -239,51 +290,9 @@ void AP_AHRS::update_trig(void) _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed(); } - Vector2f yaw_vector; - const Matrix3f &temp = get_rotation_body_to_ned(); - - // sin_yaw, cos_yaw - yaw_vector.x = temp.a.x; - yaw_vector.y = temp.b.x; - yaw_vector.normalize(); - _sin_yaw = constrain_float(yaw_vector.y, -1.0, 1.0); - _cos_yaw = constrain_float(yaw_vector.x, -1.0, 1.0); - - // cos_roll, cos_pitch - float cx2 = temp.c.x * temp.c.x; - if (cx2 >= 1.0f) { - _cos_pitch = 0; - _cos_roll = 1.0f; - } else { - _cos_pitch = safe_sqrt(1 - cx2); - _cos_roll = temp.c.z / _cos_pitch; - } - _cos_pitch = constrain_float(_cos_pitch, 0, 1.0); - _cos_roll = constrain_float(_cos_roll, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing - - // sin_roll, sin_pitch - _sin_pitch = -temp.c.x; - if (is_zero(_cos_pitch)) { - _sin_roll = sinf(roll); - } else { - _sin_roll = temp.c.y / _cos_pitch; - } - - // sanity checks - if (yaw_vector.is_inf() || yaw_vector.is_nan()) { - yaw_vector.x = 0.0f; - yaw_vector.y = 0.0f; - _sin_yaw = 0.0f; - _cos_yaw = 1.0f; - } - - if (isinf(_cos_roll) || isnan(_cos_roll)) { - _cos_roll = cosf(roll); - } - - if (isinf(_sin_roll) || isnan(_sin_roll)) { - _sin_roll = sinf(roll); - } + calc_trig(get_rotation_body_to_ned(), + _cos_roll, _cos_pitch, _cos_yaw, + _sin_roll, _sin_pitch, _sin_yaw); } /* @@ -297,3 +306,16 @@ void AP_AHRS::update_cd_values(void) if (yaw_sensor < 0) yaw_sensor += 36000; } + +/* + create a rotated view of AP_AHRS + */ +AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation) +{ + if (_view != nullptr) { + // can only have one + return nullptr; + } + _view = new AP_AHRS_View(*this, rotation); + return _view; +} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 289eac26b0..69f8bd093d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -43,9 +43,14 @@ enum AHRS_VehicleClass { }; +// forward declare view class +class AP_AHRS_View; + class AP_AHRS { public: + friend class AP_AHRS_View; + // Constructor AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : roll(0.0f), @@ -499,6 +504,9 @@ public: // Retrieves the corrected NED delta velocity in use by the inertial navigation virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); } + + // create a view + AP_AHRS_View *create_view(enum Rotation rotation); protected: AHRS_VehicleClass _vehicle_class; @@ -525,6 +533,11 @@ protected: uint8_t wind_estimation : 1; // 1 if we should do wind estimation } _flags; + // calculate sin/cos of roll/pitch/yaw from rotation + void calc_trig(const Matrix3f &rot, + float &cr, float &cp, float &cy, + float &sr, float &sp, float &sy) const; + // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude // should be called after _dcm_matrix is updated void update_trig(void); @@ -584,6 +597,9 @@ protected: // which accelerometer instance is active uint8_t _active_accel_instance; + + // optional view class + AP_AHRS_View *_view; }; #include "AP_AHRS_DCM.h" diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 6c2804e24e..60150d03cb 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -20,6 +20,7 @@ */ #include #include "AP_AHRS.h" +#include "AP_AHRS_View.h" #include #include #include @@ -99,6 +100,11 @@ void AP_AHRS_NavEKF::update(void) const Vector3f &exported_gyro_bias = get_gyro_drift(); hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y); } + + if (_view != nullptr) { + // update optional alternative attitude view + _view->update(); + } } void AP_AHRS_NavEKF::update_DCM(void) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 04e7502ec8..fd9d8fc4a7 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -19,12 +19,28 @@ */ #include "AP_AHRS_View.h" +#include AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) : 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); + break; + case ROTATION_PITCH_270: + rot_view.from_euler(0, radians(270), 0); + break; + default: + AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation); + } + // setup initial state + update(); } // update state @@ -33,6 +49,14 @@ void AP_AHRS_View::update(void) rot_body_to_ned = ahrs.get_rotation_body_to_ned(); gyro = ahrs.get_gyro(); + if (rotation != ROTATION_NONE) { + Matrix3f &r = rot_body_to_ned; + r.transpose(); + r = rot_view * r; + r.transpose(); + gyro.rotate(rotation); + } + rot_body_to_ned.to_euler(&roll, &pitch, &yaw); roll_sensor = degrees(roll) * 100; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index a8382e1a4e..d5ecc1ab6a 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -30,7 +30,7 @@ public: // update state void update(void); - + // empty virtual destructor virtual ~AP_AHRS_View() {} @@ -52,20 +52,20 @@ public: float cos_pitch() const { return trig.cos_pitch; } - float cos_yaw() const { + float cos_yaw() const { return trig.cos_yaw; } - float sin_roll() const { + float sin_roll() const { return trig.sin_roll; } float sin_pitch() const { return trig.sin_pitch; } - float sin_yaw() const { + float sin_yaw() const { return trig.sin_yaw; } - + /* wrappers around ahrs functions which pass-thru directly. See AP_AHRS.h for description of each function @@ -73,15 +73,15 @@ public: bool get_position(struct Location &loc) const { return ahrs.get_position(loc); } - + Vector3f wind_estimate(void) { return ahrs.wind_estimate(); } - + bool airspeed_estimate(float *airspeed_ret) const { return ahrs.airspeed_estimate(airspeed_ret); } - + bool airspeed_estimate_true(float *airspeed_ret) const { return ahrs.airspeed_estimate_true(airspeed_ret); } @@ -129,21 +129,22 @@ public: uint32_t getLastPosDownReset(float &posDelta) const { return ahrs.getLastPosDownReset(posDelta); } - + float roll; float pitch; float yaw; int32_t roll_sensor; int32_t pitch_sensor; int32_t yaw_sensor; - + private: const enum Rotation rotation; AP_AHRS &ahrs; - + + Matrix3f rot_view; Matrix3f rot_body_to_ned; Vector3f gyro; - + struct { float cos_roll; float cos_pitch; @@ -153,4 +154,3 @@ private: float sin_yaw; } trig; }; -