diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index a269e3c208..70614c0c7f 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -362,6 +362,8 @@ static void set_servos(void) // Doug could you please take a look at this ? RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_flap_auto); + //RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_dspoiler1); + //RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_dspoiler2); } else { if (g.mix_mode == 0) { RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, g.channel_roll.servo_out); @@ -371,6 +373,20 @@ static void set_servos(void) 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); + /*Differntial Spoilers*/ + 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); + } // spoiler deployed 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)); } diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index e44e9dd6bc..a6488a5de3 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -71,6 +71,12 @@ void update_aux_servo_function( RC_Channel_aux* rc_a, case RC_Channel_aux::k_aileron: _aux_channels[i]->set_angle(4500); break; + case RC_Channel_aux::k_dspoiler1: + _aux_channels[i]->set_angle(4500); + break; + case RC_Channel_aux::k_dspoiler2: + _aux_channels[i]->set_angle(4500); + break; default: break; } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index dc65a8a394..c4cb4433bc 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -41,6 +41,8 @@ public: k_mount2_tilt = 13, ///< mount2 pitch (tilt) k_mount2_roll = 14, ///< mount2 roll k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) + k_dspoiler1 = 16, ///< differential spoiler 1 (left wing) + k_dspoiler2 = 17, ///< differential spoiler 2 (right wing) k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t;