mirror of https://github.com/ArduPilot/ardupilot
Differential spoilers support, elevon offset
Fixed differential spoilers support, added elevon offset See PR #2935
This commit is contained in:
parent
eefdc32f51
commit
7d824247af
|
@ -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()) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
//
|
||||
|
|
Loading…
Reference in New Issue