/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
auto_state.land_complete == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.takeoff_pitch_cd,
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90) {
plane.gcs_send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
} else if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
} else {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) ||
auto_state.commanded_go_around ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
} else if (auto_state.land_complete == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
} else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
float path_progress = location_path_proportion(current_loc, prev_WP_loc, next_WP_loc);
bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000;
bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((path_progress > 0.15f && lined_up && below_prev_WP) || path_progress > 0.5f) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
}
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.takeoff_pitch_cd,
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
// configure abort altitude and pitch
// if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m
if (cmd.p1 > 0) {
auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
} else if (auto_state.takeoff_altitude_rel_cm <= 0) {
auto_state.takeoff_altitude_rel_cm = 3000;
}
if (auto_state.takeoff_pitch_cd <= 0) {
// If no takeoff command has ever been used, default to a conservative 10deg
auto_state.takeoff_pitch_cd = 1000;
}
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
landing.do_land(cmd, relative_altitude);
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// if we were in an abort we need to explicitly move out of the abort state, as it's sticky
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
}
/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) ||
auto_state.commanded_go_around ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
} else if (auto_state.land_complete == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
} else if (auto_state.land_pre_flare == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
} else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
bool on_flight_line = abs(nav_controller->crosstrack_error() < 5) && !nav_controller->data_is_stale();
bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((mission.get_prev_nav_cmd_id() == MAV_CMD_NAV_LOITER_TO_ALT) ||
(auto_state.wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
(auto_state.wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
(auto_state.wp_proportion > 0.5f)) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
}
} else if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
