AP_IOMCU: support elevon and vtail mixing in failsafe

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -788,7 +788,7 @@ void AP_IOMCU::shutdown(void)
   setup for mixing. This allows fixed wing aircraft to fly in manual
   mode if the FMU dies
  */
-bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
+bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan, float mixing_gain)
 {
     if (config.protocol_version != IOMCU_PROTOCOL_VERSION) {
         return false;
@@ -835,6 +835,7 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
     }
 
     MIX_UPDATE(mixing.rc_chan_override, override_chan);
+    MIX_UPDATE(mixing.mixing_gain, (uint16_t)(mixing_gain*1000));
 
     // and enable
     MIX_UPDATE(mixing.enabled, 1);

libraries/AP_IOMCU/AP_IOMCU.h

@@ -91,7 +91,7 @@ public:
     void shutdown();
 
     // setup for FMU failsafe mixing
-    bool setup_mixing(RCMapper *rcmap, int8_t override_chan);
+    bool setup_mixing(RCMapper *rcmap, int8_t override_chan, float mixing_gain);
     
 private:
     AP_HAL::UARTDriver &uart;

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -17,8 +17,6 @@
  */
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-
 #include <AP_Math/AP_Math.h>
 #include <AP_Math/crc.h>
 #include "iofirmware.h"
@@ -628,6 +626,6 @@ void AP_IOMCU_FW::fill_failsafe_pwm(void)
 }
 
 AP_HAL_MAIN();
-#endif // HAL_BOARD_CHIBIOS
+
 
 

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -34,6 +34,7 @@ private:
     int16_t mix_input_range(uint8_t channel, uint16_t radio_in) const;
     uint16_t mix_output_angle(uint8_t channel, int16_t angle) const;
     uint16_t mix_output_range(uint8_t channel, int16_t value) const;
+    int16_t mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const;
 
     struct PACKED {
         /* default to RSSI ADC functionality */

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -157,12 +157,15 @@ struct PACKED page_mixing {
     uint8_t rc_reversed[IOMCU_MAX_CHANNELS];
     uint8_t rc_channel[4];
 
+    // gain for elevon and vtail mixing, x1000
+    uint16_t mixing_gain;
+
     // channel which when high forces mixer
     int8_t rc_chan_override;
 
     // is the throttle an angle input?
     uint8_t throttle_is_angle;
-    
+
     // enabled needs to be 1 to enable mixing
     uint8_t enabled;
 

libraries/AP_IOMCU/iofirmware/mixer.cpp

@@ -0,0 +1,212 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  mixer for failsafe operation when FMU is dead
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <SRV_Channel/SRV_Channel.h>
+#include "iofirmware.h"
+
+#define ANGLE_SCALE ((int32_t)4500)
+#define RANGE_SCALE ((int32_t)1000)
+
+/*
+  return a RC input value scaled from -4500 to 4500
+ */
+int16_t AP_IOMCU_FW::mix_input_angle(uint8_t channel, uint16_t radio_in) const
+{
+    const uint16_t &rc_min = mixing.rc_min[channel];
+    const uint16_t &rc_max = mixing.rc_max[channel];
+    const uint16_t &rc_trim = mixing.rc_trim[channel];
+    const uint16_t &reversed = mixing.rc_reversed[channel];
+
+    int16_t ret = 0;
+    if (radio_in > rc_trim && rc_max != rc_trim) {
+        ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_max  - rc_trim);
+    } else if (radio_in < rc_trim && rc_trim != rc_min) {
+        ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_trim - rc_min);
+    }
+    if (reversed) {
+        ret = -ret;
+    }
+    return ret;
+}
+
+/*
+  return a RC input value scaled from 0 to 1000
+ */
+int16_t AP_IOMCU_FW::mix_input_range(uint8_t channel, uint16_t radio_in) const
+{
+    const uint16_t &rc_min = mixing.rc_min[channel];
+    const uint16_t &rc_max = mixing.rc_max[channel];
+    const uint16_t &reversed = mixing.rc_reversed[channel];
+
+    int16_t ret = 0;
+    if (radio_in > rc_max) {
+        ret = RANGE_SCALE;
+    } else if (radio_in < rc_min) {
+        ret = -RANGE_SCALE;
+    } else {
+        ret = (RANGE_SCALE * (int32_t)(radio_in - rc_min)) / (int32_t)(rc_max  - rc_min);
+    }
+    if (reversed) {
+        ret = -ret;
+    }
+    return ret;
+}
+
+/*
+  return an output pwm giving an angle for a servo channel
+ */
+uint16_t AP_IOMCU_FW::mix_output_angle(uint8_t channel, int16_t angle) const
+{
+    const uint16_t &srv_min = mixing.servo_min[channel];
+    const uint16_t &srv_max = mixing.servo_max[channel];
+    const uint16_t &srv_trim = mixing.servo_trim[channel];
+    const uint16_t &reversed = mixing.servo_reversed[channel];
+    if (reversed) {
+        angle = -angle;
+    }
+    angle = constrain_int16(angle, -ANGLE_SCALE, ANGLE_SCALE);
+    if (angle > 0) {
+        return srv_trim + ((int32_t)angle * (int32_t)(srv_max - srv_trim)) / ANGLE_SCALE;
+    }
+    return srv_trim - (-(int32_t)angle * (int32_t)(srv_trim - srv_min)) / ANGLE_SCALE;
+}
+
+/*
+  return an output pwm giving an range for a servo channel
+ */
+uint16_t AP_IOMCU_FW::mix_output_range(uint8_t channel, int16_t value) const
+{
+    const uint16_t &srv_min = mixing.servo_min[channel];
+    const uint16_t &srv_max = mixing.servo_max[channel];
+    const uint16_t &reversed = mixing.servo_reversed[channel];
+    value = constrain_int16(value, 0, RANGE_SCALE);
+    if (reversed) {
+        value = RANGE_SCALE - value;
+    }
+    return srv_min + ((int32_t)value * (int32_t)(srv_max - srv_min)) / RANGE_SCALE;
+}
+
+
+/*
+  elevon and vtail mixer
+ */
+int16_t AP_IOMCU_FW::mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const
+{
+    if (first_output) {
+        return (angle2 - angle1) * mixing.mixing_gain / 1000;
+    }
+    return (angle1 + angle2) * mixing.mixing_gain / 1000;
+}
+
+/*
+  run mixer. This is used when FMU is not providing inputs, or when
+  the OVERRIDE_CHAN is high. It allows for manual fixed wing flight
+ */
+void AP_IOMCU_FW::run_mixer(void)
+{
+    int16_t rcin[4] {};
+    int16_t &roll = rcin[0];
+    int16_t &pitch = rcin[1];
+    int16_t &throttle = rcin[2];
+    int16_t &rudder = rcin[3];
+
+    // get RC input angles
+    for (uint8_t i=0;i<4; i++) {
+        if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_CHANNELS) {
+            uint8_t chan = mixing.rc_channel[i]-1;
+            if (i == 2 && !mixing.throttle_is_angle) {
+                rcin[i] = mix_input_range(i, rc_input.pwm[chan]);
+            } else {
+                rcin[i] = mix_input_angle(i, rc_input.pwm[chan]);
+            }
+        }
+    }
+
+    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+        SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i];
+        uint16_t &pwm = reg_direct_pwm.pwm[i];
+        switch (function) {
+        case SRV_Channel::k_manual:
+            pwm = rc_input.pwm[i];
+            break;
+
+        case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16:
+            pwm = rc_input.pwm[(uint8_t)(function - SRV_Channel::k_rcin1)];
+            break;
+            
+        case SRV_Channel::k_aileron:
+        case SRV_Channel::k_aileron_with_input:
+        case SRV_Channel::k_flaperon_left:
+        case SRV_Channel::k_flaperon_right:
+            pwm = mix_output_angle(i, roll);
+            break;
+
+        case SRV_Channel::k_elevator:
+        case SRV_Channel::k_elevator_with_input:
+            pwm = mix_output_angle(i, pitch);
+            break;
+
+        case SRV_Channel::k_rudder:
+        case SRV_Channel::k_steering:
+            pwm = mix_output_angle(i, rudder);
+            break;
+
+        case SRV_Channel::k_throttle:
+        case SRV_Channel::k_throttleLeft:
+        case SRV_Channel::k_throttleRight:
+            if (mixing.throttle_is_angle) {
+                pwm = mix_output_angle(i, throttle);
+            } else {
+                pwm = mix_output_range(i, throttle);
+            }
+            break;
+
+        case SRV_Channel::k_flap:
+        case SRV_Channel::k_flap_auto:
+            // zero flaps
+            pwm = mix_output_range(i, 0);
+            break;
+
+        case SRV_Channel::k_elevon_left:
+        case SRV_Channel::k_dspoilerLeft1:
+        case SRV_Channel::k_dspoilerLeft2:
+            // treat differential spoilers as elevons
+            pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, true));
+            break;
+
+        case SRV_Channel::k_elevon_right:
+        case SRV_Channel::k_dspoilerRight1:
+        case SRV_Channel::k_dspoilerRight2:
+            // treat differential spoilers as elevons
+            pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, false));
+            break;
+
+        case SRV_Channel::k_vtail_left:
+            pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, false));
+            break;
+
+        case SRV_Channel::k_vtail_right:
+            pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, true));
+            break;
+            
+        default:
+            break;
+        }
+    }
+}