diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 854325b99d..4d80ed05c1 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -404,8 +404,18 @@ bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitc return false; } - // send command to backend - return _backends[instance]->get_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); + // re-use get_attitude_quaternion and convert to Euler angles + Quaternion att_quat; + if (!_backends[instance]->get_attitude_quaternion(att_quat)) { + return false; + } + + float roll_rad, pitch_rad, yaw_rad; + att_quat.to_euler(roll_rad, pitch_rad, yaw_rad); + roll_deg = degrees(roll_rad); + pitch_deg = degrees(pitch_rad); + yaw_bf_deg = degrees(yaw_rad); + return true; } // run pre-arm check. returns false on failure and fills in failure_msg diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 232e732d08..9033a91f05 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -7,24 +7,6 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate -// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame -// returns true on success -bool AP_Mount_Backend::get_attitude_euler(float& roll_deg, float& pitch_deg, float& yaw_bf_deg) -{ - // by default re-use get_attitude_quaternion and convert to Euler angles - Quaternion att_quat; - if (!get_attitude_quaternion(att_quat)) { - return false; - } - - float roll_rad, pitch_rad, yaw_rad; - att_quat.to_euler(roll_rad, pitch_rad, yaw_rad); - roll_deg = degrees(roll_rad); - pitch_deg = degrees(pitch_rad); - yaw_bf_deg = degrees(yaw_rad); - return true; -} - // set angle target in degrees // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index cdf2dab1de..f433d7aa18 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -49,9 +49,8 @@ public: // returns true if this mount can control its pan (required for multicopters) virtual bool has_pan_control() const = 0; - // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame - // returns true on success - bool get_attitude_euler(float& roll_deg, float& pitch_deg, float& yaw_bf_deg); + // get attitude as a quaternion. returns true on success + virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; // get mount's mode enum MAV_MOUNT_MODE get_mode() const { return _mode; } @@ -155,9 +154,6 @@ protected: // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal) virtual bool suppress_heartbeat() const { return false; } - // get attitude as a quaternion. returns true on success - virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; - // get pilot input (in the range -1 to +1) received through RC void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;