From ee19b09e9f3d78b18d360b8525045b3374cff927 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Aug 2013 10:18:29 +1000 Subject: [PATCH] Rover: fixes for new AP_Math location API Pair-Programmed-With: Brandon Jones --- APMrover2/APMrover2.pde | 2 +- APMrover2/commands.pde | 8 ++++---- APMrover2/commands_logic.pde | 6 +++--- APMrover2/navigation.pde | 6 +++--- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 08c1b22358..4499786a66 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -808,7 +808,7 @@ static void update_GPS(void) } } - have_position = ahrs.get_projected_position(¤t_loc); + have_position = ahrs.get_projected_position(current_loc); if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) { gps_fix_count++; diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index c1e39135d8..15fc7dd5d5 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -121,9 +121,9 @@ static void set_next_WP(const struct Location *wp) } // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_totalDistance = get_distance(current_loc, next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing_cd(¤t_loc, &next_WP); + target_bearing = get_bearing_cd(current_loc, next_WP); nav_bearing = target_bearing; // set a new crosstrack bearing @@ -142,9 +142,9 @@ static void set_guided_WP(void) next_WP = guided_WP; // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_totalDistance = get_distance(current_loc, next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing_cd(¤t_loc, &next_WP); + target_bearing = get_bearing_cd(current_loc, next_WP); // set a new crosstrack bearing // ---------------------------- diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index a6e0a62571..c9b9c3477f 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -216,7 +216,7 @@ static bool verify_nav_wp() if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"), (unsigned)nav_command_index, - (unsigned)get_distance(¤t_loc, &next_WP)); + (unsigned)get_distance(current_loc, next_WP)); return true; } @@ -224,7 +224,7 @@ static bool verify_nav_wp() if (location_passed_point(current_loc, prev_WP, next_WP)) { gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"), (unsigned)nav_command_index, - (unsigned)get_distance(¤t_loc, &next_WP)); + (unsigned)get_distance(current_loc, next_WP)); return true; } @@ -242,7 +242,7 @@ static bool verify_RTL() // have we gone past the waypoint? if (location_passed_point(current_loc, prev_WP, next_WP)) { gcs_send_text_fmt(PSTR("Reached Home dist %um"), - (unsigned)get_distance(¤t_loc, &next_WP)); + (unsigned)get_distance(current_loc, next_WP)); return true; } diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index d414df1ac3..3b11014f6b 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -17,7 +17,7 @@ static void navigate() // waypoint distance from rover // ---------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); + wp_distance = get_distance(current_loc, next_WP); if (wp_distance < 0){ gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); @@ -26,7 +26,7 @@ static void navigate() // target_bearing is where we should be heading // -------------------------------------------- - target_bearing = get_bearing_cd(¤t_loc, &next_WP); + target_bearing = get_bearing_cd(current_loc, next_WP); // nav_bearing will includes xtrac correction // ------------------------------------------ @@ -61,7 +61,7 @@ static void update_crosstrack(void) static void reset_crosstrack() { - crosstrack_bearing = get_bearing_cd(&prev_WP, &next_WP); // Used for track following + crosstrack_bearing = get_bearing_cd(prev_WP, next_WP); // Used for track following } void reached_waypoint()