mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-04 23:18:29 -04:00
Copter: control_rtl
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
26b257c8ba
commit
689e92eb33
@ -2,13 +2,16 @@
|
||||
|
||||
/*
|
||||
* control_rtl.pde - init and run calls for RTL flight mode
|
||||
*
|
||||
* There are two parts to RTL, the high level decision making which controls which state we are in
|
||||
* and the lower implementation of the waypoint or landing controllers within those states
|
||||
*/
|
||||
|
||||
// rtl_init - initialise rtl controller
|
||||
static bool rtl_init(bool ignore_checks)
|
||||
{
|
||||
if (GPS_ok() || ignore_checks) {
|
||||
do_RTL();
|
||||
rtl_climb_start();
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
@ -19,6 +22,271 @@ static bool rtl_init(bool ignore_checks)
|
||||
// should be called at 100hz or more
|
||||
static void rtl_run()
|
||||
{
|
||||
verify_RTL();
|
||||
// check if we need to move to next state
|
||||
if (rtl_state_complete) {
|
||||
switch (rtl_state) {
|
||||
case InitialClimb:
|
||||
rtl_return_start();
|
||||
break;
|
||||
case ReturnHome:
|
||||
rtl_loiterathome_start();
|
||||
break;
|
||||
case LoiterAtHome:
|
||||
if (g.rtl_alt_final > 0) {
|
||||
rtl_descent_start();
|
||||
}else{
|
||||
rtl_land_start();
|
||||
}
|
||||
break;
|
||||
case FinalDescent:
|
||||
// do nothing
|
||||
break;
|
||||
case Land:
|
||||
// do nothing
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// call the correct run function
|
||||
switch (rtl_state) {
|
||||
|
||||
case InitialClimb:
|
||||
rtl_climb_return_descent_run();
|
||||
break;
|
||||
|
||||
case ReturnHome:
|
||||
rtl_climb_return_descent_run();
|
||||
break;
|
||||
|
||||
case LoiterAtHome:
|
||||
rtl_loiterathome_run();
|
||||
break;
|
||||
|
||||
case FinalDescent:
|
||||
rtl_climb_return_descent_run();
|
||||
break;
|
||||
|
||||
case Land:
|
||||
rtl_land_run();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// rtl_climb_start - initialise climb to RTL altitude
|
||||
static void rtl_climb_start()
|
||||
{
|
||||
rtl_state = InitialClimb;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// get horizontal stopping point
|
||||
Vector3f destination;
|
||||
wp_nav.get_wp_stopping_point_xy(destination);
|
||||
destination.z = get_RTL_alt();
|
||||
|
||||
wp_nav.set_wp_destination(destination);
|
||||
|
||||
// hold current yaw during initial climb
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// rtl_return_start - initialise return to home
|
||||
static void rtl_return_start()
|
||||
{
|
||||
rtl_state = ReturnHome;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// initialise original_wp_bearing which is used to point the nose home
|
||||
wp_bearing = wp_nav.get_wp_bearing_to_destination();
|
||||
original_wp_bearing = wp_bearing;
|
||||
|
||||
// set target to above home
|
||||
Vector3f destination = Vector3f(0,0,get_RTL_alt());
|
||||
wp_nav.set_wp_destination(destination);
|
||||
|
||||
// initialise yaw to point home (maybe)
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
|
||||
}
|
||||
|
||||
// rtl_descent_start - initialise descent to final alt
|
||||
static void rtl_descent_start()
|
||||
{
|
||||
rtl_state = FinalDescent;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// set target to above home
|
||||
Vector3f destination = Vector3f(0,0,g.rtl_alt_final);
|
||||
wp_nav.set_wp_destination(destination);
|
||||
|
||||
// initialise yaw to point home (maybe)
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
|
||||
// called by rtl_run at 100hz or more
|
||||
static void rtl_climb_return_descent_run()
|
||||
{
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
// reset attitude control targets
|
||||
attitude_control.init_targets();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
}
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
||||
if (target_yaw_rate != 0) {
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
rtl_state_complete = wp_nav.reached_wp_destination();
|
||||
|
||||
// re-fetch angle targets for reporting
|
||||
const Vector3f angle_target = attitude_control.angle_ef_targets();
|
||||
control_roll = angle_target.x;
|
||||
control_pitch = angle_target.y;
|
||||
control_yaw = angle_target.z;
|
||||
}
|
||||
|
||||
// rtl_return_start - initialise return to home
|
||||
static void rtl_loiterathome_start()
|
||||
{
|
||||
rtl_state = LoiterAtHome;
|
||||
rtl_state_complete = false;
|
||||
rtl_loiter_start_time = millis();
|
||||
|
||||
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
|
||||
if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
|
||||
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
|
||||
} else {
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
|
||||
// called by rtl_run at 100hz or more
|
||||
static void rtl_loiterathome_run()
|
||||
{
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
// reset attitude control targets
|
||||
attitude_control.init_targets();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
}
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
||||
if (target_yaw_rate != 0) {
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
}
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
// To-Do: add extra check that we've reached the target yaw
|
||||
rtl_state_complete = ((millis() - rtl_loiter_start_time) > (uint32_t)g.rtl_loiter_time.get());
|
||||
|
||||
// re-fetch angle targets for reporting
|
||||
const Vector3f angle_target = attitude_control.angle_ef_targets();
|
||||
control_roll = angle_target.x;
|
||||
control_pitch = angle_target.y;
|
||||
control_yaw = angle_target.z;
|
||||
}
|
||||
|
||||
// rtl_loiterathome_start - initialise controllers to loiter over home
|
||||
static void rtl_land_start()
|
||||
{
|
||||
rtl_state = Land;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// Set wp navigation target to above home
|
||||
wp_nav.set_loiter_target(Vector3f(0,0,0));
|
||||
|
||||
// initialise altitude target to stopping point
|
||||
pos_control.set_target_to_stopping_point_z();
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// rtl_returnhome_run - return home
|
||||
// called by rtl_run at 100hz or more
|
||||
static void rtl_land_run()
|
||||
{
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
attitude_control.init_targets();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
return;
|
||||
}
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
||||
}
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter();
|
||||
|
||||
// call z-axis position controller
|
||||
float cmb_rate = get_throttle_land();
|
||||
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
rtl_state_complete = ap.land_complete;
|
||||
|
||||
// re-fetch angle targets for reporting
|
||||
const Vector3f angle_target = attitude_control.angle_ef_targets();
|
||||
control_roll = angle_target.x;
|
||||
control_pitch = angle_target.y;
|
||||
control_yaw = angle_target.z;
|
||||
}
|
||||
|
||||
|
||||
@ -213,13 +213,15 @@
|
||||
//#define WP_OPTION_ 64
|
||||
#define WP_OPTION_NEXT_CMD 128
|
||||
|
||||
// RTL state
|
||||
#define RTL_STATE_START 0
|
||||
#define RTL_STATE_INITIAL_CLIMB 1
|
||||
#define RTL_STATE_RETURNING_HOME 2
|
||||
#define RTL_STATE_LOITERING_AT_HOME 3
|
||||
#define RTL_STATE_FINAL_DESCENT 4
|
||||
#define RTL_STATE_LAND 5
|
||||
// RTL states
|
||||
enum RTLState {
|
||||
Start,
|
||||
InitialClimb,
|
||||
ReturnHome,
|
||||
LoiterAtHome,
|
||||
FinalDescent,
|
||||
Land
|
||||
};
|
||||
|
||||
// LAND state
|
||||
#define LAND_STATE_FLY_TO_LOCATION 0
|
||||
|
||||
Loading…
Reference in New Issue
Block a user