Skip to content

Commit fdc287f

Browse files
committed
AP_Mission: emit jump count even if no limit
previously if there was a jump limit we informed the user we were jumping and provided the current count and the limit. This changes things so that if there's no limit we emit the same message, with the count and "unlimited" in place of the limit number
1 parent 5824a12 commit fdc287f

File tree

1 file changed

+14
-16
lines changed

1 file changed

+14
-16
lines changed

libraries/AP_Mission/AP_Mission.cpp

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -2153,24 +2153,18 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
21532153
jump_index = cmd_index;
21542154
}
21552155

2156-
// check if jump command is 'repeat forever'
2157-
if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
2156+
// get number of times jump command has already been run
2157+
if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER ||
2158+
get_jump_times_run(temp_cmd) < temp_cmd.content.jump.num_times) {
2159+
// update the record of the number of times run
2160+
if (increment_jump_num_times_if_found && !_flags.resuming_mission) {
2161+
increment_jump_times_run(temp_cmd, send_gcs_msg);
2162+
}
21582163
// continue searching from jump target
21592164
cmd_index = temp_cmd.content.jump.target;
21602165
} else {
2161-
// get number of times jump command has already been run
2162-
int16_t jump_times_run = get_jump_times_run(temp_cmd);
2163-
if (jump_times_run < temp_cmd.content.jump.num_times) {
2164-
// update the record of the number of times run
2165-
if (increment_jump_num_times_if_found && !_flags.resuming_mission) {
2166-
increment_jump_times_run(temp_cmd, send_gcs_msg);
2167-
}
2168-
// continue searching from jump target
2169-
cmd_index = temp_cmd.content.jump.target;
2170-
} else {
2171-
// jump has been run specified number of times so move search to next command in mission
2172-
cmd_index++;
2173-
}
2166+
// jump has been run specified number of times so move search to next command in mission
2167+
cmd_index++;
21742168
}
21752169
} else {
21762170
// this is a non-jump command so return it
@@ -2306,7 +2300,11 @@ void AP_Mission::increment_jump_times_run(Mission_Command& cmd, bool send_gcs_ms
23062300
if (_jump_tracking[i].index == cmd.index) {
23072301
_jump_tracking[i].num_times_run++;
23082302
if (send_gcs_msg) {
2309-
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
2303+
if (cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
2304+
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/unlimited", _jump_tracking[i].index, _jump_tracking[i].num_times_run);
2305+
} else {
2306+
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
2307+
}
23102308
}
23112309
return;
23122310
} else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {

0 commit comments

Comments
 (0)