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

Plane: use a glide slope in RTL when descending

Browse Source 
when above the target altitude in RTL come down slowly, when below
climb rapidly

This fixes issue #39
This commit is contained in:
Andrew Tridgell 2013-07-05 14:56:58 +10:00
parent d30dd7c3f1
commit 1d6748cf3f
3 changed files with 48 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/commands.pde
View File

@ -184,20 +184,10 @@ static void set_next_WP(const struct Location *wp)
// -----------------------------------------------
target_altitude_cm = current_loc.alt;
if (prev_WP.id != MAV_CMD_NAV_TAKEOFF &&
prev_WP.alt != home.alt &&
(next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
offset_altitude_cm = next_WP.alt - prev_WP.alt;
} else {
offset_altitude_cm = 0;
}
// zero out our loiter vals to watch for missed waypoints
loiter_angle_reset();
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
setup_glide_slope();
loiter_angle_reset();
}
@ -221,11 +211,8 @@ static void set_guided_WP(void)
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude_cm = current_loc.alt;
offset_altitude_cm = next_WP.alt - prev_WP.alt;
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
setup_glide_slope();
loiter_angle_reset();
}

5
ArduPlane/commands_logic.pde
View File

@ -232,7 +232,8 @@ static bool verify_condition_command() // Returns true if command compl
static void do_RTL(void)
{
control_mode = RTL;
next_WP = home;
prev_WP = current_loc;
next_WP = home;
if (g.loiter_radius < 0) {
loiter.direction = -1;
@ -245,6 +246,8 @@ static void do_RTL(void)
// -------------------------
next_WP.alt = read_alt_to_hold();
setup_glide_slope();
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}

45
ArduPlane/navigation.pde
View File

@ -132,12 +132,12 @@ static void calc_altitude_error()
if (control_mode == FLY_BY_WIRE_B) {
return;
}
if (control_mode == AUTO && offset_altitude_cm != 0) {
// limit climb rates
if (offset_altitude_cm != 0) {
// control climb/descent rate
target_altitude_cm = next_WP.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));
// stay within a certain range
if(prev_WP.alt > next_WP.alt) {
if (prev_WP.alt > next_WP.alt) {
target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.alt, prev_WP.alt);
}else{
target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.alt, next_WP.alt);
@ -154,3 +154,42 @@ static void update_loiter()
nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
}
static void setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
/*
work out if we will gradually change altitude, or try to get to
the new altitude as quickly as possible.
*/
switch (control_mode) {
case RTL:
case GUIDED:
/* glide down slowly if above target altitude, but ascend more
rapidly if below it. See
https://github.com/diydrones/ardupilot/issues/39
*/
if (current_loc.alt > next_WP.alt) {
offset_altitude_cm = next_WP.alt - current_loc.alt;
} else {
offset_altitude_cm = 0;
}
break;
case AUTO:
if (prev_WP.id != MAV_CMD_NAV_TAKEOFF &&
prev_WP.alt != home.alt &&
(next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
offset_altitude_cm = next_WP.alt - prev_WP.alt;
} else {
offset_altitude_cm = 0;
}
break;
default:
offset_altitude_cm = 0;
break;
}
}