/*****************************************
* Throttle slew limit
*****************************************/
void Plane::throttle_slew_limit(int16_t last_throttle)
{
    uint8_t slewrate = aparm.throttle_slewrate;
    if (control_mode==AUTO) {
        if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
            slewrate = g.takeoff_throttle_slewrate;
        } else if (g.land_throttle_slewrate != 0 &&
                (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE)) {
            slewrate = g.land_throttle_slewrate;
        }
    }
    // if slew limit rate is set to zero then do not slew limit
    if (slewrate) {                   
        // limit throttle change by the given percentage per second
        float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
        // allow a minimum change of 1 PWM per cycle
        if (temp < 1) {
            temp = 1;
        }
        channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp));
    }
}

/*
  calculate yaw control for ground steering
 */
void Plane::calc_nav_yaw_ground(void)
{
    if (gps.ground_speed() < 1 && 
        channel_throttle->get_control_in() == 0 &&
        flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
        flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
        // manual rudder control while still
        steer_state.locked_course = false;
        steer_state.locked_course_err = 0;
        steering_control.steering = rudder_input;
        return;
    }

    float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
    if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
        flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
        steer_rate = 0;
    }
    if (!is_zero(steer_rate)) {
        // pilot is giving rudder input
        steer_state.locked_course = false;        
    } else if (!steer_state.locked_course) {
        // pilot has released the rudder stick or we are still - lock the course
        steer_state.locked_course = true;
        if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
            flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
            steer_state.locked_course_err = 0;
        }
    }
    if (!steer_state.locked_course) {
        // use a rate controller at the pilot specified rate
        steering_control.steering = steerController.get_steering_out_rate(steer_rate);
    } else {
        // use a error controller on the summed error
        int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
        steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
    }
    steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}

// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{

    // swash servo initialisation
    _servo_1.set_range(0,1000);
    _servo_2.set_range(0,1000);
    _servo_3.set_range(0,1000);
    _servo_4.set_angle(4500);

    // range check collective min, max and mid
    if( _collective_min >= _collective_max ) {
        _collective_min = 1000;
        _collective_max = 2000;
    }
    _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);

    // calculate collective mid point as a number from 0 to 1000
    _collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f;

    // determine roll, pitch and collective input scaling
    _roll_scaler = (float)_roll_max/4500.0f;
    _pitch_scaler = (float)_pitch_max/4500.0f;
    _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;

    // calculate factors based on swash type and servo position
    calculate_roll_pitch_collective_factors();

    // servo min/max values
    _servo_1.radio_min = 1000;
    _servo_1.radio_max = 2000;
    _servo_2.radio_min = 1000;
    _servo_2.radio_max = 2000;
    _servo_3.radio_min = 1000;
    _servo_3.radio_max = 2000;

    // mark swash as initialised
    _heliflags.swash_initialised = true;
}

// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
{
    // exit immediately if input_thr is zero
    if (input_thr <= 0.0f) {
        return 0.0f;
    }

    // TODO: does this parameter sanity check really belong here?
    g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);

    float in_min = g.throttle_min;
    float in_max = get_takeoff_trigger_throttle();

#if FRAME_CONFIG == HELI_FRAME
    // helicopters swash will move from bottom to 1/2 of mid throttle
    float out_min = 0;
#else
    // multicopters will output between spin-when-armed and 1/2 of mid throttle
    float out_min = motors.get_throttle_warn();
#endif
    float out_max = get_non_takeoff_throttle();

    if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
        in_min = channel_throttle->get_control_mid();
    }

    float input_range = in_max-in_min;
    float output_range = out_max-out_min;

    // sanity check ranges
    if (input_range <= 0.0f || output_range <= 0.0f) {
        return 0.0f;
    }

    return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}

// calculate pilot input to nudge speed up or down
//  target_speed should be in meters/sec
//  cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
//  return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle)
{
    // return immediately during RC/GCS failsafe
    if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
        return target_speed;
    }

    // return immediately if pilot is not attempting to nudge speed
    // pilot can nudge up speed if throttle (in range -100 to +100) is above 50% of center in direction of travel
    const int16_t pilot_throttle = constrain_int16(rover.channel_throttle->get_control_in(), -100, 100);
    if (((pilot_throttle <= 50) && (target_speed >= 0.0f)) ||
        ((pilot_throttle >= -50) && (target_speed <= 0.0f))) {
        return target_speed;
    }

    // sanity checks
    if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
        return target_speed;
    }

    // project vehicle's maximum speed
    const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);

    // return unadjusted target if already over vehicle's projected maximum speed
    if (fabsf(target_speed) >= vehicle_speed_max) {
        return target_speed;
    }

    const float speed_increase_max = vehicle_speed_max - fabsf(target_speed);
    float speed_nudge = ((static_cast<float>(abs(pilot_throttle)) - 50.0f) * 0.02f) * speed_increase_max;
    if (pilot_throttle < 0) {
        speed_nudge = -speed_nudge;
    }

    return target_speed + speed_nudge;
}

/*
  calculate yaw control for coordinated flight
 */
void Plane::calc_nav_yaw_coordinated(float speed_scaler)
{
    bool disable_integrator = false;
    if (control_mode == STABILIZE && rudder_input != 0) {
        disable_integrator = true;
    }

    int16_t commanded_rudder;

    // Received an external msg that guides yaw in the last 3 seconds?
    if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
            plane.guided_state.last_forced_rpy_ms.z > 0 &&
            millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
        commanded_rudder = plane.guided_state.forced_rpy_cd.z;
    } else {
        commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);

        // add in rudder mixing from roll
        commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
        commanded_rudder += rudder_input;
    }

    steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
}

// move_yaw
void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out)
{
    _yaw_servo.servo_out = constrain_int16(yaw_out, -4500, 4500);

    if (_yaw_servo.servo_out != yaw_out) {
        limit.yaw = true;
    }

    _yaw_servo.calc_pwm();

    hal.rcout->write(AP_MOTORS_MOT_4, _yaw_servo.radio_out);

    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
        // output gain to exernal gyro
        if (_acro_tail && _ext_gyro_gain_acro > 0) {
            write_aux(_ext_gyro_gain_acro);
        } else {
            write_aux(_ext_gyro_gain_std);
        }
    } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) {
        // output yaw servo to tail rsc
        write_aux(_yaw_servo.servo_out);
    }
}

/*
  setup mixer on PX4 so that if FMU dies the pilot gets manual control
 */
bool Plane::setup_failsafe_mixing(void)
{
    // we create MIXER.MIX regardless of whether we will be using it,
    // as it gives a template for the user to modify to create their
    // own CUSTOM.MIX file
    const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
    const char *custom_mixer_filename = "/fs/microsd/APM/CUSTOM.MIX";
    bool ret = false;
    char *buf = NULL;
    const uint16_t buf_size = 2048;

    if (!create_mixer_file(mixer_filename)) {
        return false;
    }

    struct stat st;
    const char *filename;
    if (stat(custom_mixer_filename, &st) == 0) {
        filename = custom_mixer_filename;
    } else {
        filename = mixer_filename;
    }

    enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
    struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};

    int px4io_fd = open("/dev/px4io", 0);
    if (px4io_fd == -1) {
        // px4io isn't started, no point in setting up a mixer
        return false;
    }

    buf = (char *)malloc(buf_size);
    if (buf == NULL) {
        goto failed;
    }
    if (load_mixer_file(filename, &buf[0], buf_size) != 0) {
        hal.console->printf("Unable to load %s\n", filename);
        goto failed;
    }

    if (old_state == AP_HAL::Util::SAFETY_ARMED) {
        // make sure the throttle has a non-zero failsafe value before we
        // disable safety. This prevents sending zero PWM during switch over
        hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min);
    }

    // we need to force safety on to allow us to load a mixer
    hal.rcout->force_safety_on();

    /* reset any existing mixer in px4io. This shouldn't be needed,
     * but is good practice */
    if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
        hal.console->printf("Unable to reset mixer\n");
        goto failed;
    }

	/* pass the buffer to the device */
    if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
        hal.console->printf("Unable to send mixer to IO\n");
        goto failed;        
    }

    // setup RC config for each channel based on user specified
    // mix/max/trim. We only do the first 8 channels due to 
    // a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
    for (uint8_t i=0; i<8; i++) {
        RC_Channel *ch = RC_Channel::rc_channel(i);
        if (ch == NULL) {
            continue;
        }
        struct pwm_output_rc_config config;
        /*
          we use a min/max of 900/2100 to allow for pass-thru of
          larger values than the RC min/max range. This mimics the APM
          behaviour of pass-thru in manual, which allows for dual-rate
          transmitter setups in manual mode to go beyond the ranges
          used in stabilised modes
         */
        config.channel = i;
        config.rc_min = 900;
        config.rc_max = 2100;
        if (rcmap.throttle()-1 == i) {
            // throttle uses a trim of 1500, so we don't get division
            // by small numbers near RC3_MIN
            config.rc_trim = 1500;
        } else {
            config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min, config.rc_max);
        }
        config.rc_dz = 0; // zero for the purposes of manual takeover
        config.rc_assignment = i;
        // we set reverse as false, as users of ArduPilot will have
        // input reversed on transmitter, so from the point of view of
        // the mixer the input is never reversed. The one exception is
        // the 2nd channel, which is reversed inside the PX4IO code,
        // so needs to be unreversed here to give sane behaviour.
        if (i == 1) {
//.........这里部分代码省略.........

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

//.........这里部分代码省略.........

            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) {

void AP_LandingGear::update(float height_above_ground_m)
{
    if (_pin_weight_on_wheels == -1) {
        last_wow_event_ms = 0;
        wow_state_current = LG_WOW_UNKNOWN;
    } else {
        LG_WOW_State wow_state_new = hal.gpio->read(_pin_weight_on_wheels) == _pin_weight_on_wheels_polarity ? LG_WOW : LG_NO_WOW;
        
        if (wow_state_new != wow_state_current) {
            // we changed states, lets note the time.
            last_wow_event_ms = AP_HAL::millis();
            log_wow_state(wow_state_new);
        }
        
        wow_state_current = wow_state_new;
    }
    
    if (_pin_deployed == -1) {
        last_gear_event_ms = 0;
        
        // If there was no pilot input and state is still unknown - leave it as it is
        if (gear_state_current != LG_UNKNOWN) {
            gear_state_current = (_deployed == true ? LG_DEPLOYED : LG_RETRACTED);
        }
    } else {
        LG_LandingGear_State gear_state_new;
        
        if (_deployed) {
            gear_state_new = (deployed() == true ? LG_DEPLOYED : LG_DEPLOYING);
        } else {
            gear_state_new = (deployed() == false ? LG_RETRACTED : LG_RETRACTING);
        }
        
        if (gear_state_new != gear_state_current) {
            // we changed states, lets note the time.
            last_gear_event_ms = AP_HAL::millis();
            
            log_wow_state(wow_state_current);
        }
        
        gear_state_current = gear_state_new;
    }

    /*
      check for height based triggering
     */
    int16_t alt_m = constrain_int16(height_above_ground_m, 0, INT16_MAX);

    if (hal.util->get_soft_armed()) {
        // only do height based triggering when armed
        if ((!_deployed || !_have_changed) &&
            _deploy_alt > 0 &&
            alt_m <= _deploy_alt &&
            _last_height_above_ground > _deploy_alt) {
            deploy();
        }
        if ((_deployed || !_have_changed) &&
            _retract_alt > 0 &&
            _retract_alt >= _deploy_alt &&
            alt_m >= _retract_alt &&
            _last_height_above_ground < _retract_alt) {
            retract();
        }
    }

    _last_height_above_ground = alt_m;
}

void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
{

    int16_t channels[11];

    float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
    float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
    int16_t rpyCenter = 1500;
    int16_t throttleBase = 1500-500*throttleScale;

    bool shift = false;

    // Neutralize camera tilt speed setpoint
    cam_tilt = 1500;

    // Detect if any shift button is pressed
    for (uint8_t i = 0 ; i < 16 ; i++) {
        if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
            shift = true;
        }
    }

    // Act if button is pressed
    // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
    for (uint8_t i = 0 ; i < 16 ; i++) {
        if ((buttons & (1 << i))) {
            handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
        }
    }

    buttons_prev = buttons;

    // Set channels to override
    if (!roll_pitch_flag) {
        channels[0] = 1500 + pitchTrim; // pitch
        channels[1] = 1500 + rollTrim;  // roll
    } else {
        // adjust roll and pitch with joystick input instead of forward and lateral
        channels[0] = constrain_int16((x+pitchTrim)*rpyScale+rpyCenter,1100,1900);
        channels[1] = constrain_int16((y+rollTrim)*rpyScale+rpyCenter,1100,1900);
    }

    channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
    channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900);                 // yaw

    if (!roll_pitch_flag) {
        // adjust forward and lateral with joystick input instead of roll and pitch
        channels[4] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
        channels[5] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900); // lateral for ROV
    } else {
        // neutralize forward and lateral input while we are adjusting roll and pitch
        channels[4] = constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900); // forward for ROV
        channels[5] = constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900); // lateral for ROV
    }

    channels[6] = 0;             // Unused
    channels[7] = cam_tilt;      // camera tilt
    channels[8] = lights1;       // lights 1
    channels[9] = lights2;       // lights 2
    channels[10] = video_switch; // video switch

    // Store old x, y, z values for use in input hold logic
    x_last = x;
    y_last = y;
    z_last = z;

    hal.rcin->set_overrides(channels, 10);
}

/*
  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();
    }
}

/*
  implement a software VTail or elevon mixer. There are 4 different mixing modes
 */
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const
{
    int16_t c1, c2;
    int16_t v1, v2;

    // first get desired elevator and rudder as -500..500 values
    c1 = chan1_out - 1500;
    c2 = chan2_out - 1500;

    // apply MIXING_OFFSET to input channels using long-integer version
    //  of formula:  x = x * (g.mixing_offset/100.0 + 1.0)
    //  -100 => 2x on 'c1', 100 => 2x on 'c2'
    if (g.mixing_offset < 0) {
        c1 = (int16_t)(((int32_t)c1) * (-g.mixing_offset+100) / 100);
    } else if (g.mixing_offset > 0) {
        c2 = (int16_t)(((int32_t)c2) * (g.mixing_offset+100) / 100);
    }

    v1 = (c1 - c2) * g.mixing_gain;
    v2 = (c1 + c2) * g.mixing_gain;

    // now map to mixed output
    switch (mixing_type) {
    case MIXING_DISABLED:
        return;

    case MIXING_UPUP:
        break;

    case MIXING_UPDN:
        v2 = -v2;
        break;

    case MIXING_DNUP:
        v1 = -v1;
        break;

    case MIXING_DNDN:
        v1 = -v1;
        v2 = -v2;
        break;

    case MIXING_UPUP_SWP:
        std::swap(v1, v2);
        break;

    case MIXING_UPDN_SWP:
        v2 = -v2;
        std::swap(v1, v2);        
        break;

    case MIXING_DNUP_SWP:
        v1 = -v1;
        std::swap(v1, v2);        
        break;

    case MIXING_DNDN_SWP:
        v1 = -v1;
        v2 = -v2;
        std::swap(v1, v2);        
        break;
    }

    // scale for a 1500 center and 900..2100 range, symmetric
    v1 = constrain_int16(v1, -600, 600);
    v2 = constrain_int16(v2, -600, 600);

    chan1_out = 1500 + v1;
    chan2_out = 1500 + v2;
}

//.........这里部分代码省略.........
                }
                RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3);
                RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4);
            }

            // directly set the radio_out values for elevon mode
            channel_roll->radio_out  =     elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX));
            channel_pitch->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->servo_out = 0;
#else
        // convert 0 to 100% into PWM
        uint8_t min_throttle = aparm.throttle_min.get();
        uint8_t max_throttle = aparm.throttle_max.get();
        if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
            min_throttle = 0;
        }
        if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
            if(aparm.takeoff_throttle_max != 0) {
                max_throttle = aparm.takeoff_throttle_max;
            } else {
                max_throttle = aparm.throttle_max;
            }
        }
        channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
                                      min_throttle,
                                      max_throttle);

        if (!hal.util->get_soft_armed()) {
            channel_throttle->servo_out = 0;
            channel_throttle->calc_pwm();
        } else if (suppress_throttle()) {
            // throttle is suppressed in auto mode
            channel_throttle->servo_out = 0;
            if (g.throttle_suppress_manual) {
                // manual pass through of throttle while throttle is suppressed
                channel_throttle->radio_out = channel_throttle->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)) {
            // manual pass through of throttle while in FBWA or
            // STABILIZE mode with THR_PASS_STAB set
            channel_throttle->radio_out = channel_throttle->radio_in;
        } else if (control_mode == GUIDED &&
                   guided_throttle_passthru) {
            // manual pass through of throttle while in GUIDED
            channel_throttle->radio_out = channel_throttle->radio_in;
        } else {
            // normal throttle calculation based on servo_out
            channel_throttle->calc_pwm();
        }

// sanity check parameters
void AP_MotorsUGV::sanity_check_parameters()
{
    _throttle_min = constrain_int16(_throttle_min, 0, 20);
    _throttle_max = constrain_int16(_throttle_max, 30, 100);
    _vector_throttle_base = constrain_float(_vector_throttle_base, 0.0f, 100.0f);
}

void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
{

    float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
    float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
    int16_t rpyCenter = 1500;
    int16_t throttleBase = 1500-500*throttleScale;

    bool shift = false;

    // Neutralize camera tilt and pan speed setpoint
    cam_tilt = 1500;
    cam_pan = 1500;

    // Detect if any shift button is pressed
    for (uint8_t i = 0 ; i < 16 ; i++) {
        if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
            shift = true;
        }
    }

    // Act if button is pressed
    // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
    for (uint8_t i = 0 ; i < 16 ; i++) {
        if ((buttons & (1 << i))) {
            handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
            // buttonDebounce = tnow_ms;
        } else if (buttons_prev & (1 << i)) {
            handle_jsbutton_release(i, shift);
        }
    }

    buttons_prev = buttons;

    // attitude mode:
    if (roll_pitch_flag == 1) {
    // adjust roll/pitch trim with joystick input instead of forward/lateral
        pitchTrim = -x * rpyScale;
        rollTrim  =  y * rpyScale;
    }

    uint32_t tnow = AP_HAL::millis();

    RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch
    RC_Channels::set_override(1, constrain_int16(rollTrim  + rpyCenter,1100,1900), tnow); // roll

    RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900), tnow); // throttle
    RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow);                 // yaw

    // maneuver mode:
    if (roll_pitch_flag == 0) {
        // adjust forward and lateral with joystick input instead of roll and pitch
        RC_Channels::set_override(4, constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
        RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
    } else {
        // neutralize forward and lateral input while we are adjusting roll and pitch
        RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
        RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
    }

    RC_Channels::set_override(6, cam_pan, tnow);       // camera pan
    RC_Channels::set_override(7, cam_tilt, tnow);      // camera tilt
    RC_Channels::set_override(8, lights1, tnow);       // lights 1
    RC_Channels::set_override(9, lights2, tnow);       // lights 2
    RC_Channels::set_override(10, video_switch, tnow); // video switch

    // Store old x, y, z values for use in input hold logic
    x_last = x;
    y_last = y;
    z_last = z;
}

// output_armed - sends commands to the motors
void AP_MotorsMatrix::output_armed()
{
    int8_t i;
    int16_t out_min = _rc_throttle->radio_min;
    int16_t out_max = _rc_throttle->radio_max;
    int16_t rc_yaw_constrained_pwm;
    int16_t rc_yaw_excess;
    int16_t upper_margin, lower_margin;
    int16_t motor_adjustment = 0;
    int16_t yaw_to_execute = 0;

    // initialize reached_limit flag
    _reached_limit = AP_MOTOR_NO_LIMITS_REACHED;

    // Throttle is 0 to 1000 only
    _rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);

    // capture desired roll, pitch, yaw and throttle from receiver
    _rc_roll->calc_pwm();
    _rc_pitch->calc_pwm();
    _rc_throttle->calc_pwm();
    _rc_yaw->calc_pwm();

    // if we are not sending a throttle output, we cut the motors
    if(_rc_throttle->servo_out == 0) {
        for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
            if( motor_enabled[i] ) {
                motor_out[i]    = _rc_throttle->radio_min;
            }
        }
        // if we have any roll, pitch or yaw input then it's breaching the limit
        if( _rc_roll->pwm_out != 0 || _rc_pitch->pwm_out != 0 ) {
            _reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT;
        }
        if( _rc_yaw->pwm_out != 0 ) {
            _reached_limit |= AP_MOTOR_YAW_LIMIT;
        }
    } else {    // non-zero throttle

        out_min = _rc_throttle->radio_min + _min_throttle;

        // initialise rc_yaw_contrained_pwm that we will certainly output and rc_yaw_excess that we will do on best-efforts basis.
        // Note: these calculations and many others below depend upon _yaw_factors always being 0, -1 or 1.
        if( _rc_yaw->pwm_out < -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
            rc_yaw_constrained_pwm = -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
            rc_yaw_excess = _rc_yaw->pwm_out+AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
        }else if( _rc_yaw->pwm_out > AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
            rc_yaw_constrained_pwm = AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
            rc_yaw_excess = _rc_yaw->pwm_out-AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
        }else{
            rc_yaw_co