aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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);
}
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) {
///