Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Ignore mission commands with unsupported altitude values

Browse Source
This commit is contained in:
Jacob Walser 2017-04-17 11:25:41 -04:00
parent 287f2e3c56
commit 004a7aceb7
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduSub/commands_logic.cpp
View File

@ -10,6 +10,20 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
DataFlash.Log_Write_Mission_Cmd(mission, cmd); DataFlash.Log_Write_Mission_Cmd(mission, cmd);
} }
Location_Class target_loc(cmd.content.location);
// target alt must be negative (underwater)
if (target_loc.alt > 0.0f) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
return true;
}
// only tested/supported alt frame so far is ALT_FRAME_ABOVE_HOME, where Home alt is always water's surface ie zero depth
if (target_loc.get_alt_frame() != Location_Class::ALT_FRAME_ABOVE_HOME) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
return true;
}
switch (cmd.id) { switch (cmd.id) {
/// ///