IOMCU: io firmware enhancements

ArduPlane/ArduPlane.cpp

@@ -244,6 +244,11 @@ void Plane::afs_fs_check(void)
     afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
 }
 
+#if HAL_WITH_IO_MCU
+#include <AP_IOMCU/AP_IOMCU.h>
+extern AP_IOMCU iomcu;
+#endif
+
 void Plane::one_second_loop()
 {
     // send a heartbeat
@@ -259,6 +264,10 @@ void Plane::one_second_loop()
     }
 #endif // CONFIG_HAL_BOARD
 
+#if HAL_WITH_IO_MCU
+    iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
+#endif
+
     // make it possible to change orientation at runtime
     ahrs.set_orientation();
 

ArduPlane/Parameters.cpp

@@ -807,7 +807,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: Advanced
     GSCALAR(flap_2_speed,           "FLAP_2_SPEED",   FLAP_2_SPEED),
 
-#if HAVE_PX4_MIXER
+#if HAVE_PX4_MIXER || HAL_WITH_IO_MCU
     // @Param: OVERRIDE_CHAN
     // @DisplayName: PX4IO override channel
     // @Description: If set to a non-zero value then this is an RC input channel number to use for giving PX4IO manual control in case the main FMU microcontroller on a PX4 or Pixhawk fails. When this RC input channel goes above 1750 the FMU microcontroller will no longer be involved in controlling the servos and instead the PX4IO microcontroller will directly control the servos. Note that PX4IO manual control will be automatically activated if the FMU crashes for any reason. This parameter allows you to test for correct manual behaviour without actually crashing the FMU. This parameter is can be set to a non-zero value either for ground testing purposes or for giving the effect of an external override control board. Please also see the docs on OVERRIDE_SAFETY. Note that you may set OVERRIDE_CHAN to the same channel as FLTMODE_CH to get PX4IO based override when in flight mode 6. Note that when override is triggered due to a FMU crash the 6 auxiliary output channels on Pixhawk will no longer be updated, so all the flight controls you need must be assigned to the first 8 channels.

ArduPlane/Parameters.h

@@ -487,7 +487,7 @@ class Parameters {
     AP_Int8 fbwa_tdrag_chan;
     AP_Int8 rangefinder_landing;
     AP_Int8 flap_slewrate;
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if HAVE_PX4_MIXER || HAL_WITH_IO_MCU
     AP_Int8 override_channel;
     AP_Int8 override_safety;
 #endif

Tools/ardupilotwaf/px4.py

@@ -211,13 +211,18 @@ def _px4_taskgen(bld, **kw):
 @feature('_px4_romfs')
 def _process_romfs(self):
     bld = self.bld
+
+    board_name = bld.env.get_flat('PX4_BOARD_NAME')
+
     file_list = [
         'init.d/rc.APM',
         'init.d/rc.error',
         (bld.env.PX4_RC_S_SCRIPT, 'init.d/rcS'),
-        'tones/startup',
-        (bld.env.PX4_BOOTLOADER, 'bootloader/fmu_bl.bin'),
+        'tones/startup'
     ]
+    if board_name != "px4fmu-v2":
+        # we omit the bootloader on px4-v2 to save flash space
+        file_list.append((bld.env.PX4_BOOTLOADER, 'bootloader/fmu_bl.bin'))
 
     if bld.env.PX4_BOARD_RC:
         board_rc = 'init.d/rc.%s' % bld.env.get_flat('PX4_BOARD_NAME')

libraries/AP_Arming/AP_Arming.cpp

@@ -79,6 +79,11 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
     AP_GROUPEND
 };
 
+#if HAL_WITH_IO_MCU
+#include <AP_IOMCU/AP_IOMCU.h>
+extern AP_IOMCU iomcu;
+#endif
+
 AP_Arming::AP_Arming()
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -508,6 +513,13 @@ bool AP_Arming::servo_checks(bool report) const
         }
     }
 
+#if HAL_WITH_IO_MCU
+    if (!iomcu.healthy()) {
+        check_failed(ARMING_CHECK_NONE, report, "IOMCU is unhealthy");
+        check_passed = false;
+    }
+#endif
+
     return check_passed;
 }
 

libraries/AP_HAL/utility/RingBuffer.h

@@ -98,7 +98,11 @@ template <class T>
 class ObjectBuffer {
 public:
     ObjectBuffer(uint32_t _size) {
-        buffer = new ByteBuffer((_size * sizeof(T))+1);
+        // we set size to 1 more than requested as the byte buffer
+        // gives one less byte than requested. We round up to a full
+        // multiple of the object size so that we always get aligned
+        // elements, which makes the readptr() method possible
+        buffer = new ByteBuffer(((_size+1) * sizeof(T)));
     }
     ~ObjectBuffer(void) {
         delete buffer;
@@ -188,6 +192,25 @@ class ObjectBuffer {
         return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T);
     }
 
+    /*
+      return a pointer to first contiguous array of available
+      objects. Return nullptr if none available
+     */
+    const T *readptr(uint32_t &n) {
+        uint32_t avail_bytes = 0;
+        const T *ret = (const T *)buffer->readptr(avail_bytes);
+        if (!ret || avail_bytes < sizeof(T)) {
+            return nullptr;
+        }
+        n = avail_bytes / sizeof(T);
+        return ret;
+    }
+
+    // advance the read pointer (discarding objects)
+    bool advance(uint32_t n) {
+        return buffer->advance(n * sizeof(T));
+    }
+
     /* update the object at the front of the queue (the one that would
        be fetched by pop()) */
     bool update(const T &object) {

libraries/AP_HAL_ChibiOS/RCInput.cpp

@@ -27,6 +27,10 @@ extern AP_IOMCU iomcu;
 
 #include <AP_Math/AP_Math.h>
 
+#ifndef HAL_NO_UARTDRIVER
+#include <GCS_MAVLink/GCS.h>
+#endif
+
 #define SIG_DETECT_TIMEOUT_US 500000
 using namespace ChibiOS;
 extern const AP_HAL::HAL& hal;
@@ -116,13 +120,24 @@ void RCInput::_timer_tick(void)
     if (!_init) {
         return;
     }
-#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE
-    uint32_t width_s0, width_s1;
 
+#if HAL_USE_ICU == TRUE
+    const uint32_t *p;
+    uint32_t n;
+    while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) {
+        rcin_prot.process_pulse_list(p, n*2, sig_reader.need_swap);
+        sig_reader.sigbuf.advance(n);
+    }
+#endif
+
+#if HAL_USE_EICU == TRUE
+    uint32_t width_s0, width_s1;
     while(sig_reader.read(width_s0, width_s1)) {
         rcin_prot.process_pulse(width_s0, width_s1);
     }
-
+#endif
+
+#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE
     if (rcin_prot.new_input()) {
         rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
         _rcin_timestamp_last_signal = AP_HAL::micros();
@@ -132,6 +147,12 @@ void RCInput::_timer_tick(void)
             _rc_values[i] = rcin_prot.read(i);
         }
         rcin_mutex.give();
+#ifndef HAL_NO_UARTDRIVER
+        if (rcin_prot.protocol_name() != last_protocol) {
+            last_protocol = rcin_prot.protocol_name();
+            gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol);
+        }
+#endif
     }
 #endif
 
@@ -168,6 +189,14 @@ void RCInput::_timer_tick(void)
  */
 bool RCInput::rc_bind(int dsmMode)
 {
+#if HAL_WITH_IO_MCU
+    rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.bind_dsm(dsmMode);
+    }
+    rcin_mutex.give();
+#endif
+
 #if HAL_USE_ICU == TRUE
     // ask AP_RCProtocol to start a bind
     rcin_prot.start_bind();

libraries/AP_HAL_ChibiOS/RCInput.h

@@ -62,6 +62,8 @@ class ChibiOS::RCInput : public AP_HAL::RCInput {
     int16_t _rssi = -1;
     uint32_t _rcin_timestamp_last_signal;
     bool _init;
+    const char *last_protocol;
+
 #if HAL_RCINPUT_WITH_AP_RADIO
     bool _radio_init;
     AP_Radio *radio;

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -140,7 +140,12 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
       set PWM to send to a set of channels if the FMU firmware dies
      */
     void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
-
+
+    /*
+      set safety mask for IOMCU
+     */
+    void set_safety_mask(uint16_t mask) { safety_mask = mask; }
+
 private:
     struct pwm_group {
         // only advanced timers can do high clocks needed for more than 400Hz

libraries/AP_HAL_ChibiOS/SoftSigReader.cpp

@@ -30,7 +30,7 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
     if (chan > ICU_CHANNEL_2) {
         return false;
     }
-    signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*_bounce_buf_size, AP_HAL::Util::MEM_DMA_SAFE);
+    signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*SOFTSIG_BOUNCE_BUF_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
     if (signal == nullptr) {
         return false;
     }
@@ -53,7 +53,7 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
     dmamode |= STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD |
         STM32_DMA_CR_MSIZE_WORD | STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE;
     dmaStreamSetMemory0(dma, signal);
-    dmaStreamSetTransactionSize(dma, _bounce_buf_size);
+    dmaStreamSetTransactionSize(dma, SOFTSIG_BOUNCE_BUF_SIZE);
     dmaStreamSetMode(dma, dmamode);
 
     icucfg.frequency = INPUT_CAPTURE_FREQUENCY;
@@ -91,54 +91,21 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
 void SoftSigReader::_irq_handler(void* self, uint32_t flags)
 {
     SoftSigReader* sig_reader = (SoftSigReader*)self;
-    sig_reader->sigbuf.push(sig_reader->signal, sig_reader->_bounce_buf_size);
+    // we need to restart the DMA as quickly as possible to prevent losing pulses, so we
+    // make a fixed length copy to a 2nd buffer. On the F100 this reduces the time with DMA
+    // disabled from 20us to under 1us
+    memcpy(sig_reader->signal2, sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
     //restart the DMA transfers
     dmaStreamDisable(sig_reader->dma);
     dmaStreamSetPeripheral(sig_reader->dma, &sig_reader->_icu_drv->tim->DMAR);
     dmaStreamSetMemory0(sig_reader->dma, sig_reader->signal);
-    dmaStreamSetTransactionSize(sig_reader->dma, sig_reader->_bounce_buf_size);
+    dmaStreamSetTransactionSize(sig_reader->dma, SOFTSIG_BOUNCE_BUF_SIZE);
     dmaStreamSetMode(sig_reader->dma, sig_reader->dmamode);
     dmaStreamEnable(sig_reader->dma);
+    sig_reader->sigbuf.push((const pulse_t *)sig_reader->signal2, SOFTSIG_BOUNCE_BUF_SIZE/2);
 }
 
 
-bool SoftSigReader::read(uint32_t &widths0, uint32_t &widths1)
-{
-    if (sigbuf.pop(widths0) && sigbuf.pop(widths1)) {
-        //the data is period and width, order depending on which
-        //channel is used and width type (0 or 1) depends on mode
-        //being set to HIGH or LOW. We need to swap the words when on
-        //channel 1
-        if (need_swap) {
-            uint32_t tmp = widths1;
-            widths1 = widths0;
-            widths0 = tmp;
-        }
-        widths1 -= widths0;
-    } else {
-        return false;
-    }
-    return true;
-}
-
-
-bool SoftSigReader::set_bounce_buf_size(uint16_t buf_size)
-{
-    if (buf_size > _bounce_buf_size) {
-        if (signal) {
-            hal.util->free_type(signal, sizeof(uint32_t)*_bounce_buf_size, AP_HAL::Util::MEM_DMA_SAFE);
-        }
-        signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*buf_size, AP_HAL::Util::MEM_DMA_SAFE);
-        if (signal == nullptr) {
-            return false;
-        }
-        _bounce_buf_size = buf_size;
-    } else {
-        _bounce_buf_size = buf_size;
-    }
-    return true;
-}
-
 #endif // HAL_USE_ICU
 
 #endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

libraries/AP_HAL_ChibiOS/SoftSigReader.h

@@ -23,29 +23,36 @@
 #if HAL_USE_ICU == TRUE
 
 #define INPUT_CAPTURE_FREQUENCY 1000000 //capture unit in microseconds
+
 #ifndef SOFTSIG_MAX_SIGNAL_TRANSITIONS
-#define SOFTSIG_MAX_SIGNAL_TRANSITIONS 256
+#define SOFTSIG_MAX_SIGNAL_TRANSITIONS 128
 #endif
-#define DEFAULT_BOUNCE_BUF_SIZE 32
+
+// we use a small bounce buffer size to minimise time in the DMA
+// callback IRQ
+#define SOFTSIG_BOUNCE_BUF_SIZE 8
 
 class ChibiOS::SoftSigReader {
+    friend class ChibiOS::RCInput;
 public:
     bool attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan, uint8_t dma_stream, uint32_t dma_channel);
-    bool read(uint32_t &widths0, uint32_t &widths1);
-    //This sets the size of bounce buffer size, which in turn controls the rate of interrupt from DMA
-    bool set_bounce_buf_size(uint16_t buf_size);
+
 private:
     uint32_t *signal;
+    uint32_t signal2[SOFTSIG_BOUNCE_BUF_SIZE];
     static void _irq_handler(void* self, uint32_t flags);
     uint8_t num_timer_channels;
-    ObjectBuffer<uint32_t> sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS};
     uint8_t enable_chan_mask;
     uint8_t max_pulse_width;
     const stm32_dma_stream_t* dma;
     uint32_t dmamode;
     ICUConfig icucfg;
     ICUDriver* _icu_drv = nullptr;
-    uint16_t _bounce_buf_size = DEFAULT_BOUNCE_BUF_SIZE;
+    typedef struct PACKED {
+        uint32_t w0;
+        uint32_t w1;
+    } pulse_t;
+    ObjectBuffer<pulse_t> sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS};
     bool need_swap;
 };
 

libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp

@@ -93,33 +93,33 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
 void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel)
 {
     eicuchannel_t channel = get_pair_channel(aux_channel);
-    uint16_t value1 = eicup->tim->CCR[channel];
-    uint16_t value2 = eicup->tim->CCR[aux_channel];
-
-    _instance->sigbuf.push(value1);
-    _instance->sigbuf.push(value2);
+    pulse_t pulse;
+    pulse.w0 = eicup->tim->CCR[channel];
+    pulse.w1 = eicup->tim->CCR[aux_channel];
 
+    _instance->sigbuf.push(pulse);
+
     //check for missed interrupt 
     uint32_t mask = (STM32_TIM_SR_CC1OF << channel) | (STM32_TIM_SR_CC1OF << aux_channel);
     if ((eicup->tim->SR & mask) != 0) {
         //we have missed some pulses
         //try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)
-        _instance->sigbuf.push(value2);
-        //second 0 width pulse to keep polarity right
-        _instance->sigbuf.push(value2);
+        pulse.w0 = 0;
+        pulse.w1 = 0;
+        _instance->sigbuf.push(pulse);
         //reset overcapture mask
         eicup->tim->SR &= ~mask;
     }
 }
 
 bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1)
 {
-    uint16_t p0, p1;
     if (sigbuf.available() >= 2) {
-        if (sigbuf.pop(p0) && sigbuf.pop(p1)) {
-            widths0 = uint16_t(p0 - last_value);
-            widths1 = uint16_t(p1 - p0);
-            last_value = p1;
+        pulse_t pulse;
+        if (sigbuf.pop(pulse)) {
+            widths0 = uint16_t(pulse.w0 - last_value);
+            widths1 = uint16_t(pulse.w1 - pulse.w0);
+            last_value = pulse.w1;
             return true;
         }
     }