diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index d6fd37fe4d..eeead3df57 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 87978a94da..8d6f7d634c 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -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;