From fde5992b6db38087ff53e2aeaed133a74feb20df Mon Sep 17 00:00:00 2001 From: LukeMike Date: Tue, 5 Jul 2016 16:15:55 +0200 Subject: [PATCH] VRBRAIN: updated AP_HAL_VRBRAIN --- .../AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h | 32 +- libraries/AP_HAL_VRBRAIN/AnalogIn.cpp | 620 +++++----- libraries/AP_HAL_VRBRAIN/AnalogIn.h | 166 +-- libraries/AP_HAL_VRBRAIN/GPIO.cpp | 478 +++----- libraries/AP_HAL_VRBRAIN/GPIO.h | 112 +- .../AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp | 717 +++++------ libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h | 38 +- libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp | 143 +++ libraries/AP_HAL_VRBRAIN/RCInput.cpp | 255 ++-- libraries/AP_HAL_VRBRAIN/RCInput.h | 69 +- libraries/AP_HAL_VRBRAIN/RCOutput.cpp | 835 +++++++++---- libraries/AP_HAL_VRBRAIN/RCOutput.h | 117 +- libraries/AP_HAL_VRBRAIN/Scheduler.cpp | 709 ++++++----- libraries/AP_HAL_VRBRAIN/Scheduler.h | 180 +-- libraries/AP_HAL_VRBRAIN/Semaphores.cpp | 39 + libraries/AP_HAL_VRBRAIN/Semaphores.h | 20 + libraries/AP_HAL_VRBRAIN/Storage.cpp | 577 +++++---- libraries/AP_HAL_VRBRAIN/Storage.h | 79 +- libraries/AP_HAL_VRBRAIN/UARTDriver.cpp | 1081 +++++++++-------- libraries/AP_HAL_VRBRAIN/UARTDriver.h | 151 +-- libraries/AP_HAL_VRBRAIN/Util.cpp | 369 +++--- libraries/AP_HAL_VRBRAIN/Util.h | 103 +- libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp | 49 + 23 files changed, 3929 insertions(+), 3010 deletions(-) create mode 100644 libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp create mode 100644 libraries/AP_HAL_VRBRAIN/Semaphores.cpp create mode 100644 libraries/AP_HAL_VRBRAIN/Semaphores.h create mode 100644 libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp diff --git a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h index 54406950d7..0b7e6dfc29 100644 --- a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h +++ b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h @@ -1,14 +1,18 @@ -#pragma once - -namespace VRBRAIN { - class VRBRAINScheduler; - class VRBRAINUARTDriver; - class VRBRAINStorage; - class VRBRAINRCInput; - class VRBRAINRCOutput; - class VRBRAINAnalogIn; - class VRBRAINAnalogSource; - class VRBRAINUtil; - class VRBRAINGPIO; - class VRBRAINDigitalSource; -} +#pragma once + +namespace VRBRAIN { + class VRBRAINScheduler; + class VRBRAINUARTDriver; + class VRBRAINStorage; + class VRBRAINRCInput; + class VRBRAINRCOutput; + class VRBRAINAnalogIn; + class VRBRAINAnalogSource; + class VRBRAINUtil; + class VRBRAINGPIO; + class VRBRAINDigitalSource; + class NSHShellStream; + class VRBRAINI2CDriver; + class VRBRAIN_I2C; + class Semaphore; +} diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp index 2b87168283..0623176956 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp @@ -1,322 +1,298 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "AnalogIn.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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[] = { -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) - { 0, 3.3f/4096 }, - { 10, 3.3f/4096 }, - { 11, 3.3f/4096 }, -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - { 10, 3.3f/4096 }, -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - { 1, 3.3f/4096 }, - { 2, 3.3f/4096 }, - { 3, 3.3f/4096 }, - { 10, 3.3f/4096 }, -#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) - { 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), - _stop_pin(-1), - _settle_time_ms(0), - _value(initial_value), - _value_ratiometric(initial_value), - _latest_value(initial_value), - _sum_count(0), - _sum_value(0), - _sum_ratiometric(0) -{ -} - -void VRBRAINAnalogSource::set_stop_pin(uint8_t p) -{ - _stop_pin = p; -} - -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 = ARRAY_SIZE(pin_scaling); - for (uint8_t i=0; isuspend_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() : - _current_stop_pin_i(0), - _board_voltage(0), - _servorail_voltage(0), - _power_flags(0) -{} - -void VRBRAINAnalogIn::init() -{ - _adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); - if (_adc_fd == -1) { - AP_HAL::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)); -} - - -/* - move to the next stop pin - */ -void VRBRAINAnalogIn::next_stop_pin(void) -{ - // find the next stop pin. We start one past the current stop pin - // and wrap completely, so we do the right thing is there is only - // one stop pin - for (uint8_t i=1; i <= VRBRAIN_ANALOG_MAX_CHANNELS; i++) { - uint8_t idx = (_current_stop_pin_i + i) % VRBRAIN_ANALOG_MAX_CHANNELS; - VRBRAIN::VRBRAINAnalogSource *c = _channels[idx]; - if (c && c->_stop_pin != -1) { - // found another stop pin - _stop_pin_change_time = AP_HAL::millis(); - _current_stop_pin_i = idx; - - // set that pin high - hal.gpio->pinMode(c->_stop_pin, 1); - hal.gpio->write(c->_stop_pin, 1); - - // set all others low - for (uint8_t j=0; j_stop_pin != -1 && j != idx) { - hal.gpio->pinMode(c2->_stop_pin, 1); - hal.gpio->write(c2->_stop_pin, 0); - } - } - break; - } - } -} - -/* - called at 1kHz - */ -void VRBRAINAnalogIn::_timer_tick(void) -{ - // read adc at 100Hz - uint32_t now = AP_HAL::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]; - - // cope with initial setup of stop pin - if (_channels[_current_stop_pin_i] == NULL || - _channels[_current_stop_pin_i]->_stop_pin == -1) { - next_stop_pin(); - } - - /* 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_pin) { - // add a value if either there is no stop pin, or - // the stop pin has been settling for enough time - if (c->_stop_pin == -1 || - (_current_stop_pin_i == j && - AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) { - c->_add_value(buf_adc[i].am_data, _board_voltage); - if (c->_stop_pin != -1 && _current_stop_pin_i == j) { - next_stop_pin(); - } - } - } - } - } - } - - // 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_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; jprintln("Out of analog channels"); - return NULL; -} - -#endif // CONFIG_HAL_BOARD +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "AnalogIn.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "GPIO.h" + +#define ANLOGIN_DEBUGGING 0 + +// base voltage scaling for 12 bit 3.3V ADC +#define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f) + +#if ANLOGIN_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[] = { +#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) + { 10, 3.3f/4096 }, + { 11, 3.3f/4096 }, +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) + { 10, 3.3f/4096 }, +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) + { 1, 3.3f/4096 }, + { 2, 3.3f/4096 }, + { 3, 3.3f/4096 }, + { 10, 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), + _stop_pin(-1), + _settle_time_ms(0), + _value(initial_value), + _value_ratiometric(initial_value), + _latest_value(initial_value), + _sum_count(0), + _sum_value(0), + _sum_ratiometric(0) +{ + + + + + +} + +void VRBRAINAnalogSource::set_stop_pin(uint8_t p) +{ + _stop_pin = p; +} + +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 = ARRAY_SIZE(pin_scaling); + for (uint8_t i=0; isuspend_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() : + _current_stop_pin_i(0), + _board_voltage(0), + _servorail_voltage(0), + _power_flags(0) +{} + +void VRBRAINAnalogIn::init() +{ + _adc_fd = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + if (_adc_fd == -1) { + AP_HAL::panic("Unable to open " ADC0_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)); +} + + +/* + move to the next stop pin + */ +void VRBRAINAnalogIn::next_stop_pin(void) +{ + // find the next stop pin. We start one past the current stop pin + // and wrap completely, so we do the right thing is there is only + // one stop pin + for (uint8_t i=1; i <= VRBRAIN_ANALOG_MAX_CHANNELS; i++) { + uint8_t idx = (_current_stop_pin_i + i) % VRBRAIN_ANALOG_MAX_CHANNELS; + VRBRAIN::VRBRAINAnalogSource *c = _channels[idx]; + if (c && c->_stop_pin != -1) { + // found another stop pin + _stop_pin_change_time = AP_HAL::millis(); + _current_stop_pin_i = idx; + + // set that pin high + hal.gpio->pinMode(c->_stop_pin, 1); + hal.gpio->write(c->_stop_pin, 1); + + // set all others low + for (uint8_t j=0; j_stop_pin != -1 && j != idx) { + hal.gpio->pinMode(c2->_stop_pin, 1); + hal.gpio->write(c2->_stop_pin, 0); + } + } + break; + } + } +} + +/* + called at 1kHz + */ +void VRBRAINAnalogIn::_timer_tick(void) +{ + // read adc at 100Hz + uint32_t now = AP_HAL::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]; + + // cope with initial setup of stop pin + if (_channels[_current_stop_pin_i] == NULL || + _channels[_current_stop_pin_i]->_stop_pin == -1) { + next_stop_pin(); + } + + /* 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_pin) { + // add a value if either there is no stop pin, or + // the stop pin has been settling for enough time + if (c->_stop_pin == -1 || + (_current_stop_pin_i == j && + AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) { + c->_add_value(buf_adc[i].am_data, _board_voltage); + if (c->_stop_pin != -1 && _current_stop_pin_i == j) { + next_stop_pin(); + } + } + } + } + } + } + +} + +AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin) +{ + for (uint8_t j=0; jprintln("Out of analog channels"); + return NULL; +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.h b/libraries/AP_HAL_VRBRAIN/AnalogIn.h index 6bb976bb6a..bbb0f79945 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.h +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.h @@ -1,83 +1,83 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include - -#define VRBRAIN_ANALOG_MAX_CHANNELS 16 - - -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRHERO_V10) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN -1 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 1 -#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(); - - // implement stop pins - void set_stop_pin(uint8_t p); - void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; } - -private: - // what pin it is attached to - int16_t _pin; - int16_t _stop_pin; - uint16_t _settle_time_ms; - - // 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(); - 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]; - - // what pin is currently held low to stop a sonar from reading - uint8_t _current_stop_pin_i; - uint32_t _stop_pin_change_time; - - uint32_t _last_run; - float _board_voltage; - float _servorail_voltage; - uint16_t _power_flags; - - void next_stop_pin(void); -}; +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#pragma once + +#include "AP_HAL_VRBRAIN.h" +#include +#include + +#define VRBRAIN_ANALOG_MAX_CHANNELS 16 + + +#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) +#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 +#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11 +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) +#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 +#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN -1 +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) +#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 +#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 1 +#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(); + + // implement stop pins + void set_stop_pin(uint8_t p); + void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; } + +private: + // what pin it is attached to + int16_t _pin; + int16_t _stop_pin; + uint16_t _settle_time_ms; + + // 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(); + 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]; + + // what pin is currently held low to stop a sonar from reading + uint8_t _current_stop_pin_i; + uint32_t _stop_pin_change_time; + + uint32_t _last_run; + float _board_voltage; + float _servorail_voltage; + uint16_t _power_flags; + + void next_stop_pin(void); +}; diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.cpp b/libraries/AP_HAL_VRBRAIN/GPIO.cpp index 737b5a20b4..dc338925a3 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.cpp +++ b/libraries/AP_HAL_VRBRAIN/GPIO.cpp @@ -1,277 +1,201 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "GPIO.h" - -#include -#include -#include -#include - -/* VRBRAIN headers */ -#include -#include -#include - -#include -#include - -#define LOW 0 -#define HIGH 1 - -extern const AP_HAL::HAL& hal; - -using namespace VRBRAIN; - -VRBRAINGPIO::VRBRAINGPIO() -{} - -void VRBRAINGPIO::init() -{ - _led_fd = open(LED_DEVICE_PATH, O_RDWR); - if (_led_fd == -1) { - AP_HAL::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"); - } - if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); - } -#if defined(LED_EXT1) - if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n"); - } -#endif -#if defined(LED_EXT2) - if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n"); - } -#endif -#if defined(LED_EXT3) - if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n"); - } -#endif - -#if defined(BUZZER_EXT) - _buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR); - if (_buzzer_fd == -1) { - AP_HAL::panic("Unable to open " BUZZER_DEVICE_PATH); - } - if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n"); - } -#endif - -#if defined(GPIO_GPIO0_OUTPUT) - stm32_configgpio(GPIO_GPIO0_OUTPUT); -#endif - -#if defined(GPIO_GPIO1_OUTPUT) - stm32_configgpio(GPIO_GPIO1_OUTPUT); -#endif -} - -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) { - case EXTERNAL_RELAY1_PIN: -#if defined(GPIO_GPIO0_OUTPUT) - return (stm32_gpioread(GPIO_GPIO0_OUTPUT))?HIGH:LOW; -#endif - break; - case EXTERNAL_RELAY2_PIN: -#if defined(GPIO_GPIO1_OUTPUT) - return (stm32_gpioread(GPIO_GPIO1_OUTPUT))?HIGH:LOW; -#endif - break; - } - return LOW; -} - -void VRBRAINGPIO::write(uint8_t pin, uint8_t value) -{ - switch (pin) { - - case HAL_GPIO_A_LED_PIN: // Arming LED - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_GREEN); - } else { - ioctl(_led_fd, LED_ON, LED_GREEN); - } - break; - - case HAL_GPIO_B_LED_PIN: // not used yet - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_BLUE); - } else { - ioctl(_led_fd, LED_ON, LED_BLUE); - } - break; - - case HAL_GPIO_C_LED_PIN: // GPS LED - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_RED); - } else { - ioctl(_led_fd, LED_ON, LED_RED); - } - break; - - case EXTERNAL_LED_GPS: -#if defined(LED_EXT1) - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_EXT1); - } else { - ioctl(_led_fd, LED_ON, LED_EXT1); - } -#endif - break; - - case EXTERNAL_LED_ARMED: -#if defined(LED_EXT2) - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_EXT2); - } else { - ioctl(_led_fd, LED_ON, LED_EXT2); - } -#endif - break; - - case EXTERNAL_LED_MOTOR1: - break; - - case EXTERNAL_LED_MOTOR2: - break; - - case BUZZER_PIN: -#if defined(BUZZER_EXT) - if (value == LOW) { - ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT); - } else { - ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT); - } -#endif - break; - - case EXTERNAL_RELAY1_PIN: -#if defined(GPIO_GPIO0_OUTPUT) - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, (value==HIGH)); -#endif - break; - case EXTERNAL_RELAY2_PIN: -#if defined(GPIO_GPIO1_OUTPUT) - stm32_gpiowrite(GPIO_GPIO1_OUTPUT, (value==HIGH)); -#endif - break; - - } -} - -void VRBRAINGPIO::toggle(uint8_t pin) -{ - switch (pin) { - - case HAL_GPIO_A_LED_PIN: // Arming LED - ioctl(_led_fd, LED_TOGGLE, LED_GREEN); - break; - - case HAL_GPIO_B_LED_PIN: // not used yet - ioctl(_led_fd, LED_TOGGLE, LED_BLUE); - break; - - case HAL_GPIO_C_LED_PIN: // GPS LED - ioctl(_led_fd, LED_TOGGLE, LED_RED); - break; - - case EXTERNAL_LED_GPS: -#if defined(LED_EXT1) - ioctl(_led_fd, LED_TOGGLE, LED_EXT1); -#endif - break; - - case EXTERNAL_LED_ARMED: -#if defined(LED_EXT2) - ioctl(_led_fd, LED_TOGGLE, LED_EXT2); -#endif - break; - - case EXTERNAL_LED_MOTOR1: - - break; - - case EXTERNAL_LED_MOTOR2: - - break; - - case BUZZER_PIN: -#if defined(BUZZER_EXT) - ioctl(_buzzer_fd, BUZZER_TOGGLE, BUZZER_EXT); -#endif - break; - - default: - write(pin, !read(pin)); - break; - } -} - -/* 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 +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include "GPIO.h" + +#include +#include +#include +#include + +/* VRBRAIN headers */ +#include +#include +#include + +#include +#include + +#define LOW 0 +#define HIGH 1 + +extern const AP_HAL::HAL& hal; + +using namespace VRBRAIN; + +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); + } + 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"); + } + 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); + } + + _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0); + if (_gpio_fmu_fd == -1) { + AP_HAL::panic("Unable to open GPIO"); + } +#ifdef GPIO_SERVO_1 + if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_1) != 0) { + hal.console->printf("GPIO: Unable to setup GPIO_1\n"); + } +#endif +#ifdef GPIO_SERVO_2 + if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_2) != 0) { + hal.console->printf("GPIO: Unable to setup GPIO_2\n"); + } +#endif +#ifdef GPIO_SERVO_3 + if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_3) != 0) { + hal.console->printf("GPIO: Unable to setup GPIO_3\n"); + } +#endif +} + +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) { + +#ifdef GPIO_SERVO_3 + case EXTERNAL_RELAY1_PIN: { + uint32_t relays = 0; + ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays); + return (relays & GPIO_SERVO_3)?HIGH:LOW; + } +#endif + + } + return LOW; +} + +void VRBRAINGPIO::write(uint8_t pin, uint8_t value) +{ + switch (pin) { + + case HAL_GPIO_A_LED_PIN: // Arming LED + if (value == LOW) { + ioctl(_led_fd, LED_OFF, LED_GREEN); + } else { + ioctl(_led_fd, LED_ON, LED_GREEN); + } + break; + + case HAL_GPIO_B_LED_PIN: // not used yet + if (value == LOW) { + ioctl(_led_fd, LED_OFF, LED_BLUE); + } else { + ioctl(_led_fd, LED_ON, LED_BLUE); + } + break; + + case HAL_GPIO_C_LED_PIN: // GPS LED + if (value == LOW) { + ioctl(_led_fd, LED_OFF, LED_RED); + } else { + ioctl(_led_fd, LED_ON, LED_RED); + } + break; + +#ifdef GPIO_SERVO_1 + case EXTERNAL_LED_GPS: + ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_1); + break; +#endif + +#ifdef GPIO_SERVO_2 + case EXTERNAL_LED_ARMED: + ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_2); + break; +#endif + +#ifdef GPIO_SERVO_3 + case EXTERNAL_RELAY1_PIN: + ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_3); + break; +#endif + + + } +} + +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) +{ + /* + we use a combination of voltage on the USB connector and the + open of the /dev/ttyACM0 character device. This copes with + systems where the VBUS may go high even with no USB connected + (such as AUAV-X2) + */ + return stm32_gpioread(GPIO_OTGFS_VBUS) && _usb_connected; +} + + +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 diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.h b/libraries/AP_HAL_VRBRAIN/GPIO.h index 5d09dd6a1b..704bb82559 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.h +++ b/libraries/AP_HAL_VRBRAIN/GPIO.h @@ -1,52 +1,60 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#pragma once - -#include "AP_HAL_VRBRAIN.h" - - # define HAL_GPIO_A_LED_PIN 25 - # define HAL_GPIO_B_LED_PIN 26 - # define HAL_GPIO_C_LED_PIN 27 - # define EXTERNAL_LED_GPS 28 - # define EXTERNAL_LED_ARMED 29 - # define EXTERNAL_LED_MOTOR1 30 - # define EXTERNAL_LED_MOTOR2 31 - # define BUZZER_PIN 32 - # define EXTERNAL_RELAY1_PIN 33 - # define EXTERNAL_RELAY2_PIN 34 - # define HAL_GPIO_LED_ON HIGH - # define HAL_GPIO_LED_OFF LOW - -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 _buzzer_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; -}; +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#pragma once + +#include "AP_HAL_VRBRAIN.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + # define HAL_GPIO_A_LED_PIN 25 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 27 + # define EXTERNAL_LED_GPS 28 + # define EXTERNAL_LED_ARMED 29 + # define EXTERNAL_LED_MOTOR1 30 + # define EXTERNAL_LED_MOTOR2 31 + + # define EXTERNAL_RELAY1_PIN 33 + + # define HAL_GPIO_LED_ON HIGH + # define HAL_GPIO_LED_OFF LOW +#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); + + // used by UART code to avoid a hw bug in the AUAV-X2 + void set_usb_connected(void) { _usb_connected = true; } + +private: + int _led_fd = -1; + int _tone_alarm_fd = -1; + int _gpio_fmu_fd = -1; + + bool _usb_connected = false; +}; + +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; +}; diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp index 3958cfec64..f6810b22cf 100644 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp +++ b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp @@ -1,354 +1,363 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include - -#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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace VRBRAIN; - -static Empty::SPIDeviceManager spiDeviceManager; -static Empty::OpticalFlow optflowDriver; -//static Empty::GPIO gpioDriver; - -static Empty::I2CDeviceManager i2c_mgr_instance; - -static VRBRAINScheduler schedulerInstance; -static VRBRAINStorage storageDriver; -static VRBRAINRCInput rcinDriver; -static VRBRAINRCOutput rcoutDriver; -static VRBRAINAnalogIn analogIn; -static VRBRAINUtil utilInstance; -static VRBRAINGPIO gpioDriver; - -//We only support 3 serials for VRBRAIN at the moment -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) -#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" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) -#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 UARTE_DEFAULT_DEVICE "/dev/ttyS1" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_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 UARTE_DEFAULT_DEVICE "/dev/ttyS1" -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) -#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 UARTE_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 UARTE_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) -#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" -#else -#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" -#endif - - -// 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 */ - NULL, // uartF - &i2c_mgr_instance, - &spiDeviceManager, /* spi */ - &analogIn, /* analogin */ - &storageDriver, /* storage */ - &uartADriver, /* console */ - &gpioDriver, /* gpio */ - &rcinDriver, /* rcinput */ - &rcoutDriver, /* rcoutput */ - &schedulerInstance, /* scheduler */ - &utilInstance, /* util */ - &optflowDriver) /* optflow */ -{} - -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 */ -bool vrbrain_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, ¶m); -} - -/* - 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); - vrbrain_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(); - hal.rcin->init(); - hal.rcout->init(); - hal.analogin->init(); - 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 (vrbrain_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); - vrbrain_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::run(int argc, char * const argv[], Callbacks* callbacks) 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); - } - - assert(callbacks); - g_callbacks = callbacks; - - for (i=0; i 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 AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_VRBRAIN hal_vrbrain; - return hal_vrbrain; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#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 "I2CDevice.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace VRBRAIN; + +static Empty::SPIDeviceManager spiDeviceManager; +//static Empty::GPIO gpioDriver; + +static VRBRAINScheduler schedulerInstance; +static VRBRAINStorage storageDriver; +static VRBRAINRCInput rcinDriver; +static VRBRAINRCOutput rcoutDriver; +static VRBRAINAnalogIn analogIn; +static VRBRAINUtil utilInstance; +static VRBRAINGPIO gpioDriver; + +static VRBRAIN::I2CDeviceManager i2c_mgr_instance; + +#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) +#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" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) +#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" +#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" +#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" +#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) +#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" +#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" +#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" +#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) +#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 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 UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) +#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" +#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" +#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" +#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) +#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" +#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" +#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" +#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#else +#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" +#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" +#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" +#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTF_DEFAULT_DEVICE "/dev/null" +#endif + +// 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"); +static VRBRAINUARTDriver uartFDriver(UARTF_DEFAULT_DEVICE, "APM_uartF"); + +HAL_VRBRAIN::HAL_VRBRAIN() : + AP_HAL::HAL( + &uartADriver, /* uartA */ + &uartBDriver, /* uartB */ + &uartCDriver, /* uartC */ + &uartDDriver, /* uartD */ + &uartEDriver, /* uartE */ + &uartFDriver, /* uartF */ + &i2c_mgr_instance, + &spiDeviceManager, /* spi */ + &analogIn, /* analogin */ + &storageDriver, /* storage */ + &uartADriver, /* console */ + &gpioDriver, /* gpio */ + &rcinDriver, /* rcinput */ + &rcoutDriver, /* rcoutput */ + &schedulerInstance, /* scheduler */ + &utilInstance, /* util */ + NULL) /* no onboard optical flow */ +{} + +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 */ +bool vrbrain_ran_overtime; + +extern const AP_HAL::HAL& hal; + +/* + set the priority of the main APM task + */ +void hal_vrbrain_set_priority(uint8_t priority) +{ + struct sched_param param; + param.sched_priority = priority; + sched_setscheduler(daemon_task, SCHED_FIFO, ¶m); +} + +/* + 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 *) +{ + hal_vrbrain_set_priority(APM_OVERTIME_PRIORITY); + vrbrain_ran_overtime = true; +} + +static AP_HAL::HAL::Callbacks* g_callbacks; + +static int main_loop(int argc, char **argv) +{ + hal.uartA->begin(115200); + hal.uartB->begin(38400); + hal.uartC->begin(57600); + hal.uartD->begin(57600); + hal.uartE->begin(57600); + hal.scheduler->init(); + hal.rcin->init(); + hal.rcout->init(); + hal.analogin->init(); + 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 + */ + hal_vrbrain_set_priority(APM_STARTUP_PRIORITY); + + schedulerInstance.hal_initialized(); + + g_callbacks->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 + */ + hal_vrbrain_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); + + g_callbacks->loop(); + + if (vrbrain_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. + */ + hal_vrbrain_set_priority(APM_MAIN_PRIORITY); + perf_count(perf_overrun); + vrbrain_ran_overtime = false; + } + + perf_end(perf_loop); + + /* + give up 250 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(250); + } + 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::run(int argc, char * const argv[], Callbacks* callbacks) 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); + } + + assert(callbacks); + g_callbacks = callbacks; + + for (i=0; i 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 AP_HAL::HAL& AP_HAL::get_HAL() { + static const HAL_VRBRAIN hal_vrbrain; + return hal_vrbrain; +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h index 50345a82ad..144540f106 100644 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h +++ b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h @@ -1,18 +1,20 @@ -#pragma once - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_HAL_VRBRAIN.h" -#include "AP_HAL_VRBRAIN_Namespace.h" -#include -#include - -class HAL_VRBRAIN : public AP_HAL::HAL { -public: - HAL_VRBRAIN(); - void run(int argc, char* const argv[], Callbacks* callbacks) const override; -}; - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include "AP_HAL_VRBRAIN.h" +#include "AP_HAL_VRBRAIN_Namespace.h" +#include +#include + +class HAL_VRBRAIN : public AP_HAL::HAL { +public: + HAL_VRBRAIN(); + void run(int argc, char* const argv[], Callbacks* callbacks) const override; +}; + +void hal_vrbrain_set_priority(uint8_t priority); + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp b/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp new file mode 100644 index 0000000000..237dfb7565 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp @@ -0,0 +1,143 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + implementation of NSH shell as a stream, for use in nsh over MAVLink + + See GCS_serial_control.cpp for usage + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include +#include +#include +#include +#include +#include +#include +#include +#include "Scheduler.h" + +extern const AP_HAL::HAL& hal; + +#include "Util.h" +using namespace VRBRAIN; + +void NSHShellStream::shell_thread(void *arg) +{ + NSHShellStream *nsh = (NSHShellStream *)arg; + close(0); + close(1); + close(2); + dup2(nsh->child.in, 0); + dup2(nsh->child.out, 1); + dup2(nsh->child.out, 2); + + nsh_consolemain(0, NULL); + + nsh->shell_stdin = -1; + nsh->shell_stdout = -1; + nsh->child.in = -1; + nsh->child.out = -1; +} + +void NSHShellStream::start_shell(void) +{ + if (hal.util->available_memory() < 8192) { + if (!showed_memory_warning) { + showed_memory_warning = true; + hal.console->printf("Not enough memory for shell\n"); + } + return; + } + if (hal.util->get_soft_armed()) { + if (!showed_armed_warning) { + showed_armed_warning = true; + hal.console->printf("Disarm to start nsh shell\n"); + } + // don't allow shell start when armed + return; + } + + int p1[2], p2[2]; + + if (pipe(p1) != 0) { + return; + } + if (pipe(p2) != 0) { + return; + } + shell_stdin = p1[0]; + shell_stdout = p2[1]; + child.in = p2[0]; + child.out = p1[1]; + + pthread_attr_t thread_attr; + struct sched_param param; + + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); + + param.sched_priority = APM_SHELL_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&shell_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::NSHShellStream::shell_thread, this); +} + +size_t NSHShellStream::write(uint8_t b) +{ + if (shell_stdout == -1) { + start_shell(); + } + if (shell_stdout != -1) { + return ::write(shell_stdout, &b, 1); + } + return 0; +} + +size_t NSHShellStream::write(const uint8_t *buffer, size_t size) +{ + size_t ret = 0; + while (size > 0) { + if (write(*buffer++) != 1) { + break; + } + ret++; + size--; + } + return ret; +} + +int16_t NSHShellStream::read() +{ + if (shell_stdin == -1) { + start_shell(); + } + if (shell_stdin != -1) { + uint8_t b; + if (::read(shell_stdin, &b, 1) == 1) { + return b; + } + } + return -1; +} + +uint32_t NSHShellStream::available() +{ + uint32_t ret = 0; + if (ioctl(shell_stdin, FIONREAD, (unsigned long)&ret) == OK) { + return ret; + } + return 0; +} + +uint32_t NSHShellStream::txspace() +{ + uint32_t ret = 0; + if (ioctl(shell_stdout, FIONWRITE, (unsigned long)&ret) == OK) { + return ret; + } + return 0; +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.cpp b/libraries/AP_HAL_VRBRAIN/RCInput.cpp index 7d832ba32f..68893b6598 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCInput.cpp @@ -1,117 +1,138 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "RCInput.h" -#include -#include - -using namespace VRBRAIN; - -extern const AP_HAL::HAL& hal; - -void VRBRAINRCInput::init() -{ - _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin"); - _rc_sub = orb_subscribe(ORB_ID(input_rc)); - if (_rc_sub == -1) { - AP_HAL::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; - _last_read = _rcin.timestamp_last_signal; - _override_valid = false; - 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); - 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 +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "RCInput.h" +#include +#include +#include +#include +#include + +using namespace VRBRAIN; + +extern const AP_HAL::HAL& hal; + +void VRBRAINRCInput::init() +{ + _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin"); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + if (_rc_sub == -1) { + AP_HAL::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; + _last_read = _rcin.timestamp_last_signal; + _override_valid = false; + 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); + 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); +} + +bool VRBRAINRCInput::rc_bind(int dsmMode) +{ + + + + + + + + + + + + + + return true; +} + +#endif diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.h b/libraries/AP_HAL_VRBRAIN/RCInput.h index 678c0fecc0..fb44b33865 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.h +++ b/libraries/AP_HAL_VRBRAIN/RCInput.h @@ -1,31 +1,38 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include -#include - -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); - - 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; -}; +#pragma once + +#include "AP_HAL_VRBRAIN.h" +#include +#include +#include + + +#ifndef RC_INPUT_MAX_CHANNELS +#define RC_INPUT_MAX_CHANNELS 18 +#endif + +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); + + 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); + + bool rc_bind(int dsmMode); + +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; +}; diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp index 3dc21e70ee..4461a0060a 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp @@ -1,219 +1,616 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "RCOutput.h" - -#include -#include -#include -#include - -#include - -extern const AP_HAL::HAL& hal; - -using namespace VRBRAIN; - -void VRBRAINRCOutput::init() -{ - _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); - _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); - if (_pwm_fd == -1) { - AP_HAL::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; - - _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; - } -} - - -void VRBRAINRCOutput::_init_alt_channels(void) -{ - -} - -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 - _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 { - // we are setting low rates on the given channels - if (chmask & 0x07) { - _rate_mask &= ~0x07; - } - if (chmask & 0x38) { - _rate_mask &= ~0x38; - } - if (chmask & 0xC0) { - _rate_mask &= ~0xC0; - } - } - - 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<printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); - } -} - -void VRBRAINRCOutput::set_failsafe_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<printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); - } -} - -bool VRBRAINRCOutput::force_safety_on(void) -{ - int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); - return (ret == OK); -} - -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) { - return; - } - if (!(_enabled_channels & (1U<= _max_channel) { - _max_channel = ch + 1; - } - if (period_us != _period[ch]) { - _period[ch] = period_us; - _need_update = true; - up_pwm_servo_set(ch, period_us); - } -} - -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 50000) { - _need_update = true; - } - - if (_need_update && _pwm_fd != -1) { - _need_update = false; - perf_begin(_perf_rcout); - ::write(_pwm_fd, _period, _max_channel*sizeof(_period[0])); - perf_end(_perf_rcout); - _last_output = now; - } -} - -#endif // CONFIG_HAL_BOARD +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "RCOutput.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace VRBRAIN; + +/* + enable RCOUT_DEBUG_LATENCY to measure output latency using a logic + analyser. AUX6 will go high during the cork/push output. + */ +#define RCOUT_DEBUG_LATENCY 0 + +void VRBRAINRCOutput::init() +{ + _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); + _pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR); + if (_pwm_fd == -1) { + AP_HAL::panic("Unable to open " PWM_OUTPUT0_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; + } + + for (uint8_t i=0; iprintf("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; + } + + // publish actuator vaules on demand + _actuator_direct_pub = NULL; + + // and armed state + _actuator_armed_pub = NULL; +} + + +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"); + } +} + +/* + set output frequency on outputs associated with fd + */ +void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t 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 + if (_output_mode == MODE_PWM_ONESHOT) { + /* + set a 1Hz update for oneshot. This periodic output will + never actually trigger, instead we will directly trigger + the pulse after each push() + */ + freq_hz = 1; + } + //::printf("SET_UPDATE_RATE %u\n", (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; + } + _freq_hz = freq_hz; + } + + /* work out the new rate mask. The outputs have 4 groups of servos. + + Group 0: channels 0 1 2 + Group 1: channels 3 4 5 + Group 2: channels 6 7 + Group 8: channels 8 9 10 11 + + 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 || freq_hz == 1) { + // we are setting high rates on the given channels + _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; + } + if (_rate_mask & 0xF00) { + _rate_mask |= 0xF00; + } + } else { + // we are setting low rates on the given channels + if (chmask & 0x07) { + _rate_mask &= ~0x07; + } + if (chmask & 0x38) { + _rate_mask &= ~0x38; + } + if (chmask & 0xC0) { + _rate_mask &= ~0xC0; + } + if (chmask & 0xF00) { + _rate_mask &= ~0xF00; + } + } + + //::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); + } +} + +/* + set output frequency + */ +void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) +{ + if (freq_hz > 50 && _output_mode == MODE_PWM_ONESHOT) { + // rate is irrelevent in oneshot + return; + } + + // re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT + if (_pwm_fd != -1 && ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) { + 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) { + 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); + } + if (alt_mask && _alt_fd != -1) { + set_freq_fd(_alt_fd, alt_mask, freq_hz); + } +} + +uint16_t VRBRAINRCOutput::get_freq(uint8_t ch) +{ + if (_rate_mask & (1U<= VRBRAIN_NUM_OUTPUT_CHANNELS) { + return; + } + if (ch >= 8 && !(_enabled_channels & (1U<= VRBRAIN_NUM_OUTPUT_CHANNELS) { + return; + } + + _enabled_channels &= ~(1U<printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); + } +} + +void VRBRAINRCOutput::set_failsafe_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<printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); + } +} + +bool VRBRAINRCOutput::force_safety_on(void) +{ + _safety_state_request = AP_HAL::Util::SAFETY_DISARMED; + _safety_state_request_last_ms = 1; + return true; +} + +void VRBRAINRCOutput::force_safety_off(void) +{ + _safety_state_request = AP_HAL::Util::SAFETY_ARMED; + _safety_state_request_last_ms = 1; +} + +void VRBRAINRCOutput::force_safety_pending_requests(void) +{ + // check if there is a pending saftey_state change. If so (timer != 0) then set it. + 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 + _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(); + } 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(); + } + } +} + +void VRBRAINRCOutput::force_safety_no_wait(void) +{ + if (_safety_state_request_last_ms != 0) { + _safety_state_request_last_ms = 1; + force_safety_pending_requests(); + } +} + +void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us) +{ + if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) { + return; + } + if (!(_enabled_channels & (1U<= _max_channel) { + _max_channel = ch + 1; + } + /* + 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 + output + */ + if (period_us != _period[ch] || + _output_mode == MODE_PWM_ONESHOT) { + _period[ch] = period_us; + _need_update = true; + up_pwm_servo_set(ch, period_us); + } +} + +uint16_t VRBRAINRCOutput::read(uint8_t ch) +{ + if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) { + return 0; + } + // if px4io has given us a value for this channel use that, + // otherwise use the value we last sent. This makes it easier to + // observe the behaviour of failsafe in px4io + for (uint8_t i=0; i= 0 && + ch < _outputs[i].outputs.noutputs && + _outputs[i].outputs.output[ch] > 0) { + return _outputs[i].outputs.output[ch]; + } + } + return _period[ch]; +} + +void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len) +{ + for (uint8_t i=0; i= VRBRAIN_NUM_OUTPUT_CHANNELS) { + return 0; + } + + return _period[ch]; +} + +void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len) +{ + for (uint8_t i=0; i 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; isafety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { + _arm_actuators(true); + } +} + +void VRBRAINRCOutput::_send_outputs(void) +{ + uint32_t now = AP_HAL::micros(); + + if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) { + // no channels enabled + _arm_actuators(false); + goto update_pwm; + } + + // always send at least at 20Hz, otherwise the IO board may think + // we are dead + if (now - _last_output > 50000) { + _need_update = true; + } + + // check for PWM count changing. This can happen then the user changes BRD_PWM_COUNT + if (now - _last_config_us > 1000000) { + if (_pwm_fd != -1) { + ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count); + } + if (_alt_fd != -1) { + ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count); + } + _last_config_us = now; + } + + if (_need_update && _pwm_fd != -1) { + _need_update = false; + perf_begin(_perf_rcout); + uint8_t to_send = _max_channel<_servo_count?_max_channel:_servo_count; + if (_sbus_enabled) { + to_send = _max_channel; + } + if (to_send > 0) { + for (int i=to_send-1; i >= 0; i--) { + if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) { + to_send = i; + } else { + break; + } + } + } + if (to_send > 0) { + ::write(_pwm_fd, _period, to_send*sizeof(_period[0])); + } + if (_max_channel > _servo_count) { + // maybe send updates to alt_fd + if (_alt_fd != -1 && _alt_servo_count > 0) { + uint8_t n = _max_channel - _servo_count; + if (n > _alt_servo_count) { + n = _alt_servo_count; + } + if (n > 0) { + ::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0])); + } + } + } + + // also publish to actuator_direct + _publish_actuators(); + + perf_end(_perf_rcout); + _last_output = now; + } + +update_pwm: + for (uint8_t i=0; i= 0 && + orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 && + rc_updated) { + orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs); + } + } + +} + +void VRBRAINRCOutput::cork() +{ +#if RCOUT_DEBUG_LATENCY + hal.gpio->pinMode(55, HAL_GPIO_OUTPUT); + hal.gpio->write(55, 1); +#endif + _corking = true; +} + +void VRBRAINRCOutput::push() +{ +#if RCOUT_DEBUG_LATENCY + hal.gpio->pinMode(55, HAL_GPIO_OUTPUT); + hal.gpio->write(55, 0); +#endif + _corking = false; + if (_output_mode == MODE_PWM_ONESHOT) { + // run timer immediately in oneshot mode + _send_outputs(); + } +} + +void VRBRAINRCOutput::_timer_tick(void) +{ + if (_output_mode != MODE_PWM_ONESHOT) { + /* in oneshot mode the timer does nothing as the outputs are + * sent from push() */ + _send_outputs(); + } + + force_safety_pending_requests(); +} + +/* + enable sbus output + */ +bool VRBRAINRCOutput::enable_sbus_out(uint16_t rate_hz) +{ + int fd = open("/dev/px4io", 0); + if (fd == -1) { + return false; + } + for (uint8_t tries=0; tries<10; tries++) { + if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) { + continue; + } + if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate_hz) != 0) { + continue; + } + close(fd); + _sbus_enabled = true; + return true; + } + close(fd); + return false; +} + +/* + setup output mode + */ +void VRBRAINRCOutput::set_output_mode(enum output_mode mode) +{ + if (_output_mode == mode) { + // no change + return; + } + if (mode == MODE_PWM_ONESHOT) { + // when using oneshot we don't want the regular pulses. The + // best we can do with the current PX4Firmware code is ask for + // 1Hz. This does still produce pulses, but the trigger calls + // 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); + } + _output_mode = mode; + if (_output_mode == MODE_PWM_ONESHOT) { + //::printf("enable oneshot\n"); + ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1); + if (_alt_fd != -1) { + ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1); + } + } else { + ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0); + if (_alt_fd != -1) { + ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0); + } + } +} + + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.h b/libraries/AP_HAL_VRBRAIN/RCOutput.h index 429c703c24..7d2dbf3749 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.h +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.h @@ -1,41 +1,76 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include - -#define VRBRAIN_NUM_OUTPUT_CHANNELS 16 - -class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput -{ -public: - void init(); - 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); - 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 set_failsafe_pwm(uint32_t chmask, uint16_t period_us); - bool force_safety_on(void); - void force_safety_off(void); - - void _timer_tick(void); - -private: - int _pwm_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; - - uint32_t _rate_mask; - uint16_t _enabled_channels; - - void _init_alt_channels(void); -}; +#pragma once + +#include "AP_HAL_VRBRAIN.h" +#include +#include +#include + +#define VRBRAIN_NUM_OUTPUT_CHANNELS 16 + +class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput +{ +public: + void init() override; + void set_freq(uint32_t chmask, uint16_t freq_hz) override; + uint16_t get_freq(uint8_t ch) override; + void enable_ch(uint8_t ch) override; + void disable_ch(uint8_t ch) override; + void write(uint8_t ch, uint16_t period_us) override; + uint16_t read(uint8_t ch) override; + void read(uint16_t* period_us, uint8_t len) override; + uint16_t read_last_sent(uint8_t ch) override; + void read_last_sent(uint16_t* period_us, uint8_t len) override; + void set_safety_pwm(uint32_t chmask, uint16_t period_us) override; + void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override; + bool force_safety_on(void) override; + void force_safety_off(void) override; + void force_safety_no_wait(void) override; + void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override { + _esc_pwm_min = min_pwm; + _esc_pwm_max = max_pwm; + } + void cork(); + void push(); + + void set_output_mode(enum output_mode mode) override; + + void _timer_tick(void); + bool enable_sbus_out(uint16_t rate_hz) override; + +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; + bool _sbus_enabled:1; + perf_counter_t _perf_rcout; + uint32_t _last_output; + uint32_t _last_config_us; + unsigned _servo_count; + unsigned _alt_servo_count; + uint32_t _rate_mask; + uint16_t _enabled_channels; + struct { + int pwm_sub; + actuator_outputs_s outputs; + } _outputs[ORB_MULTI_MAX_INSTANCES] {}; + actuator_armed_s _armed; + + orb_advert_t _actuator_direct_pub = NULL; + orb_advert_t _actuator_armed_pub = NULL; + uint16_t _esc_pwm_min = 0; + uint16_t _esc_pwm_max = 0; + + 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); + 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); +}; diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp index a2a9e4ab35..2ee883d408 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp @@ -1,321 +1,388 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_HAL_VRBRAIN.h" -#include "Scheduler.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "UARTDriver.h" -#include "AnalogIn.h" -#include "Storage.h" -#include "RCOutput.h" -#include "RCInput.h" -#include - -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() -{ - _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, ¶m); - 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, ¶m); - 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, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_io_thread, this); -} - -/** - 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; - } - uint64_t start = AP_HAL::micros64(); - uint64_t dt; - while ((dt=(AP_HAL::micros64() - 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 = AP_HAL::micros64(); - - while ((AP_HAL::micros64() - 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]) { - _timer_proc[i](); - } - } - } else if (called_from_timer_thread) { - _timer_event_missed = true; - } - - // and the failsafe, if one is setup - if (_failsafe) { - _failsafe(); - } - - // process analog input - ((VRBRAINAnalogIn *)hal.analogin)->_timer_tick(); - - _in_timer_proc = false; -} - -extern bool vrbrain_ran_overtime; - -void *VRBRAINScheduler::_timer_thread(void) -{ - uint32_t last_ran_overtime = 0; - 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(); - - if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) { - last_ran_overtime = AP_HAL::millis(); -// printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); -// hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); - } - } - 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]) { - _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; -} - -bool VRBRAINScheduler::in_timerprocess() -{ - return getpid() != _main_task_pid; -} - -void VRBRAINScheduler::system_initialized() { - if (_initialized) { - AP_HAL::panic("PANIC: scheduler::system_initialized called" - "more than once"); - } - _initialized = true; -} - -#endif +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include "AP_HAL_VRBRAIN.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "UARTDriver.h" +#include "AnalogIn.h" +#include "Storage.h" +#include "RCOutput.h" +#include "RCInput.h" +#include + +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_storage_timer(perf_alloc(PC_ELAPSED, "APM_storage_timers")), + _perf_delay(perf_alloc(PC_ELAPSED, "APM_delay")) +{} + +void VRBRAINScheduler::init() +{ + _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, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&_timer_thread_ctx, &thread_attr, &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, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&_uart_thread_ctx, &thread_attr, &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, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&_io_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_io_thread, this); + + // the storage thread runs at just above IO priority + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 1024); + + param.sched_priority = APM_STORAGE_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this); +} + +/** + 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); + memset(&wait_call, 0, sizeof(wait_call)); + 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); + delay_microseconds_semaphore(usec); + perf_end(_perf_delay); +} + +/* + wrapper around sem_post that boosts main thread priority + */ +static void sem_post_boost(sem_t *sem) +{ + hal_vrbrain_set_priority(APM_MAIN_PRIORITY_BOOST); + sem_post(sem); +} + +/* + return the main thread to normal priority + */ +static void set_normal_priority(void *sem) +{ + hal_vrbrain_set_priority(APM_MAIN_PRIORITY); +} + +/* + a variant of delay_microseconds that boosts priority to + APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC + microseconds when the time completes. This significantly improves + the regularity of timing of the main loop as it takes + */ +void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec) +{ + sem_t wait_semaphore; + static struct hrt_call wait_call; + sem_init(&wait_semaphore, 0, 0); + hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore); + sem_wait(&wait_semaphore); + hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, NULL); +} + +void VRBRAINScheduler::delay(uint16_t ms) +{ + if (in_timerprocess()) { + ::printf("ERROR: delay() from timer process\n"); + return; + } + perf_begin(_perf_delay); + uint64_t start = AP_HAL::micros64(); + + while ((AP_HAL::micros64() - 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) +{ + // disarm motors to ensure they are off during a bootloader upload + hal.rcout->force_safety_on(); + hal.rcout->force_safety_no_wait(); + + // delay to ensure the async force_saftey operation completes + delay(500); + + px4_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]) { + _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; +} + +extern bool vrbrain_ran_overtime; + +void *VRBRAINScheduler::_timer_thread(void *arg) +{ + VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + uint32_t last_ran_overtime = 0; + + while (!sched->_hal_initialized) { + poll(NULL, 0, 1); + } + while (!_vrbrain_thread_should_exit) { + sched->delay_microseconds_semaphore(1000); + + // run registered timers + perf_begin(sched->_perf_timers); + sched->_run_timers(true); + perf_end(sched->_perf_timers); + + // process any pending RC output requests + ((VRBRAINRCOutput *)hal.rcout)->_timer_tick(); + + // process any pending RC input requests + ((VRBRAINRCInput *)hal.rcin)->_timer_tick(); + + if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) { + last_ran_overtime = AP_HAL::millis(); +#if 0 + printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); + hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); +#endif + } + } + 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]) { + _io_proc[i](); + } + } + } + + _in_io_proc = false; +} + +void *VRBRAINScheduler::_uart_thread(void *arg) +{ + VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + + while (!sched->_hal_initialized) { + poll(NULL, 0, 1); + } + while (!_vrbrain_thread_should_exit) { + sched->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(); + ((VRBRAINUARTDriver *)hal.uartF)->_timer_tick(); + } + return NULL; +} + +void *VRBRAINScheduler::_io_thread(void *arg) +{ + VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + + while (!sched->_hal_initialized) { + poll(NULL, 0, 1); + } + while (!_vrbrain_thread_should_exit) { + poll(NULL, 0, 1); + + // run registered IO processes + perf_begin(sched->_perf_io_timers); + sched->_run_io(); + perf_end(sched->_perf_io_timers); + } + return NULL; +} + +void *VRBRAINScheduler::_storage_thread(void *arg) +{ + VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + + while (!sched->_hal_initialized) { + poll(NULL, 0, 1); + } + while (!_vrbrain_thread_should_exit) { + poll(NULL, 0, 10); + + // process any pending storage writes + perf_begin(sched->_perf_storage_timer); + ((VRBRAINStorage *)hal.storage)->_timer_tick(); + perf_end(sched->_perf_storage_timer); + } + return NULL; +} + +bool VRBRAINScheduler::in_timerprocess() +{ + return getpid() != _main_task_pid; +} + +void VRBRAINScheduler::system_initialized() { + if (_initialized) { + AP_HAL::panic("PANIC: scheduler::system_initialized called" + "more than once"); + } + _initialized = true; +} + +#endif diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h index 6b78c27f0c..c941a27bfe 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.h @@ -1,78 +1,102 @@ -#pragma once - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "AP_HAL_VRBRAIN_Namespace.h" -#include -#include -#include -#include - -#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 delay(uint16_t ms); - 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); - - bool in_timerprocess(); - 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_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 +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "AP_HAL_VRBRAIN_Namespace.h" +#include +#include +#include +#include + +#define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8 + +#define APM_MAIN_PRIORITY_BOOST 241 +#define APM_MAIN_PRIORITY 180 +#define APM_TIMER_PRIORITY 181 +#define APM_UART_PRIORITY 60 +#define APM_STORAGE_PRIORITY 59 +#define APM_IO_PRIORITY 58 +#define APM_SHELL_PRIORITY 57 +#define APM_OVERTIME_PRIORITY 10 +#define APM_STARTUP_PRIORITY 10 + +/* how long to boost priority of the main thread for each main + loop. This needs to be long enough for all interrupt-level drivers + (mostly SPI drivers) to run, and for the main loop of the vehicle + code to start the AHRS update. + + Priority boosting of the main thread in delay_microseconds_boost() + avoids the problem that drivers in hpwork all happen to run right + at the start of the period where the main vehicle loop is calling + wait_for_sample(). That causes main loop timing jitter, which + reduces performance. Using the priority boost the main loop + temporarily runs at a priority higher than hpwork and the timer + thread, which results in much more consistent loop timing. +*/ +#define APM_MAIN_PRIORITY_BOOST_USEC 150 + +#define APM_MAIN_THREAD_STACK_SIZE 8192 + +/* Scheduler implementation: */ +class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler { +public: + VRBRAINScheduler(); + /* AP_HAL::Scheduler methods */ + + void init(); + void delay(uint16_t ms); + void delay_microseconds(uint16_t us); + void delay_microseconds_boost(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); + + bool in_timerprocess(); + 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_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 _storage_thread_ctx; + pthread_t _uart_thread_ctx; + + static void *_timer_thread(void *arg); + static void *_io_thread(void *arg); + static void *_storage_thread(void *arg); + static void *_uart_thread(void *arg); + + 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_storage_timer; + perf_counter_t _perf_delay; +}; +#endif diff --git a/libraries/AP_HAL_VRBRAIN/Semaphores.cpp b/libraries/AP_HAL_VRBRAIN/Semaphores.cpp new file mode 100644 index 0000000000..9b64a8a21e --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/Semaphores.cpp @@ -0,0 +1,39 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include "Semaphores.h" + +extern const AP_HAL::HAL& hal; + +using namespace VRBRAIN; + +bool Semaphore::give() +{ + return pthread_mutex_unlock(&_lock) == 0; +} + +bool Semaphore::take(uint32_t timeout_ms) +{ + if (timeout_ms == 0) { + return pthread_mutex_lock(&_lock) == 0; + } + if (take_nonblocking()) { + return true; + } + uint64_t start = AP_HAL::micros64(); + do { + hal.scheduler->delay_microseconds(200); + if (take_nonblocking()) { + return true; + } + } while ((AP_HAL::micros64() - start) < timeout_ms*1000); + return false; +} + +bool Semaphore::take_nonblocking() +{ + return pthread_mutex_trylock(&_lock) == 0; +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/Semaphores.h b/libraries/AP_HAL_VRBRAIN/Semaphores.h new file mode 100644 index 0000000000..6efe91c01a --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/Semaphores.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "AP_HAL_VRBRAIN.h" +#include + +class VRBRAIN::Semaphore : public AP_HAL::Semaphore { +public: + Semaphore() { + pthread_mutex_init(&_lock, NULL); + } + bool give(); + bool take(uint32_t timeout_ms); + bool take_nonblocking(); +private: + pthread_mutex_t _lock; +}; +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/Storage.cpp b/libraries/AP_HAL_VRBRAIN/Storage.cpp index 7c0549188b..c77d286898 100644 --- a/libraries/AP_HAL_VRBRAIN/Storage.cpp +++ b/libraries/AP_HAL_VRBRAIN/Storage.cpp @@ -1,268 +1,309 @@ -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include -#include -#include -#include -#include -#include -#include - -#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) { - 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); - } - if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) { - AP_HAL::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) { - 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); - } - if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) { - AP_HAL::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) { - 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; - 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"); - } - 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) { - 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); -#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>VRBRAIN_STORAGE_LINE_SHIFT; - line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; - line++) { - _dirty_mask |= 1U << line; - } -} - -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_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_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< +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 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; + +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); + + // 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); +#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>VRBRAIN_STORAGE_LINE_SHIFT; + line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; + line++) { + _dirty_mask |= 1U << line; + } +} + +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_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::bus_lock(bool lock) +{ + + + + + + + + + + + + + + + + +} + +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_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< -#include "AP_HAL_VRBRAIN_Namespace.h" -#include - -#define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE -#define VRBRAIN_STORAGE_MAX_WRITE 512 -#define VRBRAIN_STORAGE_LINE_SHIFT 9 -#define VRBRAIN_STORAGE_LINE_SIZE (1< +#include "AP_HAL_VRBRAIN_Namespace.h" +#include + +#define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE +#define VRBRAIN_STORAGE_MAX_WRITE 512 +#define VRBRAIN_STORAGE_LINE_SHIFT 9 +#define VRBRAIN_STORAGE_LINE_SIZE (1< - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "UARTDriver.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace VRBRAIN; - -extern const AP_HAL::HAL& hal; - -VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name) : - _devpath(devpath), - _fd(-1), - _baudrate(57600), - _initialised(false), - _in_timer(false), - _perf_uart(perf_alloc(PC_ELAPSED, perf_name)), - _os_start_auto_space(-1), - _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; - } - } - - 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); - if (fcontrol == FLOW_CONTROL_AUTO) { - // reset flow control auto state machine - _total_written = 0; - _first_write_time = 0; - } - _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 ((AP_HAL::millis() - _last_initialise_attempt_ms) < 2000) { - return; - } - _last_initialise_attempt_ms = AP_HAL::millis(); - if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_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; } - -/* - return number of bytes available to be read from the buffer - */ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "UARTDriver.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "GPIO.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), + _initialised(false), + _in_timer(false), + _perf_uart(perf_alloc(PC_ELAPSED, perf_name)), + _os_start_auto_space(-1), + _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 = 4096; + 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; + } + } + + 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) { + if (strcmp(_devpath, "/dev/ttyACM0") == 0) { + ((VRBRAINGPIO *)hal.gpio)->set_usb_connected(); + } + ::printf("initialised %s OK %u %u\n", _devpath, + (unsigned)_writebuf_size, (unsigned)_readbuf_size); + } + _initialised = true; + } + _uart_owner_pid = getpid(); + +} + +void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol) +{ + if (_fd == -1) { + return; + } + struct termios t; + tcgetattr(_fd, &t); + // we already enabled CRTS_IFLOW above, just enable output flow control + if (fcontrol != FLOW_CONTROL_DISABLE) { + t.c_cflag |= CRTSCTS; + } else { + t.c_cflag &= ~CRTSCTS; + } + tcsetattr(_fd, TCSANOW, &t); + if (fcontrol == FLOW_CONTROL_AUTO) { + // reset flow control auto state machine + _total_written = 0; + _first_write_time = 0; + } + _flow_control = fcontrol; +} + +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 ((AP_HAL::millis() - _last_initialise_attempt_ms) < 2000) { + return; + } + _last_initialise_attempt_ms = AP_HAL::millis(); + if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_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; } + +/* + return number of bytes available to be read from the buffer + */ uint32_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 - */ +{ + 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 + */ uint32_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 - - // FIONWRITE is also used for auto flow control detection - // Assume output flow control is not working if: - // port is configured for auto flow control - // and this is not the first write since flow control turned on - // and no data has been removed from the buffer since flow control turned on - // and more than .5 seconds elapsed after writing a total of > 5 characters - // - - int nwrite = 0; - - if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) { - if (_flow_control == FLOW_CONTROL_AUTO) { - if (_first_write_time == 0) { - if (_total_written == 0) { - // save the remaining buffer bytes for comparison next write - _os_start_auto_space = nwrite; - } - } else { - if (_os_start_auto_space - nwrite + 1 >= _total_written && - (AP_HAL::micros64() - _first_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 = AP_HAL::micros64(); - _total_written += ret; - if (! _first_write_time && _total_written > 5) { - _first_write_time = _last_write_time; - } - return ret; - } - - if (AP_HAL::micros64() - _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 (AP_HAL::micros64() - _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 = AP_HAL::micros64(); - _initialised = true; - } -#else - _last_write_time = AP_HAL::micros64(); -#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) { - uint16_t n1 = _writebuf_size - _writebuf_head; - perf_begin(_perf_uart); - if (n1 >= n) { - // do as a single write - _write_fd(&_writebuf[_writebuf_head], n); - } else { - // split into two writes - 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) { - uint16_t n1 = _readbuf_size - _readbuf_tail; - perf_begin(_perf_uart); - if (n1 >= n) { - // one read will do - assert(_readbuf_tail+n <= _readbuf_size); - _read_fd(&_readbuf[_readbuf_tail], n); - } else { - 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 - +{ + 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 (_uart_owner_pid != getpid()){ + return -1; + } + 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 (_uart_owner_pid != getpid()){ + return 0; + } + if (!_initialised) { + try_initialise(); + 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 (_uart_owner_pid != getpid()){ + return 0; + } + if (!_initialised) { + try_initialise(); + 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 + + // FIONWRITE is also used for auto flow control detection + // Assume output flow control is not working if: + // port is configured for auto flow control + // and this is not the first write since flow control turned on + // and no data has been removed from the buffer since flow control turned on + // and more than .5 seconds elapsed after writing a total of > 5 characters + // + + int nwrite = 0; + + if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) { + if (_flow_control == FLOW_CONTROL_AUTO) { + if (_first_write_time == 0) { + if (_total_written == 0) { + // save the remaining buffer bytes for comparison next write + _os_start_auto_space = nwrite; + } + } else { + if (_os_start_auto_space - nwrite + 1 >= _total_written && + (AP_HAL::micros64() - _first_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 = AP_HAL::micros64(); + _total_written += ret; + if (! _first_write_time && _total_written > 5) { + _first_write_time = _last_write_time; + } + return ret; + } + + if (AP_HAL::micros64() - _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 (AP_HAL::micros64() - _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 = AP_HAL::micros64(); + _initialised = true; + } +#else + _last_write_time = AP_HAL::micros64(); +#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) { + uint16_t n1 = _writebuf_size - _writebuf_head; + perf_begin(_perf_uart); + if (n1 >= n) { + // do as a single write + _write_fd(&_writebuf[_writebuf_head], n); + } else { + // split into two writes + 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) { + uint16_t n1 = _readbuf_size - _readbuf_tail; + perf_begin(_perf_uart); + if (n1 >= n) { + // one read will do + assert(_readbuf_tail+n <= _readbuf_size); + _read_fd(&_readbuf[_readbuf_tail], n); + } else { + 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 + diff --git a/libraries/AP_HAL_VRBRAIN/UARTDriver.h b/libraries/AP_HAL_VRBRAIN/UARTDriver.h index 27c00d738f..61a588ea1c 100644 --- a/libraries/AP_HAL_VRBRAIN/UARTDriver.h +++ b/libraries/AP_HAL_VRBRAIN/UARTDriver.h @@ -1,77 +1,80 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include - -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 */ +#pragma once + +#include "AP_HAL_VRBRAIN.h" +#include + +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 */ uint32_t available() override; uint32_t txspace() override; int16_t read() override; - - /* 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 _first_write_time; - uint64_t _last_write_time; - - void try_initialise(void); - uint32_t _last_initialise_attempt_ms; - - uint32_t _os_start_auto_space; - uint32_t _total_read; - uint32_t _total_written; - enum flow_control _flow_control; -}; + + /* 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 _first_write_time; + uint64_t _last_write_time; + + void try_initialise(void); + uint32_t _last_initialise_attempt_ms; + + uint32_t _os_start_auto_space; + uint32_t _total_read; + uint32_t _total_written; + enum flow_control _flow_control; + + pid_t _uart_owner_pid; + +}; diff --git a/libraries/AP_HAL_VRBRAIN/Util.cpp b/libraries/AP_HAL_VRBRAIN/Util.cpp index 9d96a90c2f..816ee6364a 100644 --- a/libraries/AP_HAL_VRBRAIN/Util.cpp +++ b/libraries/AP_HAL_VRBRAIN/Util.cpp @@ -1,140 +1,229 @@ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include -#include -#include -#include -#include -#include "UARTDriver.h" -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -#include "Util.h" -using namespace VRBRAIN; - -extern bool _vrbrain_thread_should_exit; - -/* - constructor - */ -VRBRAINUtil::VRBRAINUtil(void) : Util() -{ - _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.0e6f; - 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_V45) - const char *board_type = "VRBRAINv45"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) - const char *board_type = "VRBRAINv51"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) - const char *board_type = "VRBRAINv52"; -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - const char *board_type = "VRUBRAINv51"; -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - const char *board_type = "VRUBRAINv52"; -#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) - const char *board_type = "VRHEROv10"; -#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. -*/ -uint32_t VRBRAINUtil::available_memory(void) -{ - return mallinfo().fordblks; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include +#include +#include +#include +#include +#include +#include +#include "UARTDriver.h" +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#include "Util.h" +using namespace VRBRAIN; + +extern bool _vrbrain_thread_should_exit; + +/* + constructor + */ +VRBRAINUtil::VRBRAINUtil(void) : Util() +{ + _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.0e6f; + 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_V45) + const char *board_type = "VRBRAINv45"; +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) + const char *board_type = "VRBRAINv51"; +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) + const char *board_type = "VRBRAINv52"; +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) + const char *board_type = "VRUBRAINv51"; +#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) + const char *board_type = "VRUBRAINv52"; +#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) + const char *board_type = "VRCOREv10"; +#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) + const char *board_type = "VRBRAINv54"; +#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. +*/ +uint32_t VRBRAINUtil::available_memory(void) +{ + return mallinfo().fordblks; +} + +/* + AP_HAL wrapper around PX4 perf counters + */ +VRBRAINUtil::perf_counter_t VRBRAINUtil::perf_alloc(VRBRAINUtil::perf_counter_type t, const char *name) +{ + ::perf_counter_type vrbrain_t; + switch (t) { + case VRBRAINUtil::PC_COUNT: + vrbrain_t = ::PC_COUNT; + break; + case VRBRAINUtil::PC_ELAPSED: + vrbrain_t = ::PC_ELAPSED; + break; + case VRBRAINUtil::PC_INTERVAL: + vrbrain_t = ::PC_INTERVAL; + break; + default: + return NULL; + } + return (perf_counter_t)::perf_alloc(vrbrain_t, name); +} + +void VRBRAINUtil::perf_begin(perf_counter_t h) +{ + ::perf_begin((::perf_counter_t)h); +} + +void VRBRAINUtil::perf_end(perf_counter_t h) +{ + ::perf_end((::perf_counter_t)h); +} + +void VRBRAINUtil::perf_count(perf_counter_t h) +{ + ::perf_count((::perf_counter_t)h); +} + +void VRBRAINUtil::set_imu_temp(float current) +{ + if (!_heater.target || *_heater.target == -1) { + return; + } + + // average over temperatures to remove noise + _heater.count++; + _heater.sum += current; + + // update once a second + uint32_t now = AP_HAL::millis(); + if (now - _heater.last_update_ms < 1000) { + return; + } + _heater.last_update_ms = now; + + current = _heater.sum / _heater.count; + _heater.sum = 0; + _heater.count = 0; + + // experimentally tweaked for Pixhawk2 + const float kI = 0.3f; + const float kP = 200.0f; + + float err = ((float)*_heater.target) - current; + + _heater.integrator += kI * err; + _heater.integrator = constrain_float(_heater.integrator, 0, 70); + + float output = constrain_float(kP * err + _heater.integrator, 0, 100); + + // hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err); + + if (_heater.fd == -1) { + _heater.fd = open("/dev/px4io", O_RDWR); + } + if (_heater.fd != -1) { + ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output); + } + +} + +void VRBRAINUtil::set_imu_target_temp(int8_t *target) +{ + _heater.target = target; +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_HAL_VRBRAIN/Util.h b/libraries/AP_HAL_VRBRAIN/Util.h index 0571b659ff..964c9a9122 100644 --- a/libraries/AP_HAL_VRBRAIN/Util.h +++ b/libraries/AP_HAL_VRBRAIN/Util.h @@ -1,27 +1,76 @@ -#pragma once - -#include -#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]); - - uint32_t available_memory(void) override; - -private: - int _safety_handle; -}; +#pragma once + +#include +#include "AP_HAL_VRBRAIN_Namespace.h" +#include "Semaphores.h" + +class VRBRAIN::NSHShellStream : public AP_HAL::Stream { +public: + size_t write(uint8_t); + size_t write(const uint8_t *buffer, size_t size); + int16_t read() override; + uint32_t available() override; + uint32_t txspace() override; +private: + int shell_stdin = -1; + int shell_stdout = -1; + pthread_t shell_thread_ctx; + + struct { + int in = -1; + int out = -1; + } child; + bool showed_memory_warning = false; + bool showed_armed_warning = false; + + void start_shell(void); + static void shell_thread(void *arg); +}; + +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]); + + uint32_t available_memory(void) override; + + /* + return a stream for access to nsh shell + */ + AP_HAL::Stream *get_shell_stream() { return &_shell_stream; } + perf_counter_t perf_alloc(perf_counter_type t, const char *name) override; + void perf_begin(perf_counter_t ) override; + void perf_end(perf_counter_t) override; + void perf_count(perf_counter_t) override; + + // create a new semaphore + AP_HAL::Semaphore *new_semaphore(void) override { return new VRBRAIN::Semaphore; } + + void set_imu_temp(float current) override; + void set_imu_target_temp(int8_t *target) override; + +private: + int _safety_handle; + VRBRAIN::NSHShellStream _shell_stream; + + struct { + int8_t *target; + float integrator; + uint16_t count; + float sum; + uint32_t last_update_ms; + int fd = -1; + } _heater; +}; diff --git a/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp b/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp new file mode 100644 index 0000000000..7c4638dce8 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp @@ -0,0 +1,49 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This replaces the PX4Firmware parameter system with dummy + functions. The ArduPilot parameter system uses different formatting + for FRAM and we need to ensure that the PX4 parameter system doesn't + try to access FRAM in an invalid manner + */ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include +#include +#include + +#include "systemlib/param/param.h" + +#include "uORB/uORB.h" +#include "uORB/topics/parameter_update.h" +#include +#include + +/** 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) +{ + ::printf("VRBRAIN: param_find(%s)\n", name); + return PARAM_INVALID; +} + +int param_get(param_t param, void *val) +{ + return -1; +} + +int param_set(param_t param, const void *val) +{ + return -1; +} + +int +param_set_no_notification(param_t param, const void *val) +{ + return -1; +} +#endif // CONFIG_HAL_BOARD