Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: return MAV_RESULT_COMMAND_INT_ONLY if command-long support not compiled in

This commit is contained in:
Peter Barker 2023-11-27 07:56:37 +11:00 committed by Peter Barker
parent 81ab6a9461
commit 4dae077787
2 changed files with 67 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/GCS_MAVLink/GCS.h
View File

@ -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);

106
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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)