AP_IOMCU: added initial mixing support

supports aileron, elevator, throttle and rudder

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -23,13 +23,10 @@ extern const AP_HAL::HAL &hal;
 enum ioevents {
     IOEVENT_INIT=1,
     IOEVENT_SEND_PWM_OUT,
-    IOEVENT_SET_DISARMED_PWM,
-    IOEVENT_SET_FAILSAFE_PWM,
     IOEVENT_FORCE_SAFETY_OFF,
     IOEVENT_FORCE_SAFETY_ON,
     IOEVENT_SET_ONESHOT_ON,
     IOEVENT_SET_RATES,
-    IOEVENT_GET_RCIN,
     IOEVENT_ENABLE_SBUS,
     IOEVENT_SET_HEATER_TARGET,
     IOEVENT_SET_DEFAULT_RATE,
@@ -226,7 +223,7 @@ void AP_IOMCU::thread_main(void)
         // update safety pwm
         if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
             uint8_t set = pwm_out.safety_pwm_set;
-            if (write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
+            if (write_registers(PAGE_SAFETY_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
                 pwm_out.safety_pwm_sent = set;
             }
         }
@@ -809,6 +806,7 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
         MIX_UPDATE(mixing.servo_min[i], ch->get_output_min());
         MIX_UPDATE(mixing.servo_max[i], ch->get_output_max());
         MIX_UPDATE(mixing.servo_function[i], ch->get_function());
+        MIX_UPDATE(mixing.servo_reversed[i], ch->get_reversed());
     }
     // update RCMap
     MIX_UPDATE(mixing.rc_channel[0], rcmap->roll());
@@ -826,8 +824,18 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan)
         MIX_UPDATE(mixing.rc_min[i], ch->get_radio_min());
         MIX_UPDATE(mixing.rc_max[i], ch->get_radio_max());
         MIX_UPDATE(mixing.rc_trim[i], ch->get_radio_trim());
+        MIX_UPDATE(mixing.rc_reversed[i], ch->get_reverse());
+
+        // cope with reversible throttle
+        if (i == 2 && ch->get_type() == RC_Channel::RC_CHANNEL_TYPE_ANGLE) {
+            MIX_UPDATE(mixing.throttle_is_angle, 1);
+        } else {
+            MIX_UPDATE(mixing.throttle_is_angle, 0);
+        }
     }
+
     MIX_UPDATE(mixing.rc_chan_override, override_chan);
+
     // and enable
     MIX_UPDATE(mixing.enabled, 1);
     if (changed) {

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -195,6 +195,15 @@ void AP_IOMCU_FW::update()
         sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
     }
 
+    // handle FMU failsafe
+    if (now - fmu_data_received_time > 10) {
+        // we are not getting input from the FMU. Fill in failsafe values
+        fill_failsafe_pwm();
+        chEvtSignal(thread_ctx, EVENT_MASK(IOEVENT_PWM));
+        // mark as done
+        fmu_data_received_time = now;
+    }
+    
     // run remaining functions at 1kHz
     if (now != last_loop_ms) {
         last_loop_ms = now;
@@ -208,7 +217,6 @@ void AP_IOMCU_FW::update()
 
 void AP_IOMCU_FW::pwm_out_update()
 {
-    //TODO: PWM mixing
     memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
     hal.rcout->cork();
     for (uint8_t i = 0; i < SERVO_COUNT; i++) {
@@ -258,6 +266,14 @@ void AP_IOMCU_FW::rcin_update()
         hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
     }
 
+    // check for active override channel
+    if (mixing.enabled &&
+        mixing.rc_chan_override > 0 &&
+        mixing.rc_chan_override <= IOMCU_MAX_CHANNELS) {
+        override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1800);
+    } else {
+        override_active = false;
+    }
 }
 
 void AP_IOMCU_FW::process_io_packet()
@@ -457,7 +473,12 @@ bool AP_IOMCU_FW::handle_code_write()
             break;
         }
         break;
+
     case PAGE_DIRECT_PWM: {
+        if (override_active) {
+            // no input when override is active
+            break;
+        }
         /* copy channel data */
         uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
         while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
@@ -476,12 +497,25 @@ bool AP_IOMCU_FW::handle_code_write()
         chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
         break;
     }
+
     case PAGE_MIXING: {
         uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
         memcpy(((uint16_t *)&mixing)+offset, &rx_io_packet.regs[0], num_values*2);
         break;
     }
 
+    case PAGE_SAFETY_PWM: {
+        uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
+        memcpy((&reg_safety_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
+        break;
+    }
+
+    case PAGE_FAILSAFE_PWM: {
+        uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
+        memcpy((&reg_failsafe_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2);
+        break;
+    }
+
     default:
         break;
     }
@@ -576,6 +610,23 @@ void AP_IOMCU_FW::rcout_mode_update(void)
     }
 }
 
+/*
+  fill in failsafe PWM values
+ */
+void AP_IOMCU_FW::fill_failsafe_pwm(void)
+{
+    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+        if (reg_status.flag_safety_off) {
+            reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i];
+        } else {
+            reg_direct_pwm.pwm[i] = reg_safety_pwm.pwm[i];
+        }
+    }
+    if (mixing.enabled) {
+        run_mixer();
+    }
+}
+
 AP_HAL_MAIN();
 #endif // HAL_BOARD_CHIBIOS
 

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -28,6 +28,12 @@ private:
     void safety_update();
     void rcout_mode_update();
     void page_status_update(void);
+    void fill_failsafe_pwm(void);
+    void run_mixer(void);
+    int16_t mix_input_angle(uint8_t channel, uint16_t radio_in) const;
+    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;
 
     struct PACKED {
         /* default to RSSI ADC functionality */
@@ -69,11 +75,21 @@ private:
         uint16_t pwm[IOMCU_MAX_CHANNELS];
     } reg_servo;
 
-    // PAGE_SERVO values
+    // PAGE_DIRECT_PWM values
     struct {
         uint16_t pwm[IOMCU_MAX_CHANNELS];
     } reg_direct_pwm;
 
+    // PAGE_FAILSAFE_PWM
+    struct {
+        uint16_t pwm[IOMCU_MAX_CHANNELS];
+    } reg_failsafe_pwm;
+
+    // PAGE_SAFETY_PWM
+    struct {
+        uint16_t pwm[IOMCU_MAX_CHANNELS];
+    } reg_safety_pwm;
+    
     // output rates
     struct {
         uint16_t freq;
@@ -84,6 +100,9 @@ private:
 
     // MIXER values
     struct page_mixing mixing;
+
+    // true when override channel active
+    bool override_active;
     
     // sbus rate handling
     uint32_t sbus_last_ms;

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -49,7 +49,7 @@ enum iopage {
     PAGE_SETUP = 50,
     PAGE_DIRECT_PWM = 54,
     PAGE_FAILSAFE_PWM = 55,
-    PAGE_DISARMED_PWM = 108,
+    PAGE_SAFETY_PWM = 108,
     PAGE_MIXING = 200,
 };
 
@@ -148,16 +148,23 @@ struct PACKED page_mixing {
     uint16_t servo_max[IOMCU_MAX_CHANNELS];
     uint16_t servo_trim[IOMCU_MAX_CHANNELS];
     uint8_t servo_function[IOMCU_MAX_CHANNELS];
+    uint8_t servo_reversed[IOMCU_MAX_CHANNELS];
 
     // RC input arrays are in AETR order
     uint16_t rc_min[4];
     uint16_t rc_max[4];
     uint16_t rc_trim[4];
+    uint8_t rc_reversed[IOMCU_MAX_CHANNELS];
     uint8_t rc_channel[4];
 
     // 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;
+
+    uint8_t pad; // pad to even size
 };