From 8c0f065ee48282b57c136fc9266b22e7407fbc5d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Feb 2013 19:32:44 +1100 Subject: [PATCH] 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 --- ArduPlane/commands.pde | 9 ++++++--- ArduPlane/navigation.pde | 4 ++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 5d67c8852f..5f7f3350be 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -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; diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 347f9461e6..3de3f5fa46 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -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) {