Plane: added flaperon support

this adds FLAPERON_OUTPUT and FLAP_IN_CHANNEL to allow both manual
and auto flap control for both normal flaps and flaperons

ArduPlane/Attitude.pde

@@ -679,6 +679,45 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_
     chan2_out = 1500 + v2;
 }
 
+/*
+  setup flaperon output channels
+ */
+static void flaperon_update(int8_t flap_percent)
+{
+    if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) ||
+        !RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) {
+        return;
+    }
+    int16_t ch1, ch2;
+    /*
+      flaperons are implemented as a mixer between aileron and a
+      percentage of flaps. Flap input can come from a manual channel
+      or from auto flaps. When the manual flap percentage is larger
+      than the auto flap percentage then the manual flap percentage
+      takes priority. Note that negative manual flap percentates are
+      allowed, which give spoilerons
+     */
+    ch1 = channel_roll->radio_out;
+
+    int16_t flap_trim = 1500;
+    RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
+    if (flapin != NULL) {
+        flap_trim = flapin->radio_trim;
+    }
+
+    if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
+        flapin->input();
+        int8_t manual_flap_percent = constrain_int16(flapin->norm_input() * 100, -100, 100);
+        if (abs(manual_flap_percent) > flap_percent) {
+            flap_percent = manual_flap_percent;
+        }
+    }
+    ch2 = flap_trim - flap_percent * 5;
+    channel_output_mixer(g.flaperon_output, ch1, ch2);
+    RC_Channel_aux::set_radio(RC_Channel_aux::k_flaperon1, ch1);
+    RC_Channel_aux::set_radio(RC_Channel_aux::k_flaperon2, ch2);
+}
+
 /*****************************************
 * Set the flight control servos based on the current calculated values
 *****************************************/
@@ -822,26 +861,25 @@ static void set_servos(void)
     }
 
     // Auto flap deployment
+    uint8_t auto_flap_percent = 0;
+
     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;
+        if (airspeed.use()) {
+            flapSpeedSource = target_airspeed_cm * 0.01f;
         } else {
             flapSpeedSource = aparm.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);
+            auto_flap_percent = 0;
+        } else if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
+            auto_flap_percent = g.flap_2_percent;
         } else {
-            RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, g.flap_2_percent);
+            auto_flap_percent = g.flap_1_percent;
         }
+        RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent);
     }
 
     if (control_mode >= FLY_BY_WIRE_B) {
@@ -855,6 +893,9 @@ static void set_servos(void)
         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) {

ArduPlane/Parameters.h

@@ -102,6 +102,8 @@ public:
         k_param_log_bitmask,
         k_param_BoardConfig,
         k_param_rssi_range,
+        k_param_flapin_channel,
+        k_param_flaperon_output,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -419,6 +421,8 @@ public:
     AP_Float takeoff_throttle_min_accel;
     AP_Int8 takeoff_throttle_delay;
     AP_Int8 level_roll_limit;
+    AP_Int8 flapin_channel;
+    AP_Int8 flaperon_output;
 
     // RC channels
     RC_Channel rc_1;

ArduPlane/Parameters.pde

@@ -712,6 +712,19 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(compass_enabled,        "MAG_ENABLE",     1),
 
+    // @Param: FLAP_IN_CHANNEL
+    // @DisplayName: Flap input channel
+    // @Description: An RC input channel to use for flaps control. If this is set to a RC channel number then that channel will be used for manual flaps control. When enabled, the percentage of flaps is taken as the percentage travel from the TRIM value of the channel to the MIN value of the channel. A value above the TRIM values will give inverse flaps (spoilers). This option needs to be enabled in conjunction with a FUNCTION setting on an output channel to one of the flap functions. When a FLAP_IN_CHANNEL is combined with auto-flaps the higher of the two flap percentages is taken. You must also enable a FLAPERON_OUTPUT flaperon mixer setting.
+    // @User: User
+    GSCALAR(flapin_channel,         "FLAP_IN_CHANNEL",  0),
+
+    // @Param: FLAPERON_OUTPUT
+    // @DisplayName: Flaperon output
+    // @Description: Enable flaperon output in software. If enabled then the APM will provide software flaperon mixing on the FLAPERON1 and FLAPERON2 output channels specified using the FUNCTION on two auxillary channels. There are 4 different mixing modes available, which refer to the 4 ways the flap and aileron outputs can be mapped to the two flaperon servos. Note that you must not use flaperon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable FLAPERON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. FLAPERON_OUTPUT cannot be combined with ELEVON_OUTPUT or ELEVON_MIXING.
+    // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown
+    // @User: User
+    GSCALAR(flaperon_output,        "FLAPERON_OUTPUT",  0),
+
     // @Param: FLAP_1_PERCNT
     // @DisplayName: Flap 1 percentage
     // @Description: The percentage change in flap position when FLAP_1_SPEED is reached. Use zero to disable flaps