Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: commands_logic to use auto_rtl

Browse Source
This commit is contained in:
Randy Mackay 2014-01-28 12:21:45 +09:00 committed by Andrew Tridgell
parent 1457a9d31a
commit 70d1e53b74
1 changed files with 17 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduCopter/commands_logic.pde
View File

@ -220,8 +220,8 @@ static bool verify_cond_command()
// do_RTL - start Return-to-Launch // do_RTL - start Return-to-Launch
static void do_RTL(void) static void do_RTL(void)
{ {
// set rtl state // start rtl in auto flight mode
rtl_init(true); auto_rtl_start();
} }
/********************************************************************************/ /********************************************************************************/
@ -292,15 +292,6 @@ static void do_loiter_unlimited()
{ {
Vector3f target_pos; Vector3f target_pos;
// set roll-pitch mode (no pilot input)
set_roll_pitch_mode(AUTO_RP);
// set throttle mode to AUTO which, if not already active, will default to hold at our current altitude
set_throttle_mode(AUTO_THR);
// hold yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// get current position // get current position
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
@ -318,8 +309,7 @@ static void do_loiter_unlimited()
} }
// start way point navigator and provide it the desired location // start way point navigator and provide it the desired location
set_nav_mode(NAV_WP); auto_wp_start(target_pos);
wp_nav.set_wp_destination(target_pos);
} }
// do_circle - initiate moving in a circle // do_circle - initiate moving in a circle
@ -352,15 +342,6 @@ static void do_loiter_time()
{ {
Vector3f target_pos; Vector3f target_pos;
// set roll-pitch mode (no pilot input)
set_roll_pitch_mode(AUTO_RP);
// set throttle mode to AUTO which, if not already active, will default to hold at our current altitude
set_throttle_mode(AUTO_THR);
// hold yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// get current position // get current position
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
@ -378,8 +359,7 @@ static void do_loiter_time()
} }
// start way point navigator and provide it the desired location // start way point navigator and provide it the desired location
set_nav_mode(NAV_WP); auto_wp_start(target_pos);
wp_nav.set_wp_destination(target_pos);
// setup loiter timer // setup loiter timer
loiter_time = 0; loiter_time = 0;
@ -511,18 +491,22 @@ static void do_wait_delay()
static void do_change_alt() static void do_change_alt()
{ {
// adjust target appropriately for each nav mode // adjust target appropriately for each nav mode
switch (nav_mode) { if (control_mode == AUTO) {
//case NAV_CIRCLE: switch (auto_mode) {
case NAV_LOITER: case Auto_TakeOff:
// update loiter target altitude // To-Do: adjust waypoint target altitude to new provided altitude
wp_nav.set_desired_alt(command_cond_queue.alt);
break; break;
case Auto_WP:
case NAV_WP: // To-Do; reset origin to current location + stopping distance at new altitude
// To-Do: update waypoint nav's destination altitude break;
case Auto_Land:
// ignore altitude
break;
case Auto_Circle:
// move circle altitude up to target (we will need to store this target in circle class)
break; break;
} }
}
// To-Do: store desired altitude in a variable so that it can be verified later // To-Do: store desired altitude in a variable so that it can be verified later
} }