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
This commit is contained in:
Andrew Tridgell 2013-02-13 19:32:44 +11:00
parent 891f35835e
commit 8c0f065ee4
2 changed files with 8 additions and 5 deletions

View File

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

View File

@ -113,9 +113,9 @@ static void calc_bearing_error()
static void calc_altitude_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 // 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 // stay within a certain range
if(prev_WP.alt > next_WP.alt) { if(prev_WP.alt > next_WP.alt) {