mirror of https://github.com/ArduPilot/ardupilot
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:
parent
891f35835e
commit
8c0f065ee4
|
@ -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
|
||||
} else {
|
||||
offset_altitude_cm = 0;
|
||||
}
|
||||
|
||||
// zero out our loiter vals to watch for missed waypoints
|
||||
loiter_delta = 0;
|
||||
|
|
|
@ -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…
Reference in New Issue