Skip to content

Commit

Permalink
AP_Mission: Dirty Debugs
Browse files Browse the repository at this point in the history
  • Loading branch information
@MattKear
MattKear committed Jan 9, 2024
1 parent 2f98c3e commit 51e5ebc
Showing 1 changed file with 23 additions and 0 deletions.
23 changes: 23 additions & 0 deletions libraries/AP_Mission/AP_Mission.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,12 @@ void AP_Mission::resume()
return;
}

GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-A1: _repeat_dist = %u", _repeat_dist);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-A2: _wp_index_history[LAST_WP_PASSED] = %u", _wp_index_history[LAST_WP_PASSED]);
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-A3: _wp_index_history:");
// for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "[i=%u] %u", i, _wp_index_history[i]);
// }
// rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) {
// if not already in a resume state calculate the position to rewind to
Expand Down Expand Up @@ -266,7 +272,9 @@ void AP_Mission::reset()
_prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
init_jump_tracking();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-D2: reset:");
reset_wp_history();

}

/// clear - clears out mission
Expand Down Expand Up @@ -554,6 +562,7 @@ bool AP_Mission::set_current_cmd(uint16_t index)
}

// mission command has been set, don't track history.
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-D1: set curr cmd:");
reset_wp_history();

// sanity check index and that we have a mission
Expand Down Expand Up @@ -1963,6 +1972,8 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
return false;
}

GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-B3: next cmd = %u", cmd_index);

// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// save previous nav command index
Expand All @@ -1985,10 +1996,13 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
// and prevent history being re-written until vehicle returns to interrupted position
if (_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
// update mission history. last index position is always the most recent wp loaded.
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-B2: WP Hist:");
for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
_wp_index_history[i] = _wp_index_history[i+1];
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "[i=%u] %u", i, _wp_index_history[i]);
}
_wp_index_history[AP_MISSION_MAX_WP_HISTORY-1] = _nav_cmd.index;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-B1: Saved index %u", _nav_cmd.index);
}
// check if the vehicle is resuming and has returned to where it was interrupted
if (_flags.resuming_mission && _nav_cmd.index == _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1]) {
Expand Down Expand Up @@ -2779,6 +2793,7 @@ void AP_Mission::reset_wp_history(void)
if (_flags.resuming_mission) {
return;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB-C1: reset history");
for (uint8_t i = 0; i<AP_MISSION_MAX_WP_HISTORY; i++) {
_wp_index_history[i] = AP_MISSION_CMD_INDEX_NONE;
}
Expand All @@ -2802,11 +2817,13 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// check for a recent history
if (_wp_index_history[LAST_WP_PASSED] == AP_MISSION_CMD_INDEX_NONE) {
// no saved history so can't rewind
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB1: No Hist");
return false;
}

// check that we have a valid exit position
if (_exit_position.lat == 0 && _exit_position.lng == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB2: No Exit Pos");
return false;
}

Expand All @@ -2821,12 +2838,14 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// of history array no being completely filled with valid waypoints upon resume.
if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) {
// no more stored history
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB3: No more hist");
resume_index = i+1;
break;
}

if (!read_cmd_from_storage(_wp_index_history[i], temp_cmd)) {
// if read from storage failed then don't use rewind
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB4: Failed mission read");
return false;
}

Expand All @@ -2838,6 +2857,7 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
if (rewind_distance <= 0.0f) {
// history rewound enough distance to meet _repeat_dist requirement
rewind_cmd = temp_cmd;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB5: rewound enough");
break;
}

Expand All @@ -2849,6 +2869,7 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance.
// the last read temp_cmd will be the furthest cmd back in the history array so resume to that.
rewind_cmd = temp_cmd;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: Rewind to max repeat dist (%u m)", _repeat_dist);
return true;
}

Expand All @@ -2861,6 +2882,7 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// fetch next destination wp
if (!read_cmd_from_storage(_wp_index_history[resume_index+1], temp_cmd)) {
// if read from storage failed then don't use rewind
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB7: Failed mission read 2");
return false;
}

Expand All @@ -2881,6 +2903,7 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// continues as planned without further intervention. The resume wp is not written to memory so will not perminantely change the mission.

// if we got this far then mission rewind position was successfully calculated
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: Rewind to lat=%.5f lng=%.5f", ((float)rewind_cmd.content.location.lat)*1e-7, ((float)rewind_cmd.content.location.lng)*1e-7);
return true;
}

Expand Down

0 comments on commit 51e5ebc

Please sign in to comment.