Browse Source

Plane: fixed an integer multiply error that caused poor altitude on landing

the control of altitude between waypoints was broken due to an integer
overflow
master
Andrew Tridgell 12 years ago
parent
commit
8c0f065ee4
  1. 9
      ArduPlane/commands.pde
  2. 4
      ArduPlane/navigation.pde

9
ArduPlane/commands.pde

@ -184,10 +184,13 @@ static void set_next_WP(struct Location *wp) @@ -184,10 +184,13 @@ static void set_next_WP(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))
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;
} else {
offset_altitude_cm = 0;
}
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;

4
ArduPlane/navigation.pde

@ -113,9 +113,9 @@ static void calc_bearing_error() @@ -113,9 +113,9 @@ static void calc_bearing_error()
static void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude_cm != 0) {
if (control_mode == AUTO && offset_altitude_cm != 0) {
// limit climb rates
target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30));
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) {

Loading…
Cancel
Save