diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 417bf990d2..a13663d1e1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1042,6 +1042,7 @@ private: void set_servos_controlled(void); void set_servos_old_elevons(void); void set_servos_flaps(void); + void servo_output_mixers(void); void servos_output(void); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); bool allow_reverse_thrust(void); diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index c0dd32efd5..620fd59e06 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -76,12 +76,6 @@ void Plane::failsafe_check(void) RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, rudder); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, rudder); - 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); - } - // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 669fefb5fc..db9c68aed2 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -602,6 +602,57 @@ void Plane::set_servos_flaps(void) } +/* + apply vtail and elevon mixers + the rewrites radio_out for the corresponding channels + */ +void Plane::servo_output_mixers(void) +{ + 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 (both) differential spoilers setup then apply rudder + // control into splitting the two elevons 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)) { + int16_t ch3 = channel_roll->get_radio_out(); //diff spoiler 1 + int16_t ch4 = channel_pitch->get_radio_out(); //diff spoiler 2 + // convert rudder-servo output (-4500 to 4500) to PWM offset + // value (-500 to 500) and multiply by DSPOILR_RUD_RATE/100 + // (rudder->servo_out * 500 / SERVO_MAX * dspoiler_rud_rate/100): + int16_t ruddVal = (int16_t)((int32_t)(channel_rudder->get_servo_out()) * + g.dspoiler_rud_rate / (SERVO_MAX/5)); + if (ruddVal != 0) { //if nonzero rudder then apply to spoilers + int16_t ch1 = ch3; //elevon 1 + int16_t ch2 = ch4; //elevon 2 + if (ruddVal > 0) { //apply rudder to right or left side + ch1 += ruddVal; + ch3 -= ruddVal; + } else { + ch2 += ruddVal; + ch4 -= ruddVal; + } + // change elevon 1 & 2 positions; constrain min/max: + channel_roll->set_radio_out(constrain_int16(ch1, 900, 2100)); + channel_pitch->set_radio_out(constrain_int16(ch2, 900, 2100)); + // constrain min/max for intermediate dspoiler positions: + ch3 = constrain_int16(ch3, 900, 2100); + ch4 = constrain_int16(ch4, 900, 2100); + } + // set positions of differential spoilers (convert PWM + // 900-2100 range to servo output (-4500 to 4500) + // and use function that supports rev/min/max/trim): + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, + (ch3-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, + (ch4-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); + } + } +} + + /* Set the flight control servos based on the current calculated values @@ -685,49 +736,6 @@ void Plane::set_servos(void) channel_rudder->set_radio_out(channel_rudder->get_radio_in()); } - 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 (both) differential spoilers setup then apply rudder - // control into splitting the two elevons 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)) { - int16_t ch3 = channel_roll->get_radio_out(); //diff spoiler 1 - int16_t ch4 = channel_pitch->get_radio_out(); //diff spoiler 2 - // convert rudder-servo output (-4500 to 4500) to PWM offset - // value (-500 to 500) and multiply by DSPOILR_RUD_RATE/100 - // (rudder->servo_out * 500 / SERVO_MAX * dspoiler_rud_rate/100): - int16_t ruddVal = (int16_t)((int32_t)(channel_rudder->get_servo_out()) * - g.dspoiler_rud_rate / (SERVO_MAX/5)); - if (ruddVal != 0) { //if nonzero rudder then apply to spoilers - int16_t ch1 = ch3; //elevon 1 - int16_t ch2 = ch4; //elevon 2 - if (ruddVal > 0) { //apply rudder to right or left side - ch1 += ruddVal; - ch3 -= ruddVal; - } else { - ch2 += ruddVal; - ch4 -= ruddVal; - } - // change elevon 1 & 2 positions; constrain min/max: - channel_roll->set_radio_out(constrain_int16(ch1, 900, 2100)); - channel_pitch->set_radio_out(constrain_int16(ch2, 900, 2100)); - // constrain min/max for intermediate dspoiler positions: - ch3 = constrain_int16(ch3, 900, 2100); - ch4 = constrain_int16(ch4, 900, 2100); - } - // set positions of differential spoilers (convert PWM - // 900-2100 range to servo output (-4500 to 4500) - // and use function that supports rev/min/max/trim): - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, - (ch3-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, - (ch4-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); - } - } - if (!arming.is_armed()) { //Some ESCs get noisy (beep error msgs) if PWM == 0. //This little segment aims to avoid this. @@ -804,17 +812,18 @@ void Plane::set_servos(void) void Plane::servos_output(void) { hal.rcout->cork(); - + + if (g.rudder_only != 0) { + channel_roll->set_radio_out(channel_roll->get_radio_trim()); + } + // remap servo output to SERVO* ranges if enabled g2.servo_channels.remap_servo_output(); - if (g.rudder_only == 0) { - // when in 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(); - } + // run vtail and elevon mixers + servo_output_mixers(); + channel_roll->output(); channel_pitch->output(); channel_throttle->output(); channel_rudder->output();