/*
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);
}
}
/*
Set the flight control servos based on the current calculated values
This function operates by first building up output values for
channels using set_servo() and set_radio_out(). Using
set_radio_out() is for when a raw PWM value of output is given which
does not depend on any output scaling. Using set_servo() is for when
scaling and mixing will be needed.
Finally servos_output() is called to push the final PWM values
for output channels
*/
void Plane::set_servos(void)
{
// start with output corked. the cork is released when we run
// servos_output(), which is run from all code paths in this
// function
SRV_Channels::cork();
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
if (afs.should_crash_vehicle()) {
afs.terminate_vehicle();
return;
}
// do any transition updates for quadplane
quadplane.update();
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
servos_output();
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 (!SRV_Channels::function_assigned(SRV_Channel::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;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
if (control_mode == TRAINING) {
steering_control.rudder = channel_rudder->get_control_in();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
set_servos_manual_passthrough();
} else {
set_servos_controlled();
}
// setup flap outputs
set_servos_flaps();
if (auto_throttle_mode ||
quadplane.in_assisted_flight() ||
quadplane.in_vtol_mode()) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit();
}
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:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
break;
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
break;
}
}
#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) {
// we don't run the output mixer
return;
}
}
#endif
if (landing.get_then_servos_neutral() > 0 &&
control_mode == AUTO &&
landing.get_disarm_delay() > 0 &&
landing.is_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 (landing.get_then_servos_neutral() == 1) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
} else if (landing.get_then_servos_neutral() == 2) {
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
}
uint8_t override_pct;
if (g2.ice_control.throttle_override(override_pct)) {
// the ICE controller wants to override the throttle for starting
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
}
// run output mixer and send values to the hal for output
servos_output();
}
/*****************************************
* 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();
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void) {
static uint16_t last_throttle;
bool apply_skid_mix = true; // Normaly true, false when the mixage is done by the controler with skid_steer_in = 1
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
// suppress steer if in failsafe and manual and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
}
}
if (g.skid_steer_in) {
apply_skid_mix = false;
}
} else {
if (in_reverse) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-g.throttle_max,
-g.throttle_min));
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g.throttle_min.get(),
g.throttle_max.get()));
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
if (!hal.util->get_soft_armed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
// limit throttle movement speed
throttle_slew_limit(last_throttle);
}
// record last throttle before we apply skid steering
SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle);
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = throttle_scaled - 0.5f * steering_scaled;
if (!apply_skid_mix) { // Mixage is already done by a controller so just pass the value to motor
motor1 = steering_scaled;
motor2 = throttle_scaled;
} else if (fabsf(throttle_scaled) <= 0.01f) { // Use full range for on spot turn
motor1 = steering_scaled;
motor2 = -steering_scaled;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
}
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:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0);
}
break;
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
}
break;
}
}
SRV_Channels::calc_pwm();
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
SRV_Channels::output_ch_all();
#endif
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
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->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(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out(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->radio_out = channel_roll->radio_in;
channel_pitch->radio_out = channel_pitch->radio_in;
} else {
channel_roll->radio_out = channel_roll->read();
channel_pitch->radio_out = channel_pitch->read();
}
channel_throttle->radio_out = channel_throttle->radio_in;
channel_rudder->radio_out = channel_rudder->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(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out(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->radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->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(RC_Channel_aux::k_aileron, channel_roll->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out);
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out);
} else {
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->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->servo_out < 0) {
ch1 += abs(channel_rudder->servo_out);
ch3 -= abs(channel_rudder->servo_out);
} else {
ch2 += abs(channel_rudder->servo_out);
ch4 -= abs(channel_rudder->servo_out);
}
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();
}
#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:
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_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(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent);
if (control_mode >= FLY_BY_WIRE_B) {
/* 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->radio_out = channel_rudder->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->radio_out, channel_rudder->radio_out);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
}
//send throttle to 0 or MIN_PWM if not yet armed
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::YES_MIN_PWM:
channel_throttle->radio_out = channel_throttle->radio_min;
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->radio_out = 0;
break;
default:
//keep existing behavior: do nothing to radio_out
//(don't disarm throttle channel even if AP_Arming class is)
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 (comm_get_txspace(MAVLINK_COMM_0) >=
MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
}
#endif
// 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();
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void) {
static int16_t last_throttle;
// support a separate steering channel
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
channel_steer->set_radio_out(channel_steer->read());
channel_throttle->set_radio_out(channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
channel_throttle->set_radio_out(channel_throttle->get_radio_trim());
}
} else {
channel_steer->calc_pwm();
if (in_reverse) {
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
-g.throttle_max,
-g.throttle_min));
} else {
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
g.throttle_min.get(),
g.throttle_max.get()));
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
channel_throttle->set_servo_out(0);
}
if (!hal.util->get_soft_armed()) {
channel_throttle->set_servo_out(0);
}
// convert 0 to 100% into PWM
channel_throttle->calc_pwm();
// limit throttle movement speed
throttle_slew_limit(last_throttle);
}
// record last throttle before we apply skid steering
last_throttle = channel_throttle->get_radio_out();
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5f*steering_scaled;
float motor2 = throttle_scaled - 0.5f*steering_scaled;
channel_steer->set_servo_out(4500*motor1);
channel_throttle->set_servo_out(100*motor2);
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
}
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(channel_throttle->get_radio_trim());
break;
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
channel_steer->output();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
#endif
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void)
{
static int16_t last_throttle;
// support a separate steering channel
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
channel_steer->radio_out = channel_steer->read();
channel_throttle->radio_out = channel_throttle->read();
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
channel_throttle->radio_out = channel_throttle->radio_trim;
}
} else {
channel_steer->calc_pwm();
if (in_reverse) {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
-g.throttle_max,
-g.throttle_min);
} else {
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
channel_throttle->servo_out = 0;
}
// convert 0 to 100% into PWM
channel_throttle->calc_pwm();
// limit throttle movement speed
throttle_slew_limit(last_throttle);
}
// record last throttle before we apply skid steering
last_throttle = channel_throttle->radio_out;
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5f*steering_scaled;
float motor2 = throttle_scaled - 0.5f*steering_scaled;
channel_steer->servo_out = 4500*motor1;
channel_throttle->servo_out = 100*motor2;
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
channel_steer->output();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
#endif
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void set_servos(void)
{
int16_t last_throttle = g.channel_throttle.radio_out;
if (control_mode == MANUAL) {
// do a direct pass through of radio values
if (g.mix_mode == 0) {
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
g.channel_roll.radio_out = hal.rcin->read(CH_ROLL);
g.channel_pitch.radio_out = hal.rcin->read(CH_PITCH);
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
// setup extra aileron channel. We want this to come from the
// main aileron 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
int16_t aileron_in = g.channel_roll.pwm_to_angle_dz(0);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, aileron_in);
// this aileron 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);
// copy flap control from transmitter
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_flap_auto);
if (g.mix_mode != 0) {
// set any differential spoilers to follow the elevons in
// manual mode.
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, g.channel_roll.radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, g.channel_pitch.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(RC_Channel_aux::k_aileron, g.channel_roll.servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, g.channel_roll.servo_out);
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = g.channel_pitch.servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * g.channel_roll.servo_out);
ch2 = g.channel_pitch.servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * g.channel_roll.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) * g.channel_rudder.servo_out < 0) {
ch1 += abs(g.channel_rudder.servo_out);
ch3 -= abs(g.channel_rudder.servo_out);
} else {
ch2 += abs(g.channel_rudder.servo_out);
ch4 -= abs(g.channel_rudder.servo_out);
}
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
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
#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
if (obc.crash_plane()) {
g.channel_roll.servo_out = -4500;
g.channel_pitch.servo_out = -4500;
g.channel_rudder.servo_out = -4500;
g.channel_throttle.servo_out = 0;
}
#endif
// push out the PWM values
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
}
g.channel_rudder.calc_pwm();
#if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0;
#else
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
if (suppress_throttle()) {
// throttle is suppressed in auto mode
g.channel_throttle.servo_out = 0;
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
} else {
g.channel_throttle.calc_pwm();
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == FLY_BY_WIRE_A)) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
} else {
// normal throttle calculation based on servo_out
g.channel_throttle.calc_pwm();
}
#endif
}
// Auto flap deployment
if(control_mode < FLY_BY_WIRE_B) {
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_flap_auto);
} else if (control_mode >= FLY_BY_WIRE_B) {
int16_t flapSpeedSource = 0;
// FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
if (control_mode == FLY_BY_WIRE_B) {
flapSpeedSource = target_airspeed_cm * 0.01;
} else if (airspeed.use()) {
flapSpeedSource = g.airspeed_cruise_cm * 0.01;
} else {
flapSpeedSource = g.throttle_cruise;
}
if ( g.flap_1_speed != 0 && flapSpeedSource > g.flap_1_speed) {
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 0);
} else if (g.flap_2_speed != 0 && flapSpeedSource > g.flap_2_speed) {
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, g.flap_1_percent);
} else {
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, g.flap_2_percent);
}
}
if (control_mode >= FLY_BY_WIRE_B) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit(last_throttle);
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos
hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
g.rc_7.output_ch(CH_7);
g.rc_8.output_ch(CH_8);
# if CONFIG_HAL_BOARD == HAL_BOARD_APM2
g.rc_9.output_ch(CH_9);
g.rc_10.output_ch(CH_10);
g.rc_11.output_ch(CH_11);
# endif
#endif
}
