diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 867ec88903..ccea42410f 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -657,8 +657,6 @@ protected: MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet); MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet); - virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const; - MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc); @@ -730,12 +728,16 @@ protected: */ uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size); +#if AP_MAVLINK_COMMAND_LONG_ENABLED // converts a COMMAND_LONG packet to a COMMAND_INT packet, where // the command-long packet is assumed to be in the supplied frame. // If location is not present in the command then just omit frame. // this method ensures the passed-in structure is entirely // initialised. virtual void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT); + virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const; + MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); +#endif // methods to extract a Location object from a command_int bool location_from_command_t(const mavlink_command_int_t &in, Location &out); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f2fb1c5bb0..f7ead3489f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4757,6 +4757,49 @@ MAV_RESULT GCS_MAVLINK::handle_command_component_arm_disarm(const mavlink_comman return MAV_RESULT_UNSUPPORTED; } +bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Location &out) +{ + if (!command_long_stores_location((MAV_CMD)in.command)) { + return false; + } + + // integer storage imposes limits on the altitudes we can accept: + if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) { + return false; + } + + Location::AltFrame frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in.frame, frame)) { + // unknown coordinate frame + return false; + } + + out.lat = in.x; + out.lng = in.y; + + out.set_alt_cm(int32_t(in.z * 100), frame); + + return true; +} + +bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command) +{ + switch(command) { + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_ROI: + case MAV_CMD_DO_SET_ROI_LOCATION: + // case MAV_CMD_NAV_TAKEOFF: // technically yes, but we don't do lat/lng + // case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_DO_REPOSITION: + case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: + return true; + default: + return false; + } + return false; +} + +#if AP_MAVLINK_COMMAND_LONG_ENABLED // when conveyed via COMMAND_LONG, a command doesn't come with an // explicit frame. When conveying a location they do have an assumed // frame in ArduPilot, and this function returns that frame. @@ -4801,49 +4844,6 @@ MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_lo return handle_command_int_packet(command_int, msg); } -bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Location &out) -{ - if (!command_long_stores_location((MAV_CMD)in.command)) { - return false; - } - - // integer storage imposes limits on the altitudes we can accept: - if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) { - return false; - } - - Location::AltFrame frame; - if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in.frame, frame)) { - // unknown coordinate frame - return false; - } - - out.lat = in.x; - out.lng = in.y; - - out.set_alt_cm(int32_t(in.z * 100), frame); - - return true; -} - -#if AP_MAVLINK_COMMAND_LONG_ENABLED -bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command) -{ - switch(command) { - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_ROI: - case MAV_CMD_DO_SET_ROI_LOCATION: - // case MAV_CMD_NAV_TAKEOFF: // technically yes, but we don't do lat/lng - // case MAV_CMD_NAV_VTOL_TAKEOFF: - case MAV_CMD_DO_REPOSITION: - case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: - return true; - default: - return false; - } - return false; -} - // returns a value suitable for COMMAND_INT.x or y based on a value // coming in from COMMAND_LONG.p5 or p6: static int32_t convert_COMMAND_LONG_loc_param(float param, bool stores_location) @@ -4909,6 +4909,26 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) hal.util->persistent_data.last_mavlink_cmd = 0; } + +#else +void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) +{ + // decode packet + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(&msg, &packet); + + // send ACK or NAK + mavlink_msg_command_ack_send( + chan, + packet.command, + MAV_RESULT_COMMAND_INT_ONLY, + 0, + 0, + msg.sysid, + msg.compid + ); + +} #endif // AP_MAVLINK_COMMAND_LONG_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc)