/* allow for runtime change of control channel ordering */ void Plane::set_control_channels(void) { if (g.rudder_only) { // in rudder only mode the roll and rudder channels are the // same. channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1); } else { channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); } channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1); channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1); // set rc channel ranges channel_roll->set_angle(SERVO_MAX); channel_pitch->set_angle(SERVO_MAX); channel_rudder->set_angle(SERVO_MAX); if (!have_reverse_thrust()) { // normal operation channel_throttle->set_range(100); } else { // reverse thrust if (have_reverse_throttle_rc_option) { // when we have a reverse throttle RC option setup we use throttle // as a range, and rely on the RC switch to get reverse thrust channel_throttle->set_range(100); } else { channel_throttle->set_angle(100); } SRV_Channels::set_angle(SRV_Channel::k_throttle, 100); SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 100); SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100); } if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } if (!quadplane.enable) { // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. For quadplanes we use AP_Motors // scaling g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); } }
/* initialise RC output for main channels. This is done early to allow for BRD_SAFETYENABLE=0 and early servo control */ void Plane::init_rc_out_main() { /* change throttle trim to minimum throttle. This prevents a configuration error where the user sets CH3_TRIM incorrectly and the motor may start on power up */ if (!have_reverse_thrust()) { SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle); } SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); // setup PX4 to output the min throttle when safety off if arming // is setup for min on disarm if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } }
/* 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); } }