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

Copter: Guided: stop aircraft if an invalid command is sent

This commit is contained in:
Leonard Hall 2021-07-09 14:57:15 +09:30 committed by Andrew Tridgell
parent 03373c6962
commit d51ab7fcc9
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/GCS_Mavlink.cpp
View File

@ -1119,6 +1119,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
// input is not valid so stop
copter.mode_guided.init(true);
break; break;
} }
@ -1221,11 +1223,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
if (!pos_ignore) { if (!pos_ignore) {
// sanity check location // sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) { if (!check_latlng(packet.lat_int, packet.lon_int)) {
// input is not valid so stop
copter.mode_guided.init(true);
break; break;
} }
Location::AltFrame frame; Location::AltFrame frame;
if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) { if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
// unknown coordinate frame // unknown coordinate frame
// input is not valid so stop
copter.mode_guided.init(true);
break; break;
} }
loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame}; loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
@ -1262,10 +1268,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
// convert Location to vector from ekf origin for posvel controller // convert Location to vector from ekf origin for posvel controller
if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) { if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
// posvel controller does not support alt-above-terrain // posvel controller does not support alt-above-terrain
// input is not valid so stop
copter.mode_guided.init(true);
break; break;
} }
Vector3f pos_neu_cm; Vector3f pos_neu_cm;
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
// input is not valid so stop
copter.mode_guided.init(true);
break; break;
} }
copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);