From 87a260656e14199ad098fb0842ebd5d13fc6cb7e Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Thu, 21 Jul 2011 23:36:22 +0000 Subject: [PATCH] fix float issues - crosstrack & target alt git-svn-id: https://arducopter.googlecode.com/svn/trunk@2935 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index c7e0e24c84..8fffd2a76c 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -181,7 +181,7 @@ static void calc_altitude_error() static void calc_altitude_smoothing_error() { // limit climb rates - we draw a straight line between first location and edge of waypoint_radius - target_altitude = next_WP.alt - ((wp_distance * (next_WP.alt - prev_WP.alt)) / (wp_totalDistance - g.waypoint_radius)); + target_altitude = next_WP.alt - ((float)(wp_distance * (next_WP.alt - prev_WP.alt)) / (float)(wp_totalDistance - g.waypoint_radius)); // stay within a certain range if(prev_WP.alt > next_WP.alt){ @@ -212,7 +212,7 @@ static void update_crosstrack(void) // Crosstrack Error // ---------------- if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100; nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing);