diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 380128e180..028fd5649e 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -117,6 +117,9 @@ public: // reset the current attitude, used on new IMU calibration virtual void reset(bool recover_eulers=false) = 0; + // reset the current attitude, used on new IMU calibration + virtual void reset_attitude(const float &roll, const float &pitch, const float &yaw) = 0; + // how often our attitude representation has gone out of range uint8_t renorm_range_count; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index b0240e6690..a7b09921d2 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -117,6 +117,12 @@ AP_AHRS_DCM::reset(bool recover_eulers) } } +// reset the current attitude, used by HIL +void AP_AHRS_DCM::reset_attitude(const float &roll, const float &pitch, const float &yaw) +{ + _dcm_matrix.from_euler(roll, pitch, yaw); +} + /* * check the DCM matrix for pathological values */ diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index b3372d9bec..f3a8cf64a8 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -55,6 +55,9 @@ public: void update(void); void reset(bool recover_eulers = false); + // reset the current attitude, used on new IMU calibration + void reset_attitude(const float &roll, const float &pitch, const float &yaw); + // dead-reckoning support bool get_position(struct Location &loc); diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.h b/libraries/AP_AHRS/AP_AHRS_HIL.h index 7081114510..7c55d8f4c0 100644 --- a/libraries/AP_AHRS/AP_AHRS_HIL.h +++ b/libraries/AP_AHRS/AP_AHRS_HIL.h @@ -36,6 +36,10 @@ public: void reset(bool recover_eulers=false) { } + void reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) { + // not implemented - use setHil() + } + float get_error_rp(void) { return 0; }