From 3a1e9e43a1c33c8c80680e70c2652eedf2a272f9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Mar 2013 13:14:14 +1100 Subject: [PATCH] Rover: use new angle wrap code --- APMrover2/navigation.pde | 20 +++----------------- APMrover2/test.pde | 2 +- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 8d05a12b56..b48c1e3ce9 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -42,32 +42,18 @@ static void calc_bearing_error() { static butter10hz1_6 butter; - bearing_error_cd = wrap_180(nav_bearing - ahrs.yaw_sensor); + bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor); bearing_error_cd = butter.filter(bearing_error_cd); } -static long wrap_360(long error) -{ - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; -} - -static long wrap_180(long error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - static void update_crosstrack(void) { // Crosstrack Error // ---------------- - if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following + if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing = wrap_360(nav_bearing); + nav_bearing = wrap_360_cd(nav_bearing); } } diff --git a/APMrover2/test.pde b/APMrover2/test.pde index a97256117b..8981dc728b 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -517,7 +517,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) if (compass.healthy) { Vector3f maggy = compass.get_offsets(); cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(heading) * 100)) /100, + (wrap_360_cd(ToDeg(heading) * 100)) /100, (int)compass.mag_x, (int)compass.mag_y, (int)compass.mag_z,