AP_HAL: New VRBRAIN board definition and libraries

libraries/AP_HAL/AP_HAL_Boards.h

@@ -17,6 +17,7 @@
 #define HAL_BOARD_PX4      5
 #define HAL_BOARD_FLYMAPLE 6
 #define HAL_BOARD_LINUX    7
+#define HAL_BOARD_VRBRAIN  8
 #define HAL_BOARD_EMPTY    99
 
 
@@ -101,6 +102,14 @@
 #define HAL_STORAGE_SIZE            4096
 #define HAL_STORAGE_SIZE_AVAILABLE  HAL_STORAGE_SIZE
 
+#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#define AP_HAL_BOARD_DRIVER AP_HAL_VRBRAIN
+#define HAL_BOARD_NAME "VRBRAIN"
+#define HAL_CPU_CLASS HAL_CPU_CLASS_150
+#define HAL_OS_POSIX_IO 1
+#define HAL_STORAGE_SIZE            4096
+#define HAL_STORAGE_SIZE_AVAILABLE  HAL_STORAGE_SIZE
+
 #else
 #error "Unknown CONFIG_HAL_BOARD type"
 #endif

libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h

@@ -0,0 +1,13 @@
+
+#ifndef __AP_HAL_VRBRAIN_H__
+#define __AP_HAL_VRBRAIN_H__
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "HAL_VRBRAIN_Class.h"
+#include "AP_HAL_VRBRAIN_Main.h"
+
+#endif // CONFIG_HAL_BOARD
+#endif // __AP_HAL_VRBRAIN_H__
+

libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Main.h

@@ -0,0 +1,14 @@
+#ifndef __AP_HAL_VRBRAIN_MAIN_H__
+#define __AP_HAL_VRBRAIN_MAIN_H__
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#define AP_HAL_MAIN() \
+    extern "C" __EXPORT int SKETCH_MAIN(int argc, char * const argv[]); \
+    int SKETCH_MAIN(int argc, char * const argv[]) {	\
+	hal.init(argc, argv); \
+	return OK; \
+    }
+
+#endif
+#endif // __AP_HAL_VRBRAIN_MAIN_H__

libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h

@@ -0,0 +1,19 @@
+
+#ifndef __AP_HAL_VRBRAIN_NAMESPACE_H__
+#define __AP_HAL_VRBRAIN_NAMESPACE_H__
+
+namespace VRBRAIN {
+	class VRBRAINScheduler;
+	class VRBRAINUARTDriver;
+	class VRBRAINStorage;
+	class VRBRAINRCInput;
+	class VRBRAINRCOutput;
+	class VRBRAINAnalogIn;
+	class VRBRAINAnalogSource;
+	class VRBRAINUtil;
+        class VRBRAINGPIO;
+        class VRBRAINDigitalSource;
+}
+
+#endif //__AP_HAL_VRBRAIN_NAMESPACE_H__
+

libraries/AP_HAL_VRBRAIN/AnalogIn.cpp

@@ -0,0 +1,320 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AnalogIn.h"
+#include <drivers/drv_adc.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/config.h>
+#include <arch/board/board.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
+#include <uORB/topics/system_power.h>
+#include <GCS_MAVLink.h>
+#include <errno.h>
+
+#define ANLOGIN_DEBUGGING 0
+
+// base voltage scaling for 12 bit 3.3V ADC
+#define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f)
+
+#if ANALOGIN_DEBUGGING
+ # define Debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  scaling table between ADC count and actual input voltage, to account
+  for voltage dividers on the board. 
+ */
+static const struct {
+    uint8_t pin;
+    float scaling;
+} pin_scaling[] = {
+#ifdef CONFIG_ARCH_BOARD_VRBRAIN_V4
+    { 0,  3.3f/4096 },
+    { 10, 3.3f/4096 },
+#elif CONFIG_ARCH_BOARD_VRBRAIN_V5
+    { 0,  3.3f/4096 },
+    { 10, 3.3f/4096 },
+    { 11, 3.3f/4096 },
+#elif CONFIG_ARCH_BOARD_VRHERO_V1
+    { 10, 3.3f/4096 },
+    { 11, 3.3f/4096 },
+    { 14, 3.3f/4096 },
+    { 15, 3.3f/4096 },
+#else
+#error "Unknown board type for AnalogIn scaling"
+#endif
+};
+
+using namespace VRBRAIN;
+
+VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) :
+	_pin(pin),
+    _value(initial_value),
+    _value_ratiometric(initial_value),
+    _latest_value(initial_value),
+    _sum_count(0),
+    _sum_value(0),
+    _sum_ratiometric(0)
+{
+
+
+
+
+
+}
+
+float VRBRAINAnalogSource::read_average()
+{
+    if (_sum_count == 0) {
+        return _value;
+    }
+    hal.scheduler->suspend_timer_procs();
+    _value = _sum_value / _sum_count;
+    _value_ratiometric = _sum_ratiometric / _sum_count;
+    _sum_value = 0;
+    _sum_ratiometric = 0;
+    _sum_count = 0;
+    hal.scheduler->resume_timer_procs();
+    return _value;
+}
+
+float VRBRAINAnalogSource::read_latest()
+{
+    return _latest_value;
+}
+
+/*
+  return scaling from ADC count to Volts
+ */
+float VRBRAINAnalogSource::_pin_scaler(void)
+{
+    float scaling = VRBRAIN_VOLTAGE_SCALING;
+    uint8_t num_scalings = sizeof(pin_scaling)/sizeof(pin_scaling[0]);
+    for (uint8_t i=0; i<num_scalings; i++) {
+        if (pin_scaling[i].pin == _pin) {
+            scaling = pin_scaling[i].scaling;
+            break;
+        }
+    }
+    return scaling;
+}
+
+/*
+  return voltage in Volts
+ */
+float VRBRAINAnalogSource::voltage_average()
+{
+    return _pin_scaler() * read_average();
+}
+
+/*
+  return voltage in Volts, assuming a ratiometric sensor powered by
+  the 5V rail
+ */
+float VRBRAINAnalogSource::voltage_average_ratiometric()
+{
+    voltage_average();
+    return _pin_scaler() * _value_ratiometric;
+}
+
+/*
+  return voltage in Volts
+ */
+float VRBRAINAnalogSource::voltage_latest()
+{
+    return _pin_scaler() * read_latest();
+}
+
+void VRBRAINAnalogSource::set_pin(uint8_t pin)
+{
+    if (_pin == pin) {
+        return;
+    }
+    hal.scheduler->suspend_timer_procs();
+    _pin = pin;
+    _sum_value = 0;
+    _sum_ratiometric = 0;
+    _sum_count = 0;
+    _latest_value = 0;
+    _value = 0;
+    _value_ratiometric = 0;
+    hal.scheduler->resume_timer_procs();
+}
+
+/*
+  apply a reading in ADC counts
+ */
+void VRBRAINAnalogSource::_add_value(float v, float vcc5V)
+{
+    _latest_value = v;
+    _sum_value += v;
+    if (vcc5V < 3.0f) {
+        _sum_ratiometric += v;
+    } else {
+        // this compensates for changes in the 5V rail relative to the
+        // 3.3V reference used by the ADC.
+        _sum_ratiometric += v * 5.0f / vcc5V;
+    }
+    _sum_count++;
+    if (_sum_count == 254) {
+        _sum_value /= 2;
+        _sum_ratiometric /= 2;
+        _sum_count /= 2;
+    }
+}
+
+
+VRBRAINAnalogIn::VRBRAINAnalogIn() :
+	_board_voltage(0),
+    _servorail_voltage(0),
+    _power_flags(0)    
+{}
+
+void VRBRAINAnalogIn::init(void* machtnichts)
+{
+	_adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
+    if (_adc_fd == -1) {
+        hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH);
+	}
+    _battery_handle   = orb_subscribe(ORB_ID(battery_status));
+    _servorail_handle = orb_subscribe(ORB_ID(servorail_status));
+    _system_power_handle = orb_subscribe(ORB_ID(system_power));
+}
+
+/*
+  called at 1kHz
+ */
+void VRBRAINAnalogIn::_timer_tick(void)
+{
+    // read adc at 100Hz
+    uint32_t now = hal.scheduler->micros();
+    uint32_t delta_t = now - _last_run;
+    if (delta_t < 10000) {
+        return;
+    }
+    _last_run = now;
+
+    struct adc_msg_s buf_adc[VRBRAIN_ANALOG_MAX_CHANNELS];
+
+    /* read all channels available */
+    int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc));
+    if (ret > 0) {
+        // match the incoming channels to the currently active pins
+        for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
+
+
+
+
+
+
+
+        }
+        for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
+            Debug("chan %u value=%u\n",
+                  (unsigned)buf_adc[i].am_channel,
+                  (unsigned)buf_adc[i].am_data);
+            for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
+                VRBRAIN::VRBRAINAnalogSource *c = _channels[j];
+                if (c != NULL && buf_adc[i].am_channel == c->_pin) {
+                    c->_add_value(buf_adc[i].am_data, _board_voltage);
+                }
+            }
+        }
+    }
+
+
+    // check for new battery data on FMUv1
+    if (_battery_handle != -1) {
+        struct battery_status_s battery;
+        bool updated = false;
+        if (orb_check(_battery_handle, &updated) == 0 && updated) {
+            orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
+            if (battery.timestamp != _battery_timestamp) {
+                _battery_timestamp = battery.timestamp;
+                for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
+                    VRBRAIN::VRBRAINAnalogSource *c = _channels[j];
+                    if (c == NULL) continue;
+                    if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
+                        c->_add_value(battery.voltage_v / VRBRAIN_VOLTAGE_SCALING, 0);
+                    }
+                    if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN) {
+                        // scale it back to voltage, knowing that the
+                        // px4io code scales by 90.0/5.0
+                        c->_add_value(battery.current_a * (5.0f/90.0f) / VRBRAIN_VOLTAGE_SCALING, 0);
+                    }
+                }
+            }
+        }
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+}
+
+AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin)
+{
+    for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
+        if (_channels[j] == NULL) {
+            _channels[j] = new VRBRAINAnalogSource(pin, 0.0);
+            return _channels[j];
+        }
+    }
+    hal.console->println("Out of analog channels");
+    return NULL;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/AnalogIn.h

@@ -0,0 +1,78 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_HAL_VRBRAIN_ANALOGIN_H__
+#define __AP_HAL_VRBRAIN_ANALOGIN_H__
+
+#include <AP_HAL_VRBRAIN.h>
+#include <pthread.h>
+#include <uORB/uORB.h>
+
+#define VRBRAIN_ANALOG_MAX_CHANNELS 16
+
+
+#if  defined(CONFIG_ARCH_BOARD_VRBRAIN_V4)
+// these are virtual pins that read from the ORB
+#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN     100
+#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN     101
+#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V5)
+#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN     100
+#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN     101
+#elif defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN     100
+#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN     101
+#endif
+
+class VRBRAIN::VRBRAINAnalogSource : public AP_HAL::AnalogSource {
+public:
+    friend class VRBRAIN::VRBRAINAnalogIn;
+    VRBRAINAnalogSource(int16_t pin, float initial_value);
+    float read_average();
+    float read_latest();
+    void set_pin(uint8_t p);
+    float voltage_average();
+    float voltage_latest();
+    float voltage_average_ratiometric();
+
+    // stop pins not implemented on VRBRAIN yet
+    void set_stop_pin(uint8_t p) {}
+    void set_settle_time(uint16_t settle_time_ms) {}
+
+private:
+    // what pin it is attached to
+    int16_t _pin;
+
+    // what value it has
+    float _value;
+    float _value_ratiometric;
+    float _latest_value;
+    uint8_t _sum_count;
+    float _sum_value;
+    float _sum_ratiometric;
+    void _add_value(float v, float vcc5V);
+    float _pin_scaler();
+};
+
+class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn {
+public:
+    VRBRAINAnalogIn();
+    void init(void* implspecific);
+    AP_HAL::AnalogSource* channel(int16_t pin);
+    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; }
+
+private:
+    int _adc_fd;
+    int _battery_handle;
+    int _servorail_handle;
+    int _system_power_handle;
+    uint64_t _battery_timestamp;
+    uint64_t _servorail_timestamp;
+    VRBRAIN::VRBRAINAnalogSource* _channels[VRBRAIN_ANALOG_MAX_CHANNELS];
+    uint32_t _last_run;
+    float _board_voltage;
+    float _servorail_voltage;
+    uint16_t _power_flags;
+};
+#endif // __AP_HAL_VRBRAIN_ANALOGIN_H__

libraries/AP_HAL_VRBRAIN/GPIO.cpp

@@ -0,0 +1,258 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include "GPIO.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+/* VRBRAIN headers */
+#include <drivers/drv_led.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_gpio.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+
+#define LOW     0
+#define HIGH    1
+
+extern const AP_HAL::HAL& hal;
+
+using namespace VRBRAIN;
+
+VRBRAINGPIO::VRBRAINGPIO()
+{}
+
+void VRBRAINGPIO::init()
+{
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V4) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V5) || defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+    _led_fd = open(LED_DEVICE_PATH, O_RDWR);
+    if (_led_fd == -1) {
+        hal.scheduler->panic("Unable to open " LED_DEVICE_PATH);
+    }
+    if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
+        hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
+    }
+    if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) {
+         hal.console->printf("GPIO: Unable to setup GPIO LED RED\n");
+    }
+#endif
+    _tone_alarm_fd = open("/dev/tone_alarm", O_WRONLY);
+    if (_tone_alarm_fd == -1) {
+        hal.scheduler->panic("Unable to open /dev/tone_alarm");
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+}
+
+void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
+{
+    switch (pin) {
+
+
+
+    }
+}
+
+int8_t VRBRAINGPIO::analogPinToDigitalPin(uint8_t pin)
+{
+    return -1;
+}
+
+
+uint8_t VRBRAINGPIO::read(uint8_t pin) {
+
+    switch (pin) {
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    }
+    return LOW;
+}
+
+void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
+{
+    switch (pin) {
+
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V4) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V5) || defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+        case HAL_GPIO_A_LED_PIN:    // Arming LED
+            if (value == LOW) {
+                ioctl(_led_fd, LED_OFF, LED_RED);
+            } else {
+                ioctl(_led_fd, LED_ON, LED_RED);
+            }
+            break;
+
+        case HAL_GPIO_B_LED_PIN:    // not used yet 
+            break;
+
+        case HAL_GPIO_C_LED_PIN:    // GPS LED 
+            if (value == LOW) { 
+                ioctl(_led_fd, LED_OFF, LED_BLUE);
+            } else { 
+                ioctl(_led_fd, LED_ON, LED_BLUE);
+            }
+            break;
+#endif
+
+        case VRBRAIN_GPIO_PIEZO_PIN:    // Piezo beeper 
+            if (value == LOW) { // this is inverted 
+                ioctl(_tone_alarm_fd, TONE_SET_ALARM, 3);    // Alarm on !! 
+                //::write(_tone_alarm_fd, &user_tune, sizeof(user_tune));
+            } else { 
+                ioctl(_tone_alarm_fd, TONE_SET_ALARM, 0);    // Alarm off !! 
+            }
+            break;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    }
+}
+
+void VRBRAINGPIO::toggle(uint8_t pin)
+{
+    write(pin, !read(pin));
+}
+
+/* Alternative interface: */
+AP_HAL::DigitalSource* VRBRAINGPIO::channel(uint16_t n) {
+    return new VRBRAINDigitalSource(0);
+}
+
+/* Interrupt interface: */
+bool VRBRAINGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode)
+{
+    return true;
+}
+
+/*
+  return true when USB connected
+ */
+bool VRBRAINGPIO::usb_connected(void)
+{
+    return stm32_gpioread(GPIO_OTGFS_VBUS);
+}
+
+
+VRBRAINDigitalSource::VRBRAINDigitalSource(uint8_t v) :
+    _v(v)
+{}
+
+void VRBRAINDigitalSource::mode(uint8_t output)
+{}
+
+uint8_t VRBRAINDigitalSource::read() {
+    return _v;
+}
+
+void VRBRAINDigitalSource::write(uint8_t value) {
+    _v = value;
+}
+
+void VRBRAINDigitalSource::toggle() {
+    _v = !_v;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/GPIO.h

@@ -0,0 +1,67 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_HAL_VRBRAIN_GPIO_H__
+#define __AP_HAL_VRBRAIN_GPIO_H__
+
+#include <AP_HAL_VRBRAIN.h>
+
+#define VRBRAIN_GPIO_PIEZO_PIN              110
+
+
+
+
+
+
+
+
+
+
+
+
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+ # define HAL_GPIO_A_LED_PIN        27
+ # define HAL_GPIO_B_LED_PIN        26
+ # define HAL_GPIO_C_LED_PIN        25
+ # define HAL_GPIO_LED_ON           LOW
+ # define HAL_GPIO_LED_OFF          HIGH
+#endif
+
+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);
+
+    /* Alternative interface: */
+    AP_HAL::DigitalSource* channel(uint16_t n);
+
+    /* Interrupt interface: */
+    bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
+
+    /* return true if USB cable is connected */
+    bool usb_connected(void);
+
+private:
+    int _led_fd;
+    int _tone_alarm_fd;
+
+
+};
+
+class VRBRAIN::VRBRAINDigitalSource : public AP_HAL::DigitalSource {
+public:
+	VRBRAINDigitalSource(uint8_t v);
+    void    mode(uint8_t output);
+    uint8_t read();
+    void    write(uint8_t value);
+    void    toggle();
+private:
+    uint8_t _v;
+};
+
+#endif // __AP_HAL_VRBRAIN_GPIO_H__

libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp

@@ -0,0 +1,311 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include <AP_HAL_VRBRAIN.h>
+#include "AP_HAL_VRBRAIN_Namespace.h"
+#include "HAL_VRBRAIN_Class.h"
+#include "Scheduler.h"
+#include "UARTDriver.h"
+#include "Storage.h"
+#include "RCInput.h"
+#include "RCOutput.h"
+#include "AnalogIn.h"
+#include "Util.h"
+#include "GPIO.h"
+
+#include <AP_HAL_Empty.h>
+#include <AP_HAL_Empty_Private.h>
+
+#include <stdlib.h>
+#include <systemlib/systemlib.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <pthread.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+
+using namespace VRBRAIN;
+
+static Empty::EmptySemaphore  i2cSemaphore;
+static Empty::EmptyI2CDriver  i2cDriver(&i2cSemaphore);
+static Empty::EmptySPIDeviceManager spiDeviceManager;
+//static Empty::EmptyGPIO gpioDriver;
+
+static VRBRAINScheduler schedulerInstance;
+static VRBRAINStorage storageDriver;
+static VRBRAINRCInput rcinDriver;
+static VRBRAINRCOutput rcoutDriver;
+static VRBRAINAnalogIn analogIn;
+static VRBRAINUtil utilInstance;
+static VRBRAINGPIO gpioDriver;
+
+
+
+
+
+
+
+
+#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
+#define UARTB_DEFAULT_DEVICE "/dev/ttyS1"
+#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
+#define UARTD_DEFAULT_DEVICE "/dev/null"
+#define UARTE_DEFAULT_DEVICE "/dev/null"
+
+
+// 3 UART drivers, for GPS plus two mavlink-enabled devices
+static VRBRAINUARTDriver uartADriver(UARTA_DEFAULT_DEVICE, "APM_uartA");
+static VRBRAINUARTDriver uartBDriver(UARTB_DEFAULT_DEVICE, "APM_uartB");
+static VRBRAINUARTDriver uartCDriver(UARTC_DEFAULT_DEVICE, "APM_uartC");
+static VRBRAINUARTDriver uartDDriver(UARTD_DEFAULT_DEVICE, "APM_uartD");
+static VRBRAINUARTDriver uartEDriver(UARTE_DEFAULT_DEVICE, "APM_uartE");
+
+HAL_VRBRAIN::HAL_VRBRAIN() :
+    AP_HAL::HAL(
+        &uartADriver,  /* uartA */
+        &uartBDriver,  /* uartB */
+        &uartCDriver,  /* uartC */
+        &uartDDriver,  /* uartD */
+        &uartEDriver,  /* uartE */
+        &i2cDriver, /* i2c */
+        &spiDeviceManager, /* spi */
+        &analogIn, /* analogin */
+        &storageDriver, /* storage */
+        &uartADriver, /* console */
+        &gpioDriver, /* gpio */
+        &rcinDriver,  /* rcinput */
+        &rcoutDriver, /* rcoutput */
+        &schedulerInstance, /* scheduler */
+        &utilInstance) /* util */
+{}
+
+bool _vrbrain_thread_should_exit = false;        /**< Daemon exit flag */
+static bool thread_running = false;        /**< Daemon status flag */
+static int daemon_task;                /**< Handle of daemon task / thread */
+static bool ran_overtime;
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  set the priority of the main APM task
+ */
+static void set_priority(uint8_t priority)
+{
+    struct sched_param param;
+    param.sched_priority = priority;
+    sched_setscheduler(daemon_task, SCHED_FIFO, &param);    
+}
+
+/*
+  this is called when loop() takes more than 1 second to run. If that
+  happens then something is blocking for a long time in the main
+  sketch - probably waiting on a low priority driver. Set the priority
+  of the APM task low to let the driver run.
+ */
+static void loop_overtime(void *)
+{
+    set_priority(APM_OVERTIME_PRIORITY);
+    ran_overtime = true;
+}
+
+static int main_loop(int argc, char **argv)
+{
+    extern void setup(void);
+    extern void loop(void);
+
+
+    hal.uartA->begin(115200);
+    hal.uartB->begin(38400);
+    hal.uartC->begin(57600);
+    hal.uartD->begin(57600);
+    hal.uartE->begin(57600);
+    hal.scheduler->init(NULL);
+    hal.rcin->init(NULL);
+    hal.rcout->init(NULL);
+    hal.analogin->init(NULL);
+    hal.gpio->init();
+
+
+    /*
+      run setup() at low priority to ensure CLI doesn't hang the
+      system, and to allow initial sensor read loops to run
+     */
+    set_priority(APM_STARTUP_PRIORITY);
+
+    schedulerInstance.hal_initialized();
+
+    setup();
+    hal.scheduler->system_initialized();
+
+    perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
+    perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
+    struct hrt_call loop_overtime_call;
+
+    thread_running = true;
+
+    /*
+      switch to high priority for main loop
+     */
+    set_priority(APM_MAIN_PRIORITY);
+
+    while (!_vrbrain_thread_should_exit) {
+        perf_begin(perf_loop);
+        
+        /*
+          this ensures a tight loop waiting on a lower priority driver
+          will eventually give up some time for the driver to run. It
+          will only ever be called if a loop() call runs for more than
+          0.1 second
+         */
+        hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
+
+        loop();
+
+        if (ran_overtime) {
+            /*
+              we ran over 1s in loop(), and our priority was lowered
+              to let a driver run. Set it back to high priority now.
+             */
+            set_priority(APM_MAIN_PRIORITY);
+            perf_count(perf_overrun);
+            ran_overtime = false;
+        }
+
+        perf_end(perf_loop);
+
+        /*
+          give up 500 microseconds of time, to ensure drivers get a
+          chance to run. This relies on the accurate semaphore wait
+          using hrt in semaphore.cpp
+         */
+        hal.scheduler->delay_microseconds(500);
+    }
+    thread_running = false;
+    return 0;
+}
+
+static void usage(void)
+{
+    printf("Usage: %s [options] {start,stop,status}\n", SKETCHNAME);
+    printf("Options:\n");
+    printf("\t-d  DEVICE         set terminal device (default %s)\n", UARTA_DEFAULT_DEVICE);
+    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("\n");
+}
+
+
+void HAL_VRBRAIN::init(int argc, char * const argv[]) const
+{
+    int i;
+    const char *deviceA = UARTA_DEFAULT_DEVICE;
+    const char *deviceC = UARTC_DEFAULT_DEVICE;
+    const char *deviceD = UARTD_DEFAULT_DEVICE;
+    const char *deviceE = UARTE_DEFAULT_DEVICE;
+
+    if (argc < 1) {
+        printf("%s: missing command (try '%s start')", 
+               SKETCHNAME, SKETCHNAME);
+        usage();
+        exit(1);
+    }
+
+    for (i=0; i<argc; i++) {
+        if (strcmp(argv[i], "start") == 0) {
+            if (thread_running) {
+                printf("%s already running\n", SKETCHNAME);
+                /* this is not an error */
+                exit(0);
+            }
+
+            uartADriver.set_device_path(deviceA);
+            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);
+
+            _vrbrain_thread_should_exit = false;
+            daemon_task = task_spawn_cmd(SKETCHNAME,
+                                     SCHED_FIFO,
+                                     APM_MAIN_PRIORITY,
+                                     8192,
+                                     main_loop,
+                                     NULL);
+            exit(0);
+        }
+
+        if (strcmp(argv[i], "stop") == 0) {
+            _vrbrain_thread_should_exit = true;
+            exit(0);
+        }
+ 
+        if (strcmp(argv[i], "status") == 0) {
+            if (_vrbrain_thread_should_exit && thread_running) {
+                printf("\t%s is exiting\n", SKETCHNAME);
+            } else if (thread_running) {
+                printf("\t%s is running\n", SKETCHNAME);
+            } else {
+                printf("\t%s is not started\n", SKETCHNAME);
+            }
+            exit(0);
+        }
+
+        if (strcmp(argv[i], "-d") == 0) {
+            // set terminal device
+            if (argc > i + 1) {
+                deviceA = strdup(argv[i+1]);
+            } else {
+                printf("missing parameter to -d DEVICE\n");
+                usage();
+                exit(1);
+            }
+        }
+
+        if (strcmp(argv[i], "-d2") == 0) {
+            // set uartC terminal device
+            if (argc > i + 1) {
+                deviceC = strdup(argv[i+1]);
+            } else {
+                printf("missing parameter to -d2 DEVICE\n");
+                usage();
+                exit(1);
+            }
+        }
+
+        if (strcmp(argv[i], "-d3") == 0) {
+            // set uartD terminal device
+            if (argc > i + 1) {
+                deviceD = strdup(argv[i+1]);
+            } else {
+                printf("missing parameter to -d3 DEVICE\n");
+                usage();
+                exit(1);
+            }
+        }
+
+        if (strcmp(argv[i], "-d4") == 0) {
+            // set uartE 2nd GPS device
+            if (argc > i + 1) {
+                deviceE = strdup(argv[i+1]);
+            } else {
+                printf("missing parameter to -d4 DEVICE\n");
+                usage();
+                exit(1);
+            }
+        }
+    }
+ 
+    usage();
+    exit(1);
+}
+
+const HAL_VRBRAIN AP_HAL_VRBRAIN;
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+

libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h

@@ -0,0 +1,23 @@
+
+#ifndef __AP_HAL_VRBRAIN_CLASS_H__
+#define __AP_HAL_VRBRAIN_CLASS_H__
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include <AP_HAL_VRBRAIN.h>
+#include "AP_HAL_VRBRAIN_Namespace.h"
+#include <systemlib/visibility.h>
+#include <systemlib/perf_counter.h>
+
+class HAL_VRBRAIN : public AP_HAL::HAL {
+public:
+    HAL_VRBRAIN();
+    void init(int argc, char * const argv[]) const;
+};
+
+extern const HAL_VRBRAIN AP_HAL_VRBRAIN;
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#endif // __AP_HAL_VRBRAIN_CLASS_H__

libraries/AP_HAL_VRBRAIN/RCInput.cpp

@@ -0,0 +1,117 @@
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "RCInput.h"
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+
+using namespace VRBRAIN;
+
+extern const AP_HAL::HAL& hal;
+
+void VRBRAINRCInput::init(void* unused)
+{
+	_perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
+	_rc_sub = orb_subscribe(ORB_ID(input_rc));
+	if (_rc_sub == -1) {
+		hal.scheduler->panic("Unable to subscribe to input_rc");		
+	}
+	clear_overrides();
+        pthread_mutex_init(&rcin_mutex, NULL);
+}
+
+bool VRBRAINRCInput::new_input() 
+{
+    pthread_mutex_lock(&rcin_mutex);
+    bool valid = _rcin.timestamp_last_signal != _last_read || _override_valid;
+    pthread_mutex_unlock(&rcin_mutex);
+    return valid;
+}
+
+uint8_t VRBRAINRCInput::num_channels() 
+{
+    pthread_mutex_lock(&rcin_mutex);
+    uint8_t n = _rcin.channel_count;
+    pthread_mutex_unlock(&rcin_mutex);
+    return n;
+}
+
+uint16_t VRBRAINRCInput::read(uint8_t ch)
+{
+	if (ch >= RC_INPUT_MAX_CHANNELS) {
+		return 0;
+	}
+        pthread_mutex_lock(&rcin_mutex);
+	_last_read = _rcin.timestamp_last_signal;
+	_override_valid = false;
+	if (_override[ch]) {
+            uint16_t v = _override[ch];
+            pthread_mutex_unlock(&rcin_mutex);
+            return v;
+	}
+	if (ch >= _rcin.channel_count) {
+            pthread_mutex_unlock(&rcin_mutex);
+            return 0;
+	}
+	uint16_t v = _rcin.values[ch];
+        pthread_mutex_unlock(&rcin_mutex);
+        return v;
+}
+
+uint8_t VRBRAINRCInput::read(uint16_t* periods, uint8_t len)
+{
+	if (len > RC_INPUT_MAX_CHANNELS) {
+		len = RC_INPUT_MAX_CHANNELS;
+	}
+	for (uint8_t i = 0; i < len; i++){
+		periods[i] = read(i);
+	}
+	return len;
+}
+
+bool VRBRAINRCInput::set_overrides(int16_t *overrides, uint8_t len)
+{
+	bool res = false;
+	for (uint8_t i = 0; i < len; i++) {
+		res |= set_override(i, overrides[i]);
+	}
+	return res;
+}
+
+bool VRBRAINRCInput::set_override(uint8_t channel, int16_t override) {
+	if (override < 0) {
+		return false; /* -1: no change. */
+	}
+	if (channel >= RC_INPUT_MAX_CHANNELS) {
+		return false;
+	}
+	_override[channel] = override;
+	if (override != 0) {
+		_override_valid = true;
+		return true;
+	}
+	return false;
+}
+
+void VRBRAINRCInput::clear_overrides()
+{
+	for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
+		set_override(i, 0);
+	}
+}
+
+void VRBRAINRCInput::_timer_tick(void)
+{
+	perf_begin(_perf_rcin);
+	bool rc_updated = false;
+	if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
+            pthread_mutex_lock(&rcin_mutex);
+            orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
+            pthread_mutex_unlock(&rcin_mutex);
+	}
+        // note, we rely on the vehicle code checking new_input() 
+        // and a timeout for the last valid input to handle failsafe
+	perf_end(_perf_rcin);
+}
+
+#endif

libraries/AP_HAL_VRBRAIN/RCInput.h

@@ -0,0 +1,35 @@
+
+#ifndef __AP_HAL_VRBRAIN_RCINPUT_H__
+#define __AP_HAL_VRBRAIN_RCINPUT_H__
+
+#include <AP_HAL_VRBRAIN.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/perf_counter.h>
+#include <pthread.h>
+
+class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput {
+public:
+    void init(void* machtnichts);
+    bool new_input();
+    uint8_t num_channels();
+    uint16_t read(uint8_t ch);
+    uint8_t read(uint16_t* periods, uint8_t len);
+
+    bool set_overrides(int16_t *overrides, uint8_t len);
+    bool set_override(uint8_t channel, int16_t override);
+    void clear_overrides();
+
+    void _timer_tick(void);
+
+private:
+    /* override state */
+    uint16_t _override[RC_INPUT_MAX_CHANNELS];
+    struct rc_input_values _rcin;
+    int _rc_sub;
+    uint64_t _last_read;
+    bool _override_valid;
+    perf_counter_t _perf_rcin;
+    pthread_mutex_t rcin_mutex;
+};
+
+#endif // __AP_HAL_VRBRAIN_RCINPUT_H__

libraries/AP_HAL_VRBRAIN/RCOutput.cpp

@@ -0,0 +1,265 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "RCOutput.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_pwm_output.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace VRBRAIN;
+
+void VRBRAINRCOutput::init(void* unused)
+{
+    _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
+    _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+    if (_pwm_fd == -1) {
+        hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
+    }
+    if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
+        hal.console->printf("RCOutput: Unable to setup IO arming\n");
+    }
+    if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
+        hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
+    }
+    _rate_mask = 0;
+    _alt_fd = -1;    
+    _servo_count = 0;
+    _alt_servo_count = 0;
+
+    if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
+        hal.console->printf("RCOutput: Unable to get servo count\n");        
+        return;
+    }
+
+    _alt_fd = open("/dev/px4fmu", O_RDWR);
+    if (_alt_fd == -1) {
+        hal.console->printf("RCOutput: failed to open /dev/px4fmu");
+        return;
+    }
+}
+
+
+void VRBRAINRCOutput::_init_alt_channels(void)
+{
+    if (_alt_fd == -1) {
+        return;
+    }
+    if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
+        hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
+        return;
+    }
+    if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
+        hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
+        return;
+    }
+    if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
+        hal.console->printf("RCOutput: Unable to get servo count\n");        
+    }
+}
+
+void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    // we can't set this per channel yet
+    if (freq_hz > 50) {
+        // we're being asked to set the alt rate
+        if (ioctl(_pwm_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;
+        }
+        _freq_hz = freq_hz;
+    }
+
+    /* work out the new rate mask. The PX4IO board has 3 groups of servos. 
+
+       Group 0: channels 0 1
+       Group 1: channels 4 5 6 7
+       Group 2: channels 2 3
+
+       Channels within a group must be set to the same rate.
+
+       For the moment we never set the channels above 8 to more than
+       50Hz
+     */
+    if (freq_hz > 50) {
+        // we are setting high rates on the given channels
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V4) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V5) || defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+        _rate_mask |= chmask & 0xFF;
+        if (_rate_mask & 0x07) {
+            _rate_mask |= 0x07;
+        }
+        if (_rate_mask & 0x38) {
+            _rate_mask |= 0x38;
+        }
+        if (_rate_mask & 0xC0) {
+            _rate_mask |= 0xC0;
+        }
+#else
+        _rate_mask |= chmask & 0xFF;
+        if (_rate_mask & 0x3) {
+            _rate_mask |= 0x3;
+        }
+        if (_rate_mask & 0xc) {
+            _rate_mask |= 0xc;
+        }
+        if (_rate_mask & 0xF0) {
+            _rate_mask |= 0xF0;
+        }
+#endif
+    } else {
+        // we are setting low rates on the given channels
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V4) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V5) || defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+        if (chmask & 0x07) {
+            _rate_mask &= ~0x07;
+        }
+        if (chmask & 0x38) {
+            _rate_mask &= ~0x38;
+        }
+        if (chmask & 0xC0) {
+            _rate_mask &= ~0xC0;
+        }
+#else
+        if (chmask & 0x3) {
+            _rate_mask &= ~0x3;
+        }
+        if (chmask & 0xc) {
+            _rate_mask &= ~0xc;
+        }
+        if (chmask & 0xf0) {
+            _rate_mask &= ~0xf0;
+        }
+#endif
+    }
+
+    if (ioctl(_pwm_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);
+    }
+}
+
+uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
+{
+    if (_rate_mask & (1U<<ch)) {
+        return _freq_hz;
+    }
+    return 50;
+}
+
+void VRBRAINRCOutput::enable_ch(uint8_t ch)
+{
+    if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
+        // this is the first enable of an auxillary channel - setup
+        // aux channels now. This delayed setup makes it possible to
+        // use BRD_PWM_COUNT to setup the number of PWM channels.
+        _init_alt_channels();
+    }
+    _enabled_channels |= (1U<<ch);
+}
+
+void VRBRAINRCOutput::disable_ch(uint8_t ch)
+{
+    _enabled_channels &= ~(1U<<ch);
+}
+
+void VRBRAINRCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
+{
+    struct pwm_output_values pwm_values;
+    memset(&pwm_values, 0, sizeof(pwm_values));
+    for (uint8_t i=0; i<_servo_count; i++) {
+        if ((1UL<<i) & chmask) {
+            pwm_values.values[i] = period_us;
+        }
+        pwm_values.channel_count++;
+    }
+    int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
+    if (ret != OK) {
+        hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
+    }
+}
+
+void VRBRAINRCOutput::force_safety_off(void)
+{
+    int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+    if (ret != OK) {
+        hal.console->printf("Failed to force safety off\n");
+    }
+}
+
+void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
+{
+    if (ch >= _servo_count + _alt_servo_count) {
+        return;
+    }
+    if (!(_enabled_channels & (1U<<ch))) {
+        // not enabled
+        return;
+    }
+    if (ch >= _max_channel) {
+        _max_channel = ch + 1;
+    }
+    if (period_us != _period[ch]) {
+        _period[ch] = period_us;
+        _need_update = true;
+    }
+}
+
+void VRBRAINRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+    for (uint8_t i=0; i<len; i++) {
+        write(i, period_us[i]);
+    }
+}
+
+uint16_t VRBRAINRCOutput::read(uint8_t ch)
+{
+    if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
+        return 0;
+    }
+    return _period[ch];
+}
+
+void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len)
+{
+    for (uint8_t i=0; i<len; i++) {
+        period_us[i] = read(i);
+    }
+}
+
+void VRBRAINRCOutput::_timer_tick(void)
+{
+    uint32_t now = hal.scheduler->micros();
+
+    // always send at least at 20Hz, otherwise the IO board may think
+    // we are dead
+    if (now - _last_output > 50000) {
+        _need_update = true;
+    }
+
+    if (_need_update && _pwm_fd != -1) {
+        _need_update = false;
+        perf_begin(_perf_rcout);
+        if (_max_channel <= _servo_count) {
+            ::write(_pwm_fd, _period, _max_channel*sizeof(_period[0]));
+        } else {
+            // we're using both sets of outputs
+            ::write(_pwm_fd, _period, _servo_count*sizeof(_period[0]));
+            if (_alt_fd != -1 && _alt_servo_count > 0) {
+                uint8_t n = _max_channel - _servo_count;
+                if (n > _alt_servo_count) {
+                    n = _alt_servo_count;
+                }
+                ::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
+            }
+        }
+        perf_end(_perf_rcout);
+        _last_output = now;
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/RCOutput.h

@@ -0,0 +1,44 @@
+
+#ifndef __AP_HAL_VRBRAIN_RCOUTPUT_H__
+#define __AP_HAL_VRBRAIN_RCOUTPUT_H__
+
+#include <AP_HAL_VRBRAIN.h>
+#include <systemlib/perf_counter.h>
+
+#define VRBRAIN_NUM_OUTPUT_CHANNELS 16
+
+class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput
+{
+public:
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     disable_ch(uint8_t ch);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+    void     set_safety_pwm(uint32_t chmask, uint16_t period_us);
+    void     force_safety_off(void);
+
+    void _timer_tick(void);
+
+private:
+    int _pwm_fd;
+    int _alt_fd;
+    uint16_t _freq_hz;
+    uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
+    volatile uint8_t _max_channel;
+    volatile bool _need_update;
+    perf_counter_t  _perf_rcout;
+    uint32_t _last_output;
+    unsigned _servo_count;
+    unsigned _alt_servo_count;
+    uint32_t _rate_mask;
+    uint16_t _enabled_channels;
+
+    void _init_alt_channels(void);
+};
+
+#endif // __AP_HAL_VRBRAIN_RCOUTPUT_H__

libraries/AP_HAL_VRBRAIN/Scheduler.cpp

@@ -0,0 +1,336 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include "AP_HAL_VRBRAIN.h"
+#include "Scheduler.h"
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <sched.h>
+#include <errno.h>
+#include <stdio.h>
+#include <drivers/drv_hrt.h>
+#include <nuttx/arch.h>
+#include <systemlib/systemlib.h>
+#include <pthread.h>
+#include <poll.h>
+
+#include "UARTDriver.h"
+#include "AnalogIn.h"
+#include "Storage.h"
+#include "RCOutput.h"
+#include "RCInput.h"
+
+using namespace VRBRAIN;
+
+extern const AP_HAL::HAL& hal;
+
+extern bool _vrbrain_thread_should_exit;
+
+VRBRAINScheduler::VRBRAINScheduler() :
+    _perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")),
+    _perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")),
+	_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
+{}
+
+void VRBRAINScheduler::init(void *unused)
+{
+    _sketch_start_time = hrt_absolute_time();
+
+    _main_task_pid = getpid();
+
+    // setup the timer thread - this will call tasks at 1kHz
+	pthread_attr_t thread_attr;
+	struct sched_param param;
+
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 2048);
+
+	param.sched_priority = APM_TIMER_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+	pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_timer_thread, this);
+
+    // the UART thread runs at a medium priority
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 2048);
+
+	param.sched_priority = APM_UART_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+	pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_uart_thread, this);
+
+    // the IO thread runs at lower priority
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 2048);
+
+	param.sched_priority = APM_IO_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+	pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_io_thread, this);
+}
+
+uint32_t VRBRAINScheduler::micros()
+{
+    return (uint32_t)(hrt_absolute_time() - _sketch_start_time);
+}
+
+uint32_t VRBRAINScheduler::millis()
+{
+    return hrt_absolute_time() / 1000;
+}
+
+/**
+   delay for a specified number of microseconds using a semaphore wait
+ */
+void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec)
+{
+    sem_t wait_semaphore;
+    struct hrt_call wait_call;
+    sem_init(&wait_semaphore, 0, 0);
+    hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore);
+    sem_wait(&wait_semaphore);
+}
+
+void VRBRAINScheduler::delay_microseconds(uint16_t usec)
+{
+    perf_begin(_perf_delay);
+    if (usec >= 500) {
+        delay_microseconds_semaphore(usec);
+        perf_end(_perf_delay);
+        return;
+    }
+	uint32_t start = micros();
+    uint32_t dt;
+	while ((dt=(micros() - start)) < usec) {
+		up_udelay(usec - dt);
+	}
+    perf_end(_perf_delay);
+}
+
+void VRBRAINScheduler::delay(uint16_t ms)
+{
+    if (in_timerprocess()) {
+        ::printf("ERROR: delay() from timer process\n");
+        return;
+    }
+    perf_begin(_perf_delay);
+	uint64_t start = hrt_absolute_time();
+    
+    while ((hrt_absolute_time() - start)/1000 < ms && 
+           !_vrbrain_thread_should_exit) {
+        delay_microseconds_semaphore(1000);
+        if (_min_delay_cb_ms <= ms) {
+            if (_delay_cb) {
+                _delay_cb();
+            }
+        }
+    }
+    perf_end(_perf_delay);
+    if (_vrbrain_thread_should_exit) {
+        exit(1);
+    }
+}
+
+void VRBRAINScheduler::register_delay_callback(AP_HAL::Proc proc,
+                                            uint16_t min_time_ms) 
+{
+    _delay_cb = proc;
+    _min_delay_cb_ms = min_time_ms;
+}
+
+void VRBRAINScheduler::register_timer_process(AP_HAL::MemberProc proc)
+{
+    for (uint8_t i = 0; i < _num_timer_procs; i++) {
+        if (_timer_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_timer_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) {
+        _timer_proc[_num_timer_procs] = proc;
+        _num_timer_procs++;
+    } else {
+        hal.console->printf("Out of timer processes\n");
+    }
+}
+
+void VRBRAINScheduler::register_io_process(AP_HAL::MemberProc proc)
+{
+    for (uint8_t i = 0; i < _num_io_procs; i++) {
+        if (_io_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_io_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) {
+        _io_proc[_num_io_procs] = proc;
+        _num_io_procs++;
+    } else {
+        hal.console->printf("Out of IO processes\n");
+    }
+}
+
+void VRBRAINScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
+{
+    _failsafe = failsafe;
+}
+
+void VRBRAINScheduler::suspend_timer_procs()
+{
+    _timer_suspended = true;
+}
+
+void VRBRAINScheduler::resume_timer_procs()
+{
+    _timer_suspended = false;
+    if (_timer_event_missed == true) {
+        _run_timers(false);
+        _timer_event_missed = false;
+    }
+}
+
+void VRBRAINScheduler::reboot(bool hold_in_bootloader)
+{
+	systemreset(hold_in_bootloader);
+}
+
+void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
+{
+    if (_in_timer_proc) {
+        return;
+    }
+    _in_timer_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the timer based drivers
+        for (int i = 0; i < _num_timer_procs; i++) {
+            if (_timer_proc[i] != NULL) {
+                _timer_proc[i]();
+            }
+        }
+    } else if (called_from_timer_thread) {
+        _timer_event_missed = true;
+    }
+
+    // and the failsafe, if one is setup
+    if (_failsafe != NULL) {
+        _failsafe();
+    }
+
+    // process analog input
+    ((VRBRAINAnalogIn *)hal.analogin)->_timer_tick();
+
+    _in_timer_proc = false;
+}
+
+void *VRBRAINScheduler::_timer_thread(void)
+{
+    while (!_hal_initialized) {
+        poll(NULL, 0, 1);        
+    }
+    while (!_vrbrain_thread_should_exit) {
+        delay_microseconds_semaphore(1000);
+
+        // run registered timers
+        perf_begin(_perf_timers);
+        _run_timers(true);
+        perf_end(_perf_timers);
+
+        // process any pending RC output requests
+        ((VRBRAINRCOutput *)hal.rcout)->_timer_tick();
+
+        // process any pending RC input requests
+        ((VRBRAINRCInput *)hal.rcin)->_timer_tick();
+    }
+    return NULL;
+}
+
+void VRBRAINScheduler::_run_io(void)
+{
+    if (_in_io_proc) {
+        return;
+    }
+    _in_io_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the IO based drivers
+        for (int i = 0; i < _num_io_procs; i++) {
+            if (_io_proc[i] != NULL) {
+                _io_proc[i]();
+            }
+        }
+    }
+
+    _in_io_proc = false;
+}
+
+void *VRBRAINScheduler::_uart_thread(void)
+{
+    while (!_hal_initialized) {
+        poll(NULL, 0, 1);        
+    }
+    while (!_vrbrain_thread_should_exit) {
+        delay_microseconds_semaphore(1000);
+
+        // process any pending serial bytes
+        ((VRBRAINUARTDriver *)hal.uartA)->_timer_tick();
+        ((VRBRAINUARTDriver *)hal.uartB)->_timer_tick();
+        ((VRBRAINUARTDriver *)hal.uartC)->_timer_tick();
+        ((VRBRAINUARTDriver *)hal.uartD)->_timer_tick();
+        ((VRBRAINUARTDriver *)hal.uartE)->_timer_tick();
+    }
+    return NULL;
+}
+
+void *VRBRAINScheduler::_io_thread(void)
+{
+    while (!_hal_initialized) {
+        poll(NULL, 0, 1);        
+    }
+    while (!_vrbrain_thread_should_exit) {
+        poll(NULL, 0, 1);
+
+        // process any pending storage writes
+        ((VRBRAINStorage *)hal.storage)->_timer_tick();
+
+        // run registered IO processes
+        perf_begin(_perf_io_timers);
+        _run_io();
+        perf_end(_perf_io_timers);
+    }
+    return NULL;
+}
+
+void VRBRAINScheduler::panic(const prog_char_t *errormsg)
+{
+    write(1, errormsg, strlen(errormsg));
+    write(1, "\n", 1);
+    hal.scheduler->delay_microseconds(10000);
+    _vrbrain_thread_should_exit = true;
+    exit(1);
+}
+
+bool VRBRAINScheduler::in_timerprocess()
+{
+    return getpid() != _main_task_pid;
+}
+
+bool VRBRAINScheduler::system_initializing() {
+    return !_initialized;
+}
+
+void VRBRAINScheduler::system_initialized() {
+    if (_initialized) {
+        panic(PSTR("PANIC: scheduler::system_initialized called"
+                   "more than once"));
+    }
+    _initialized = true;
+}
+
+#endif

libraries/AP_HAL_VRBRAIN/Scheduler.h

@@ -0,0 +1,89 @@
+
+#ifndef __AP_HAL_VRBRAIN_SCHEDULER_H__
+#define __AP_HAL_VRBRAIN_SCHEDULER_H__
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AP_HAL_VRBRAIN_Namespace.h"
+#include <sys/time.h>
+#include <signal.h>
+#include <pthread.h>
+#include <systemlib/perf_counter.h>
+
+#define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8
+
+#define APM_MAIN_PRIORITY    180
+#define APM_TIMER_PRIORITY   181
+#define APM_UART_PRIORITY     60
+#define APM_IO_PRIORITY       59
+#define APM_OVERTIME_PRIORITY 10
+#define APM_STARTUP_PRIORITY  10
+
+/* Scheduler implementation: */
+class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler {
+public:
+	VRBRAINScheduler();
+    /* AP_HAL::Scheduler methods */
+
+    void     init(void *unused);
+    void     delay(uint16_t ms);
+    uint32_t millis();
+    uint32_t micros();
+    void     delay_microseconds(uint16_t us);
+    void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
+    void     register_timer_process(AP_HAL::MemberProc);
+    void     register_io_process(AP_HAL::MemberProc);
+    void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
+    void     suspend_timer_procs();
+    void     resume_timer_procs();
+    void     reboot(bool hold_in_bootloader);
+    void     panic(const prog_char_t *errormsg);
+
+    bool     in_timerprocess();
+    bool     system_initializing();
+    void     system_initialized();
+    void     hal_initialized() { _hal_initialized = true; }
+    
+private:
+    bool _initialized;
+    volatile bool _hal_initialized;
+    AP_HAL::Proc _delay_cb;
+    uint16_t _min_delay_cb_ms;
+    AP_HAL::Proc _failsafe;
+    volatile bool _timer_pending;
+    uint64_t _sketch_start_time;
+
+    volatile bool _timer_suspended;
+
+    AP_HAL::MemberProc _timer_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_timer_procs;
+    volatile bool _in_timer_proc;
+
+    AP_HAL::MemberProc _io_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_io_procs;
+    volatile bool _in_io_proc;
+
+    volatile bool _timer_event_missed;
+
+    pid_t _main_task_pid;
+    pthread_t _timer_thread_ctx;
+    pthread_t _io_thread_ctx;
+    pthread_t _uart_thread_ctx;
+
+    void *_timer_thread(void);
+    void *_io_thread(void);
+    void *_uart_thread(void);
+
+    void _run_timers(bool called_from_timer_thread);
+    void _run_io(void);
+
+    void delay_microseconds_semaphore(uint16_t us);
+
+    perf_counter_t  _perf_timers;
+    perf_counter_t  _perf_io_timers;
+    perf_counter_t  _perf_delay;
+};
+#endif
+#endif // __AP_HAL_VRBRAIN_SCHEDULER_H__
+
+

libraries/AP_HAL_VRBRAIN/Storage.cpp

@@ -0,0 +1,336 @@
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include <assert.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "Storage.h"
+using namespace VRBRAIN;
+
+/*
+  This stores eeprom data in the VRBRAIN MTD interface with a 4k size, and
+  a in-memory buffer. This keeps the latency and devices IOs down.
+ */
+
+// 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 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;
+
+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) {
+        hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE);
+    }
+    uint32_t v;
+    if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
+        hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE);
+    }
+    if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
+        hal.scheduler->panic("Failed to read signature from " MTD_PARAMS_FILE);
+    }
+    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) {
+        hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE);
+    }
+    uint32_t v = MTD_SIGNATURE;
+    if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
+        hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE);
+    }
+    if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
+        hal.scheduler->panic("Failed to write signature in " MTD_PARAMS_FILE);
+    }
+    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) {
+        hal.scheduler->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;
+    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);
+        hal.scheduler->panic("Unable to write MTD for upgrade");        
+    }
+    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);
+
+        // VRBRAIN should always have /fs/mtd_params
+        if (!_have_mtd) {
+            hal.scheduler->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);
+#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) {
+            hal.scheduler->panic("Failed to open " MTD_PARAMS_FILE);
+	}
+	if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
+            hal.scheduler->panic("Failed to read " MTD_PARAMS_FILE);
+	}
+	close(fd);
+	_initialised = true;
+}
+
+/*
+  mark some lines as dirty. Note that there is no attempt to avoid
+  the race condition between this code and the _timer_tick() code
+  below, which both update _dirty_mask. If we lose the race then the
+  result is that a line is written more than once, but it won't result
+  in a line not being written.
+ */
+void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length)
+{
+	uint16_t end = loc + length;
+	while (loc < end) {
+		uint8_t line = (loc >> VRBRAIN_STORAGE_LINE_SHIFT);
+		_dirty_mask |= 1 << line;
+		loc += VRBRAIN_STORAGE_LINE_SIZE;
+	}
+}
+
+uint8_t VRBRAINStorage::read_byte(uint16_t loc)
+{
+	if (loc >= sizeof(_buffer)) {
+		return 0;
+	}
+	_storage_open();
+	return _buffer[loc];
+}
+
+uint16_t VRBRAINStorage::read_word(uint16_t loc)
+{
+	uint16_t value;
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return 0;
+	}
+	_storage_open();
+	memcpy(&value, &_buffer[loc], sizeof(value));
+	return value;
+}
+
+uint32_t VRBRAINStorage::read_dword(uint16_t loc)
+{
+	uint32_t value;
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return 0;
+	}
+	_storage_open();
+	memcpy(&value, &_buffer[loc], sizeof(value));
+	return value;
+}
+
+void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n)
+{
+	if (loc >= sizeof(_buffer)-(n-1)) {
+		return;
+	}
+	_storage_open();
+	memcpy(dst, &_buffer[loc], n);
+}
+
+void VRBRAINStorage::write_byte(uint16_t loc, uint8_t value)
+{
+	if (loc >= sizeof(_buffer)) {
+		return;
+	}
+	if (_buffer[loc] != value) {
+		_storage_open();
+		_buffer[loc] = value;
+		_mark_dirty(loc, sizeof(value));
+	}
+}
+
+void VRBRAINStorage::write_word(uint16_t loc, uint16_t value)
+{
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return;
+	}
+	if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], &value, sizeof(value));
+		_mark_dirty(loc, sizeof(value));
+	}
+}
+
+void VRBRAINStorage::write_dword(uint16_t loc, uint32_t value)
+{
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return;
+	}
+	if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], &value, sizeof(value));
+		_mark_dirty(loc, sizeof(value));
+	}
+}
+
+void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n)
+{
+	if (loc >= sizeof(_buffer)-(n-1)) {
+		return;
+	}
+	if (memcmp(src, &_buffer[loc], n) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], src, n);
+		_mark_dirty(loc, n);
+	}
+}
+
+void VRBRAINStorage::_timer_tick(void)
+{
+	if (!_initialised || _dirty_mask == 0) {
+		return;
+	}
+	perf_begin(_perf_storage);
+
+	if (_fd == -1) {
+		_fd = open(MTD_PARAMS_FILE, O_WRONLY);
+		if (_fd == -1) {
+			perf_end(_perf_storage);
+			perf_count(_perf_errors);
+			return;	
+		}
+	}
+
+	// write out the first dirty set of lines. We don't write more
+	// than one to keep the latency of this call to a minimum
+	uint8_t i, n;
+	for (i=0; i<VRBRAIN_STORAGE_NUM_LINES; i++) {
+		if (_dirty_mask & (1<<i)) {
+			break;
+		}
+	}
+	if (i == VRBRAIN_STORAGE_NUM_LINES) {
+		// this shouldn't be possible
+		perf_end(_perf_storage);
+		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));
+	}
+
+	/*
+	  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.
+	 */
+	if (lseek(_fd, i<<VRBRAIN_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<VRBRAIN_STORAGE_LINE_SHIFT)) {
+		_dirty_mask &= ~write_mask;
+		if (write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT) != n<<VRBRAIN_STORAGE_LINE_SHIFT) {
+			// write error - likely EINTR
+			_dirty_mask |= write_mask;
+			close(_fd);
+			_fd = -1;
+			perf_count(_perf_errors);
+		}
+		if (_dirty_mask == 0) {
+			if (fsync(_fd) != 0) {
+				close(_fd);
+				_fd = -1;
+				perf_count(_perf_errors);
+			}
+		}
+	}
+	perf_end(_perf_storage);
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_VRBRAIN/Storage.h

@@ -0,0 +1,49 @@
+
+
+#ifndef __AP_HAL_VRBRAIN_STORAGE_H__
+#define __AP_HAL_VRBRAIN_STORAGE_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_VRBRAIN_Namespace.h"
+#include <systemlib/perf_counter.h>
+
+#define VRBRAIN_STORAGE_SIZE 4096
+#define VRBRAIN_STORAGE_MAX_WRITE 512
+#define VRBRAIN_STORAGE_LINE_SHIFT 9
+#define VRBRAIN_STORAGE_LINE_SIZE (1<<VRBRAIN_STORAGE_LINE_SHIFT)
+#define VRBRAIN_STORAGE_NUM_LINES (VRBRAIN_STORAGE_SIZE/VRBRAIN_STORAGE_LINE_SIZE)
+
+class VRBRAIN::VRBRAINStorage : public AP_HAL::Storage {
+public:
+	VRBRAINStorage();
+
+    void init(void* machtnichts) {}
+    uint8_t  read_byte(uint16_t loc);
+    uint16_t read_word(uint16_t loc);
+    uint32_t read_dword(uint16_t loc);
+    void     read_block(void *dst, uint16_t src, size_t n);
+
+    void write_byte(uint16_t loc, uint8_t value);
+    void write_word(uint16_t loc, uint16_t value);
+    void write_dword(uint16_t loc, uint32_t value);
+    void write_block(uint16_t dst, const void* src, size_t n);
+
+    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;
+    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);
+};
+
+#endif // __AP_HAL_VRBRAIN_STORAGE_H__

libraries/AP_HAL_VRBRAIN/UARTDriver.cpp

@@ -0,0 +1,535 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "UARTDriver.h"
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <drivers/drv_hrt.h>
+#include <assert.h>
+
+using namespace VRBRAIN;
+
+extern const AP_HAL::HAL& hal;
+
+VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name) :
+	_devpath(devpath),
+    _fd(-1),
+    _baudrate(57600),
+    _perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
+    _initialised(false),
+    _in_timer(false),
+    _flow_control(FLOW_CONTROL_DISABLE)
+{
+}
+
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  this UART driver maps to a serial device in /dev
+ */
+
+void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
+{
+    if (strcmp(_devpath, "/dev/null") == 0) {
+        // leave uninitialised
+        return;
+    }
+
+    uint16_t min_tx_buffer = 1024;
+    uint16_t min_rx_buffer = 512;
+    if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
+        min_tx_buffer = 16384;
+        min_rx_buffer = 1024;
+    }
+    // on VRBRAIN we have enough memory to have a larger transmit and
+    // receive buffer for all ports. This means we don't get delays
+    // while waiting to write GPS config packets
+    if (txS < min_tx_buffer) {
+        txS = min_tx_buffer;
+    }
+    if (rxS < min_rx_buffer) {
+        rxS = min_rx_buffer;
+    }
+
+    /*
+      allocate the read buffer
+      we allocate buffers before we successfully open the device as we
+      want to allocate in the early stages of boot, and cause minimum
+      thrashing of the heap once we are up. The ttyACM0 driver may not
+      connect for some time after boot
+     */
+	if (rxS != 0 && rxS != _readbuf_size) {
+        _initialised = false;
+        while (_in_timer) {
+            hal.scheduler->delay(1);
+        }
+		_readbuf_size = rxS;
+		if (_readbuf != NULL) {
+			free(_readbuf);
+		}
+		_readbuf = (uint8_t *)malloc(_readbuf_size);
+		_readbuf_head = 0;
+		_readbuf_tail = 0;
+	}
+
+    if (b != 0) {
+        _baudrate = b;
+    }
+
+    /*
+      allocate the write buffer
+     */
+	if (txS != 0 && txS != _writebuf_size) {
+        _initialised = false;
+        while (_in_timer) {
+            hal.scheduler->delay(1);
+        }
+		_writebuf_size = txS;
+		if (_writebuf != NULL) {
+			free(_writebuf);
+		}
+		_writebuf = (uint8_t *)malloc(_writebuf_size+16);
+		_writebuf_head = 0;
+		_writebuf_tail = 0;
+	}
+
+	if (_fd == -1) {
+        _fd = open(_devpath, O_RDWR);
+		if (_fd == -1) {
+			return;
+		}
+
+        // work out the OS write buffer size by looking at how many
+        // bytes could be written when we first open the port
+        int nwrite = 0;
+        if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
+            _os_write_buffer_size = nwrite;
+            if (_os_write_buffer_size & 1) {
+                // it is reporting one short
+                _os_write_buffer_size += 1;
+            }
+        }
+	}
+
+	if (_baudrate != 0) {
+		// set the baud rate
+		struct termios t;
+		tcgetattr(_fd, &t);
+		cfsetspeed(&t, _baudrate);
+		// disable LF -> CR/LF
+		t.c_oflag &= ~ONLCR;
+		tcsetattr(_fd, TCSANOW, &t);
+
+        // separately setup IFLOW if we can. We do this as a 2nd call
+        // as if the port has no RTS pin then the tcsetattr() call
+        // will fail, and if done as one call then it would fail to
+        // set the baudrate.
+		tcgetattr(_fd, &t);
+		t.c_cflag |= CRTS_IFLOW;
+		tcsetattr(_fd, TCSANOW, &t);
+	}
+
+    if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) {
+        if (!_initialised) {
+            ::printf("initialised %s OK %u %u\n", _devpath, 
+                     (unsigned)_writebuf_size, (unsigned)_readbuf_size);
+        }
+        _initialised = true;
+    }
+}
+
+void VRBRAINUARTDriver::set_flow_control(enum flow_control flow_control)
+{
+	if (_fd == -1) {
+        return;
+    }
+    struct termios t;
+    tcgetattr(_fd, &t);
+    // we already enabled CRTS_IFLOW above, just enable output flow control
+    if (flow_control != FLOW_CONTROL_DISABLE) {
+        t.c_cflag |= CRTSCTS;
+    } else {
+        t.c_cflag &= ~CRTSCTS;
+    }
+    tcsetattr(_fd, TCSANOW, &t);
+    _flow_control = flow_control;
+}
+
+void VRBRAINUARTDriver::begin(uint32_t b)
+{
+	begin(b, 0, 0);
+}
+
+
+/*
+  try to initialise the UART. This is used to cope with the way NuttX
+  handles /dev/ttyACM0 (the USB port). The port appears in /dev on
+  boot, but cannot be opened until a USB cable is connected and the
+  host starts the CDCACM communication.
+ */
+void VRBRAINUARTDriver::try_initialise(void)
+{
+    if (_initialised) {
+        return;
+    }
+    if ((hal.scheduler->millis() - _last_initialise_attempt_ms) < 2000) {
+        return;
+    }
+    _last_initialise_attempt_ms = hal.scheduler->millis();
+    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED) {
+        begin(0);
+    }
+}
+
+
+void VRBRAINUARTDriver::end()
+{
+    _initialised = false;
+    while (_in_timer) hal.scheduler->delay(1);
+    if (_fd != -1) {
+        close(_fd);
+        _fd = -1;
+    }
+    if (_readbuf) {
+        free(_readbuf);
+        _readbuf = NULL;
+    }
+    if (_writebuf) {
+        free(_writebuf);
+        _writebuf = NULL;
+    }
+    _readbuf_size = _writebuf_size = 0;
+    _writebuf_head = 0;
+    _writebuf_tail = 0;
+    _readbuf_head = 0;
+    _readbuf_tail = 0;
+}
+
+void VRBRAINUARTDriver::flush() {}
+
+bool VRBRAINUARTDriver::is_initialized()
+{ 
+    try_initialise();
+    return _initialised; 
+}
+
+void VRBRAINUARTDriver::set_blocking_writes(bool blocking)
+{
+    _nonblocking_writes = !blocking;
+}
+
+bool VRBRAINUARTDriver::tx_pending() { return false; }
+
+/*
+  buffer handling macros
+ */
+#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
+#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
+#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
+#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
+#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
+
+/*
+  return number of bytes available to be read from the buffer
+ */
+int16_t VRBRAINUARTDriver::available()
+{ 
+	if (!_initialised) {
+        try_initialise();
+		return 0;
+	}
+    uint16_t _tail;
+    return BUF_AVAILABLE(_readbuf);
+}
+
+/*
+  return number of bytes that can be added to the write buffer
+ */
+int16_t VRBRAINUARTDriver::txspace()
+{ 
+	if (!_initialised) {
+        try_initialise();
+		return 0;
+	}
+    uint16_t _head;
+    return BUF_SPACE(_writebuf);
+}
+
+/*
+  read one byte from the read buffer
+ */
+int16_t VRBRAINUARTDriver::read()
+{ 
+	uint8_t c;
+    if (!_initialised) {
+        try_initialise();
+        return -1;
+    }
+	if (_readbuf == NULL) {
+		return -1;
+	}
+    if (BUF_EMPTY(_readbuf)) {
+        return -1;
+    }
+    c = _readbuf[_readbuf_head];
+    BUF_ADVANCEHEAD(_readbuf, 1);
+	return c;
+}
+
+/* 
+   write one byte to the buffer
+ */
+size_t VRBRAINUARTDriver::write(uint8_t c)
+{ 
+    if (!_initialised) {
+        try_initialise();
+        return 0;
+    }
+    if (hal.scheduler->in_timerprocess()) {
+        // not allowed from timers
+        return 0;
+    }
+    uint16_t _head;
+
+    while (BUF_SPACE(_writebuf) == 0) {
+        if (_nonblocking_writes) {
+            return 0;
+        }
+        hal.scheduler->delay(1);
+    }
+    _writebuf[_writebuf_tail] = c;
+    BUF_ADVANCETAIL(_writebuf, 1);
+    return 1;
+}
+
+/*
+  write size bytes to the write buffer
+ */
+size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
+{
+	if (!_initialised) {
+        try_initialise();
+		return 0;
+	}
+    if (hal.scheduler->in_timerprocess()) {
+        // not allowed from timers
+        return 0;
+    }
+
+    if (!_nonblocking_writes) {
+        /*
+          use the per-byte delay loop in write() above for blocking writes
+         */
+        size_t ret = 0;
+        while (size--) {
+            if (write(*buffer++) != 1) break;
+            ret++;
+        }
+        return ret;
+    }
+
+    uint16_t _head, space;
+    space = BUF_SPACE(_writebuf);
+    if (space == 0) {
+        return 0;
+    }
+    if (size > space) {
+        size = space;
+    }
+    if (_writebuf_tail < _head) {
+        // perform as single memcpy
+        assert(_writebuf_tail+size <= _writebuf_size);
+        memcpy(&_writebuf[_writebuf_tail], buffer, size);
+        BUF_ADVANCETAIL(_writebuf, size);
+        return size;
+    }
+
+    // perform as two memcpy calls
+    uint16_t n = _writebuf_size - _writebuf_tail;
+    if (n > size) n = size;
+    assert(_writebuf_tail+n <= _writebuf_size);
+    memcpy(&_writebuf[_writebuf_tail], buffer, n);
+    BUF_ADVANCETAIL(_writebuf, n);
+    buffer += n;
+    n = size - n;
+    if (n > 0) {
+        assert(_writebuf_tail+n <= _writebuf_size);
+        memcpy(&_writebuf[_writebuf_tail], buffer, n);
+        BUF_ADVANCETAIL(_writebuf, n);
+    }        
+    return size;
+}
+
+/*
+  try writing n bytes, handling an unresponsive port
+ */
+int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
+{
+    int ret = 0;
+
+    // the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
+    // in NuttX on ttyACM0
+    int nwrite = 0;
+
+    if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
+        if (nwrite == 0 &&
+            _flow_control == FLOW_CONTROL_AUTO &&
+            _last_write_time != 0 &&
+            _total_written != 0 &&
+            _os_write_buffer_size == _total_written &&
+            hrt_elapsed_time(&_last_write_time) > 500*1000UL) {
+            // it doesn't look like hw flow control is working
+            ::printf("disabling flow control on %s _total_written=%u\n", 
+                     _devpath, (unsigned)_total_written);
+            set_flow_control(FLOW_CONTROL_DISABLE);
+        }
+        if (nwrite > n) {
+            nwrite = n;
+        }
+        if (nwrite > 0) {
+            ret = ::write(_fd, buf, nwrite);
+        }
+    }
+
+    if (ret > 0) {
+        BUF_ADVANCEHEAD(_writebuf, ret);
+        _last_write_time = hrt_absolute_time();
+        _total_written += ret;
+        return ret;
+    }
+
+    if (hrt_absolute_time() - _last_write_time > 2000 &&
+        _flow_control == FLOW_CONTROL_DISABLE) {
+#if 0
+        // this trick is disabled for now, as it sometimes blocks on
+        // re-opening the ttyACM0 port, which would cause a crash
+        if (hrt_absolute_time() - _last_write_time > 2000000) {
+            // we haven't done a successful write for 2 seconds - try
+            // reopening the port        
+            _initialised = false;
+            ::close(_fd);
+            _fd = ::open(_devpath, O_RDWR);
+            if (_fd == -1) {
+                fprintf(stdout, "Failed to reopen UART device %s - %s\n",
+                        _devpath, strerror(errno));
+                // leave it uninitialised
+                return n;
+            }
+            
+            _last_write_time = hrt_absolute_time();
+            _initialised = true;
+        }
+#else
+        _last_write_time = hrt_absolute_time();
+#endif
+        // we haven't done a successful write for 2ms, which means the 
+        // port is running at less than 500 bytes/sec. Start
+        // discarding bytes, even if this is a blocking port. This
+        // prevents the ttyACM0 port blocking startup if the endpoint
+        // is not connected
+        BUF_ADVANCEHEAD(_writebuf, n);
+        return n;
+    }
+    return ret;
+}
+
+/*
+  try reading n bytes, handling an unresponsive port
+ */
+int VRBRAINUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
+{
+    int ret = 0;
+
+    // the FIONREAD check is to cope with broken O_NONBLOCK behaviour
+    // in NuttX on ttyACM0
+    int nread = 0;
+    if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) {
+        if (nread > n) {
+            nread = n;
+        }
+        if (nread > 0) {
+            ret = ::read(_fd, buf, nread);
+        }
+    }
+    if (ret > 0) {
+        BUF_ADVANCETAIL(_readbuf, ret);
+        _total_read += ret;
+    }
+    return ret;
+}
+
+
+/*
+  push any pending bytes to/from the serial port. This is called at
+  1kHz in the timer thread. Doing it this way reduces the system call
+  overhead in the main task enormously. 
+ */
+void VRBRAINUARTDriver::_timer_tick(void)
+{
+    uint16_t n;
+
+    if (!_initialised) return;
+
+    // don't try IO on a disconnected USB port
+    if (strcmp(_devpath, "/dev/ttyACM0") == 0 && !hal.gpio->usb_connected()) {
+        return;
+    }
+
+    _in_timer = true;
+
+    // write any pending bytes
+    uint16_t _tail;
+    n = BUF_AVAILABLE(_writebuf);
+    if (n > 0) {
+        perf_begin(_perf_uart);
+        if (_tail > _writebuf_head) {
+            // do as a single write
+            _write_fd(&_writebuf[_writebuf_head], n);
+        } else {
+            // split into two writes
+            uint16_t n1 = _writebuf_size - _writebuf_head;
+            int ret = _write_fd(&_writebuf[_writebuf_head], n1);
+            if (ret == n1 && n != n1) {
+                _write_fd(&_writebuf[_writebuf_head], n - n1);                
+            }
+        }
+        perf_end(_perf_uart);
+    }
+
+    // try to fill the read buffer
+    uint16_t _head;
+    n = BUF_SPACE(_readbuf);
+    if (n > 0) {
+        perf_begin(_perf_uart);
+        if (_readbuf_tail < _head) {
+            // one read will do
+            assert(_readbuf_tail+n <= _readbuf_size);
+            _read_fd(&_readbuf[_readbuf_tail], n);
+        } else {
+            uint16_t n1 = _readbuf_size - _readbuf_tail;
+            assert(_readbuf_tail+n1 <= _readbuf_size);
+            int ret = _read_fd(&_readbuf[_readbuf_tail], n1);
+            if (ret == n1 && n != n1) {
+                assert(_readbuf_tail+(n-n1) <= _readbuf_size);
+                _read_fd(&_readbuf[_readbuf_tail], n - n1);                
+            }
+        }
+        perf_end(_perf_uart);
+    }
+
+    _in_timer = false;
+}
+
+#endif // CONFIG_HAL_BOARD
+

libraries/AP_HAL_VRBRAIN/UARTDriver.h

@@ -0,0 +1,80 @@
+
+#ifndef __AP_HAL_VRBRAIN_UARTDRIVER_H__
+#define __AP_HAL_VRBRAIN_UARTDRIVER_H__
+
+#include <AP_HAL_VRBRAIN.h>
+#include <systemlib/perf_counter.h>
+
+class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver {
+public:
+	VRBRAINUARTDriver(const char *devpath, const char *perf_name);
+    /* VRBRAIN implementations of UARTDriver virtual methods */
+    void begin(uint32_t b);
+    void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+    void end();
+    void flush();
+    bool is_initialized();
+    void set_blocking_writes(bool blocking);
+    bool tx_pending();
+
+    /* VRBRAIN implementations of Stream virtual methods */
+    int16_t available();
+    int16_t txspace();
+    int16_t read();
+
+    /* VRBRAIN implementations of Print virtual methods */
+    size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
+
+    void set_device_path(const char *path) {
+	    _devpath = path;
+    }
+
+    void _timer_tick(void);
+
+    int _get_fd(void) {
+	    return _fd;
+    }
+
+    void set_flow_control(enum flow_control flow_control);
+    enum flow_control get_flow_control(void) { return _flow_control; }
+
+private:
+    const char *_devpath;
+    int _fd;
+    uint32_t _baudrate;
+    volatile bool _initialised;
+    volatile bool _in_timer;
+
+    bool _nonblocking_writes;
+
+    // we use in-task ring buffers to reduce the system call cost
+    // of ::read() and ::write() in the main loop
+    uint8_t *_readbuf;
+    uint16_t _readbuf_size;
+
+    // _head is where the next available data is. _tail is where new
+    // data is put
+    volatile uint16_t _readbuf_head;
+    volatile uint16_t _readbuf_tail;
+
+    uint8_t *_writebuf;
+    uint16_t _writebuf_size;
+    volatile uint16_t _writebuf_head;
+    volatile uint16_t _writebuf_tail;
+    perf_counter_t  _perf_uart;
+
+    int _write_fd(const uint8_t *buf, uint16_t n);
+    int _read_fd(uint8_t *buf, uint16_t n);
+    uint64_t _last_write_time;
+
+    void try_initialise(void);
+    uint32_t _last_initialise_attempt_ms;
+
+    uint32_t _os_write_buffer_size;
+    uint32_t _total_read;
+    uint32_t _total_written;
+    enum flow_control _flow_control;
+};
+
+#endif // __AP_HAL_VRBRAIN_UARTDRIVER_H__

libraries/AP_HAL_VRBRAIN/Util.cpp

@@ -0,0 +1,139 @@
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <apps/nsh.h>
+#include <fcntl.h>
+#include "UARTDriver.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/safety.h>
+#include <systemlib/board_serial.h>
+
+extern const AP_HAL::HAL& hal;
+
+#include "Util.h"
+using namespace VRBRAIN;
+
+extern bool _vrbrain_thread_should_exit;
+
+/*
+  constructor
+ */
+VRBRAINUtil::VRBRAINUtil(void)
+{
+    _safety_handle = orb_subscribe(ORB_ID(safety));
+}
+
+
+/*
+  start an instance of nsh
+ */
+bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream)
+{
+	VRBRAINUARTDriver *uart = (VRBRAINUARTDriver *)stream;
+	int fd;
+
+	// trigger exit in the other threads. This stops use of the
+	// various driver handles, and especially the px4io handle,
+	// which otherwise would cause a crash if px4io is stopped in
+	// the shell
+	_vrbrain_thread_should_exit = true;
+
+	// take control of stream fd
+	fd = uart->_get_fd();
+
+	// mark it blocking (nsh expects a blocking fd)
+        unsigned v;
+        v = fcntl(fd, F_GETFL, 0);
+        fcntl(fd, F_SETFL, v & ~O_NONBLOCK);	
+
+	// setup the UART on stdin/stdout/stderr
+	close(0);
+	close(1);
+	close(2);
+	dup2(fd, 0);
+	dup2(fd, 1);
+	dup2(fd, 2);
+
+	nsh_consolemain(0, NULL);
+
+	// this shouldn't happen
+	hal.console->printf("shell exited\n");
+	return true;
+}
+
+/*
+  return state of safety switch
+ */
+enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void)
+{
+    if (_safety_handle == -1) {
+        _safety_handle = orb_subscribe(ORB_ID(safety));
+    }
+    if (_safety_handle == -1) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    struct safety_s safety;
+    if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    if (!safety.safety_switch_available) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    if (safety.safety_off) {
+        return AP_HAL::Util::SAFETY_ARMED;
+    }
+    return AP_HAL::Util::SAFETY_DISARMED;
+}
+
+void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec)
+{
+    timespec ts;
+    ts.tv_sec = time_utc_usec/1.0e6;
+    ts.tv_nsec = (time_utc_usec % 1000000) * 1000;
+    clock_settime(CLOCK_REALTIME, &ts);    
+}
+
+/*
+  display VRBRAIN system identifer - board type and serial number
+ */
+bool VRBRAINUtil::get_system_id(char buf[40])
+{
+    uint8_t serialid[12];
+    memset(serialid, 0, sizeof(serialid));
+    get_board_serial(serialid);
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V4)
+    const char *board_type = "VRBRAINv4";
+#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V5)
+    const char *board_type = "VRBRAINv5";
+#elif defined(CONFIG_ARCH_BOARD_VRHERO_V1)
+    const char *board_type = "VRHEROv1";
+#endif
+    // this format is chosen to match the human_readable_serial()
+    // function in auth.c
+    snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+             board_type,
+             (unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3], 
+             (unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7], 
+             (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]); 
+    return true;
+}
+
+/**
+   how much free memory do we have in bytes.
+*/
+uint16_t VRBRAINUtil::available_memory(void)
+{
+    struct mallinfo mem;
+    mem = mallinfo();
+    if (mem.fordblks > 0xFFFF) {
+        return 0xFFFF;
+    }
+    return mem.fordblks;
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

libraries/AP_HAL_VRBRAIN/Util.h

@@ -0,0 +1,31 @@
+
+#ifndef __AP_HAL_VRBRAIN_UTIL_H__
+#define __AP_HAL_VRBRAIN_UTIL_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_VRBRAIN_Namespace.h"
+
+class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
+public:
+	VRBRAINUtil(void);
+    bool run_debug_shell(AP_HAL::BetterStream *stream);
+
+    enum safety_state safety_switch_state(void);
+
+    /*
+      set system clock in UTC microseconds
+     */
+    void set_system_clock(uint64_t time_utc_usec);
+
+    /*
+      get system identifier (STM32 serial number)
+     */
+    bool get_system_id(char buf[40]);
+
+    uint16_t available_memory(void);
+
+private:
+    int _safety_handle;
+};
+
+#endif // __AP_HAL_VRBRAIN_UTIL_H__