Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added "slow WP" option for RTL.

Browse Source
This commit is contained in:
Jason Short 2012-02-19 13:11:59 -08:00
parent ba2cfee2fb
commit 79a9665c9d
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/ArduCopter.pde
View File

@ -666,9 +666,10 @@ static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative) // The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon; static int16_t nav_lon;
// The Commanded ROll from the autopilot based on optical flow sensor. // 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. // 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 // called after a GPS read
static void update_navigation() 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){ switch(control_mode){
case AUTO: case AUTO:
// note: wp_control is handled by commands_logic // note: wp_control is handled by commands_logic
@ -1696,6 +1697,7 @@ static void update_navigation()
} }
wp_control = WP_MODE; wp_control = WP_MODE;
slow_wp = true;
// calculates desired Yaw // calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
@ -2109,7 +2111,7 @@ static void update_nav_wp()
// calc error to target // calc error to target
calc_location_error(&next_WP); 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 // use error as the desired rate towards the target
calc_nav_rate(speed); calc_nav_rate(speed);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference