コード例 #1
0
ファイル: servos.cpp プロジェクト: mblsktxy/ardupilot
/*
  support for twin-engine planes
 */
void Plane::servos_twin_engine_mix(void)
{
    float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
    float rud_gain = float(plane.g2.rudd_dt_gain) / 100;
    rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / float(SERVO_MAX);

#if ADVANCED_FAILSAFE == ENABLED
    if (afs.should_crash_vehicle()) {
        // when in AFS failsafe force rudder input for differential thrust to zero
        rudder_dt = 0;
    }
#endif

    float throttle_left, throttle_right;

    if (throttle < 0 && have_reverse_thrust() && allow_reverse_thrust()) {
        // doing reverse thrust
        throttle_left  = constrain_float(throttle + 50 * rudder_dt, -100, 0);
        throttle_right = constrain_float(throttle - 50 * rudder_dt, -100, 0);
    } else if (throttle <= 0) {
        throttle_left  = throttle_right = 0;
    } else {
        // doing forward thrust
        throttle_left  = constrain_float(throttle + 50 * rudder_dt, 0, 100);
        throttle_right = constrain_float(throttle - 50 * rudder_dt, 0, 100);
    }
    if (!hal.util->get_soft_armed()) {
        if (arming.arming_required() == AP_Arming::Required::YES_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_throttleLeft, 0);
            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0);
        }
    } else {
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left);
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right);
        throttle_slew_limit(SRV_Channel::k_throttleLeft);
        throttle_slew_limit(SRV_Channel::k_throttleRight);
    }
}
コード例 #2
0
ファイル: servos.cpp プロジェクト: Coonat2/ardupilot
/*
  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::YES_ZERO_PWM) {
            SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
        } else {
            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 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, channel_throttle->get_control_in_zero_dz());
        }
    } else if (g.throttle_passthru_stabilize && 
               (control_mode == STABILIZE || 
                control_mode == TRAINING ||
                control_mode == ACRO ||
                control_mode == FLY_BY_WIRE_A ||
                control_mode == AUTOTUNE) &&
               !failsafe.throttle_counter) {
        // 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 if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
               guided_throttle_passthru) {
        // manual pass through of throttle while in GUIDED
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
    } 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));
    }

    // suppress throttle when soaring is active
    if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE ||
        control_mode == AUTO || control_mode == LOITER) &&
        g2.soaring_controller.is_active() &&
        g2.soaring_controller.get_throttle_suppressed()) {
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
    }
}
コード例 #3
0
ファイル: Attitude.cpp プロジェクト: Mosheyosef/ardupilot
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Plane::set_servos(void)
{
    int16_t last_throttle = channel_throttle->get_radio_out();

    // do any transition updates for quadplane
    quadplane.update();    

    if (control_mode == AUTO && auto_state.idle_mode) {
        // special handling for balloon launch
        set_servos_idle();
        return;
    }

    /*
      see if we are doing ground steering.
     */
    if (!steering_control.ground_steering) {
        // we are not at an altitude for ground steering. Set the nose
        // wheel to the rudder just in case the barometer has drifted
        // a lot
        steering_control.steering = steering_control.rudder;
    } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
        // we are within the ground steering altitude but don't have a
        // dedicated steering channel. Set the rudder to the ground
        // steering output
        steering_control.rudder = steering_control.steering;
    }
    channel_rudder->set_servo_out(steering_control.rudder);

    // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
    steering_control.ground_steering = false;

    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering);

    if (control_mode == MANUAL) {
        // do a direct pass through of radio values
        if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
            channel_roll->set_radio_out(channel_roll->get_radio_in());
            channel_pitch->set_radio_out(channel_pitch->get_radio_in());
        } else {
            channel_roll->set_radio_out(channel_roll->read());
            channel_pitch->set_radio_out(channel_pitch->read());
        }
        channel_throttle->set_radio_out(channel_throttle->get_radio_in());
        channel_rudder->set_radio_out(channel_rudder->get_radio_in());

        // setup extra channels. We want this to come from the
        // main input channel, but using the 2nd channels dead
        // zone, reverse and min/max settings. We need to use
        // pwm_to_angle_dz() to ensure we don't trim the value for the
        // deadzone of the main aileron channel, otherwise the 2nd
        // aileron won't quite follow the first one
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));

        // this variant assumes you have the corresponding
        // input channel setup in your transmitter for manual control
        // of the 2nd aileron
        RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
        RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);

        if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
            // set any differential spoilers to follow the elevons in
            // manual mode. 
            RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->get_radio_out());
            RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->get_radio_out());
        }
    } else {
        if (g.mix_mode == 0) {
            // both types of secondary aileron are slaved to the roll servo out
            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());

            // both types of secondary elevator are slaved to the pitch servo out
            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
            RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
        }else{
            /*Elevon mode*/
            float ch1;
            float ch2;
            ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
            ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());

			/* Differential Spoilers
               If differential spoilers are setup, then we translate
               rudder control into splitting of the two ailerons on
               the side of the aircraft where we want to induce
               additional drag.
             */
			if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
				float ch3 = ch1;
				float ch4 = ch2;
				if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) {
				    ch1 += abs(channel_rudder->get_servo_out());
				    ch3 -= abs(channel_rudder->get_servo_out());
				} else {
					ch2 += abs(channel_rudder->get_servo_out());
				    ch4 -= abs(channel_rudder->get_servo_out());
				}
				RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3);
				RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4);
			}

            // directly set the radio_out values for elevon mode
            channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
            channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
        }

        // push out the PWM values
        if (g.mix_mode == 0) {
            channel_roll->calc_pwm();
            channel_pitch->calc_pwm();
        }
        channel_rudder->calc_pwm();

#if THROTTLE_OUT == 0
        channel_throttle->set_servo_out(0);
#else
        // 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 (control_mode == AUTO) {
            if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
                min_throttle = 0;
            }

            if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
                if(aparm.takeoff_throttle_max != 0) {
                    max_throttle = aparm.takeoff_throttle_max;
                } else {
                    max_throttle = aparm.throttle_max;
                }
            }
        }

        uint32_t now = millis();
        if (battery.overpower_detected()) {
            // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
            // throttle limit will attack by 10% per second

            if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust
                throttle_watt_limit_max < max_throttle - 25 &&
                now - throttle_watt_limit_timer_ms >= 1) {
                // always allow for 25% throttle available regardless of battery status
                throttle_watt_limit_timer_ms = now;
                throttle_watt_limit_max++;

            } else if (channel_throttle->get_servo_out() < 0 &&
                min_throttle < 0 && // reverse thrust is available
                throttle_watt_limit_min < -(min_throttle) - 25 &&
                now - throttle_watt_limit_timer_ms >= 1) {
                // always allow for 25% throttle available regardless of battery status
                throttle_watt_limit_timer_ms = now;
                throttle_watt_limit_min++;
            }

        } else if (now - throttle_watt_limit_timer_ms >= 1000) {
            // it has been 1 second since last over-current, check if we can resume higher throttle.
            // this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
            // throttle limit will release by 1% per second
            if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust
                throttle_watt_limit_max > 0) { // and we're currently limiting it
                throttle_watt_limit_timer_ms = now;
                throttle_watt_limit_max--;

            } else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust
                throttle_watt_limit_min > 0) { // and we're limiting it
                throttle_watt_limit_timer_ms = now;
                throttle_watt_limit_min--;
            }
        }

        max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max);
        if (min_throttle < 0) {
            min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
        }

        channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), 
                                                      min_throttle,
                                                      max_throttle));

        if (!hal.util->get_soft_armed()) {
            channel_throttle->set_servo_out(0);
            channel_throttle->calc_pwm();                
        } else if (suppress_throttle()) {
            // throttle is suppressed in auto mode
            channel_throttle->set_servo_out(0);
            if (g.throttle_suppress_manual) {
                // manual pass through of throttle while throttle is suppressed
                channel_throttle->set_radio_out(channel_throttle->get_radio_in());
            } else {
                channel_throttle->calc_pwm();                
            }
        } else if (g.throttle_passthru_stabilize && 
                   (control_mode == STABILIZE || 
                    control_mode == TRAINING ||
                    control_mode == ACRO ||
                    control_mode == FLY_BY_WIRE_A ||
                    control_mode == AUTOTUNE) &&
                   !failsafe.ch3_counter) {
            // manual pass through of throttle while in FBWA or
            // STABILIZE mode with THR_PASS_STAB set
            channel_throttle->set_radio_out(channel_throttle->get_radio_in());
        } else if (control_mode == GUIDED && 
                   guided_throttle_passthru) {
            // manual pass through of throttle while in GUIDED
            channel_throttle->set_radio_out(channel_throttle->get_radio_in());
        } else if (quadplane.in_vtol_mode()) {
            // ask quadplane code for forward throttle
            channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
            channel_throttle->calc_pwm();
        } else {
            // normal throttle calculation based on servo_out
            channel_throttle->calc_pwm();
        }
#endif
    }

    // Auto flap deployment
    int8_t auto_flap_percent = 0;
    int8_t manual_flap_percent = 0;
    static int8_t last_auto_flap;
    static int8_t last_manual_flap;

    // work out any manual flap input
    RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
    if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
        flapin->input();
        manual_flap_percent = flapin->percent_input();
    }

    if (auto_throttle_mode) {
        int16_t flapSpeedSource = 0;
        if (ahrs.airspeed_sensor_enabled()) {
            flapSpeedSource = target_airspeed_cm * 0.01f;
        } else {
            flapSpeedSource = aparm.throttle_cruise;
        }
        if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
            auto_flap_percent = g.flap_2_percent;
        } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
            auto_flap_percent = g.flap_1_percent;
        } //else flaps stay at default zero deflection

        /*
          special flap levels for takeoff and landing. This works
          better than speed based flaps as it leads to less
          possibility of oscillation
         */
        if (control_mode == AUTO) {
            switch (flight_stage) {
            case AP_SpdHgtControl::FLIGHT_TAKEOFF:
            case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
                if (g.takeoff_flap_percent != 0) {
                    auto_flap_percent = g.takeoff_flap_percent;
                }
                break;
            case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
            case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
            case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
                if (g.land_flap_percent != 0) {
                    auto_flap_percent = g.land_flap_percent;
                }
                break;
            default:
                break;
            }
        }
    }

    // manual flap input overrides auto flap input
    if (abs(manual_flap_percent) > auto_flap_percent) {
        auto_flap_percent = manual_flap_percent;
    }

    flap_slew_limit(last_auto_flap, auto_flap_percent);
    flap_slew_limit(last_manual_flap, manual_flap_percent);

    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent);
    RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent);

    if (control_mode >= FLY_BY_WIRE_B ||
        quadplane.in_assisted_flight() ||
        quadplane.in_vtol_mode()) {
        /* only do throttle slew limiting in modes where throttle
         *  control is automatic */
        throttle_slew_limit(last_throttle);
    }

    if (control_mode == TRAINING) {
        // copy rudder in training mode
        channel_rudder->set_radio_out(channel_rudder->get_radio_in());
    }

    if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
        flaperon_update(auto_flap_percent);
    }
    if (g.vtail_output != MIXING_DISABLED) {
        channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder);
    } else if (g.elevon_output != MIXING_DISABLED) {
        channel_output_mixer(g.elevon_output, channel_pitch, channel_roll);
    }

    if (!arming.is_armed()) {
        //Some ESCs get noisy (beep error msgs) if PWM == 0.
        //This little segment aims to avoid this.
        switch (arming.arming_required()) { 
        case AP_Arming::NO:
            //keep existing behavior: do nothing to radio_out
            //(don't disarm throttle channel even if AP_Arming class is)
            break;

        case AP_Arming::YES_ZERO_PWM:
            channel_throttle->set_radio_out(0);
            break;

        case AP_Arming::YES_MIN_PWM:
        default:
            channel_throttle->set_radio_out(throttle_min());
            break;
        }
    }

#if OBC_FAILSAFE == ENABLED
    // this is to allow the failsafe module to deliberately crash 
    // the plane. Only used in extreme circumstances to meet the
    // OBC rules
    obc.check_crash_plane();
#endif

#if HIL_SUPPORT
    if (g.hil_mode == 1) {
        // get the servos to the GCS immediately for HIL
        if (HAVE_PAYLOAD_SPACE(MAVLINK_COMM_0, RC_CHANNELS_SCALED)) {
            send_servo_out(MAVLINK_COMM_0);
        }
        if (!g.hil_servos) {
            return;
        }
    }
#endif

    if (g.land_then_servos_neutral > 0 &&
            control_mode == AUTO &&
            g.land_disarm_delay > 0 &&
            auto_state.land_complete &&
            !arming.is_armed()) {
        // after an auto land and auto disarm, set the servos to be neutral just
        // in case we're upside down or some crazy angle and straining the servos.
        if (g.land_then_servos_neutral == 1) {
            channel_roll->set_radio_out(channel_roll->get_radio_trim());
            channel_pitch->set_radio_out(channel_pitch->get_radio_trim());
            channel_rudder->set_radio_out(channel_rudder->get_radio_trim());
        } else if (g.land_then_servos_neutral == 2) {
            channel_roll->disable_out();
            channel_pitch->disable_out();
            channel_rudder->disable_out();
        }
    }

    // send values to the PWM timers for output
    // ----------------------------------------
    if (g.rudder_only == 0) {
        // when we RUDDER_ONLY mode we don't send the channel_roll
        // output and instead rely on KFF_RDDRMIX. That allows the yaw
        // damper to operate.
        channel_roll->output();
    }
    channel_pitch->output();
    channel_throttle->output();
    channel_rudder->output();
    RC_Channel_aux::output_ch_all();
}
コード例 #4
0
ファイル: servos.cpp プロジェクト: waltermayorga/ardupilot
/*
  setup output channels all non-manual modes
 */
void Plane::set_servos_controlled(void)
{
    if (g.mix_mode != 0) {
        set_servos_old_elevons();
    } else {
        // both types of secondary aileron are slaved to the roll servo out
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
        
        // both types of secondary elevator are slaved to the pitch servo out
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
        RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());

        // push out the PWM values
        channel_roll->calc_pwm();
        channel_pitch->calc_pwm();
    }

    channel_rudder->calc_pwm();
    
    // 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 (control_mode == AUTO) {
        if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
            min_throttle = 0;
        }
        
        if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
            if(aparm.takeoff_throttle_max != 0) {
                max_throttle = aparm.takeoff_throttle_max;
            } else {
                max_throttle = aparm.throttle_max;
            }
        }
    }

    // apply watt limiter
    throttle_watt_limiter(min_throttle, max_throttle);

    channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), 
                                                    min_throttle,
                                                    max_throttle));
    
    if (!hal.util->get_soft_armed()) {
        channel_throttle->set_servo_out(0);
        channel_throttle->calc_pwm();
    } else if (suppress_throttle()) {
        // throttle is suppressed in auto mode
        channel_throttle->set_servo_out(0);
        if (g.throttle_suppress_manual) {
            // manual pass through of throttle while throttle is suppressed
            channel_throttle->set_radio_out(channel_throttle->get_radio_in());
        } else {
            channel_throttle->calc_pwm();
        }
    } else if (g.throttle_passthru_stabilize && 
               (control_mode == STABILIZE || 
                control_mode == TRAINING ||
                control_mode == ACRO ||
                control_mode == FLY_BY_WIRE_A ||
                control_mode == AUTOTUNE) &&
               !failsafe.ch3_counter) {
        // manual pass through of throttle while in FBWA or
        // STABILIZE mode with THR_PASS_STAB set
        channel_throttle->set_radio_out(channel_throttle->get_radio_in());
    } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
               guided_throttle_passthru) {
        // manual pass through of throttle while in GUIDED
        channel_throttle->set_radio_out(channel_throttle->get_radio_in());
    } else if (quadplane.in_vtol_mode()) {
        // ask quadplane code for forward throttle
        channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
        channel_throttle->calc_pwm();
    } else {
        // normal throttle calculation based on servo_out
        channel_throttle->calc_pwm();
    }
}
コード例 #5
0
ファイル: servos.cpp プロジェクト: mblsktxy/ardupilot
/*
  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
}