aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add rtl to control_auto

Browse Source
This commit is contained in:
Randy Mackay 2014-01-28 12:20:39 +09:00 committed by Andrew Tridgell
parent 1f07c2efe0
commit 7970d7112f
1 changed files with 26 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/control_auto.pde
View File

@ -5,25 +5,16 @@
* *
* This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
* Command execution code (i.e. command_logic.pde) should: * Command execution code (i.e. command_logic.pde) should:
* a) switch to Auto mode with set_mode() function. This will cause auto_init to be called * a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called
* b) call one of the three auto initialisation functions: auto_wp(), auto_takeoff, auto_land * b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
* c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed * c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
* The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
* correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature * correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
*/ */
enum AutoMode {
Auto_TakeOff,
Auto_WP,
Auto_Land,
Auto_Circle
};
static AutoMode auto_mode; // controls which auto controller is run
/* /*
* While in the auto flight mode, navigation or do/now commands can be run. * While in the auto flight mode, navigation or do/now commands can be run.
* Code in this file implements the navigation commands which * Code in this file implements the navigation commands
*/ */
// auto_init - initialise auto controller // auto_init - initialise auto controller
@ -58,6 +49,10 @@ static void auto_run()
auto_land_run(); auto_land_run();
break; break;
case Auto_RTL:
auto_rtl_run();
break;
case Auto_Circle: case Auto_Circle:
auto_circle_run(); auto_circle_run();
break; break;
@ -235,9 +230,28 @@ static void auto_land_run()
control_yaw = angle_target.z; control_yaw = angle_target.z;
} }
// auto_rtl_start - initialises RTL in AUTO flight mode
static void auto_rtl_start()
{
auto_mode = Auto_RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks
rtl_init(true);
}
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void auto_rtl_run()
{
// call regular rtl flight mode run function
rtl_run();
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode // auto_circle_start - initialises controller to fly a circle in AUTO flight mode
static void auto_circle_start(const Vector3f& center) static void auto_circle_start(const Vector3f& center)
{ {
auto_mode = Auto_Circle;
// set circle center // set circle center
circle_nav.set_center(center); circle_nav.set_center(center);
} }