/*
pass through channels in manual mode
*/
void Plane::set_servos_manual_passthrough(void)
{
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
}
/*
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() && get_throttle_input() >= 90 &&
landing.request_go_around()) {
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());
}
void Plane::calc_airspeed_errors()
{
float airspeed_measured = 0;
// we use the airspeed estimate function not direct sensor as TECS
// may be using synthetic airspeed
ahrs.airspeed_estimate(&airspeed_measured);
// FBW_B/cruise airspeed target
if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) {
if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) {
target_airspeed_cm = aparm.airspeed_cruise_cm;
} else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) {
float control_min = 0.0f;
float control_mid = 0.0f;
const float control_max = channel_throttle->get_range();
const float control_in = get_throttle_input();
switch (channel_throttle->get_type()) {
case RC_Channel::RC_CHANNEL_TYPE_ANGLE:
control_min = -control_max;
break;
case RC_Channel::RC_CHANNEL_TYPE_RANGE:
control_mid = channel_throttle->get_control_mid();
break;
}
if (control_in <= control_mid) {
target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise_cm,
control_in,
control_min, control_mid);
} else {
target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise_cm, aparm.airspeed_max * 100,
control_in,
control_mid, control_max);
}
} else {
target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
}
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();
} else if ((control_mode == &mode_auto) &&
(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
target_airspeed_cm = SpdHgt_Controller->get_land_airspeed() * 100;
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
} else {
// Normal airspeed target
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
// Set target to current airspeed + ground speed undershoot,
// but only when this is faster than the target airspeed commanded
// above.
if (auto_throttle_mode &&
aparm.min_gndspeed_cm > 0 &&
control_mode != &mode_circle) {
int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed_cm) {
target_airspeed_cm = min_gnd_target_airspeed;
}
}
// Bump up the target airspeed based on throttle nudging
if (throttle_allows_nudging && airspeed_nudge_cm > 0) {
target_airspeed_cm += airspeed_nudge_cm;
}
// Apply airspeed limit
if (target_airspeed_cm > (aparm.airspeed_max * 100))
target_airspeed_cm = (aparm.airspeed_max * 100);
// use the TECS view of the target airspeed for reporting, to take
// account of the landing speed
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured;
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (get_throttle_input() != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode and not in CRUISE or FBWB
// modes then disallow rudder arming/disarming
if (auto_throttle_mode &&
(control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
setup output channels all non-manual modes
*/
void Plane::set_servos_controlled(void)
{
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// allow landing to override servos if it would like to
landing.override_servos();
}
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
if (min_throttle < 0 && !allow_reverse_thrust()) {
// reverse thrust is available but inhibited.
min_throttle = 0;
}
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
} else if (landing.is_flaring()) {
min_throttle = 0;
}
// apply watt limiter
throttle_watt_limiter(min_throttle, max_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
if (!hal.util->get_soft_armed()) {
if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0);
}
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
}
} else if (control_mode == &mode_stabilize ||
control_mode == &mode_training ||
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune) {
// a manual throttle mode
if (failsafe.throttle_counter) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else if (g.throttle_passthru_stabilize) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
}
} else if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
}
#if SOARING_ENABLED == ENABLED
// suppress throttle when soaring is active
if ((control_mode == &mode_fbwb || control_mode == &mode_cruise ||
control_mode == &mode_auto || control_mode == &mode_loiter) &&
g2.soaring_controller.is_active() &&
g2.soaring_controller.get_throttle_suppressed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
#endif
}
