/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this should never be reached
set_mode(RTL);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
auto_state.land_complete = false;
auto_state.land_pre_flare = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint8_t nav_cmd_id;
// we should be either running a mission or RTLing home
if (mission.state() == AP_Mission::MISSION_RUNNING) {
nav_cmd_id = mission.get_current_nav_cmd().id;
}else{
nav_cmd_id = auto_rtl_command.id;
}
switch(nav_cmd_id) {
case MAV_CMD_NAV_TAKEOFF:
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
break;
case MAV_CMD_NAV_LAND:
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
break;
default:
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
steer_state.hold_course_cd = -1;
auto_state.land_complete = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
set_mode(RTL, MODE_REASON_MISSION_END);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (landing.is_complete()) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
} else {
calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
// we should be either running a mission or RTLing home
if (mission.state() == AP_Mission::MISSION_RUNNING) {
nav_cmd_id = mission.get_current_nav_cmd().id;
}else{
nav_cmd_id = auto_rtl_command.id;
}
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
auto_state.land_complete = false;
auto_state.land_pre_flare = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
set_mode(RTL, MODE_REASON_MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
// allow landing to restrict the roll limits
nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL);
if (landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
