From 2184ff1e58f283282dce5742c204e4a0aeb3d4b5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 17 Mar 2014 16:45:45 +0900 Subject: [PATCH] Plane: set_next_WP fn accepts Location --- ArduPlane/commands.pde | 7 +++---- ArduPlane/commands_logic.pde | 12 ++++++------ 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 353f1e0e16..d7483683fd 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -13,10 +13,9 @@ static int32_t read_alt_to_hold() /* - * This function stores waypoint commands - * It looks to see what the next command type is and finds the last command. + * set_next_WP - sets the target location the vehicle should fly to */ -static void set_next_WP(const AP_Mission::Mission_Command& cmd) +static void set_next_WP(const struct Location& loc) { // copy the current WP into the OldWP slot // --------------------------------------- @@ -24,7 +23,7 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd) // Load the next_WP slot // --------------------- - next_WP_loc = cmd.content.location; + next_WP_loc = loc; // if lat and lon is zero, then use current lat/lon // this allows a mission to contain a "loiter on the spot" diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 544eb1f7c7..caa516fc91 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -257,7 +257,7 @@ static void do_RTL(void) static void do_takeoff(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 takeoff_pitch_cd = (int)cmd.p1 * 100; takeoff_altitude_cm = next_WP_loc.alt; @@ -269,12 +269,12 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd) static void do_nav_wp(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); } static void do_land(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); } static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) @@ -288,20 +288,20 @@ static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); loiter_set_direction_wp(cmd); } static void do_loiter_turns(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); loiter.total_cd = cmd.p1 * 36000UL; loiter_set_direction_wp(cmd); } static void do_loiter_time(const AP_Mission::Mission_Command& cmd) { - set_next_WP(cmd); + set_next_WP(cmd.content.location); // we set start_time_ms when we reach the waypoint loiter.start_time_ms = 0; loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds