Copter: loiter copter without accepting user input at end of auto mission

This commit is contained in:
Jonathan Challinger 2014-10-12 01:06:51 -07:00 committed by Randy Mackay
parent ef12ea4d3b
commit d7d8330303
3 changed files with 47 additions and 2 deletions

View File

@ -281,7 +281,7 @@ static void exit_mission()
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if (!set_mode(LOITER)) {
if(!auto_loiter_start()) {
set_mode(LAND);
}
}else{

View File

@ -21,6 +21,8 @@
static bool auto_init(bool ignore_checks)
{
if ((GPS_ok() && inertial_nav.position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
@ -79,6 +81,10 @@ static void auto_run()
auto_nav_guided_run();
break;
#endif
case Auto_Loiter:
auto_loiter_run();
break;
}
}
@ -415,6 +421,44 @@ void auto_nav_guided_run()
}
#endif // NAV_GUIDED
bool auto_loiter_start()
{
if (!GPS_ok()) {
return false;
}
auto_mode = Auto_Loiter;
Vector3f origin = inertial_nav.get_position();
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
set_auto_yaw_mode(AUTO_YAW_HOLD);
return true;
}
void auto_loiter_run() {
if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
return;
}
float target_yaw_rate = 0;
if(!failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
}
wp_nav.update_wpnav();
pos_control.update_z_controller();
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_default_auto_yaw_mode(bool rtl)

View File

@ -179,7 +179,8 @@ enum AutoMode {
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline,
Auto_NavGuided
Auto_NavGuided,
Auto_Loiter
};
// Guided modes