VRBRAIN / AP_HAL_VRBRAIN: updated AP_HAL

libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h

@@ -15,4 +15,6 @@ namespace VRBRAIN {
     class VRBRAINI2CDriver;
     class VRBRAIN_I2C;
     class Semaphore;
+    class VRBRAINCAN;
+    class VRBRAINCANManager;
 }

libraries/AP_HAL_VRBRAIN/AnalogIn.cpp

@@ -52,6 +52,7 @@ static const struct {
 #else
 #error "Unknown board type for AnalogIn scaling"
 #endif
+    { 0, 0.f          },
 };
 
 using namespace VRBRAIN;
@@ -237,6 +238,11 @@ void VRBRAINAnalogIn::next_stop_pin(void)
  */
 void VRBRAINAnalogIn::_timer_tick(void)
 {
+    if (_adc_fd == -1) {
+        // not initialised yet
+        return;
+    }
+
     // read adc at 100Hz
     uint32_t now = AP_HAL::micros();
     uint32_t delta_t = now - _last_run;

libraries/AP_HAL_VRBRAIN/AnalogIn.h

@@ -53,15 +53,15 @@ private:
 class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn {
 public:
     VRBRAINAnalogIn();
-    void init();
-    AP_HAL::AnalogSource* channel(int16_t pin);
+    void init() override;
+    AP_HAL::AnalogSource* channel(int16_t pin) override;
     void _timer_tick(void);
-    float board_voltage(void) { return _board_voltage; }
-    float servorail_voltage(void) { return _servorail_voltage; }
-    uint16_t power_status_flags(void) { return _power_flags; }
+    float board_voltage(void) override { return _board_voltage; }
+    float servorail_voltage(void) override { return _servorail_voltage; }
+    uint16_t power_status_flags(void) override { return _power_flags; }
 
 private:
-    int _adc_fd;
+    int _adc_fd = -1;
     int _battery_handle;
     int _servorail_handle;
     int _system_power_handle;

libraries/AP_HAL_VRBRAIN/CAN.h

@@ -0,0 +1,317 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *
+ * With modifications for Ardupilot CAN driver
+ * Copyright (C) 2017 Eugene Shamaev
+ */
+
+#pragma once
+
+#include "AP_HAL_VRBRAIN.h"
+#include <systemlib/perf_counter.h>
+#include <AP_HAL/CAN.h>
+#include <pthread.h>
+#include <semaphore.h>
+
+#include "bxcan.h"
+#include "AP_HAL/utility/RingBuffer.h"
+                                      
+#if defined(GPIO_CAN2_RX) && defined(GPIO_CAN2_TX)
+#define CAN_STM32_NUM_IFACES 2
+#else
+#define CAN_STM32_NUM_IFACES 1
+#endif
+
+#define CAN_STM32_RX_QUEUE_SIZE 64
+
+namespace VRBRAIN {
+/**
+ * Driver error codes.
+ * These values can be returned from driver functions negated.
+ */
+static const int16_t ErrUnknown = 1000; ///< Reserved for future use
+static const int16_t ErrNotImplemented = 1001; ///< Feature not implemented
+static const int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported
+static const int16_t ErrLogic = 1003; ///< Internal logic error
+static const int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
+static const int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
+static const int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
+static const int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
+static const int16_t ErrFilterNumConfigs = 1008; ///< Auto bit rate detection could not be finished
+
+/**
+ * RX queue item.
+ * The application shall not use this directly.
+ */
+struct CanRxItem {
+    uint64_t utc_usec;
+    uavcan::CanFrame frame;
+    uavcan::CanIOFlags flags;
+    CanRxItem() :
+        utc_usec(0), flags(0)
+    {
+    }
+};
+
+struct CriticalSectionLocker {
+    const irqstate_t flags_;
+
+    CriticalSectionLocker() :
+        flags_(irqsave())
+    {
+    }
+
+    ~CriticalSectionLocker()
+    {
+        irqrestore(flags_);
+    }
+};
+
+namespace clock {
+uint64_t getUtcUSecFromCanInterrupt();
+uavcan::MonotonicTime getMonotonic();
+}
+
+class BusEvent: uavcan::Noncopyable {
+public:
+    BusEvent(VRBRAINCANManager& can_driver);
+    ~BusEvent();
+
+    bool wait(uavcan::MonotonicDuration duration);
+    static void signalFromCallOut(BusEvent *sem);
+
+    void signalFromInterrupt();
+    sem_t _wait_semaphore;
+    volatile uint16_t _signal;
+};
+
+class VRBRAINCAN: public AP_HAL::CAN {
+    struct Timings {
+        uint16_t prescaler;
+        uint8_t sjw;
+        uint8_t bs1;
+        uint8_t bs2;
+
+        Timings() :
+            prescaler(0), sjw(0), bs1(0), bs2(0)
+        {
+        }
+    };
+
+    struct TxItem {
+        uavcan::MonotonicTime deadline;
+        uavcan::CanFrame frame;
+
+        bool pending;
+
+        bool loopback;
+
+        bool abort_on_error;
+
+        TxItem() :
+            pending(false), loopback(false), abort_on_error(false)
+        {
+        }
+    };
+
+    enum {
+        NumTxMailboxes = 3
+    };
+    enum {
+        NumFilters = 14
+    };
+
+    static const uint32_t TSR_ABRQx[NumTxMailboxes];
+
+    ObjectBuffer<CanRxItem> rx_queue_;
+    bxcan::CanType* const can_;
+    uint64_t error_cnt_;
+    uint32_t served_aborts_cnt_;
+    BusEvent* update_event_;
+    TxItem pending_tx_[NumTxMailboxes];
+    uint8_t peak_tx_mailbox_index_;
+    const uint8_t self_index_;
+
+    bool had_activity_;
+
+    uint32_t bitrate_;
+
+    int computeTimings(uint32_t target_bitrate, Timings& out_timings);
+
+    virtual int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+                         uavcan::CanIOFlags flags) override;
+
+    virtual int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+                            uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override;
+
+    virtual int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override;
+
+    virtual uint16_t getNumFilters() const override
+    {
+        return NumFilters;
+    }
+
+    void handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, uint64_t utc_usec);
+
+    bool waitMsrINakBitStateChange(bool target_state);
+
+    bool initialized_;
+public:
+    enum {
+        MaxRxQueueCapacity = 254
+    };
+
+    enum OperatingMode {
+        NormalMode, SilentMode
+    };
+
+    VRBRAINCAN(bxcan::CanType* can, BusEvent* update_event, uint8_t self_index, uint8_t rx_queue_capacity) :
+        rx_queue_(rx_queue_capacity), can_(can), error_cnt_(0), served_aborts_cnt_(0), update_event_(
+            update_event), peak_tx_mailbox_index_(0), self_index_(self_index), had_activity_(false), bitrate_(
+                0), initialized_(false)
+    {
+        UAVCAN_ASSERT(self_index_ < CAN_STM32_NUM_IFACES);
+    }
+
+    /**
+     * Initializes the hardware CAN controller.
+     * Assumes:
+     *   - Iface clock is enabled
+     *   - Iface has been resetted via RCC
+     *   - Caller will configure NVIC by itself
+     */
+    int init(const uint32_t bitrate, const OperatingMode mode);
+
+    void set_update_event(BusEvent* update_event)
+    {
+        update_event_ = update_event;
+    }
+
+    void handleTxInterrupt(uint64_t utc_usec);
+    void handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec);
+
+    /**
+     * This method is used to count errors and abort transmission on error if necessary.
+     * This functionality used to be implemented in the SCE interrupt handler, but that approach was
+     * generating too much processing overhead, especially on disconnected interfaces.
+     *
+     * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
+     */
+    void pollErrorFlagsFromISR();
+
+    void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
+
+    bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const;
+    bool isRxBufferEmpty() const;
+
+    /**
+     * Total number of hardware failures and other kinds of errors (e.g. queue overruns).
+     * May increase continuously if the interface is not connected to the bus.
+     */
+    virtual uint64_t getErrorCount() const override;
+
+    /**
+     * Number of times the driver exercised library's requirement to abort transmission on first error.
+     * This is an atomic read, it doesn't require a critical section.
+     * See @ref uavcan::CanIOFlagAbortOnError.
+     */
+    uint32_t getVoluntaryTxAbortCount() const
+    {
+        return served_aborts_cnt_;
+    }
+
+    /**
+     * Returns the number of frames pending in the RX queue.
+     * This is intended for debug use only.
+     */
+    unsigned getRxQueueLength() const;
+
+    /**
+     * Whether this iface had at least one successful IO since the previous call of this method.
+     * This is designed for use with iface activity LEDs.
+     */
+    bool hadActivity();
+
+    /**
+     * Peak number of TX mailboxes used concurrently since initialization.
+     * Range is [1, 3].
+     * Value of 3 suggests that priority inversion could be taking place.
+     */
+    uint8_t getPeakNumTxMailboxesUsed() const
+    {
+        return uint8_t(peak_tx_mailbox_index_ + 1);
+    }
+
+    bool begin(uint32_t bitrate) override;
+    void end() override
+    {
+    }
+
+    void reset() override;
+
+    int32_t tx_pending() override;
+    int32_t available() override;
+
+    bool is_initialized() override;
+};
+
+class VRBRAINCANManager: public AP_HAL::CANManager {
+    BusEvent update_event_;
+    VRBRAINCAN if0_;
+    VRBRAINCAN if1_;
+
+    virtual int16_t select(uavcan::CanSelectMasks& inout_masks,
+                           const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override;
+
+    void initOnce(uint8_t can_number);
+
+    bool initialized_;
+
+    VRBRAINCAN* ifaces[CAN_STM32_NUM_IFACES];
+    uint8_t _ifaces_num;
+    uint8_t _ifaces_out_to_in[CAN_STM32_NUM_IFACES];
+
+    AP_UAVCAN *p_uavcan;
+
+public:
+    VRBRAINCANManager();
+
+    /**
+     * This function returns select masks indicating which interfaces are available for read/write.
+     */
+    uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const;
+
+    /**
+     * Whether there's at least one interface where receive() would return a frame.
+     */
+    bool hasReadableInterfaces() const;
+
+    /**
+     * Returns zero if OK.
+     * Returns negative value if failed (e.g. invalid bitrate).
+     */
+    int init(const uint32_t bitrate, const VRBRAINCAN::OperatingMode mode, uint8_t can_number);
+
+    virtual VRBRAINCAN* getIface(uint8_t iface_index) override;
+    VRBRAINCAN* getIface_out_to_in(uint8_t iface_index);
+
+    virtual uint8_t getNumIfaces() const override
+    {
+        return _ifaces_num;
+    }
+
+    /**
+     * Whether at least one iface had at least one successful IO since previous call of this method.
+     * This is designed for use with iface activity LEDs.
+     */
+    bool hadActivity();
+
+    bool begin(uint32_t bitrate, uint8_t can_number) override;
+
+    bool is_initialized() override;
+    void initialized(bool val) override;
+
+    AP_UAVCAN *get_UAVCAN(void) override;
+    void set_UAVCAN(AP_UAVCAN *uavcan) override;
+};
+}

libraries/AP_HAL_VRBRAIN/Device.cpp

@@ -0,0 +1,155 @@
+/*
+ * This file 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 file 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/>.
+ */
+#include "Device.h"
+
+#include <arch/board/board.h>
+#include "board_config.h"
+#include <stdio.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include "Scheduler.h"
+#include "Semaphores.h"
+
+extern bool _vrbrain_thread_should_exit;
+
+namespace VRBRAIN {
+
+static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+/*
+  per-bus callback thread
+*/
+void *DeviceBus::bus_thread(void *arg)
+{
+    struct DeviceBus *binfo = (struct DeviceBus *)arg;
+
+    // setup a name for the thread
+    char name[] = "XXX:X";
+    switch (binfo->hal_device->bus_type()) {
+    case AP_HAL::Device::BUS_TYPE_I2C:
+        snprintf(name, sizeof(name), "I2C:%u",
+                 binfo->hal_device->bus_num());
+        break;
+
+    case AP_HAL::Device::BUS_TYPE_SPI:
+        snprintf(name, sizeof(name), "SPI:%u",
+                 binfo->hal_device->bus_num());
+        break;
+    default:
+        break;
+    }
+    pthread_setname_np(pthread_self(), name);
+    
+    while (!_vrbrain_thread_should_exit) {
+        uint64_t now = AP_HAL::micros64();
+        DeviceBus::callback_info *callback;
+
+        // find a callback to run
+        for (callback = binfo->callbacks; callback; callback = callback->next) {
+            if (now >= callback->next_usec) {
+                while (now >= callback->next_usec) {
+                    callback->next_usec += callback->period_usec;
+                }
+                // call it with semaphore held
+                if (binfo->semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+                    callback->cb();
+                    binfo->semaphore.give();
+                }
+            }
+        }
+
+        // work out when next loop is needed
+        uint64_t next_needed = 0;
+        now = AP_HAL::micros64();
+
+        for (callback = binfo->callbacks; callback; callback = callback->next) {
+            if (next_needed == 0 ||
+                callback->next_usec < next_needed) {
+                next_needed = callback->next_usec;
+                if (next_needed < now) {
+                    next_needed = now;
+                }
+            }
+        }
+
+        // delay for at most 50ms, to handle newly added callbacks
+        uint32_t delay = 50000;
+        if (next_needed >= now && next_needed - now < delay) {
+            delay = next_needed - now;
+        }
+        // don't delay for less than 400usec, so one thread doesn't
+        // completely dominate the CPU
+        if (delay < 400) {
+            delay = 400;
+        }
+        hal.scheduler->delay_microseconds(delay);
+    }
+    return nullptr;
+}
+    
+AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
+{
+    if (!thread_started) {
+        thread_started = true;
+    
+        pthread_attr_t thread_attr;
+        struct sched_param param;
+    
+        pthread_attr_init(&thread_attr);
+        pthread_attr_setstacksize(&thread_attr, 1024);
+    
+        param.sched_priority = thread_priority;
+        (void)pthread_attr_setschedparam(&thread_attr, &param);
+        pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+        hal_device = _hal_device;
+        
+        pthread_create(&thread_ctx, &thread_attr, &DeviceBus::bus_thread, this);
+    }
+    DeviceBus::callback_info *callback = new DeviceBus::callback_info;
+    if (callback == nullptr) {
+        return nullptr;
+    }
+    callback->cb = cb;
+    callback->period_usec = period_usec;
+    callback->next_usec = AP_HAL::micros64() + period_usec;
+
+    // add to linked list of callbacks on thread
+    callback->next = callbacks;
+    callbacks = callback;
+
+    return callback;
+}
+
+/*
+ * Adjust the timer for the next call: it needs to be called from the bus
+ * thread, otherwise it will race with it
+ */
+bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    if (!pthread_equal(pthread_self(), thread_ctx)) {
+        fprintf(stderr, "can't adjust timer from unknown thread context\n");
+        return false;
+    }
+
+    DeviceBus::callback_info *callback = static_cast<DeviceBus::callback_info *>(h);
+
+    callback->period_usec = period_usec;
+    callback->next_usec = AP_HAL::micros64() + period_usec;
+
+    return true;
+}
+
+}

libraries/AP_HAL_VRBRAIN/Device.h

@@ -0,0 +1,49 @@
+/*
+ * This file 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 file 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/>.
+ */
+#pragma once
+
+#include <inttypes.h>
+#include <AP_HAL/HAL.h>
+#include "Semaphores.h"
+#include "Scheduler.h"
+
+namespace VRBRAIN {
+
+class DeviceBus {
+public:
+    DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) :
+        thread_priority(_thread_priority) {}
+    
+    struct DeviceBus *next;
+    Semaphore semaphore;
+
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device);
+    bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
+    static void *bus_thread(void *arg);
+    
+private:
+    struct callback_info {
+        struct callback_info *next;
+        AP_HAL::Device::PeriodicCb cb;
+        uint32_t period_usec;
+        uint64_t next_usec;        
+    } *callbacks;
+    uint8_t thread_priority;
+    pthread_t thread_ctx;
+    bool thread_started;
+    AP_HAL::Device *hal_device;
+};
+
+}

libraries/AP_HAL_VRBRAIN/GPIO.cpp

@@ -29,6 +29,7 @@ VRBRAINGPIO::VRBRAINGPIO()
 
 void VRBRAINGPIO::init()
 {
+
     _led_fd = open(LED0_DEVICE_PATH, O_RDWR);
     if (_led_fd == -1) {
         AP_HAL::panic("Unable to open " LED0_DEVICE_PATH);
@@ -39,10 +40,13 @@ void VRBRAINGPIO::init()
     if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) {
         hal.console->printf("GPIO: Unable to setup GPIO LED RED\n");
     }
+
     if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) {
         hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n");
     }
 
+
+
     _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
     if (_tone_alarm_fd == -1) {
         AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
@@ -67,6 +71,11 @@ void VRBRAINGPIO::init()
         hal.console->printf("GPIO: Unable to setup GPIO_3\n");
     }
 #endif
+#ifdef GPIO_SERVO_4
+    if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_4) != 0) {
+        hal.console->printf("GPIO: Unable to setup GPIO_4\n");
+    }
+#endif
 }
 
 void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
@@ -92,6 +101,14 @@ uint8_t VRBRAINGPIO::read(uint8_t pin) {
         }
 #endif
 
+#ifdef GPIO_SERVO_4
+        case EXTERNAL_RELAY2_PIN: {
+            uint32_t relays = 0;
+            ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
+            return (relays & GPIO_SERVO_4)?HIGH:LOW;
+        }
+#endif
+
     }
     return LOW;
 }
@@ -142,6 +159,11 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
             break;
 #endif
 
+#ifdef GPIO_SERVO_4
+        case EXTERNAL_RELAY2_PIN:
+            ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_4);
+            break;
+#endif
 
     }
 }

libraries/AP_HAL_VRBRAIN/GPIO.h

@@ -11,7 +11,8 @@
  # define EXTERNAL_LED_MOTOR1       30
  # define EXTERNAL_LED_MOTOR2       31
 
- # define EXTERNAL_RELAY1_PIN       33
+ # define EXTERNAL_RELAY1_PIN       34
+ # define EXTERNAL_RELAY2_PIN       33
 
  # define HAL_GPIO_LED_ON           HIGH
  # define HAL_GPIO_LED_OFF          LOW
@@ -20,21 +21,21 @@
 class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO {
 public:
 	VRBRAINGPIO();
-    void    init();
-    void    pinMode(uint8_t pin, uint8_t output);
-    int8_t  analogPinToDigitalPin(uint8_t pin);
-    uint8_t read(uint8_t pin);
-    void    write(uint8_t pin, uint8_t value);
-    void    toggle(uint8_t pin);
+    void    init() override;
+    void    pinMode(uint8_t pin, uint8_t output) override;
+    int8_t  analogPinToDigitalPin(uint8_t pin) override;
+    uint8_t read(uint8_t pin) override;
+    void    write(uint8_t pin, uint8_t value) override;
+    void    toggle(uint8_t pin) override;
 
     /* Alternative interface: */
-    AP_HAL::DigitalSource* channel(uint16_t n);
+    AP_HAL::DigitalSource* channel(uint16_t n) override;
 
     /* Interrupt interface: */
-    bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
+    bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override;
 
     /* return true if USB cable is connected */
-    bool usb_connected(void);
+    bool usb_connected(void) override;
 
     // used by UART code to avoid a hw bug in the AUAV-X2
     void set_usb_connected(void) { _usb_connected = true; }

libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp

@@ -2,6 +2,10 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 
+#include <AP_HAL/utility/RCOutput_Tap.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+
 #include "AP_HAL_VRBRAIN.h"
 #include "AP_HAL_VRBRAIN_Namespace.h"
 #include "HAL_VRBRAIN_Class.h"
@@ -14,9 +18,11 @@
 #include "Util.h"
 #include "GPIO.h"
 #include "I2CDevice.h"
+#include "SPIDevice.h"
 
-#include <AP_HAL_Empty/AP_HAL_Empty.h>
-#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#if HAL_WITH_UAVCAN
+#include "CAN.h"
+#endif
 
 #include <stdlib.h>
 #include <systemlib/systemlib.h>
@@ -28,8 +34,8 @@
 #include <drivers/drv_hrt.h>
 
 using namespace VRBRAIN;
+using namespace ap;
 
-static Empty::SPIDeviceManager spiDeviceManager;
 //static Empty::GPIO gpioDriver;
 
 static VRBRAINScheduler schedulerInstance;
@@ -41,6 +47,7 @@ static VRBRAINUtil utilInstance;
 static VRBRAINGPIO gpioDriver;
 
 static VRBRAIN::I2CDeviceManager i2c_mgr_instance;
+static VRBRAIN::SPIDeviceManager spi_mgr_instance;
 
 #if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
@@ -67,14 +74,14 @@ static VRBRAIN::I2CDeviceManager i2c_mgr_instance;
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
 #define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
-#define UARTD_DEFAULT_DEVICE "/dev/null"
+#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
 #define UARTE_DEFAULT_DEVICE "/dev/null"
 #define UARTF_DEFAULT_DEVICE "/dev/null"
 #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
 #define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
-#define UARTD_DEFAULT_DEVICE "/dev/null"
+#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
 #define UARTE_DEFAULT_DEVICE "/dev/null"
 #define UARTF_DEFAULT_DEVICE "/dev/null"
 #elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
@@ -117,7 +124,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
         &uartEDriver,  /* uartE */
         &uartFDriver,  /* uartF */
         &i2c_mgr_instance,
-        &spiDeviceManager, /* spi */
+        &spi_mgr_instance,
         &analogIn, /* analogin */
         &storageDriver, /* storage */
         &uartADriver, /* console */
@@ -126,7 +133,8 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
         &rcoutDriver, /* rcoutput */
         &schedulerInstance, /* scheduler */
         &utilInstance, /* util */
-        nullptr)    /* no onboard optical flow */
+        nullptr,    /* no onboard optical flow */
+        nullptr)   /* CAN */
 {}
 
 bool _vrbrain_thread_should_exit = false;        /**< Daemon exit flag */
@@ -168,11 +176,9 @@ static int main_loop(int argc, char **argv)
     hal.uartD->begin(57600);
     hal.uartE->begin(57600);
     hal.scheduler->init();
-    hal.rcin->init();
-    hal.rcout->init();
-    hal.analogin->init();
-    hal.gpio->init();
 
+    // init the I2C wrapper class
+    VRBRAIN_I2C::init_lock();
     
     /*
       run setup() at low priority to ensure CLI doesn't hang the
@@ -240,6 +246,7 @@ static void usage(void)
     printf("\t-d2 DEVICE         set second terminal device (default %s)\n", UARTC_DEFAULT_DEVICE);
     printf("\t-d3 DEVICE         set 3rd terminal device (default %s)\n", UARTD_DEFAULT_DEVICE);
     printf("\t-d4 DEVICE         set 2nd GPS device (default %s)\n", UARTE_DEFAULT_DEVICE);
+    printf("\t-d5 DEVICE         set uartF (default %s)\n", UARTF_DEFAULT_DEVICE);
     printf("\n");
 }
 
@@ -251,6 +258,7 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
     const char *deviceC = UARTC_DEFAULT_DEVICE;
     const char *deviceD = UARTD_DEFAULT_DEVICE;
     const char *deviceE = UARTE_DEFAULT_DEVICE;
+    const char *deviceF = UARTF_DEFAULT_DEVICE;
 
     if (argc < 1) {
         printf("%s: missing command (try '%s start')", 
@@ -274,8 +282,10 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
             uartCDriver.set_device_path(deviceC);
             uartDDriver.set_device_path(deviceD);
             uartEDriver.set_device_path(deviceE);
-            printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\n", 
-                   SKETCHNAME, deviceA, deviceC, deviceD, deviceE);
+            uartFDriver.set_device_path(deviceF);
+
+            printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s uartF=%s\n",
+                   SKETCHNAME, deviceA, deviceC, deviceD, deviceE, deviceF);
 
             _vrbrain_thread_should_exit = false;
             daemon_task = px4_task_spawn_cmd(SKETCHNAME,
@@ -346,6 +356,17 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
                 exit(1);
             }
         }
+
+        if (strcmp(argv[i], "-d5") == 0) {
+            // set uartF
+            if (argc > i + 1) {
+                deviceF = strdup(argv[i+1]);
+            } else {
+                printf("missing parameter to -d5 DEVICE\n");
+                usage();
+                exit(1);
+            }
+        }
     }
  
     usage();

libraries/AP_HAL_VRBRAIN/I2CDevice.cpp

@@ -17,48 +17,132 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include "Util.h"
+#include "Scheduler.h"
 
 namespace VRBRAIN {
 
 uint8_t VRBRAIN::VRBRAIN_I2C::instance;
+pthread_mutex_t VRBRAIN::VRBRAIN_I2C::instance_lock;
+
+DeviceBus I2CDevice::businfo[I2CDevice::num_buses];
+
+/*
+  constructor for I2C wrapper class
+ */    
+VRBRAIN_I2C::VRBRAIN_I2C(uint8_t bus) :
+  I2C(devname, devpath, map_bus_number(bus), 0, 100000UL)
+{}
+
+/*
+  map ArduPilot bus numbers to VRBRAIN bus numbers
+ */    
+uint8_t VRBRAIN_I2C::map_bus_number(uint8_t bus) const
+{
+    switch (bus) {
+    case 0:
+        // map to internal bus
+#ifdef PX4_I2C_BUS_ONBOARD
+        return PX4_I2C_BUS_ONBOARD;
+#else
+        return 0;
+#endif
+
+    case 1:
+        // map to expansion bus
+#ifdef PX4_I2C_BUS_EXPANSION
+        return PX4_I2C_BUS_EXPANSION;
+#else
+        return 1;
+#endif
+    case 2:
+        // map to expansion bus 2
+#ifdef PX4_I2C_BUS_EXPANSION1
+        return PX4_I2C_BUS_EXPANSION1;
+#else
+        return 2;
+#endif
+    }
+    // default to bus 1
+    return 1;
+}
     
 /*
   implement wrapper for VRBRAIN I2C driver
  */
-bool VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
+bool VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers)
 {
+    set_address(address);
     if (!init_done) {
+        if (pthread_mutex_lock(&instance_lock) != 0) {
+            return false;
+        }
         init_done = true;
         // we do late init() so we can setup the device paths
+        
         snprintf(devname, sizeof(devname), "AP_I2C_%u", instance);
         snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance);
         init_ok = (init() == OK);
         if (init_ok) {
             instance++;
         }
+        pthread_mutex_unlock(&instance_lock);
     }
     if (!init_ok) {
         return false;
     }
-    set_address(address);
-    bool ret = (transfer(send, send_len, recv, recv_len) == OK);
-    return ret;
+
+    if (split_transfers) {
+        /*
+          splitting the transfer() into two pieces avoids a stop condition
+          with SCL low which is not supported on some devices (such as
+          LidarLite blue label)
+        */
+        if (send && send_len) {
+            if (transfer(send, send_len, nullptr, 0) != OK) {
+                return false;
+            }
+        }
+        if (recv && recv_len) {
+            if (transfer(nullptr, 0, recv, recv_len) != OK) {
+                return false;
+            }
+        }
+    } else {
+        // combined transfer
+        if (transfer(send, send_len, recv, recv_len) != OK) {
+            return false;
+        }
+    }
+    return true;
 }
 
 I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
-    _bus(bus),
+    _busnum(bus),
+    _vrbraindev(_busnum),
     _address(address)
 {
+    set_device_bus(bus);
+    set_device_address(address);
+    asprintf(&pname, "I2C:%u:%02x",
+             (unsigned)bus, (unsigned)address);
+    perf = perf_alloc(PC_ELAPSED, pname);
 }
     
 I2CDevice::~I2CDevice()
 {
+    printf("I2C device bus %u address 0x%02x closed\n", 
+           (unsigned)_busnum, (unsigned)_address);
+    perf_free(perf);
+    free(pname);
 }
 
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
-    return _bus.do_transfer(_address, send, send_len, recv, recv_len);
+    perf_begin(perf);
+    bool ret = _vrbraindev.do_transfer(_address, send, send_len, recv, recv_len, _split_transfers);
+    perf_end(perf);
+    return ret;
 }
 
 bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
@@ -67,6 +151,34 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
     return false;
 }
 
+    
+/*
+  register a periodic callback
+*/
+AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    if (_busnum >= num_buses) {
+        return nullptr;
+    }
+    struct DeviceBus &binfo = businfo[_busnum];
+    return binfo.register_periodic_callback(period_usec, cb, this);
+}
+    
+
+/*
+  adjust a periodic callback
+*/
+bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    if (_busnum >= num_buses) {
+        return false;
+    }
+
+    struct DeviceBus &binfo = businfo[_busnum];
+
+    return binfo.adjust_timer(h, period_usec);
+}
+
 AP_HAL::OwnPtr<AP_HAL::I2CDevice>
 I2CDeviceManager::get_device(uint8_t bus, uint8_t address,
                              uint32_t bus_clock,

libraries/AP_HAL_VRBRAIN/I2CDevice.h

@@ -21,9 +21,9 @@
 #include <AP_HAL/HAL.h>
 #include <AP_HAL/I2CDevice.h>
 #include <AP_HAL/utility/OwnPtr.h>
-#include <AP_HAL_Empty/AP_HAL_Empty.h>
-#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#include "Semaphores.h"
 #include "I2CWrapper.h"
+#include "Device.h"
 
 namespace VRBRAIN {
 
@@ -41,7 +41,7 @@ public:
     void set_address(uint8_t address) override { _address = address; }
 
     /* See AP_HAL::I2CDevice::set_retries() */
-    void set_retries(uint8_t retries) override { _retries = retries; }
+    void set_retries(uint8_t retries) override { _vrbraindev.set_retries(retries); }
 
     /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
     bool set_speed(enum Device::Speed speed) override { return true; }
@@ -55,27 +55,30 @@ public:
 
     /* See AP_HAL::Device::register_periodic_callback() */
     AP_HAL::Device::PeriodicHandle register_periodic_callback(
-        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
-    {
-        /* Not implemented yet */
-        return nullptr;
-    }
+        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
     /* See AP_HAL::Device::adjust_periodic_callback() */
-    bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
-    {
-        /* Not implemented yet */
-        return false;
+    bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
+
+    AP_HAL::Semaphore* get_semaphore() override {
+        // if asking for invalid bus number use bus 0 semaphore
+        return &businfo[_busnum<num_buses?_busnum:0].semaphore;
     }
 
-    AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
+    void set_split_transfers(bool set) override {
+        _split_transfers = set;
+    }
     
 private:
-    // we use an empty semaphore as the underlying I2C class already has a semaphore
-    Empty::Semaphore semaphore;
-    VRBRAIN_I2C _bus;
+    static const uint8_t num_buses = 3;
+    static DeviceBus businfo[num_buses];
+    
+    uint8_t _busnum;
+    VRBRAIN_I2C _vrbraindev;
     uint8_t _address;
-    uint8_t _retries = 0;
+    perf_counter_t perf;
+    char *pname;
+    bool _split_transfers;
 };
 
 class I2CDeviceManager : public AP_HAL::I2CDeviceManager {

libraries/AP_HAL_VRBRAIN/I2CWrapper.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include <AP_HAL/AP_HAL.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
@@ -13,11 +15,23 @@ extern const AP_HAL::HAL& hal;
  */
 class VRBRAIN::VRBRAIN_I2C : public device::I2C {
 public:
-    VRBRAIN_I2C(uint8_t bus) : I2C(devname, devpath, bus, 0, 400000UL) { }
-    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
+    VRBRAIN_I2C(uint8_t bus);
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers);
+
+    void set_retries(uint8_t retries) {
+        _retries = retries;
+    }
+
+    uint8_t map_bus_number(uint8_t bus) const;
+
+    // setup instance_lock
+    static void init_lock(void) {
+        pthread_mutex_init(&instance_lock, nullptr);
+    }
     
 private:
     static uint8_t instance;
+    static pthread_mutex_t instance_lock;
     bool init_done;
     bool init_ok;
     char devname[10];

libraries/AP_HAL_VRBRAIN/RCInput.cpp

@@ -8,6 +8,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 
+#include <GCS_MAVLink/GCS.h>
+
 using namespace VRBRAIN;
 
 extern const AP_HAL::HAL& hal;
@@ -26,10 +28,22 @@ void VRBRAINRCInput::init()
 bool VRBRAINRCInput::new_input() 
 {
     pthread_mutex_lock(&rcin_mutex);
-    bool valid = _rcin.timestamp_last_signal != _last_read || _override_valid;
+    bool valid = _rcin.timestamp_last_signal != _last_read;
+    if (_rcin.rc_failsafe) {
+        // don't consider input valid if we are in RC failsafe.
+        valid = false;
+    }
+    if (_override_valid) {
+        // if we have RC overrides active, then always consider it valid
+        valid = true;
+    }
     _last_read = _rcin.timestamp_last_signal;
     _override_valid = false;
     pthread_mutex_unlock(&rcin_mutex);
+    if (_rcin.input_source != last_input_source) {
+        gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", input_source_name(_rcin.input_source));
+        last_input_source = _rcin.input_source;
+    }
     return valid;
 }
 
@@ -103,6 +117,30 @@ void VRBRAINRCInput::clear_overrides()
     }
 }
 
+const char *VRBRAINRCInput::input_source_name(uint8_t id) const
+{
+    switch(id) {
+    case input_rc_s::RC_INPUT_SOURCE_UNKNOWN:         return "UNKNOWN";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM:      return "PX4FMU_PPM";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM:       return "PX4IO_PPM";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM:  return "PX4IO_SPEKTRUM";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS:      return "PX4IO_SBUS";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24:      return "PX4IO_ST24";
+    case input_rc_s::RC_INPUT_SOURCE_MAVLINK:         return "MAVLINK";
+    case input_rc_s::RC_INPUT_SOURCE_QURT:            return "QURT";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SPEKTRUM: return "PX4FMU_SPEKTRUM";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS:     return "PX4FMU_SBUS";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24:     return "PX4FMU_ST24";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD:     return "PX4FMU_SUMD";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM:      return "PX4FMU_DSM";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD:      return "PX4IO_SUMD";
+    case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SRXL:     return "PX4FMU_SRXL";
+    case input_rc_s::RC_INPUT_SOURCE_PX4IO_SRXL:      return "PX4IO_SRXL";
+    default:                                          return "ERROR";
+    }
+}
+
+
 void VRBRAINRCInput::_timer_tick(void)
 {
     perf_begin(_perf_rcin);
@@ -110,6 +148,10 @@ void VRBRAINRCInput::_timer_tick(void)
     if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
         pthread_mutex_lock(&rcin_mutex);
         orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
+        if (_rcin.rssi != 0 || _rssi != -1) {
+            // always zero means not supported
+            _rssi = _rcin.rssi;
+        }
         pthread_mutex_unlock(&rcin_mutex);
     }
     // note, we rely on the vehicle code checking new_input()
@@ -132,6 +174,9 @@ bool VRBRAINRCInput::rc_bind(int dsmMode)
 
 
 
+
+
+
     return true;
 }
 

libraries/AP_HAL_VRBRAIN/RCInput.h

@@ -12,19 +12,24 @@
 
 class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput {
 public:
-    void init();
-    bool new_input();
-    uint8_t num_channels();
-    uint16_t read(uint8_t ch);
-    uint8_t read(uint16_t* periods, uint8_t len);
+    void init() override;
+    bool new_input() override;
+    uint8_t num_channels() override;
+    uint16_t read(uint8_t ch) override;
+    uint8_t read(uint16_t* periods, uint8_t len) override;
 
-    bool set_overrides(int16_t *overrides, uint8_t len);
-    bool set_override(uint8_t channel, int16_t override);
-    void clear_overrides();
+    int16_t get_rssi(void) override {
+        return _rssi;
+    }
+        
+    
+    bool set_overrides(int16_t *overrides, uint8_t len) override;
+    bool set_override(uint8_t channel, int16_t override) override;
+    void clear_overrides() override;
 
     void _timer_tick(void);
 
-    bool rc_bind(int dsmMode);
+    bool rc_bind(int dsmMode) override;
 
 private:
     /* override state */
@@ -35,4 +40,8 @@ private:
     bool _override_valid;
     perf_counter_t _perf_rcin;
     pthread_mutex_t rcin_mutex;
+    int16_t _rssi = -1;
+
+    uint8_t last_input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
+    const char *input_source_name(uint8_t id) const;
 };

libraries/AP_HAL_VRBRAIN/RCOutput.cpp

@@ -9,11 +9,17 @@
 #include <unistd.h>
 
 #include <drivers/drv_pwm_output.h>
-#include <uORB/topics/actuator_direct.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_sbus.h>
 
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#if HAL_WITH_UAVCAN
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 using namespace VRBRAIN;
@@ -38,7 +44,8 @@ void VRBRAINRCOutput::init()
         hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
     }
 
-    _rate_mask = 0;
+    _rate_mask_main = 0;
+    _rate_mask_alt = 0;
     _alt_fd = -1;    
     _servo_count = 0;
     _alt_servo_count = 0;
@@ -53,24 +60,22 @@ void VRBRAINRCOutput::init()
     }
 
 
-//    _alt_fd = open("/dev/px4fmu", O_RDWR);
-//    if (_alt_fd == -1) {
-//        hal.console->printf("RCOutput: failed to open /dev/px4fmu");
-//        return;
-//    }
+
+
+
+
+
+
+
+
+
 
 
     // ensure not to write zeros to disabled channels
-    _enabled_channels = 0;
     for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) {
         _period[i] = PWM_IGNORE_THIS_CHANNEL;
+        _last_sent[i] = PWM_IGNORE_THIS_CHANNEL;
     }
-
-    // publish actuator vaules on demand
-    _actuator_direct_pub = nullptr;
-
-    // and armed state
-    _actuator_armed_pub = nullptr;
 }
 
 
@@ -95,8 +100,14 @@ void VRBRAINRCOutput::_init_alt_channels(void)
 /*
   set output frequency on outputs associated with fd
  */
-void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) 
+void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask) 
 {
+    if (_output_mode == MODE_PWM_BRUSHED) {
+        freq_hz /= 8; // divide by 8 for 8MHz clock
+        // remember max period
+        _period_max = 1000000UL/freq_hz;
+    }
+    
     // we can't set this per channel
     if (freq_hz > 50 || freq_hz == 1) {
         // we're being asked to set the alt rate
@@ -108,7 +119,7 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
              */
             freq_hz = 1;
         }
-        //::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz);
+        //::printf("SET_UPDATE_RATE %d %u\n", fd, (unsigned)freq_hz);
         if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
             hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
             return;
@@ -118,10 +129,15 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
 
     /* work out the new rate mask. The outputs have 4 groups of servos. 
 
+       Group 0: channels 0 1 2 3
+       Group 1: channels 4 5
+       Group 2: channels 6 7
+       Group 3: channels 8 9 10
+
        Group 0: channels 0 1 2
        Group 1: channels 3 4 5
        Group 2: channels 6 7
-       Group 8: channels 8 9 10 11
+       Group 3: channels 8 9 10 11
 
        Channels within a group must be set to the same rate.
 
@@ -130,38 +146,71 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
      */
     if (freq_hz > 50 || freq_hz == 1) {
         // we are setting high rates on the given channels
-        _rate_mask |= chmask & 0xFF;
-        if (_rate_mask & 0x07) {
-            _rate_mask |= 0x07;
+        rate_mask |= chmask & 0xFFF;
+#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
+        if (rate_mask & 0x0F) {
+            rate_mask |= 0x0F;
         }
-        if (_rate_mask & 0x38) {
-            _rate_mask |= 0x38;
+        if (rate_mask & 0x30) {
+            rate_mask |= 0x30;
         }
-        if (_rate_mask & 0xC0) {
-            _rate_mask |= 0xC0;
+        if (rate_mask & 0xC0) {
+            rate_mask |= 0xC0;
         }
-        if (_rate_mask & 0xF00) {
-            _rate_mask |= 0xF00;
+        if (rate_mask & 0x700) {
+            rate_mask |= 0x700;
         }
+#else
+        if (rate_mask & 0x07) {
+            rate_mask |= 0x07;
+        }
+        if (rate_mask & 0x38) {
+            rate_mask |= 0x38;
+        }
+        if (rate_mask & 0xC0) {
+            rate_mask |= 0xC0;
+        }
+        if (rate_mask & 0xF00) {
+            rate_mask |= 0xF00;
+        }
+#endif
     } else {
         // we are setting low rates on the given channels
-        if (chmask & 0x07) {
-            _rate_mask &= ~0x07;
+#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
+        if (chmask & 0x0F) {
+            rate_mask &= ~0x0F;
         }
-        if (chmask & 0x38) {
-            _rate_mask &= ~0x38;
+        if (chmask & 0x30) {
+            rate_mask &= ~0x30;
         }
         if (chmask & 0xC0) {
-            _rate_mask &= ~0xC0;
+            rate_mask &= ~0xC0;
+        }
+        if (chmask & 0x700) {
+            rate_mask &= ~0x700;
+        }
+#else
+        if (chmask & 0x07) {
+            rate_mask &= ~0x07;
+        }
+        if (chmask & 0x38) {
+            rate_mask &= ~0x38;
+        }
+        if (chmask & 0xC0) {
+            rate_mask &= ~0xC0;
         }
         if (chmask & 0xF00) {
-            _rate_mask &= ~0xF00;
+            rate_mask &= ~0xF00;
         }
+#endif
     }
 
-    //::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask);
-    if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
-        hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
+    if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, rate_mask) != 0) {
+        hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)rate_mask);
+    }
+
+    if (_output_mode == MODE_PWM_BRUSHED) {
+        ioctl(fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
     }    
 }
 
@@ -180,31 +229,33 @@ void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
         hal.console->printf("RCOutput: Unable to get servo count\n");        
         return;
     }
-    if (_alt_fd != -1 && ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
-        hal.console->printf("RCOutput: Unable to get alt servo count\n");        
-        return;
-    }
     
     // greater than 400 doesn't give enough room at higher periods for
     // the down pulse
-    if (freq_hz > 400) {
+    if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) {
         freq_hz = 400;
     }
     uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
     uint32_t alt_mask = chmask >> _servo_count;
     if (primary_mask && _pwm_fd != -1) {
-        set_freq_fd(_pwm_fd, primary_mask, freq_hz);
+        set_freq_fd(_pwm_fd, primary_mask, freq_hz, _rate_mask_main);
     }
     if (alt_mask && _alt_fd != -1) {
-        set_freq_fd(_alt_fd, alt_mask, freq_hz);
+        set_freq_fd(_alt_fd, alt_mask, freq_hz, _rate_mask_alt);
     }
 }
 
 uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
 {
-    if (_rate_mask & (1U<<ch)) {
+    if (ch < _servo_count) {
+        if (_rate_mask_main & (1U<<ch)) {
             return _freq_hz;
         }
+    } else if (_alt_fd != -1) {
+        if (_rate_mask_alt & (1U<<(ch-_servo_count))) {
+            return _freq_hz;
+        }
+    }
     return 50;
 }
 
@@ -222,6 +273,7 @@ void VRBRAINRCOutput::enable_ch(uint8_t ch)
     _enabled_channels |= (1U<<ch);
     if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
         _period[ch] = 0;
+        _last_sent[ch] = 0;
     }
 }
 
@@ -283,19 +335,20 @@ void VRBRAINRCOutput::force_safety_off(void)
 void VRBRAINRCOutput::force_safety_pending_requests(void)
 {
     // check if there is a pending saftey_state change. If so (timer != 0) then set it.
+    uint32_t now = AP_HAL::millis();
     if (_safety_state_request_last_ms != 0 &&
-            AP_HAL::millis() - _safety_state_request_last_ms >= 100) {
-        if (hal.util->safety_switch_state() == _safety_state_request) {
-            // current switch state matches request, stop attempting
+        now - _safety_state_request_last_ms >= 100) {
+        if (hal.util->safety_switch_state() == _safety_state_request &&
+            _safety_state_request_last_ms != 1) {
             _safety_state_request_last_ms = 0;
         } else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) {
             // current != requested, set it
             ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
-            _safety_state_request_last_ms = AP_HAL::millis();
+            _safety_state_request_last_ms = now;
         } else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) {
             // current != requested, set it
             ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-            _safety_state_request_last_ms = AP_HAL::millis();
+            _safety_state_request_last_ms = now;
         }
     }
 }
@@ -320,6 +373,24 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
     if (ch >= _max_channel) {
         _max_channel = ch + 1;
     }
+
+    // keep unscaled value
+    _last_sent[ch] = period_us;
+        
+    if (_output_mode == MODE_PWM_BRUSHED) {
+        // map from the PWM range to 0 t0 100% duty cycle. For 16kHz
+        // this ends up being 0 to 500 pulse width in units of
+        // 125usec.
+        if (period_us <= _esc_pwm_min) {
+            period_us = 0;
+        } else if (period_us >= _esc_pwm_max) {
+            period_us = _period_max;
+        } else {
+            uint32_t pdiff = period_us - _esc_pwm_min;
+            period_us = pdiff*_period_max/(_esc_pwm_max - _esc_pwm_min);
+        }
+    }
+    
     /*
       only mark an update is needed if there has been a change, or we
       are in oneshot mode. In oneshot mode we always need to send the
@@ -329,7 +400,7 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
         _output_mode == MODE_PWM_ONESHOT) {
         _period[ch] = period_us;
         _need_update = true;
-        up_pwm_servo_set(ch, period_us);
+//        up_pwm_servo_set(ch, period_us);
     }
 }
 
@@ -364,7 +435,7 @@ uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch)
         return 0;
     }
 
-    return _period[ch];
+    return _last_sent[ch];
 }
 
 void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
@@ -386,7 +457,11 @@ void VRBRAINRCOutput::_arm_actuators(bool arm)
 
 	_armed.timestamp = hrt_absolute_time();
     _armed.armed = arm;
-    _armed.ready_to_arm = arm;
+    if (arm) {
+        // this latches ready_to_arm to true once set once. Otherwise
+        // we have a race condition with px4io safety switch update
+        _armed.ready_to_arm = true;
+    }
     _armed.lockdown = false;
     _armed.force_failsafe = false;
 
@@ -397,51 +472,6 @@ void VRBRAINRCOutput::_arm_actuators(bool arm)
     }
 }
 
-/*
-  publish new outputs to the actuator_direct topic
- */
-void VRBRAINRCOutput::_publish_actuators(void)
-{
-	struct actuator_direct_s actuators;
-
-    if (_esc_pwm_min == 0 ||
-        _esc_pwm_max == 0) {
-        // not initialised yet
-        return;
-    }
-
-	actuators.nvalues = _max_channel;
-    if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
-        actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
-    }
-    // don't publish more than 8 actuators for now, as the uavcan ESC
-    // driver refuses to update any motors if you try to publish more
-    // than 8
-    if (actuators.nvalues > 8) {
-        actuators.nvalues = 8;
-    }
-    bool armed = hal.util->get_soft_armed();
-	actuators.timestamp = hrt_absolute_time();
-    for (uint8_t i=0; i<actuators.nvalues; i++) {
-        if (!armed) {
-            actuators.values[i] = 0;
-        } else {
-            actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
-        }
-        // actuator values are from -1 to 1
-        actuators.values[i] = actuators.values[i]*2 - 1;
-    }
-
-    if (_actuator_direct_pub == nullptr) {
-        _actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
-    } else {
-        orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
-    }
-    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
-        _arm_actuators(true);
-    }
-}
-
 void VRBRAINRCOutput::_send_outputs(void)
 {
     uint32_t now = AP_HAL::micros();
@@ -478,7 +508,7 @@ void VRBRAINRCOutput::_send_outputs(void)
         }
         if (to_send > 0) {
             for (int i=to_send-1; i >= 0; i--) {
-                if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) {
+                if (_period[i] == PWM_IGNORE_THIS_CHANNEL) {
                     to_send = i;
                 } else {
                     break;
@@ -486,6 +516,8 @@ void VRBRAINRCOutput::_send_outputs(void)
             }
         }
         if (to_send > 0) {
+            _arm_actuators(true);
+
             ::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
         }
         if (_max_channel > _servo_count) {
@@ -501,8 +533,35 @@ void VRBRAINRCOutput::_send_outputs(void)
             }
         }
 
-        // also publish to actuator_direct
-        _publish_actuators();
+#if HAL_WITH_UAVCAN
+        if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1)
+        {
+            for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+                if (hal.can_mgr[i] != nullptr)
+                {
+                    AP_UAVCAN *ap_uc = hal.can_mgr[i]->get_UAVCAN();
+                    if (ap_uc != nullptr)
+                    {
+                        if (ap_uc->rc_out_sem_take())
+                        {
+                            for (uint8_t j = 0; j < _max_channel; j++)
+                            {
+                                ap_uc->rco_write(_period[j], j);
+                            }
+
+                            if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
+                                ap_uc->rco_arm_actuators(true);
+                            } else {
+                                ap_uc->rco_arm_actuators(false);
+                            }
+
+                            ap_uc->rc_out_sem_give();
+                        }
+                    }
+                }
+            }
+        }
+#endif // HAL_WITH_UAVCAN
 
         perf_end(_perf_rcout);
         _last_output = now;
@@ -531,21 +590,20 @@ void VRBRAINRCOutput::cork()
 
 void VRBRAINRCOutput::push()
 {
-    if (!_corking) {
-        return;
-    }
 #if RCOUT_DEBUG_LATENCY
     hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
     hal.gpio->write(55, 0);
 #endif
+    if (_corking) {
         _corking = false;
         if (_output_mode == MODE_PWM_ONESHOT) {
             // run timer immediately in oneshot mode
             _send_outputs();
         }
     }
+}
 
-void VRBRAINRCOutput::_timer_tick(void)
+void VRBRAINRCOutput::timer_tick(void)
 {
     if (_output_mode != MODE_PWM_ONESHOT && !_corking) {
         /* in oneshot mode the timer does nothing as the outputs are
@@ -596,22 +654,48 @@ void VRBRAINRCOutput::set_output_mode(enum output_mode mode)
         // mean the timer is constantly reset, so no pulses are
         // produced except when triggered by push() when the main loop
         // is running
-        set_freq(_rate_mask, 1);
+        set_freq_fd(_pwm_fd, _rate_mask_main, 1, _rate_mask_main);
+        if (_alt_fd != -1) {
+            set_freq_fd(_alt_fd, _rate_mask_alt, 1, _rate_mask_alt);
+        }
     }
     _output_mode = mode;
-    if (_output_mode == MODE_PWM_ONESHOT) {
-        //::printf("enable oneshot\n");
+    switch (_output_mode) {
+    case MODE_PWM_ONESHOT:
         ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
         if (_alt_fd != -1) {
             ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
         }
-    } else {
+        break;
+    case MODE_PWM_NORMAL:
         ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0);
         if (_alt_fd != -1) {
             ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0);
         }
+        break;
+    case MODE_PWM_BRUSHED:
+        // setup an 8MHz clock. This has the effect of scaling all outputs by 8x
+        ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
+        if (_alt_fd != -1) {
+            ioctl(_alt_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
+        }
+        break;
     }
 }
 
 
+// set default output update rate
+void VRBRAINRCOutput::set_default_rate(uint16_t rate_hz)
+{
+    if (rate_hz != _default_rate_hz) {
+        // set servo update rate for first 8 pwm channels
+        ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
+        if (_alt_fd != -1) {
+            // set servo update rate for auxiliary channels
+            ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
+        }
+        _default_rate_hz = rate_hz;
+    }
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/RCOutput.h

@@ -29,19 +29,28 @@ public:
         _esc_pwm_min = min_pwm;
         _esc_pwm_max = max_pwm;
     }
+    float    scale_esc_to_unity(uint16_t pwm) override {
+        return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
+    }
     void cork();
     void push();
 
     void set_output_mode(enum output_mode mode) override;
 
-    void _timer_tick(void);
+    void timer_tick(void) override;
     bool enable_px4io_sbus_out(uint16_t rate_hz) override;
 
+    // set default output update rate
+    void set_default_rate(uint16_t rate_hz) override;
+
 private:
     int _pwm_fd;
     int _alt_fd;
     uint16_t _freq_hz;
     uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
+    // we keep the last_sent value separately, as we need to keep the unscaled
+    // value for systems with brushed motors which scale outputs
+    uint16_t _last_sent[VRBRAIN_NUM_OUTPUT_CHANNELS];
     volatile uint8_t _max_channel;
     volatile bool _need_update;
     bool _sbus_enabled:1;
@@ -50,27 +59,28 @@ private:
     uint32_t _last_config_us;
     unsigned _servo_count;
     unsigned _alt_servo_count;
-    uint32_t _rate_mask;
+    uint32_t _rate_mask_main;
+    uint32_t _rate_mask_alt;
     uint16_t _enabled_channels;
+    uint32_t _period_max;
     struct {
         int pwm_sub;
         actuator_outputs_s outputs;
     } _outputs[ORB_MULTI_MAX_INSTANCES] {};
     actuator_armed_s _armed;
 
-    orb_advert_t _actuator_direct_pub = nullptr;
-    orb_advert_t _actuator_armed_pub = nullptr;
-    uint16_t _esc_pwm_min = 0;
-    uint16_t _esc_pwm_max = 0;
+    orb_advert_t _actuator_armed_pub;
+    uint16_t _esc_pwm_min;
+    uint16_t _esc_pwm_max;
 
     void _init_alt_channels(void);
-    void _publish_actuators(void);
     void _arm_actuators(bool arm);
-    void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
+    void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask);
     bool _corking;
     enum output_mode _output_mode = MODE_PWM_NORMAL;
     void _send_outputs(void);
     enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
     uint32_t _safety_state_request_last_ms = 0;
     void force_safety_pending_requests(void);
+    uint16_t _default_rate_hz = 50;
 };

libraries/AP_HAL_VRBRAIN/SPIDevice.cpp

@@ -0,0 +1,329 @@
+/*
+ * This file 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 file 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/>.
+ */
+#include "SPIDevice.h"
+
+#include <arch/board/board.h>
+#include "board_config.h"
+#include <drivers/device/spi.h>
+#include <stdio.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include "Scheduler.h"
+#include "Semaphores.h"
+
+/*
+  NuttX on STM32 doesn't produce the exact SPI bus frequency
+  requested. This is a table mapping requested to achieved SPI
+  frequency for a 168 MHz processor :
+
+  2  -> 1.3 MHz
+  4  -> 2.6 MHz
+  6  -> 5.3 MHz
+  8  -> 5.3 MHz
+  10 -> 5.3 MHz
+  11 -> 10
+  12 -> 10
+  13 -> 10
+  14 -> 10
+  16 -> 10
+  18 -> 10
+  20 -> 10
+  21 -> 20
+  28 -> 20
+
+For a 180 MHz processor :
+
+ 2  -> 1.4 MHz
+ 4  -> 2.8 MHz
+ 6  -> 5.6 MHz
+ 8  -> 5.6 MHz
+ 10 -> 5.6 MHz
+ 11 -> 5.6 MHz
+ 12 -> 11.25 MHz
+ 13 -> 11.25 MHz
+ 14 -> 11.25 MHz
+ 16 -> 11.25 MHz
+ 18 -> 11.25 MHz
+ 20 -> 11.25 MHz
+ 22 -> 11.25 MHz
+ 24 -> 22.5 MHz
+ 28 -> 22.5 MHz
+
+ */
+
+namespace VRBRAIN {
+
+#define MHZ (1000U*1000U)
+#define KHZ (1000U)
+
+SPIDesc SPIDeviceManager::device_table[] = {
+
+#if defined(SPIDEV_MS5611)
+    SPIDesc("ms5611",            PX4_SPI_BUS_1, (spi_dev_e)SPIDEV_MS5611,      SPIDEV_MODE3, 20*MHZ, 20*MHZ),
+#endif
+#if defined(SPIDEV_EXP_MS5611)
+    SPIDesc("ms5611_ext",        PX4_SPI_BUS_1, (spi_dev_e)SPIDEV_EXP_MS5611,  SPIDEV_MODE3, 20*MHZ, 20*MHZ),
+#endif
+#if defined(SPIDEV_EXP_MPU6000)
+    SPIDesc("mpu6000_ext",       PX4_SPI_BUS_1, (spi_dev_e)SPIDEV_EXP_MPU6000, SPIDEV_MODE3, 500*KHZ, 8*MHZ),
+#endif
+#if defined(SPIDEV_EXP_HMC5983)
+    SPIDesc("hmc5983_ext",       PX4_SPI_BUS_1, (spi_dev_e)SPIDEV_EXP_HMC5983, SPIDEV_MODE3, 11*MHZ, 11*MHZ),
+#endif
+#if defined(SPIDEV_MPU6000)
+    SPIDesc("mpu6000",           PX4_SPI_BUS_2, (spi_dev_e)SPIDEV_MPU6000,     SPIDEV_MODE3, 500*KHZ, 8*MHZ),
+#endif
+#if defined(SPIDEV_IMU_MS5611)
+    SPIDesc("ms5611_imu",        PX4_SPI_BUS_2, (spi_dev_e)SPIDEV_IMU_MS5611,  SPIDEV_MODE3, 20*MHZ, 20*MHZ),
+#endif
+#if defined(SPIDEV_IMU_MPU6000)
+    SPIDesc("mpu6000_imu",       PX4_SPI_BUS_2, (spi_dev_e)SPIDEV_IMU_MPU6000, SPIDEV_MODE3, 500*KHZ, 8*MHZ),
+#endif
+#if defined(SPIDEV_IMU_HMC5983)
+    SPIDesc("hmc5983_imu",       PX4_SPI_BUS_2, (spi_dev_e)SPIDEV_IMU_HMC5983, SPIDEV_MODE3, 11*MHZ, 11*MHZ),
+#endif
+#if defined(SPIDEV_MPU9250)
+    SPIDesc("mpu9250",           PX4_SPI_BUS_2, (spi_dev_e)SPIDEV_MPU9250,     SPIDEV_MODE3, 1*MHZ, 8*MHZ),
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    SPIDesc(nullptr, 0, (spi_dev_e)0, (spi_mode_e)0, 0, 0),
+};
+
+SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
+    : bus(_bus)
+    , device_desc(_device_desc)
+{
+    set_device_bus(_bus.bus);
+    set_device_address(_device_desc.device);
+    set_speed(AP_HAL::Device::SPEED_LOW);
+    SPI_SELECT(bus.dev, device_desc.device, false);
+    asprintf(&pname, "SPI:%s:%u:%u",
+             device_desc.name,
+             (unsigned)bus.bus, (unsigned)device_desc.device);
+    perf = perf_alloc(PC_ELAPSED, pname);
+    printf("SPI device %s on %u:%u at speed %u mode %u\n",
+           device_desc.name,
+           (unsigned)bus.bus, (unsigned)device_desc.device,
+           (unsigned)frequency, (unsigned)device_desc.mode);
+}
+
+SPIDevice::~SPIDevice()
+{
+    printf("SPI device %s on %u:%u closed\n", device_desc.name,
+           (unsigned)bus.bus, (unsigned)device_desc.device);
+    perf_free(perf);
+    free(pname);
+}
+
+bool SPIDevice::set_speed(AP_HAL::Device::Speed speed)
+{
+    switch (speed) {
+    case AP_HAL::Device::SPEED_HIGH:
+        frequency = device_desc.highspeed;
+        break;
+    case AP_HAL::Device::SPEED_LOW:
+        frequency = device_desc.lowspeed;
+        break;
+    }
+    return true;
+}
+
+/*
+  low level transfer function
+ */
+void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len)
+{
+    /*
+      to accomodate the method in PX4 drivers of using interrupt
+      context for SPI device transfers we need to check if PX4 has
+      registered a driver on this bus.  If not then we can avoid the
+      irqsave/irqrestore and get bus parallelism for DMA enabled
+      buses.
+
+      There is a race in this if a PX4 driver starts while we are
+      running this, but that would only happen at early boot and is
+      very unlikely
+
+      yes, this is a nasty hack. Suggestions for a better method
+      appreciated.
+     */
+    bool use_irq_save = up_spi_use_irq_save(bus.dev);
+    irqstate_t state;
+    if (use_irq_save) {
+        state = irqsave();
+    }
+    perf_begin(perf);
+    SPI_LOCK(bus.dev, true);
+    SPI_SETFREQUENCY(bus.dev, frequency);
+    SPI_SETMODE(bus.dev, device_desc.mode);
+    SPI_SETBITS(bus.dev, 8);
+    SPI_SELECT(bus.dev, device_desc.device, true);
+    SPI_EXCHANGE(bus.dev, send, recv, len);
+    if (!cs_forced) {
+        SPI_SELECT(bus.dev, device_desc.device, false);
+    }
+    SPI_LOCK(bus.dev, false);
+    perf_end(perf);
+    if (use_irq_save) {
+        irqrestore(state);
+    }
+}
+
+bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
+                         uint8_t *recv, uint32_t recv_len)
+{
+    if (send_len == recv_len && send == recv) {
+        // simplest cases, needed for DMA
+        do_transfer(send, recv, recv_len);
+        return true;
+    }
+    uint8_t buf[send_len+recv_len];
+    if (send_len > 0) {
+        memcpy(buf, send, send_len);
+    }
+    if (recv_len > 0) {
+        memset(&buf[send_len], 0, recv_len);
+    }
+    do_transfer(buf, buf, send_len+recv_len);
+    if (recv_len > 0) {
+        memcpy(recv, &buf[send_len], recv_len);
+    }
+    return true;
+}
+
+bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
+{
+    uint8_t buf[len];
+    memcpy(buf, send, len);
+    do_transfer(buf, buf, len);
+    memcpy(recv, buf, len);
+    return true;
+}
+
+AP_HAL::Semaphore *SPIDevice::get_semaphore()
+{
+    return &bus.semaphore;
+}
+
+
+AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    return bus.register_periodic_callback(period_usec, cb, this);
+}
+
+bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    return bus.adjust_timer(h, period_usec);
+}
+
+/*
+  allow for control of SPI chip select pin
+ */
+bool SPIDevice::set_chip_select(bool set)
+{
+    cs_forced = set;
+    SPI_SELECT(bus.dev, device_desc.device, set);
+    return true;
+}
+    
+
+/*
+  return a SPIDevice given a string device name
+ */
+AP_HAL::OwnPtr<AP_HAL::SPIDevice>
+SPIDeviceManager::get_device(const char *name)
+{
+    /* Find the bus description in the table */
+    uint8_t i;
+    for (i = 0; device_table[i].name; i++) {
+        if (strcmp(device_table[i].name, name) == 0) {
+            break;
+        }
+    }
+    if (device_table[i].name == nullptr) {
+        printf("SPI: Invalid device name: %s\n", name);
+        return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
+    }
+
+    SPIDesc &desc = device_table[i];
+
+    // find the bus
+    SPIBus *busp;
+    for (busp = buses; busp; busp = (SPIBus *)busp->next) {
+        if (busp->bus == desc.bus) {
+            break;
+        }
+    }
+    if (busp == nullptr) {
+        // create a new one
+        busp = new SPIBus;
+        if (busp == nullptr) {
+            return nullptr;
+        }
+        busp->next = buses;
+        busp->bus = desc.bus;
+        busp->dev = up_spiinitialize(desc.bus);
+        buses = busp;
+    }
+
+    return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(*busp, desc));
+}
+
+}

libraries/AP_HAL_VRBRAIN/SPIDevice.h

@@ -0,0 +1,113 @@
+/*
+ * This file 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 file 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/>.
+ */
+#pragma once
+
+#include <inttypes.h>
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/SPIDevice.h>
+#include <drivers/device/spi.h>
+#include "Semaphores.h"
+#include "Device.h"
+#include "Scheduler.h"
+
+namespace VRBRAIN {
+
+class SPIDesc;
+    
+class SPIBus : public DeviceBus {
+public:
+    SPIBus(void) :
+        DeviceBus(APM_SPI_PRIORITY) {}
+    struct spi_dev_s *dev;
+    uint8_t bus;
+};
+
+struct SPIDesc {
+    SPIDesc(const char *_name, uint8_t _bus, 
+            enum spi_dev_e _device, enum spi_mode_e _mode,
+            uint32_t _lowspeed, uint32_t _highspeed)
+        : name(_name), bus(_bus), device(_device), mode(_mode),
+          lowspeed(_lowspeed), highspeed(_highspeed)
+    {
+    }
+
+    const char *name;
+    uint8_t bus;
+    enum spi_dev_e device;
+    enum spi_mode_e mode;
+    uint32_t lowspeed;
+    uint32_t highspeed;
+};
+    
+    
+class SPIDevice : public AP_HAL::SPIDevice {
+public:
+    SPIDevice(SPIBus &_bus, SPIDesc &_device_desc);
+
+    virtual ~SPIDevice();
+
+    /* See AP_HAL::Device::set_speed() */
+    bool set_speed(AP_HAL::Device::Speed speed) override;
+
+    // low level transfer function
+    void do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len);
+    
+    /* See AP_HAL::Device::transfer() */
+    bool transfer(const uint8_t *send, uint32_t send_len,
+                  uint8_t *recv, uint32_t recv_len) override;
+
+    /* See AP_HAL::SPIDevice::transfer_fullduplex() */
+    bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
+                             uint32_t len) override;
+
+    /* See AP_HAL::Device::get_semaphore() */
+    AP_HAL::Semaphore *get_semaphore() override;
+
+    /* See AP_HAL::Device::register_periodic_callback() */
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(
+        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
+
+    /* See AP_HAL::Device::adjust_periodic_callback() */
+    bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
+
+    bool set_chip_select(bool set) override;
+    
+private:
+    SPIBus &bus;
+    SPIDesc &device_desc;
+    uint32_t frequency;
+    perf_counter_t perf;
+    char *pname;
+    bool cs_forced;
+    static void *spi_thread(void *arg);
+};
+
+class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
+public:
+    friend class SPIDevice;
+
+    static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr)
+    {
+        return static_cast<SPIDeviceManager*>(spi_mgr);
+    }
+
+    AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name);
+
+private:
+    static SPIDesc device_table[];
+    SPIBus *buses;
+};
+
+}

libraries/AP_HAL_VRBRAIN/Scheduler.cpp

@@ -20,7 +20,14 @@
 #include "Storage.h"
 #include "RCOutput.h"
 #include "RCInput.h"
+
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#if HAL_WITH_UAVCAN
+#include "CAN.h"
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#endif
 
 using namespace VRBRAIN;
 
@@ -83,6 +90,34 @@ void VRBRAINScheduler::init()
     pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this);
 }
 
+void VRBRAINScheduler::create_uavcan_thread()
+{
+#if HAL_WITH_UAVCAN
+    pthread_attr_t thread_attr;
+    struct sched_param param;
+
+     //the UAVCAN thread runs at medium priority
+    pthread_attr_init(&thread_attr);
+    pthread_attr_setstacksize(&thread_attr, 8192);
+
+    param.sched_priority = APM_CAN_PRIORITY;
+    (void) pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+        if (hal.can_mgr[i] != nullptr) {
+            if (hal.can_mgr[i]->get_UAVCAN() != nullptr) {
+                _uavcan_thread_arg *arg = new _uavcan_thread_arg;
+                arg->sched = this;
+                arg->uavcan_number = i;
+
+                pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg);
+            }
+        }
+    }
+#endif
+}
+
 /**
    delay for a specified number of microseconds using a semaphore wait
  */
@@ -266,6 +301,8 @@ void *VRBRAINScheduler::_timer_thread(void *arg)
     VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
     uint32_t last_ran_overtime = 0;
 
+    pthread_setname_np(pthread_self(), "apm_timer");
+
     while (!sched->_hal_initialized) {
         poll(nullptr, 0, 1);
     }
@@ -278,7 +315,7 @@ void *VRBRAINScheduler::_timer_thread(void *arg)
         perf_end(sched->_perf_timers);
 
         // process any pending RC output requests
-        ((VRBRAINRCOutput *)hal.rcout)->_timer_tick();
+        hal.rcout->timer_tick();
 
         // process any pending RC input requests
         ((VRBRAINRCInput *)hal.rcin)->_timer_tick();
@@ -317,6 +354,8 @@ void *VRBRAINScheduler::_uart_thread(void *arg)
 {
     VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
 
+    pthread_setname_np(pthread_self(), "apm_uart");
+
     while (!sched->_hal_initialized) {
         poll(nullptr, 0, 1);
     }
@@ -338,11 +377,13 @@ void *VRBRAINScheduler::_io_thread(void *arg)
 {
     VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
 
+    pthread_setname_np(pthread_self(), "apm_io");
+
     while (!sched->_hal_initialized) {
         poll(nullptr, 0, 1);
     }
     while (!_vrbrain_thread_should_exit) {
-        poll(nullptr, 0, 1);
+        sched->delay_microseconds_semaphore(1000);
 
         // run registered IO processes
         perf_begin(sched->_perf_io_timers);
@@ -356,11 +397,13 @@ void *VRBRAINScheduler::_storage_thread(void *arg)
 {
     VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
 
+    pthread_setname_np(pthread_self(), "apm_storage");
+
     while (!sched->_hal_initialized) {
         poll(nullptr, 0, 1);
     }
     while (!_vrbrain_thread_should_exit) {
-        poll(nullptr, 0, 10);
+        sched->delay_microseconds_semaphore(10000);
 
         // process any pending storage writes
         perf_begin(sched->_perf_storage_timer);
@@ -370,12 +413,43 @@ void *VRBRAINScheduler::_storage_thread(void *arg)
     return nullptr;
 }
 
+#if HAL_WITH_UAVCAN
+void *VRBRAINScheduler::_uavcan_thread(void *arg)
+{
+    VRBRAINScheduler *sched = ((_uavcan_thread_arg *) arg)->sched;
+    uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number;
+
+    char name[15];
+    snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number);
+    pthread_setname_np(pthread_self(), name);
+
+    while (!sched->_hal_initialized) {
+        poll(nullptr, 0, 1);
+    }
+
+    while (!_px4_thread_should_exit) {
+        if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->is_initialized()) {
+            if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) {
+                (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic();
+            } else {
+                sched->delay_microseconds_semaphore(10000);
+            }
+        } else {
+            sched->delay_microseconds_semaphore(10000);
+        }
+    }
+
+    return nullptr;
+}
+#endif
+
 bool VRBRAINScheduler::in_main_thread() const
 {
     return getpid() == _main_task_pid;
 }
 
-void VRBRAINScheduler::system_initialized() {
+void VRBRAINScheduler::system_initialized()
+{
     if (_initialized) {
         AP_HAL::panic("PANIC: scheduler::system_initialized called"
                       "more than once");

libraries/AP_HAL_VRBRAIN/Scheduler.h

@@ -13,6 +13,9 @@
 #define APM_MAIN_PRIORITY_BOOST 241
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      181
+#define APM_SPI_PRIORITY        242
+#define APM_CAN_PRIORITY        179
+#define APM_I2C_PRIORITY        178
 #define APM_UART_PRIORITY        60
 #define APM_STORAGE_PRIORITY     59
 #define APM_IO_PRIORITY          58
@@ -59,6 +62,8 @@ public:
     void     system_initialized();
     void     hal_initialized() { _hal_initialized = true; }
 
+    void create_uavcan_thread() override;
+
 private:
     bool _initialized;
     volatile bool _hal_initialized;
@@ -83,11 +88,18 @@ private:
     pthread_t _io_thread_ctx;
     pthread_t _storage_thread_ctx;
     pthread_t _uart_thread_ctx;
+    pthread_t _uavcan_thread_ctx;
+
+    struct _uavcan_thread_arg {
+        VRBRAINScheduler *sched;
+        uint8_t uavcan_number;
+    };
 
     static void *_timer_thread(void *arg);
     static void *_io_thread(void *arg);
     static void *_storage_thread(void *arg);
     static void *_uart_thread(void *arg);
+    static void *_uavcan_thread(void *arg);
 
     void _run_timers(bool called_from_timer_thread);
     void _run_io(void);

libraries/AP_HAL_VRBRAIN/Semaphores.cpp

@@ -3,6 +3,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 
 #include "Semaphores.h"
+#include <nuttx/arch.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -15,6 +16,10 @@ bool Semaphore::give()
 
 bool Semaphore::take(uint32_t timeout_ms) 
 {
+    if (up_interrupt_context()) {
+        // don't ever wait on a semaphore in interrupt context
+        return take_nonblocking();
+    }
     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
         return pthread_mutex_lock(&_lock) == 0;
     }

libraries/AP_HAL_VRBRAIN/Storage.cpp

@@ -9,6 +9,9 @@
 #include <errno.h>
 #include <stdio.h>
 #include <ctype.h>
+#include <nuttx/progmem.h>
+
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 #include "Storage.h"
 using namespace VRBRAIN;
@@ -21,158 +24,33 @@ using namespace VRBRAIN;
 // name the storage file after the sketch so you can use the same sd
 // card for ArduCopter and ArduPlane
 #define STORAGE_DIR "/fs/microsd/APM"
-#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
-#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak"
 //#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
 #define MTD_PARAMS_FILE "/fs/mtd"
-#define MTD_SIGNATURE 0x14012014
-#define MTD_SIGNATURE_OFFSET (8192-4)
-#define STORAGE_RENAME_OLD_FILE 0
 
 extern const AP_HAL::HAL& hal;
 
+extern "C" int mtd_main(int, char **);
+
 VRBRAINStorage::VRBRAINStorage(void) :
-    _fd(-1),
-    _dirty_mask(0),
     _perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")),
     _perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors"))
 {
 }
 
-/*
-  get signature from bytes at offset MTD_SIGNATURE_OFFSET
- */
-uint32_t VRBRAINStorage::_mtd_signature(void)
-{
-    int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY);
-    if (mtd_fd == -1) {
-        AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
-    }
-    uint32_t v;
-    if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
-        AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
-    }
-    bus_lock(true);
-    if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
-        AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
-    }
-    bus_lock(false);
-    close(mtd_fd);
-    return v;
-}
-
-/*
-  put signature bytes at offset MTD_SIGNATURE_OFFSET
- */
-void VRBRAINStorage::_mtd_write_signature(void)
-{
-    int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
-    if (mtd_fd == -1) {
-        AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
-    }
-    uint32_t v = MTD_SIGNATURE;
-    if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
-        AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
-    }
-    bus_lock(true);
-    if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
-        AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
-    }
-    bus_lock(false);
-    close(mtd_fd);
-}
-
-/*
-  upgrade from microSD to MTD (FRAM)
- */
-void VRBRAINStorage::_upgrade_to_mtd(void)
-{
-    // the MTD is completely uninitialised - try to get a
-    // copy from OLD_STORAGE_FILE
-    int old_fd = open(OLD_STORAGE_FILE, O_RDONLY);
-    if (old_fd == -1) {
-        ::printf("Failed to open %s\n", OLD_STORAGE_FILE);
-        return;
-    }
-
-    int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
-    if (mtd_fd == -1) {
-        AP_HAL::panic("Unable to open MTD for upgrade");
-    }
-
-    if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
-        close(old_fd);
-        close(mtd_fd);
-        ::printf("Failed to read %s\n", OLD_STORAGE_FILE);
-        return;        
-    }
-    close(old_fd);
-    ssize_t ret;
-    bus_lock(true);
-    if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
-        ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
-        AP_HAL::panic("Unable to write MTD for upgrade");
-    }
-    bus_lock(false);
-    close(mtd_fd);
-#if STORAGE_RENAME_OLD_FILE
-    rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
-#endif
-    ::printf("Upgraded MTD from %s\n", OLD_STORAGE_FILE);
-}
-            
-
 void VRBRAINStorage::_storage_open(void)
 {
     if (_initialised) {
         return;
     }
 
-        struct stat st;
-        _have_mtd = (stat(MTD_PARAMS_FILE, &st) == 0);
+    _dirty_mask.clearall();
 
-        // VRBRAIN should always have /fs/mtd_params
-        if (!_have_mtd) {
-            AP_HAL::panic("Failed to find " MTD_PARAMS_FILE);
-        }
-
-        /*
-          cope with upgrading from OLD_STORAGE_FILE to MTD
-         */
-        bool good_signature = (_mtd_signature() == MTD_SIGNATURE);
-        if (stat(OLD_STORAGE_FILE, &st) == 0) {
-            if (good_signature) {
-#if STORAGE_RENAME_OLD_FILE
-                rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
+    // load from storage backend
+#if USE_FLASH_STORAGE
+    _flash_load();
+#else
+    _mtd_load();
 #endif
-            } else {
-                _upgrade_to_mtd();
-            }
-        }
-
-        // we write the signature every time, even if it already is
-        // good, as this gives us a way to detect if the MTD device is
-        // functional. It is better to panic now than to fail to save
-        // parameters in flight
-        _mtd_write_signature();
-
-	_dirty_mask = 0;
-	int fd = open(MTD_PARAMS_FILE, O_RDONLY);
-	if (fd == -1) {
-            AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
-	}
-        const uint16_t chunk_size = 128;
-        for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
-            bus_lock(true);
-            ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
-            bus_lock(false);
-            if (ret != chunk_size) {
-                ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
-                         (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
-                AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
-            }
-	}
-	close(fd);
 
 #ifdef SAVE_STORAGE_FILE
     fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
@@ -195,10 +73,10 @@ void VRBRAINStorage::_storage_open(void)
 void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length)
 {
     uint16_t end = loc + length;
-    for (uint8_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT;
+    for (uint16_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT;
          line <= end>>VRBRAIN_STORAGE_LINE_SHIFT;
          line++) {
-        _dirty_mask |= 1U << line;
+        _dirty_mask.set(line);
     }
 }
 
@@ -221,35 +99,16 @@ void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n)
         memcpy(&_buffer[loc], src, n);
         _mark_dirty(loc, n);
     }
-}
-
-void VRBRAINStorage::bus_lock(bool lock)
-{
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 }
 
 void VRBRAINStorage::_timer_tick(void)
 {
-	if (!_initialised || _dirty_mask == 0) {
+    if (!_initialised || _dirty_mask.empty()) {
         return;
     }
     perf_begin(_perf_storage);
 
+#if !USE_FLASH_STORAGE
     if (_fd == -1) {
         _fd = open(MTD_PARAMS_FILE, O_WRONLY);
         if (_fd == -1) {
@@ -258,12 +117,13 @@ void VRBRAINStorage::_timer_tick(void)
             return;
         }
     }
+#endif
 
-	// write out the first dirty set of lines. We don't write more
+    // write out the first dirty line. We don't write more
     // than one to keep the latency of this call to a minimum
-	uint8_t i, n;
+    uint16_t i;
     for (i=0; i<VRBRAIN_STORAGE_NUM_LINES; i++) {
-		if (_dirty_mask & (1<<i)) {
+        if (_dirty_mask.get(i)) {
             break;
         }
     }
@@ -273,37 +133,183 @@ void VRBRAINStorage::_timer_tick(void)
         perf_count(_perf_errors);
         return;
     }
-	uint32_t write_mask = (1U<<i);
-	// see how many lines to write
-	for (n=1; (i+n) < VRBRAIN_STORAGE_NUM_LINES &&
-		     n < (VRBRAIN_STORAGE_MAX_WRITE>>VRBRAIN_STORAGE_LINE_SHIFT); n++) {
-		if (!(_dirty_mask & (1<<(n+i)))) {
-			break;
-		}		
-		// mark that line clean
-		write_mask |= (1<<(n+i));
+
+    // save to storage backend
+#if USE_FLASH_STORAGE
+    _flash_write(i);
+#else
+    _mtd_write(i);
+#endif
+    
+    perf_end(_perf_storage);
 }
 
+#if !USE_FLASH_STORAGE
+void VRBRAINStorage::bus_lock(bool lock)
+{
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
     /*
-	  write the lines. This also updates _dirty_mask. Note that
-	  because this is a SCHED_FIFO thread it will not be preempted
-	  by the main task except during blocking calls. This means we
-	  don't need a semaphore around the _dirty_mask updates.
+      this is needed on Pixracer where the ms5611 may be on the same
+      bus as FRAM, and the NuttX ramtron driver does not go via the
+      PX4 spi bus abstraction. The ramtron driver relies on
+      SPI_LOCK(). We need to prevent the ms5611 reads which happen in
+      interrupt context from interfering with the FRAM operations. As
+      the px4 spi bus abstraction just uses interrupt blocking as the
+      locking mechanism we need to block interrupts here as well.
     */
-	if (lseek(_fd, i<<VRBRAIN_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<VRBRAIN_STORAGE_LINE_SHIFT)) {
-		_dirty_mask &= ~write_mask;
+    if (lock) {
+        irq_state = irqsave();
+    } else {
+        irqrestore(irq_state);
+    }
+#endif
+}
+
+
+/*
+  write one storage line. This also updates _dirty_mask. 
+*/
+void VRBRAINStorage::_mtd_write(uint16_t line)
+{
+    uint16_t ofs = line * VRBRAIN_STORAGE_LINE_SIZE;
+    if (lseek(_fd, ofs, SEEK_SET) != ofs) {
+        return;
+    }
+
+    // mark the line clean
+    _dirty_mask.clear(line);
+    
     bus_lock(true);
-		ssize_t ret = write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT);
+    ssize_t ret = write(_fd, &_buffer[ofs], VRBRAIN_STORAGE_LINE_SIZE);
     bus_lock(false);
-                if (ret != n<<VRBRAIN_STORAGE_LINE_SHIFT) {
+
+    if (ret != VRBRAIN_STORAGE_LINE_SIZE) {
         // write error - likely EINTR
-                    _dirty_mask |= write_mask;
+        _dirty_mask.set(line);
         close(_fd);
         _fd = -1;
         perf_count(_perf_errors);
     }
 }
-	perf_end(_perf_storage);
+
+/*
+  load all data from mtd
+ */
+void VRBRAINStorage::_mtd_load(void)
+{
+    if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "start " MTD_PARAMS_FILE)) {
+        printf("mtd: started OK\n");
+        if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "readtest " MTD_PARAMS_FILE)) {
+            printf("mtd: readtest OK\n");
+        } else {
+            AP_BoardConfig::sensor_config_error("mtd: failed readtest");
+        }
+    } else {
+        AP_BoardConfig::sensor_config_error("mtd: failed start");
     }
 
+    int fd = open(MTD_PARAMS_FILE, O_RDONLY);
+    if (fd == -1) {
+        AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
+    }
+    const uint16_t chunk_size = 128;
+    for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
+        bus_lock(true);
+        ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
+        bus_lock(false);
+        if (ret != chunk_size) {
+            ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
+                     (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
+            AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
+        }
+    }
+    close(fd);
+}
+
+#else // USE_FLASH_STORAGE
+
+/*
+  load all data from flash
+ */
+void VRBRAINStorage::_flash_load(void)
+{
+    _flash_page = up_progmem_npages() - 2;
+    if (up_progmem_pagesize(_flash_page) != 128*1024U ||
+        up_progmem_pagesize(_flash_page+1) != 128*1024U) {
+        AP_HAL::panic("Bad flash page sizes %u %u",
+                      up_progmem_pagesize(_flash_page),
+                      up_progmem_pagesize(_flash_page+1));
+    }
+        
+    printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
+    
+    if (!_flash.init()) {
+        AP_HAL::panic("unable to init flash storage");
+    }
+}
+
+/*
+  write one storage line. This also updates _dirty_mask. 
+*/
+void VRBRAINStorage::_flash_write(uint16_t line)
+{
+    if (_flash.write(line*VRBRAIN_STORAGE_LINE_SIZE, VRBRAIN_STORAGE_LINE_SIZE)) {
+        // mark the line clean
+        _dirty_mask.clear(line);
+    } else {
+        perf_count(_perf_errors);
+    }
+}
+
+/*
+  callback to write data to flash
+ */
+bool VRBRAINStorage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
+{
+    size_t base_address = up_progmem_getaddress(_flash_page+sector);
+    bool ret = up_progmem_write(base_address+offset, data, length) == length;
+    if (!ret && _flash_erase_ok()) {
+        // we are getting flash write errors while disarmed. Try
+        // re-writing all of flash
+        uint32_t now = AP_HAL::millis();
+        if (now - _last_re_init_ms > 5000) {
+            _last_re_init_ms = now;
+            bool ok = _flash.re_initialise();
+            printf("Storage: failed at %u:%u for %u - re-init %u\n",
+                   sector, offset, length, (unsigned)ok);
+        }
+    }
+    return ret;
+}
+
+/*
+  callback to read data from flash
+ */
+bool VRBRAINStorage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
+{
+    size_t base_address = up_progmem_getaddress(_flash_page+sector);
+    const uint8_t *b = ((const uint8_t *)base_address)+offset;
+    memcpy(data, b, length);
+    return true;
+}
+
+/*
+  callback to erase flash sector
+ */
+bool VRBRAINStorage::_flash_erase_sector(uint8_t sector)
+{
+    return up_progmem_erasepage(_flash_page+sector) > 0;
+}
+
+/*
+  callback to check if erase is allowed
+ */
+bool VRBRAINStorage::_flash_erase_ok(void)
+{
+    // only allow erase while disarmed
+    return !hal.util->get_soft_armed();
+}
+#endif // USE_FLASH_STORAGE
+
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/Storage.h

@@ -1,12 +1,29 @@
 #pragma once
 
+/*
+  we can optionally use flash for storage instead of FRAM. That allows
+  ArduPilot to run on a wider range of boards and reduces board cost
+ */
+#ifndef USE_FLASH_STORAGE
+#define USE_FLASH_STORAGE 0
+#endif
+
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_VRBRAIN_Namespace.h"
 #include <systemlib/perf_counter.h>
+#include <AP_Common/Bitmask.h>
+#include <AP_FlashStorage/AP_FlashStorage.h>
 
 #define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE
-#define VRBRAIN_STORAGE_MAX_WRITE 512
+
+#if USE_FLASH_STORAGE
+// when using flash storage we use a small line size to make storage
+// compact and minimise the number of erase cycles needed
+#define VRBRAIN_STORAGE_LINE_SHIFT 3
+#else
 #define VRBRAIN_STORAGE_LINE_SHIFT 9
+#endif
+
 #define VRBRAIN_STORAGE_LINE_SIZE (1<<VRBRAIN_STORAGE_LINE_SHIFT)
 #define VRBRAIN_STORAGE_NUM_LINES (VRBRAIN_STORAGE_SIZE/VRBRAIN_STORAGE_LINE_SIZE)
 
@@ -21,22 +38,41 @@ public:
     void _timer_tick(void);
 
 private:
-    int _fd;
     volatile bool _initialised;
     void _storage_create(void);
     void _storage_open(void);
     void _mark_dirty(uint16_t loc, uint16_t length);
     uint8_t _buffer[VRBRAIN_STORAGE_SIZE] __attribute__((aligned(4)));
-    volatile uint32_t _dirty_mask;
+    Bitmask _dirty_mask{VRBRAIN_STORAGE_NUM_LINES};
     perf_counter_t  _perf_storage;
     perf_counter_t  _perf_errors;
-    bool _have_mtd;
-    void _upgrade_to_mtd(void);
-    uint32_t _mtd_signature(void);
-    void _mtd_write_signature(void);
-
-
-
+    uint32_t _last_re_init_ms;
 
+#if !USE_FLASH_STORAGE
+    int _fd = -1;
     void bus_lock(bool lock);
+    void _mtd_load(void);
+    void _mtd_write(uint16_t line);
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+    irqstate_t irq_state;
+#endif
+
+#else // USE_FLASH_STORAGE
+    bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length);
+    bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length);
+    bool _flash_erase_sector(uint8_t sector);
+    bool _flash_erase_ok(void);
+    uint8_t _flash_page;
+    bool _flash_failed;
+    
+    AP_FlashStorage _flash{_buffer,
+            128*1024U, 
+            FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_erase_sector, bool, uint8_t),
+            FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_erase_ok, bool)};
+    
+    void _flash_load(void);
+    void _flash_write(uint16_t line);
+#endif
 };

libraries/AP_HAL_VRBRAIN/UARTDriver.cpp

@@ -26,6 +26,7 @@ VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name)
     _baudrate(57600),
     _initialised(false),
     _in_timer(false),
+    _unbuffered_writes(false),
     _perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
     _os_start_auto_space(-1),
     _flow_control(FLOW_CONTROL_DISABLE)
@@ -69,7 +70,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
       thrashing of the heap once we are up. The ttyACM0 driver may not
       connect for some time after boot
      */
-    if (rxS != 0 && rxS != _readbuf.get_size()) {
+    if (rxS != _readbuf.get_size()) {
         _initialised = false;
         while (_in_timer) {
             hal.scheduler->delay(1);
@@ -85,7 +86,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
     /*
       allocate the write buffer
      */
-    if (txS != 0 && txS != _writebuf.get_size()) {
+    if (txS != _writebuf.get_size()) {
         _initialised = false;
         while (_in_timer) {
             hal.scheduler->delay(1);
@@ -128,8 +129,6 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         }
         _initialised = true;
     }
-    _uart_owner_pid = getpid();
-
 }
 
 void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
@@ -154,6 +153,44 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
     _flow_control = fcontrol;
 }
 
+void VRBRAINUARTDriver::configure_parity(uint8_t v) {
+    if (_fd == -1) {
+        return;
+    }
+    struct termios t;
+    tcgetattr(_fd, &t);
+    if (v != 0) {
+        // enable parity
+        t.c_cflag |= PARENB;
+        if (v == 1) {
+            t.c_cflag |= PARODD;
+        } else {
+            t.c_cflag &= ~PARODD;
+        }
+    }
+    else {
+        // disable parity
+        t.c_cflag &= ~PARENB;
+    }
+    tcsetattr(_fd, TCSANOW, &t);
+}
+
+void VRBRAINUARTDriver::set_stop_bits(int n) {
+    if (_fd == -1) {
+        return;
+    }
+    struct termios t;
+    tcgetattr(_fd, &t);
+    if (n > 1) t.c_cflag |= CSTOPB;
+    else t.c_cflag &= ~CSTOPB;
+    tcsetattr(_fd, TCSANOW, &t);
+}
+
+bool VRBRAINUARTDriver::set_unbuffered_writes(bool on) {
+    _unbuffered_writes = on;
+    return _unbuffered_writes;
+}
+
 void VRBRAINUARTDriver::begin(uint32_t b)
 {
 	begin(b, 0, 0);
@@ -240,61 +277,81 @@ uint32_t VRBRAINUARTDriver::txspace()
  */
 int16_t VRBRAINUARTDriver::read()
 {
-    if (_uart_owner_pid != getpid()){
+    if (!_semaphore.take_nonblocking()) {
         return -1;
     }
     if (!_initialised) {
         try_initialise();
+        _semaphore.give();
         return -1;
     }
 
     uint8_t byte;
-    if (!_readbuf.read_byte(&byte))
+    if (!_readbuf.read_byte(&byte)) {
+        _semaphore.give();
         return -1;
+    }
 
+    _semaphore.give();
     return byte;
 }
 
 /*
-   write one byte to the buffer
+   write one byte
  */
 size_t VRBRAINUARTDriver::write(uint8_t c)
 {
-    if (_uart_owner_pid != getpid()){
-        return 0;
+    if (!_semaphore.take_nonblocking()) {
+        return -1;
     }
     if (!_initialised) {
         try_initialise();
+        _semaphore.give();
         return 0;
     }
 
+    if (_unbuffered_writes) {
+        // write one byte to the file descriptor
+        return _write_fd(&c, 1);
+    }
+
     while (_writebuf.space() == 0) {
         if (_nonblocking_writes) {
+            _semaphore.give();
             return 0;
         }
+        _semaphore.give();
         hal.scheduler->delay(1);
+        if (!_semaphore.take_nonblocking()) {
+            return -1;
         }
-    return _writebuf.write(&c, 1);
+    }
+    size_t ret = _writebuf.write(&c, 1);
+    _semaphore.give();
+    return ret;
 }
 
 /*
-  write size bytes to the write buffer
+ * write size bytes
  */
 size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
 {
-    if (_uart_owner_pid != getpid()){
-        return 0;
+    if (!_semaphore.take_nonblocking()) {
+        return -1;
     }
     if (!_initialised) {
         try_initialise();
+        _semaphore.give();
 		return 0;
 	}
 
+    size_t ret = 0;
+    
     if (!_nonblocking_writes) {
+        _semaphore.give();
         /*
           use the per-byte delay loop in write() above for blocking writes
          */
-        size_t ret = 0;
         while (size--) {
             if (write(*buffer++) != 1) break;
             ret++;
@@ -302,7 +359,14 @@ size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
         return ret;
     }
 
-    return _writebuf.write(buffer, size);
+    if (_unbuffered_writes) {
+        // write buffer straight to the file descriptor
+        ret = _write_fd(buffer, size);
+    } else {
+        ret = _writebuf.write(buffer, size);
+    }
+    _semaphore.give();
+    return ret;
 }
 
 /*

libraries/AP_HAL_VRBRAIN/UARTDriver.h

@@ -1,9 +1,10 @@
 #pragma once
 
 #include <AP_HAL/utility/RingBuffer.h>
-#include <systemlib/perf_counter.h>
 
 #include "AP_HAL_VRBRAIN.h"
+#include <systemlib/perf_counter.h>
+#include "Semaphores.h"
 
 class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver {
 public:
@@ -39,6 +40,10 @@ public:
     void set_flow_control(enum flow_control flow_control);
     enum flow_control get_flow_control(void) { return _flow_control; }
 
+    void configure_parity(uint8_t v);
+    void set_stop_bits(int n);
+    bool set_unbuffered_writes(bool on);
+
 private:
     const char *_devpath;
     int _fd;
@@ -47,12 +52,12 @@ private:
     volatile bool _in_timer;
 
     bool _nonblocking_writes;
+    bool _unbuffered_writes;
 
     // we use in-task ring buffers to reduce the system call cost
     // of ::read() and ::write() in the main loop
-    ByteBuffer _readbuf;
-    ByteBuffer _writebuf;
-
+    ByteBuffer _readbuf{0};
+    ByteBuffer _writebuf{0};
     perf_counter_t  _perf_uart;
 
     int _write_fd(const uint8_t *buf, uint16_t n);
@@ -68,6 +73,5 @@ private:
     uint32_t _total_written;
     enum flow_control _flow_control;
 
-    pid_t _uart_owner_pid;
-
+    Semaphore _semaphore;
 };

libraries/AP_HAL_VRBRAIN/Util.cpp

@@ -73,6 +73,10 @@ bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream)
  */
 enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void)
 {
+#if !HAL_HAVE_SAFETY_SWITCH
+    return AP_HAL::Util::SAFETY_NONE;
+#endif
+
     if (_safety_handle == -1) {
         _safety_handle = orb_subscribe(ORB_ID(safety));
     }
@@ -96,7 +100,7 @@ void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec)
 {
     timespec ts;
     ts.tv_sec = time_utc_usec/1000000ULL;
-    ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000UL;
+    ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL;
     clock_settime(CLOCK_REALTIME, &ts);    
 }
 
@@ -202,8 +206,12 @@ void VRBRAINUtil::set_imu_temp(float current)
     // experimentally tweaked for Pixhawk2
     const float kI = 0.3f;
     const float kP = 200.0f;
+    float target = (float)(*_heater.target);
 
-    float err = ((float)*_heater.target) - current;
+    // limit to 65 degrees to prevent damage
+    target = constrain_float(target, 0, 65);
+    
+    float err = target - current;
 
     _heater.integrator += kI * err;
     _heater.integrator = constrain_float(_heater.integrator, 0, 70);
@@ -226,4 +234,39 @@ void VRBRAINUtil::set_imu_target_temp(int8_t *target)
     _heater.target = target;
 }
 
+
+extern "C" {
+    extern void *fat_dma_alloc(size_t);
+    extern void fat_dma_free(void *, size_t);
+}
+
+/*
+  allocate DMA-capable memory if possible. Otherwise return normal
+  memory.
+*/
+void *VRBRAINUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type)
+{
+
+
+
+
+
+
+
+    return calloc(1, size);
+
+}
+void VRBRAINUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
+{
+
+
+
+
+
+
+
+    return free(ptr);
+
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

libraries/AP_HAL_VRBRAIN/Util.h

@@ -61,6 +61,10 @@ public:
     void set_imu_temp(float current) override;
     void set_imu_target_temp(int8_t *target) override;
 
+    // allocate and free DMA-capable memory if possible. Otherwise return normal memory
+    void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override;
+    void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
+
 private:
     int _safety_handle;
     VRBRAIN::NSHShellStream _shell_stream;

libraries/AP_HAL_VRBRAIN/bxcan.h

@@ -0,0 +1,294 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Bit definitions were copied from NuttX STM32 CAN driver.
+ *
+ * With modifications for Ardupilot CAN driver
+ * Copyright (C) 2017 Eugene Shamaev
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <chip/stm32_tim.h>
+#include <syslog.h>
+#include <nuttx/config.h>
+#include <nuttx/fs/fs.h>
+#include <nuttx/irq.h>
+#include <nuttx/mm.h>
+#include <pthread.h>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+// #undef'ed at the end of this file
+# define constexpr const
+#endif
+
+namespace VRBRAIN {
+namespace bxcan {
+
+# define CAN_IRQ_ATTACH(irq, handler)                          \
+   do {                                                      \
+        const int res = irq_attach(irq, handler);          \
+        (void)res;                                         \
+        assert(res >= 0);                                  \
+        up_enable_irq(irq);                                \
+    } while(0)
+
+struct TxMailboxType {
+    volatile uint32_t TIR;
+    volatile uint32_t TDTR;
+    volatile uint32_t TDLR;
+    volatile uint32_t TDHR;
+};
+
+struct RxMailboxType {
+    volatile uint32_t RIR;
+    volatile uint32_t RDTR;
+    volatile uint32_t RDLR;
+    volatile uint32_t RDHR;
+};
+
+struct FilterRegisterType {
+    volatile uint32_t FR1;
+    volatile uint32_t FR2;
+};
+
+struct CanType {
+    volatile uint32_t MCR; /*!< CAN master control register,         Address offset: 0x00          */
+    volatile uint32_t MSR; /*!< CAN master status register,          Address offset: 0x04          */
+    volatile uint32_t TSR; /*!< CAN transmit status register,        Address offset: 0x08          */
+    volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register,         Address offset: 0x0C          */
+    volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register,         Address offset: 0x10          */
+    volatile uint32_t IER; /*!< CAN interrupt enable register,       Address offset: 0x14          */
+    volatile uint32_t ESR; /*!< CAN error status register,           Address offset: 0x18          */
+    volatile uint32_t BTR; /*!< CAN bit timing register,             Address offset: 0x1C          */
+    uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F                                            */
+    TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox,                      Address offset: 0x180 - 0x1AC */
+    RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox,                    Address offset: 0x1B0 - 0x1CC */
+    uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF                                            */
+    volatile uint32_t FMR; /*!< CAN filter master register,          Address offset: 0x200         */
+    volatile uint32_t FM1R; /*!< CAN filter mode register,            Address offset: 0x204         */
+    uint32_t RESERVED2; /*!< Reserved, 0x208                                                    */
+    volatile uint32_t FS1R; /*!< CAN filter scale register,           Address offset: 0x20C         */
+    uint32_t RESERVED3; /*!< Reserved, 0x210                                                    */
+    volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214         */
+    uint32_t RESERVED4; /*!< Reserved, 0x218                                                    */
+    volatile uint32_t FA1R; /*!< CAN filter activation register,      Address offset: 0x21C         */
+    uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F                                              */
+    FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register,                 Address offset: 0x240-0x31C   */
+};
+
+/**
+ * CANx register sets
+ */
+CanType* const Can[2] = { reinterpret_cast<CanType*>(STM32_CAN1_BASE), reinterpret_cast<CanType*>(STM32_CAN2_BASE) };
+
+/* CAN master control register */
+
+constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
+constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
+constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
+constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
+constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
+constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
+constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
+constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
+constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */
+constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */
+
+/* CAN master status register */
+
+constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
+constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
+constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
+constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
+constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
+constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
+constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
+constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */
+constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */
+
+/* CAN transmit status register */
+
+constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
+constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
+constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
+constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
+constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
+constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
+constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
+constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */
+constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */
+constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */
+constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */
+constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */
+constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */
+constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */
+constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */
+constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
+constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
+constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */
+constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */
+constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */
+constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */
+constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */
+constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */
+
+/* CAN receive FIFO 0/1 registers */
+
+constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
+constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
+constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
+constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
+constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
+
+/* CAN interrupt enable register */
+
+constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
+constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
+constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
+constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
+constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
+constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
+constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
+constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
+constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
+constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */
+constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */
+constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */
+constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */
+constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */
+
+/* CAN error status register */
+
+constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
+constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
+constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
+constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
+constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
+constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */
+constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */
+constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */
+constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */
+constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */
+constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */
+constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */
+constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */
+constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
+constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
+constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
+constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
+
+/* CAN bit timing register */
+
+constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
+constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
+constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
+constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
+constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
+constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
+constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
+constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
+constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/
+constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/
+
+constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
+constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
+constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
+
+/* TX mailbox identifier register */
+
+constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
+constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
+constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
+constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
+constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
+constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
+constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
+
+/* Mailbox data length control and time stamp register */
+
+constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
+constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
+constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
+constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
+constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
+
+/* Mailbox data low register */
+
+constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
+constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
+constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
+constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
+constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
+constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
+constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
+constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
+
+/* Mailbox data high register */
+
+constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
+constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
+constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
+constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
+constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
+constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
+constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
+constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
+
+/* Rx FIFO mailbox identifier register */
+
+constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
+constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
+constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
+constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
+constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
+constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
+
+/* Receive FIFO mailbox data length control and time stamp register */
+
+constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
+constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
+constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
+constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
+constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
+constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
+
+/* Receive FIFO mailbox data low register */
+
+constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
+constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
+constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
+constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
+constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
+constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
+constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
+constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
+
+/* Receive FIFO mailbox data high register */
+
+constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
+constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
+constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
+constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
+constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
+constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
+constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
+constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
+
+/* CAN filter master register */
+
+constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0:  Filter Init Mode */
+}
+}
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+# undef constexpr
+#endif

libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp

@@ -16,17 +16,16 @@
 
 #include "uORB/uORB.h"
 #include "uORB/topics/parameter_update.h"
-#include <uORB/topics/uavcan_parameter_request.h>
-#include <uORB/topics/uavcan_parameter_value.h>
         
 /** parameter update topic */
 ORB_DEFINE(parameter_update, struct parameter_update_s);
-ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
-ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
 
 param_t param_find(const char *name)
 {
+#if 0
+    // useful for driver debugging
     ::printf("VRBRAIN: param_find(%s)\n", name);
+#endif
     return PARAM_INVALID;
 }