diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp index a2598b4ac7..4aa009c4b3 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.cpp +++ b/libraries/AP_Gimbal/AP_Gimbal.cpp @@ -178,6 +178,12 @@ void AP_Gimbal::update_target(Vector3f newTarget) } +Vector3f AP_Gimbal::getGimbalEstimateEF() +{ + Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z); + return eulerEst; +} + uint8_t AP_Gimbal::isCopterFliped(){ return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f; } \ No newline at end of file diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h index 2329747469..71420664e2 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.h +++ b/libraries/AP_Gimbal/AP_Gimbal.h @@ -43,6 +43,8 @@ public: void update_target(Vector3f newTarget); void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg); + Vector3f getGimbalEstimateEF(); + struct Measurament { float delta_time; Vector3f delta_angles; diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 015a7a96c8..5dc4dd6144 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -87,7 +87,8 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan) { - // do nothing - we rely on the mount sending the messages directly + Vector3f est = _gimbal.getGimbalEstimateEF(); + mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100); } /*