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;
};
-