diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index e8340155e8..1c13e6fefd 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -379,6 +379,18 @@ void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan) } } +// 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) +{ + if (!check_instance(instance)) { + return false; + } + + // send command to backend + return _backends[instance]->get_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); +} + // run pre-arm check. returns false on failure and fills in failure_msg // any failure_msg returned will not include a prefix bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b5576aef72..caa9763906 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -154,6 +154,10 @@ public: // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS void send_gimbal_device_attitude_status(mavlink_channel_t chan); + // 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); + // run pre-arm check. returns false on failure and fills in failure_msg // any failure_msg returned will not include a prefix bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9033a91f05..232e732d08 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -7,6 +7,24 @@ 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 b9dedcaf88..14b6d8b720 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -49,6 +49,10 @@ 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 mount's mode enum MAV_MOUNT_MODE get_mode() const { return _mode; }