ardupilot/ArduCopter/mode_smart_rtl.cpp

211 lines
7.5 KiB
C++
Raw Normal View History

2017-07-26 14:14:40 -03:00
#include "Copter.h"
#if MODE_SMARTRTL_ENABLED == ENABLED
2017-07-26 14:14:40 -03:00
/*
2017-09-08 23:45:31 -03:00
* Init and run calls for Smart_RTL flight mode
2017-07-26 14:14:40 -03:00
*
* This code uses the SmartRTL path that is already in memory, and feeds it into WPNav, one point at a time.
2017-07-26 14:14:40 -03:00
* Once the copter is close to home, it will run a standard land controller.
*/
bool ModeSmartRTL::init(bool ignore_checks)
2017-07-26 14:14:40 -03:00
{
2019-02-27 23:56:36 -04:00
if (g2.smart_rtl.is_active()) {
2017-07-26 14:14:40 -03:00
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// set current target to a reasonable stopping point
Vector3p stopping_point;
2021-06-21 04:22:48 -03:00
pos_control->get_stopping_point_xy_cm(stopping_point.xy());
pos_control->get_stopping_point_z_cm(stopping_point.z);
wp_nav->set_wp_destination(stopping_point.tofloat());
2017-07-26 14:14:40 -03:00
// initialise yaw to obey user parameter
auto_yaw.set_mode_to_default(true);
2017-07-26 14:14:40 -03:00
// wait for cleanup of return path
smart_rtl_state = SubMode::WAIT_FOR_PATH_CLEANUP;
2017-07-26 14:14:40 -03:00
return true;
}
return false;
2017-07-26 14:14:40 -03:00
}
2017-09-08 23:45:31 -03:00
// perform cleanup required when leaving smart_rtl
void ModeSmartRTL::exit()
2017-07-26 14:14:40 -03:00
{
2017-09-08 23:45:31 -03:00
g2.smart_rtl.cancel_request_for_thorough_cleanup();
2017-07-26 14:14:40 -03:00
}
void ModeSmartRTL::run()
2017-07-26 14:14:40 -03:00
{
2017-09-08 23:45:31 -03:00
switch (smart_rtl_state) {
case SubMode::WAIT_FOR_PATH_CLEANUP:
wait_cleanup_run();
2017-07-26 14:14:40 -03:00
break;
case SubMode::PATH_FOLLOW:
path_follow_run();
2017-07-26 14:14:40 -03:00
break;
case SubMode::PRELAND_POSITION:
pre_land_position_run();
2017-07-26 14:14:40 -03:00
break;
case SubMode::DESCEND:
descent_run(); // Re-using the descend method from normal rtl mode.
2017-07-26 14:14:40 -03:00
break;
case SubMode::LAND:
land_run(true); // Re-using the land method from normal rtl mode.
2017-07-26 14:14:40 -03:00
break;
}
}
bool ModeSmartRTL::is_landing() const
{
return smart_rtl_state == SubMode::LAND;
}
void ModeSmartRTL::wait_cleanup_run()
2017-07-26 14:14:40 -03:00
{
// hover at current target position
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2017-07-26 14:14:40 -03:00
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
2017-07-26 14:14:40 -03:00
// check if return path is computed and if yes, begin journey home
2017-09-08 23:45:31 -03:00
if (g2.smart_rtl.request_thorough_cleanup()) {
path_follow_last_pop_fail_ms = 0;
smart_rtl_state = SubMode::PATH_FOLLOW;
2017-07-26 14:14:40 -03:00
}
}
void ModeSmartRTL::path_follow_run()
2017-07-26 14:14:40 -03:00
{
2018-04-29 17:48:43 -03:00
float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio && g2.smart_rtl.use_pilot_yaw()) {
2018-04-29 17:48:43 -03:00
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
2017-07-26 14:14:40 -03:00
// if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) {
Vector3f dest_NED;
// this pop_point can fail if the IO task currently has the
// path semaphore.
if (g2.smart_rtl.pop_point(dest_NED)) {
path_follow_last_pop_fail_ms = 0;
2017-09-08 23:45:31 -03:00
if (g2.smart_rtl.get_num_points() == 0) {
// this is the very last point, add 2m to the target alt and move to pre-land state
dest_NED.z -= 2.0f;
smart_rtl_state = SubMode::PRELAND_POSITION;
wp_nav->set_wp_destination_NED(dest_NED);
2020-01-08 07:18:39 -04:00
} else {
2021-03-11 08:07:50 -04:00
// peek at the next point. this can fail if the IO task currently has the path semaphore
Vector3f next_dest_NED;
if (g2.smart_rtl.peek_point(next_dest_NED)) {
wp_nav->set_wp_destination_NED(dest_NED);
if (g2.smart_rtl.get_num_points() == 1) {
// this is the very last point, add 2m to the target alt
next_dest_NED.z -= 2.0f;
}
wp_nav->set_wp_destination_next_NED(next_dest_NED);
2020-01-08 07:18:39 -04:00
} else {
2021-03-11 08:07:50 -04:00
// this can only happen if peek failed to take the semaphore
// send next point anyway which will cause the vehicle to slow at the next point
wp_nav->set_wp_destination_NED(dest_NED);
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2020-01-08 07:18:39 -04:00
}
2017-07-26 14:14:40 -03:00
}
} else if (g2.smart_rtl.get_num_points() == 0) {
// We should never get here; should always have at least
// two points and the "zero points left" is handled above.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
smart_rtl_state = SubMode::PRELAND_POSITION;
} else if (path_follow_last_pop_fail_ms == 0) {
// first time we've failed to pop off (ever, or after a success)
path_follow_last_pop_fail_ms = AP_HAL::millis();
} else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) {
// we failed to pop a point off for 10 seconds. This is
// almost certainly a bug.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
smart_rtl_state = SubMode::PRELAND_POSITION;
2017-07-26 14:14:40 -03:00
}
}
// update controllers
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2017-07-26 14:14:40 -03:00
wp_nav->update_wpnav();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
2017-07-26 14:14:40 -03:00
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
2017-07-26 14:14:40 -03:00
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
2017-07-26 14:14:40 -03:00
}
}
void ModeSmartRTL::pre_land_position_run()
2017-07-26 14:14:40 -03:00
{
// if we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) {
// choose descend and hold, or land based on user parameter rtl_alt_final
if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
land_start();
smart_rtl_state = SubMode::LAND;
2017-07-26 14:14:40 -03:00
} else {
set_descent_target_alt(copter.g.rtl_alt_final);
descent_start();
smart_rtl_state = SubMode::DESCEND;
2017-07-26 14:14:40 -03:00
}
}
// update controllers
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2017-07-26 14:14:40 -03:00
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
2017-07-26 14:14:40 -03:00
}
2017-09-08 23:45:31 -03:00
// save current position for use by the smart_rtl flight mode
void ModeSmartRTL::save_position()
2017-07-26 14:14:40 -03:00
{
const bool should_save_position = motors->armed() && (copter.flightmode->mode_number() != Mode::Number::SMART_RTL);
2017-07-26 14:14:40 -03:00
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
2017-07-26 14:14:40 -03:00
}
2021-07-06 18:02:26 -03:00
bool ModeSmartRTL::get_wp(Location& destination) const
2019-06-05 07:47:32 -03:00
{
// provide target in states which use wp_nav
switch (smart_rtl_state) {
case SubMode::WAIT_FOR_PATH_CLEANUP:
case SubMode::PATH_FOLLOW:
case SubMode::PRELAND_POSITION:
case SubMode::DESCEND:
return wp_nav->get_wp_destination_loc(destination);
case SubMode::LAND:
2019-06-05 07:47:32 -03:00
return false;
}
// we should never get here but just in case
return false;
}
uint32_t ModeSmartRTL::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t ModeSmartRTL::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}
#endif