diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 8dc31bbfa3..0b64b1063b 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif -// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame -// returns true on success -bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +// get attitude as a quaternion. returns true on success. +// att_quat will be an earth-frame quaternion rotated such that +// yaw is in body-frame. +bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } + return backend->get_attitude_quaternion(att_quat); +} +// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame +// returns true on success +bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +{ // re-use get_attitude_quaternion and convert to Euler angles Quaternion att_quat; - if (!backend->get_attitude_quaternion(att_quat)) { + if (!get_attitude_quaternion(instance, att_quat)) { return false; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e9153..d34f581636 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -206,6 +206,11 @@ public: bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const; #endif + // get attitude as a quaternion. returns true on success. + // att_quat will be an earth-frame quaternion rotated such that + // yaw is in body-frame. + bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat); + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);