/*
set the target altitude to the current altitude, with ALT_OFFSET adjustment
*/
void Plane::set_target_altitude_current_adjusted(void)
{
set_target_altitude_current();
// use adjusted_altitude_cm() to take account of ALTITUDE_OFFSET
target_altitude.amsl_cm = adjusted_altitude_cm();
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f);
if (is_zero(elevator_input) && !is_zero(last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
// check for FBWB altitude limit
check_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}
void Plane::set_guided_WP(void)
{
if (g.loiter_radius < 0 || guided_WP_loc.flags.loiter_ccw) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP_loc = current_loc;
// Load the next_WP slot
// ---------------------
next_WP_loc = guided_WP_loc;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
set_target_altitude_current();
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
// reset loiter start time.
loiter.start_time_ms = 0;
// start in non-VTOL mode
auto_state.vtol_loiter = false;
loiter_angle_reset();
}
void Plane::set_guided_WP(void)
{
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP_loc = current_loc;
// Load the next_WP slot
// ---------------------
next_WP_loc = guided_WP_loc;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
set_target_altitude_current();
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
loiter_angle_reset();
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
uint32_t now = micros();
if (now - target_altitude.last_elev_check_us >= 100000) {
// we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and
// give below 1cm altitude change, which would result in no climb or descent
float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6;
dt = constrain_float(dt, 0.1, 0.15);
target_altitude.last_elev_check_us = now;
float elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100;
change_target_altitude(alt_change_cm);
if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
target_altitude.last_elevator_input = elevator_input;
}
check_fbwb_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
calc_throttle();
calc_nav_pitch();
}
void Plane::set_mode(enum FlightMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// zero locked course
steer_state.locked_course_err = 0;
// set mode
previous_mode = control_mode;
control_mode = mode;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
do_RTL();
break;
case LOITER:
auto_throttle_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
void Plane::set_mode(enum FlightMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// not in pre-flare
auto_state.land_pre_flare = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// always reset this because we don't know who called set_mode. In evasion
// behavior you should set this flag after set_mode so you know the evasion
// logic is controlling the mode. This allows manual override of the mode
// to exit evasion behavior automatically but if the mode is manually switched
// then we won't resume AUTO after an evasion
adsb_state.is_evading = false;
// set mode
previous_mode = control_mode;
control_mode = mode;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
auto_state.vtol_mode = false;
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
do_RTL();
break;
case LOITER:
auto_throttle_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// not in pre-flare
auto_state.land_pre_flare = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.last_forced_rpy_ms.zero();
guided_state.last_forced_throttle_ms = 0;
// set mode
previous_mode = control_mode;
control_mode = mode;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_loiter = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
auto_navigation_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
auto_navigation_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
auto_navigation_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
auto_navigation_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
auto_navigation_mode = true;
if (quadplane.available() && quadplane.enable == 2) {
auto_state.vtol_mode = true;
} else {
auto_state.vtol_mode = false;
}
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
break;
case LOITER:
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
break;
case AVOID_ADSB:
case GUIDED:
auto_throttle_mode = true;
auto_navigation_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
auto_navigation_mode = false;
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
adsb.set_is_auto_mode(auto_navigation_mode);
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
/*
* ArduSoar support function
*
* Peter Braswell, Samuel Tabor, Andrey Kolobov, and Iain Guilliard
*/
void Plane::update_soaring() {
if (!g2.soaring_controller.is_active()) {
return;
}
g2.soaring_controller.update_vario();
// Check for throttle suppression change.
switch (control_mode){
case AUTO:
g2.soaring_controller.suppress_throttle();
break;
case FLY_BY_WIRE_B:
case CRUISE:
if (!g2.soaring_controller.suppress_throttle()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
set_mode(RTL, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING);
}
break;
case LOITER:
// Do nothing. We will switch back to auto/rtl before enabling throttle.
break;
default:
// This does not affect the throttle since suppressed is only checked in the above three modes.
// It ensures that the soaring always starts with throttle suppressed though.
g2.soaring_controller.set_throttle_suppressed(true);
break;
}
// Nothing to do if we are in powered flight
if (!g2.soaring_controller.get_throttle_suppressed() && aparm.throttle_max > 0) {
return;
}
switch (control_mode){
case AUTO:
case FLY_BY_WIRE_B:
case CRUISE:
// Test for switch into thermalling mode
g2.soaring_controller.update_cruising();
if (g2.soaring_controller.check_thermal_criteria()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
set_mode(LOITER, MODE_REASON_SOARING_THERMAL_DETECTED);
}
break;
case LOITER:
// Update thermal estimate and check for switch back to AUTO
g2.soaring_controller.update_thermalling(); // Update estimate
if (g2.soaring_controller.check_cruise_criteria()) {
// Exit as soon as thermal state estimate deteriorates
switch (previous_mode) {
case FLY_BY_WIRE_B:
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
set_mode(RTL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case CRUISE: {
// return to cruise with old ground course
CruiseState cruise = cruise_state;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
set_mode(CRUISE, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
cruise_state = cruise;
set_target_altitude_current();
break;
}
case AUTO:
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
set_mode(AUTO, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
default:
break;
}
} else {
// still in thermal - need to update the wp location
g2.soaring_controller.get_target(next_WP_loc);
}
break;
default:
// nothing to do
break;
}
}
