ardupilot/ArduCopter/control_rtl.pde

275 lines
8.1 KiB
Plaintext

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* 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) {
rtl_climb_start();
return true;
}else{
return false;
}
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
static void rtl_run()
{
// 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(),true);
}
// check if we've completed this stage of RTL
rtl_state_complete = wp_nav.reached_wp_destination();
}
// 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(),true);
}
// 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());
}
// 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;
}