mirror of https://github.com/ArduPilot/ardupilot
380 lines
12 KiB
Plaintext
380 lines
12 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 && !failsafe.radio) {
|
|
rtl_descent_start();
|
|
}else{
|
|
rtl_land_start();
|
|
}
|
|
break;
|
|
case FinalDescent:
|
|
// do nothing
|
|
break;
|
|
case Land:
|
|
// do nothing - rtl_land_run will take care of disarming motors
|
|
break;
|
|
}
|
|
}
|
|
|
|
// call the correct run function
|
|
switch (rtl_state) {
|
|
|
|
case InitialClimb:
|
|
rtl_climb_return_run();
|
|
break;
|
|
|
|
case ReturnHome:
|
|
rtl_climb_return_run();
|
|
break;
|
|
|
|
case LoiterAtHome:
|
|
rtl_loiterathome_run();
|
|
break;
|
|
|
|
case FinalDescent:
|
|
rtl_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;
|
|
|
|
// initialise waypoint and spline controller
|
|
wp_nav.wp_and_spline_init();
|
|
|
|
// get horizontal stopping point
|
|
Vector3f destination;
|
|
wp_nav.get_wp_stopping_point_xy(destination);
|
|
|
|
#if AC_RALLY == ENABLED
|
|
// rally_point.alt will be max of RTL_ALT and the height of the nearest rally point (if there is one)
|
|
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt());
|
|
destination.z = rally_point.alt;
|
|
#else
|
|
destination.z = get_RTL_alt();
|
|
#endif
|
|
|
|
// set the destination
|
|
wp_nav.set_wp_destination(destination);
|
|
wp_nav.set_fast_waypoint(true);
|
|
|
|
// 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;
|
|
|
|
// set target to above home/rally point
|
|
#if AC_RALLY == ENABLED
|
|
Vector3f destination = pv_location_to_vector(rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()));
|
|
#else
|
|
Vector3f destination = Vector3f(0,0,get_RTL_alt());
|
|
#endif
|
|
|
|
wp_nav.set_wp_destination(destination);
|
|
|
|
// initialise yaw to point home (maybe)
|
|
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
|
|
}
|
|
|
|
// rtl_climb_return_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_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.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
|
}else{
|
|
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
|
attitude_control.angle_ef_roll_pitch_yaw(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.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
|
}else{
|
|
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
|
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
|
}
|
|
|
|
// check if we've completed this stage of RTL
|
|
if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
|
|
if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
|
|
// check if heading is within 2 degrees of heading when vehicle was armed
|
|
if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {
|
|
rtl_state_complete = true;
|
|
}
|
|
} else {
|
|
// we have loitered long enough
|
|
rtl_state_complete = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// rtl_descent_start - initialise descent to final alt
|
|
static void rtl_descent_start()
|
|
{
|
|
rtl_state = FinalDescent;
|
|
rtl_state_complete = false;
|
|
|
|
// Set wp navigation target to above home
|
|
wp_nav.set_loiter_target(wp_nav.get_wp_destination());
|
|
|
|
// initialise altitude target to stopping point
|
|
pos_control.set_target_to_stopping_point_z();
|
|
|
|
// initialise yaw
|
|
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
}
|
|
|
|
// rtl_descent_run - implements the final descent to the RTL_ALT
|
|
// called by rtl_run at 100hz or more
|
|
static void rtl_descent_run()
|
|
{
|
|
// if not auto armed set throttle to zero and exit immediately
|
|
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
|
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 input
|
|
float target_yaw_rate = 0;
|
|
if (!failsafe.radio) {
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// process pilot's roll and pitch input
|
|
wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
|
|
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
|
} else {
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs while descending
|
|
wp_nav.clear_pilot_desired_acceleration();
|
|
}
|
|
|
|
// run loiter controller
|
|
wp_nav.update_loiter();
|
|
|
|
// call z-axis position controller
|
|
pos_control.set_alt_target_with_slew(g.rtl_alt_final, G_Dt);
|
|
pos_control.update_z_controller();
|
|
|
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
|
|
|
// check if we've reached within 20cm of final altitude
|
|
rtl_state_complete = fabs(g.rtl_alt_final - inertial_nav.get_altitude()) < 20.0f;
|
|
}
|
|
|
|
// 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(wp_nav.get_wp_destination());
|
|
|
|
// 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 || !inertial_nav.position_ok()) {
|
|
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) {
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// process pilot's roll and pitch input
|
|
wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
|
|
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
|
} else {
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs while landing
|
|
wp_nav.clear_pilot_desired_acceleration();
|
|
}
|
|
|
|
// 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.angle_ef_roll_pitch_rate_ef_yaw(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;
|
|
|
|
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
|
// disarm when the landing detector says we've landed and throttle is at minimum
|
|
if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
|
|
init_disarm_motors();
|
|
}
|
|
#else
|
|
// disarm when the landing detector says we've landed
|
|
if (ap.land_complete) {
|
|
init_disarm_motors();
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// get_RTL_alt - return altitude which vehicle should return home at
|
|
// altitude is in cm above home
|
|
static float get_RTL_alt()
|
|
{
|
|
// maximum of current altitude and rtl altitude
|
|
float rtl_alt = max(current_loc.alt, g.rtl_altitude);
|
|
|
|
#if AC_FENCE == ENABLED
|
|
// ensure not above fence altitude if alt fence is enabled
|
|
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
|
rtl_alt = min(rtl_alt, fence.get_safe_alt()*100.0f);
|
|
}
|
|
#endif
|
|
|
|
return rtl_alt;
|
|
}
|
|
|