Browse Source

AP_AHRS: added get_DCM_rotation_body_to_ned()

this specifically asks for the DCM attitude, which will be used for
the comass calibration to help determine compass orientation
mission-4.1.18
Andrew Tridgell 7 years ago
parent
commit
1056e64419
  1. 3
      libraries/AP_AHRS/AP_AHRS.h
  2. 3
      libraries/AP_AHRS/AP_AHRS_DCM.h

3
libraries/AP_AHRS/AP_AHRS.h

@ -263,6 +263,9 @@ public: @@ -263,6 +263,9 @@ public:
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
// get rotation matrix specifically from DCM backend (used for compass calibrator)
virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
// get our current position estimate. Return true if a position is available,
// otherwise false. This call fills in lat, lng and alt
virtual bool get_position(struct Location &loc) const = 0;

3
libraries/AP_AHRS/AP_AHRS_DCM.h

@ -53,6 +53,9 @@ public: @@ -53,6 +53,9 @@ public:
return _body_dcm_matrix;
}
// get rotation matrix specifically from DCM backend (used for compass calibrator)
const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; }
// return the current drift correction integrator value
const Vector3f &get_gyro_drift() const override {
return _omega_I;

Loading…
Cancel
Save