Plane: more fixes for glide slope calculations

This commit is contained in:
Andrew Tridgell 2014-08-04 16:55:15 +10:00
parent 52a3dc2bde
commit f983040401
3 changed files with 8 additions and 4 deletions

View File

@ -349,11 +349,15 @@ static bool above_location_current(const Location &loc)
if (!loc.flags.relative_alt) { if (!loc.flags.relative_alt) {
loc_alt -= home.alt*0.01f; loc_alt -= home.alt*0.01f;
} }
return terrain_alt > loc.alt; return terrain_alt > loc_alt;
} }
#endif #endif
return current_loc.alt > loc.alt; float loc_alt_cm = loc.alt;
if (!loc.flags.relative_alt) {
loc_alt_cm -= home.alt;
}
return current_loc.alt > loc_alt_cm;
} }
/* /*

View File

@ -48,7 +48,7 @@ static void set_next_WP(const struct Location& loc)
// used to control FBW and limit the rate of climb // used to control FBW and limit the rate of climb
// ----------------------------------------------- // -----------------------------------------------
set_target_altitude_current(); set_target_altitude_location(next_WP_loc);
// zero out our loiter vals to watch for missed waypoints // zero out our loiter vals to watch for missed waypoints
loiter_angle_reset(); loiter_angle_reset();

View File

@ -250,10 +250,10 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
static void do_RTL(void) static void do_RTL(void)
{ {
control_mode = RTL;
prev_WP_loc = current_loc; prev_WP_loc = current_loc;
next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
setup_terrain_target_alt(next_WP_loc); setup_terrain_target_alt(next_WP_loc);
set_target_altitude_location(next_WP_loc);
if (g.loiter_radius < 0) { if (g.loiter_radius < 0) {
loiter.direction = -1; loiter.direction = -1;