Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow exiting loiter_to_alt in cases where you get stuck

Browse Source 
- check for scenarios where updrafts can keep you from loitering down indefinitely.
This commit is contained in:
Tom Pittenger 2017-02-21 03:56:32 -08:00 committed by Tom Pittenger
parent f5c89117f6
commit 4ccd59f394
3 changed files with 46 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/Plane.h
View File

@ -641,9 +641,20 @@ private:
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
int8_t direction;
// when loitering and an altitude is involved, this flag is true when it has been reached at least once
bool reached_target_alt;
// check for scenarios where updrafts can keep you from loitering down indefinitely.
bool unable_to_acheive_target_alt;
// start time of the loiter. Milliseconds.
uint32_t start_time_ms;
// altitude at start of loiter loop lap. Used to detect delta alt of each lap.
// only valid when sum_cd > 36000
int32_t start_lap_alt_cm;
int32_t next_sum_lap_cd;
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
uint32_t time_max_ms;
} loiter;

28
ArduPlane/commands_logic.cpp
View File

@ -499,12 +499,8 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
set_next_WP(loc);
loiter_set_direction_wp(cmd);
// used to signify primary turns goal not yet met when non-zero
condition_value = next_WP_loc.alt;
if (condition_value == 0) {
// the value of 0 is used to signify it has been reached. Lets bump alt to 1 which is 10cm. Close enough!
condition_value = 1;
}
// init to 0, set to 1 when altitude is reached
condition_value = 0;
}
/********************************************************************************/
@ -674,7 +670,7 @@ bool Plane::verify_loiter_time()
}
if (result) {
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete");
gcs_send_text(MAV_SEVERITY_INFO,"Loiter time complete");
auto_state.vtol_loiter = false;
}
return result;
@ -702,7 +698,7 @@ bool Plane::verify_loiter_turns()
}
if (result) {
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
gcs_send_text(MAV_SEVERITY_INFO,"Loiter orbits complete");
}
return result;
}
@ -717,12 +713,16 @@ bool Plane::verify_loiter_to_alt()
bool result = false;
update_loiter(mission.get_current_nav_cmd().p1);
//has target altitude been reached?
if (condition_value != 0) {
// primary goal, loiter alt
if (labs(condition_value - current_loc.alt) < 500 && loiter.sum_cd > 1) {
// condition_value == 0 means alt has never been reached
if (condition_value == 0) {
// primary goal, loiter to alt
if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
if (loiter.unable_to_acheive_target_alt) {
gcs_send_text_fmt(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100);
}
condition_value = 1;
result = verify_loiter_heading(true);
}
} else {
@ -731,7 +731,7 @@ bool Plane::verify_loiter_to_alt()
}
if (result) {
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER alt complete");
gcs_send_text(MAV_SEVERITY_INFO,"Loiter to alt complete");
}
return result;
}

23
ArduPlane/navigation.cpp
View File

@ -22,6 +22,8 @@ void Plane::loiter_angle_reset(void)
{
loiter.sum_cd = 0;
loiter.total_cd = 0;
loiter.reached_target_alt = false;
loiter.unable_to_acheive_target_alt = false;
}
/*
@ -30,21 +32,38 @@ void Plane::loiter_angle_reset(void)
*/
void Plane::loiter_angle_update(void)
{
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
static const int32_t lap_check_interval_cd = 3*36000;
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd = lap_check_interval_cd;
} else {
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
}
loiter.old_target_bearing_cd = target_bearing_cd;
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
loiter.reached_target_alt = true;
loiter.unable_to_acheive_target_alt = false;
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
} else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
// check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long
loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd += lap_check_interval_cd;
}
}
//****************************************************************