diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ea175d143b..6512c116ab 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -806,6 +806,13 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.set_yaw_speed.relative_angle = packet.param3; // 0 = absolute angle, 1 = relative angle break; + case MAV_CMD_DO_WINCH: // MAV ID: 42600 + cmd.content.winch.num = packet.param1; // winch number + cmd.content.winch.action = packet.param2; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum + cmd.content.winch.release_length = packet.param3; // cable distance to unwind in meters, negative numbers to wind in cable + cmd.content.winch.release_rate = packet.param4; // release rate in meters/second + break; + default: // unrecognised command return MAV_MISSION_UNSUPPORTED; @@ -1255,6 +1262,13 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param3 = cmd.content.set_yaw_speed.relative_angle; // 0 = absolute angle, 1 = relative angle break; + case MAV_CMD_DO_WINCH: + packet.param1 = cmd.content.winch.num; // winch number + packet.param2 = cmd.content.winch.action; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum + packet.param3 = cmd.content.winch.release_length; // cable distance to unwind in meters, negative numbers to wind in cable + packet.param4 = cmd.content.winch.release_rate; // release rate in meters/second + break; + default: // unrecognised command return false; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 0df0710cc4..f981b19239 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -184,6 +184,14 @@ public: uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle }; + // winch command structure + struct PACKED Winch_Command { + uint8_t num; // winch number + uint8_t action; // action (0 = relax, 1 = length control, 2 = rate control) + float release_length; // cable distance to unwind in meters, negative numbers to wind in cable + float release_rate; // release rate in meters/second + }; + union PACKED Content { // jump structure Jump_Command jump; @@ -245,6 +253,9 @@ public: // navigation delay Set_Yaw_Speed set_yaw_speed; + // do-winch + Winch_Command winch; + // location Location location; // Waypoint location