diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index ed2301f26b..6d83cf3630 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -680,6 +680,15 @@ void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16 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; @@ -868,12 +877,6 @@ void Plane::set_servos(void) 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 @@ -1120,6 +1123,43 @@ void Plane::set_servos(void) 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()) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 98f26d8a56..71c85c6ced 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -863,6 +863,22 @@ const AP_Param::Info Plane::var_info[] = { // @User: User GSCALAR(rudder_only, "RUDDER_ONLY", 0), + // @Param: MIXING_OFFSET + // @DisplayName: Mixing Offset + // @Description: The offset for the Vtail and elevon output mixers, as a percentage. This can be used in combination with MIXING_GAIN to configure how the control surfaces respond to input. The response to aileron or elevator input can be increased by setting this parameter to a positive or negative value. A common usage is to enter a positive value to increase the aileron response of the elevons of a flying wing. The default value of zero will leave the aileron-input response equal to the elevator-input response. + // @Units: percent + // @Range: -1000 1000 + // @User: User + GSCALAR(mixing_offset, "MIXING_OFFSET", 0), + + // @Param: DSPOILR_RUD_RATE + // @DisplayName: Differential spoilers rudder rate + // @Description: Sets the amount of deflection that the rudder output will apply to the differential spoilers, as a percentage. The default value of 100 results in full rudder applying full deflection. A value of 0 will result in the differential spoilers exactly following the elevons (no rudder effect). + // @Units: percent + // @Range: -1000 1000 + // @User: User + GSCALAR(dspoiler_rud_rate, "DSPOILR_RUD_RATE", DSPOILR_RUD_RATE_DEFAULT), + // @Param: SYS_NUM_RESETS // @DisplayName: Num Resets // @Description: Number of APM board resets diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index cd83eef9b1..73f905f206 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -341,6 +341,9 @@ public: k_param_pidNavPitchAltitude, // unused k_param_pidWheelSteer, // unused + k_param_mixing_offset, + k_param_dspoiler_rud_rate, + k_param_DataFlash = 253, // Logging Group // 254,255: reserved @@ -463,6 +466,8 @@ public: AP_Int8 reverse_elevons; AP_Int8 reverse_ch1_elevon; AP_Int8 reverse_ch2_elevon; + AP_Int16 mixing_offset; + AP_Int16 dspoiler_rud_rate; AP_Int16 num_resets; AP_Int32 log_bitmask; AP_Int8 reset_switch_chan; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 51b9d90b95..d0cc8cf6f1 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -220,6 +220,10 @@ # define ELEVON_CH2_REVERSE DISABLED #endif +#ifndef DSPOILR_RUD_RATE_DEFAULT + #define DSPOILR_RUD_RATE_DEFAULT 100 +#endif + ////////////////////////////////////////////////////////////////////////////// // CAMERA TRIGGER AND CONTROL //