/* 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 }