From 79a9665c9de9840f7d9ccb518f0ded00a6c67c62 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:11:59 -0800 Subject: [PATCH] Added "slow WP" option for RTL. --- ArduCopter/ArduCopter.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 91c5ea52aa..68bcde7f26 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -666,9 +666,10 @@ static int16_t nav_lat; // The desired bank towards East (Positive) or West (Negative) static int16_t nav_lon; // The Commanded ROll from the autopilot based on optical flow sensor. -static int32_t of_roll = 0; +static int32_t of_roll; // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. -static int32_t of_pitch = 0; +static int32_t of_pitch; +static bool slow_wp = false; //////////////////////////////////////////////////////////////////////////////// @@ -1655,8 +1656,8 @@ void update_throttle_mode(void) // called after a GPS read static void update_navigation() { - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ + // wp_distance is in CM + // -------------------- switch(control_mode){ case AUTO: // note: wp_control is handled by commands_logic @@ -1696,6 +1697,7 @@ static void update_navigation() } wp_control = WP_MODE; + slow_wp = true; // calculates desired Yaw #if FRAME_CONFIG == HELI_FRAME @@ -2109,7 +2111,7 @@ static void update_nav_wp() // calc error to target calc_location_error(&next_WP); - int16_t speed = calc_desired_speed(g.waypoint_speed_max); + int16_t speed = calc_desired_speed(g.waypoint_speed_max, slow_wp); // use error as the desired rate towards the target calc_nav_rate(speed); // rotate pitch and roll to the copter frame of reference