aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

VRBRAIN: updated AP_HAL_VRBRAIN

Browse Source
This commit is contained in:
LukeMike 2016-07-05 16:15:55 +02:00 committed by Andrew Tridgell
parent 5255f55cc3
commit fde5992b6d
23 changed files with 3929 additions and 3010 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h
View File

@ -1,14 +1,18 @@
#pragma once #pragma once
namespace VRBRAIN { namespace VRBRAIN {
class VRBRAINScheduler; class VRBRAINScheduler;
class VRBRAINUARTDriver; class VRBRAINUARTDriver;
class VRBRAINStorage; class VRBRAINStorage;
class VRBRAINRCInput; class VRBRAINRCInput;
class VRBRAINRCOutput; class VRBRAINRCOutput;
class VRBRAINAnalogIn; class VRBRAINAnalogIn;
class VRBRAINAnalogSource; class VRBRAINAnalogSource;
class VRBRAINUtil; class VRBRAINUtil;
class VRBRAINGPIO; class VRBRAINGPIO;
class VRBRAINDigitalSource; class VRBRAINDigitalSource;
} class NSHShellStream;
class VRBRAINI2CDriver;
class VRBRAIN_I2C;
class Semaphore;
}

620
libraries/AP_HAL_VRBRAIN/AnalogIn.cpp
View File

@ -1,322 +1,298 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AnalogIn.h" #include "AnalogIn.h"
#include <drivers/drv_adc.h> #include <drivers/drv_adc.h>
#include <stdio.h> #include <stdio.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <nuttx/analog/adc.h> #include <nuttx/analog/adc.h>
#include <nuttx/config.h> #include <nuttx/config.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h> #include <uORB/topics/servorail_status.h>
#include <uORB/topics/system_power.h> #include <uORB/topics/system_power.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <errno.h> #include <errno.h>
#include <AP_Vehicle/AP_Vehicle.h> #include "GPIO.h"
#define ANLOGIN_DEBUGGING 0 #define ANLOGIN_DEBUGGING 0
// base voltage scaling for 12 bit 3.3V ADC // base voltage scaling for 12 bit 3.3V ADC
#define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f) #define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f)
#if ANALOGIN_DEBUGGING #if ANLOGIN_DEBUGGING
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) # define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else #else
# define Debug(fmt, args ...) # define Debug(fmt, args ...)
#endif #endif
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
/* /*
scaling table between ADC count and actual input voltage, to account scaling table between ADC count and actual input voltage, to account
for voltage dividers on the board. for voltage dividers on the board.
*/ */
static const struct { static const struct {
uint8_t pin; uint8_t pin;
float scaling; float scaling;
} pin_scaling[] = { } pin_scaling[] = {
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) #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)
{ 0, 3.3f/4096 }, { 10, 3.3f/4096 },
{ 10, 3.3f/4096 }, { 11, 3.3f/4096 },
{ 11, 3.3f/4096 }, #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) { 10, 3.3f/4096 },
{ 10, 3.3f/4096 }, #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) { 1, 3.3f/4096 },
{ 1, 3.3f/4096 }, { 2, 3.3f/4096 },
{ 2, 3.3f/4096 }, { 3, 3.3f/4096 },
{ 3, 3.3f/4096 }, { 10, 3.3f/4096 },
{ 10, 3.3f/4096 }, #else
#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) #error "Unknown board type for AnalogIn scaling"
{ 10, 3.3f/4096 }, #endif
{ 11, 3.3f/4096 }, };
{ 14, 3.3f/4096 },
{ 15, 3.3f/4096 }, using namespace VRBRAIN;
#else
#error "Unknown board type for AnalogIn scaling" VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) :
#endif _pin(pin),
}; _stop_pin(-1),
_settle_time_ms(0),
using namespace VRBRAIN; _value(initial_value),
_value_ratiometric(initial_value),
VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) : _latest_value(initial_value),
_pin(pin), _sum_count(0),
_stop_pin(-1), _sum_value(0),
_settle_time_ms(0), _sum_ratiometric(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)
void VRBRAINAnalogSource::set_stop_pin(uint8_t p) {
{ _stop_pin = p;
_stop_pin = p; }
}
float VRBRAINAnalogSource::read_average()
float VRBRAINAnalogSource::read_average() {
{ if (_sum_count == 0) {
if (_sum_count == 0) { return _value;
return _value; }
} hal.scheduler->suspend_timer_procs();
hal.scheduler->suspend_timer_procs(); _value = _sum_value / _sum_count;
_value = _sum_value / _sum_count; _value_ratiometric = _sum_ratiometric / _sum_count;
_value_ratiometric = _sum_ratiometric / _sum_count; _sum_value = 0;
_sum_value = 0; _sum_ratiometric = 0;
_sum_ratiometric = 0; _sum_count = 0;
_sum_count = 0; hal.scheduler->resume_timer_procs();
hal.scheduler->resume_timer_procs(); return _value;
return _value; }
}
float VRBRAINAnalogSource::read_latest()
float VRBRAINAnalogSource::read_latest() {
{ return _latest_value;
return _latest_value; }
}
/*
/* return scaling from ADC count to Volts
return scaling from ADC count to Volts */
*/ float VRBRAINAnalogSource::_pin_scaler(void)
float VRBRAINAnalogSource::_pin_scaler(void) {
{ float scaling = VRBRAIN_VOLTAGE_SCALING;
float scaling = VRBRAIN_VOLTAGE_SCALING; uint8_t num_scalings = ARRAY_SIZE(pin_scaling);
uint8_t num_scalings = ARRAY_SIZE(pin_scaling); for (uint8_t i=0; i<num_scalings; i++) {
for (uint8_t i=0; i<num_scalings; i++) { if (pin_scaling[i].pin == _pin) {
if (pin_scaling[i].pin == _pin) { scaling = pin_scaling[i].scaling;
scaling = pin_scaling[i].scaling; break;
break; }
} }
} return scaling;
return scaling; }
}
/*
/* return voltage in Volts
return voltage in Volts */
*/ float VRBRAINAnalogSource::voltage_average()
float VRBRAINAnalogSource::voltage_average() {
{ return _pin_scaler() * read_average();
return _pin_scaler() * read_average(); }
}
/*
/* return voltage in Volts, assuming a ratiometric sensor powered by
return voltage in Volts, assuming a ratiometric sensor powered by the 5V rail
the 5V rail */
*/ float VRBRAINAnalogSource::voltage_average_ratiometric()
float VRBRAINAnalogSource::voltage_average_ratiometric() {
{ voltage_average();
voltage_average(); return _pin_scaler() * _value_ratiometric;
return _pin_scaler() * _value_ratiometric; }
}
/*
/* return voltage in Volts
return voltage in Volts */
*/ float VRBRAINAnalogSource::voltage_latest()
float VRBRAINAnalogSource::voltage_latest() {
{ return _pin_scaler() * read_latest();
return _pin_scaler() * read_latest(); }
}
void VRBRAINAnalogSource::set_pin(uint8_t pin)
void VRBRAINAnalogSource::set_pin(uint8_t pin) {
{ if (_pin == pin) {
if (_pin == pin) { return;
return; }
} hal.scheduler->suspend_timer_procs();
hal.scheduler->suspend_timer_procs(); _pin = pin;
_pin = pin; _sum_value = 0;
_sum_value = 0; _sum_ratiometric = 0;
_sum_ratiometric = 0; _sum_count = 0;
_sum_count = 0; _latest_value = 0;
_latest_value = 0; _value = 0;
_value = 0; _value_ratiometric = 0;
_value_ratiometric = 0; hal.scheduler->resume_timer_procs();
hal.scheduler->resume_timer_procs(); }
}
/*
/* apply a reading in ADC counts
apply a reading in ADC counts */
*/ void VRBRAINAnalogSource::_add_value(float v, float vcc5V)
void VRBRAINAnalogSource::_add_value(float v, float vcc5V) {
{ _latest_value = v;
_latest_value = v; _sum_value += v;
_sum_value += v; if (vcc5V < 3.0f) {
if (vcc5V < 3.0f) { _sum_ratiometric += v;
_sum_ratiometric += v; } else {
} else { // this compensates for changes in the 5V rail relative to the
// this compensates for changes in the 5V rail relative to the // 3.3V reference used by the ADC.
// 3.3V reference used by the ADC. _sum_ratiometric += v * 5.0f / vcc5V;
_sum_ratiometric += v * 5.0f / vcc5V; }
} _sum_count++;
_sum_count++; if (_sum_count == 254) {
if (_sum_count == 254) { _sum_value /= 2;
_sum_value /= 2; _sum_ratiometric /= 2;
_sum_ratiometric /= 2; _sum_count /= 2;
_sum_count /= 2; }
} }
}
VRBRAINAnalogIn::VRBRAINAnalogIn() :
VRBRAINAnalogIn::VRBRAINAnalogIn() : _current_stop_pin_i(0),
_current_stop_pin_i(0), _board_voltage(0),
_board_voltage(0), _servorail_voltage(0),
_servorail_voltage(0), _power_flags(0)
_power_flags(0) {}
{}
void VRBRAINAnalogIn::init()
void VRBRAINAnalogIn::init() {
{ _adc_fd = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
_adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_adc_fd == -1) {
if (_adc_fd == -1) { AP_HAL::panic("Unable to open " ADC0_DEVICE_PATH);
AP_HAL::panic("Unable to open " ADC_DEVICE_PATH); }
} _battery_handle = orb_subscribe(ORB_ID(battery_status));
_battery_handle = orb_subscribe(ORB_ID(battery_status)); _servorail_handle = orb_subscribe(ORB_ID(servorail_status));
_servorail_handle = orb_subscribe(ORB_ID(servorail_status)); _system_power_handle = orb_subscribe(ORB_ID(system_power));
_system_power_handle = orb_subscribe(ORB_ID(system_power)); }
}
/*
/* move to the next stop pin
move to the next stop pin */
*/ void VRBRAINAnalogIn::next_stop_pin(void)
void VRBRAINAnalogIn::next_stop_pin(void) {
{ // find the next stop pin. We start one past the current stop pin
// 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
// and wrap completely, so we do the right thing is there is only // one stop pin
// one stop pin for (uint8_t i=1; i <= VRBRAIN_ANALOG_MAX_CHANNELS; i++) {
for (uint8_t i=1; i <= VRBRAIN_ANALOG_MAX_CHANNELS; i++) { uint8_t idx = (_current_stop_pin_i + i) % VRBRAIN_ANALOG_MAX_CHANNELS;
uint8_t idx = (_current_stop_pin_i + i) % VRBRAIN_ANALOG_MAX_CHANNELS; VRBRAIN::VRBRAINAnalogSource *c = _channels[idx];
VRBRAIN::VRBRAINAnalogSource *c = _channels[idx]; if (c && c->_stop_pin != -1) {
if (c && c->_stop_pin != -1) { // found another stop pin
// found another stop pin _stop_pin_change_time = AP_HAL::millis();
_stop_pin_change_time = AP_HAL::millis(); _current_stop_pin_i = idx;
_current_stop_pin_i = idx;
// set that pin high
// set that pin high hal.gpio->pinMode(c->_stop_pin, 1);
hal.gpio->pinMode(c->_stop_pin, 1); hal.gpio->write(c->_stop_pin, 1);
hal.gpio->write(c->_stop_pin, 1);
// set all others low
// set all others low for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) { VRBRAIN::VRBRAINAnalogSource *c2 = _channels[j];
VRBRAIN::VRBRAINAnalogSource *c2 = _channels[j]; if (c2 && c2->_stop_pin != -1 && j != idx) {
if (c2 && c2->_stop_pin != -1 && j != idx) { hal.gpio->pinMode(c2->_stop_pin, 1);
hal.gpio->pinMode(c2->_stop_pin, 1); hal.gpio->write(c2->_stop_pin, 0);
hal.gpio->write(c2->_stop_pin, 0); }
} }
} break;
break; }
} }
} }
}
/*
/* called at 1kHz
called at 1kHz */
*/ void VRBRAINAnalogIn::_timer_tick(void)
void VRBRAINAnalogIn::_timer_tick(void) {
{ // read adc at 100Hz
// read adc at 100Hz uint32_t now = AP_HAL::micros();
uint32_t now = AP_HAL::micros(); uint32_t delta_t = now - _last_run;
uint32_t delta_t = now - _last_run; if (delta_t < 10000) {
if (delta_t < 10000) { return;
return; }
} _last_run = now;
_last_run = now;
struct adc_msg_s buf_adc[VRBRAIN_ANALOG_MAX_CHANNELS];
struct adc_msg_s buf_adc[VRBRAIN_ANALOG_MAX_CHANNELS];
// cope with initial setup of stop pin
// cope with initial setup of stop pin if (_channels[_current_stop_pin_i] == NULL ||
if (_channels[_current_stop_pin_i] == NULL || _channels[_current_stop_pin_i]->_stop_pin == -1) {
_channels[_current_stop_pin_i]->_stop_pin == -1) { next_stop_pin();
next_stop_pin(); }
}
/* read all channels available */
/* read all channels available */ int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc));
int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc)); if (ret > 0) {
if (ret > 0) { // match the incoming channels to the currently active pins
// match the incoming channels to the currently active pins for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) { Debug("chan %u value=%u\n",
Debug("chan %u value=%u\n", (unsigned)buf_adc[i].am_channel,
(unsigned)buf_adc[i].am_channel, (unsigned)buf_adc[i].am_data);
(unsigned)buf_adc[i].am_data); for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) { VRBRAIN::VRBRAINAnalogSource *c = _channels[j];
VRBRAIN::VRBRAINAnalogSource *c = _channels[j]; if (c != NULL && buf_adc[i].am_channel == c->_pin) {
if (c != NULL && buf_adc[i].am_channel == c->_pin) { // add a value if either there is no stop pin, or
// add a value if either there is no stop pin, or // the stop pin has been settling for enough time
// the stop pin has been settling for enough time if (c->_stop_pin == -1 ||
if (c->_stop_pin == -1 || (_current_stop_pin_i == j &&
(_current_stop_pin_i == j && AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) {
AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) { c->_add_value(buf_adc[i].am_data, _board_voltage);
c->_add_value(buf_adc[i].am_data, _board_voltage); if (c->_stop_pin != -1 && _current_stop_pin_i == j) {
if (c->_stop_pin != -1 && _current_stop_pin_i == j) { next_stop_pin();
next_stop_pin(); }
} }
} }
} }
} }
} }
}
}
// check for new battery data on FMUv1
if (_battery_handle != -1) { AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin)
struct battery_status_s battery; {
bool updated = false; for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
if (orb_check(_battery_handle, &updated) == 0 && updated) { if (_channels[j] == NULL) {
orb_copy(ORB_ID(battery_status), _battery_handle, &battery); _channels[j] = new VRBRAINAnalogSource(pin, 0.0f);
if (battery.timestamp != _battery_timestamp) { return _channels[j];
_battery_timestamp = battery.timestamp; }
for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) { }
VRBRAIN::VRBRAINAnalogSource *c = _channels[j]; hal.console->println("Out of analog channels");
if (c == NULL) continue; return NULL;
if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN) { }
c->_add_value(battery.voltage_v / VRBRAIN_VOLTAGE_SCALING, 0);
} #endif // CONFIG_HAL_BOARD
if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN) {
// scale it back to voltage, knowing that the
// px4io code scales by 90.0/5.0
c->_add_value(battery.current_a * (5.0f/90.0f) / VRBRAIN_VOLTAGE_SCALING, 0);
}
}
}
}
}
}
AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin)
{
for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
if (_channels[j] == NULL) {
_channels[j] = new VRBRAINAnalogSource(pin, 0.0f);
return _channels[j];
}
}
hal.console->println("Out of analog channels");
return NULL;
}
#endif // CONFIG_HAL_BOARD

166
libraries/AP_HAL_VRBRAIN/AnalogIn.h
View File

@ -1,83 +1,83 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once #pragma once
#include "AP_HAL_VRBRAIN.h" #include "AP_HAL_VRBRAIN.h"
#include <pthread.h> #include <pthread.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#define VRBRAIN_ANALOG_MAX_CHANNELS 16 #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) #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_VOLTAGE_PIN 10
#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11 #define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 #define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10
#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN -1 #define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN -1
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 #define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10
#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 1 #define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 1
#endif #endif
class VRBRAIN::VRBRAINAnalogSource : public AP_HAL::AnalogSource { class VRBRAIN::VRBRAINAnalogSource : public AP_HAL::AnalogSource {
public: public:
friend class VRBRAIN::VRBRAINAnalogIn; friend class VRBRAIN::VRBRAINAnalogIn;
VRBRAINAnalogSource(int16_t pin, float initial_value); VRBRAINAnalogSource(int16_t pin, float initial_value);
float read_average(); float read_average();
float read_latest(); float read_latest();
void set_pin(uint8_t p); void set_pin(uint8_t p);
float voltage_average(); float voltage_average();
float voltage_latest(); float voltage_latest();
float voltage_average_ratiometric(); float voltage_average_ratiometric();
// implement stop pins // implement stop pins
void set_stop_pin(uint8_t p); void set_stop_pin(uint8_t p);
void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; } void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; }
private: private:
// what pin it is attached to // what pin it is attached to
int16_t _pin; int16_t _pin;
int16_t _stop_pin; int16_t _stop_pin;
uint16_t _settle_time_ms; uint16_t _settle_time_ms;
// what value it has // what value it has
float _value; float _value;
float _value_ratiometric; float _value_ratiometric;
float _latest_value; float _latest_value;
uint8_t _sum_count; uint8_t _sum_count;
float _sum_value; float _sum_value;
float _sum_ratiometric; float _sum_ratiometric;
void _add_value(float v, float vcc5V); void _add_value(float v, float vcc5V);
float _pin_scaler(); float _pin_scaler();
}; };
class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn { class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn {
public: public:
VRBRAINAnalogIn(); VRBRAINAnalogIn();
void init(); void init();
AP_HAL::AnalogSource* channel(int16_t pin); AP_HAL::AnalogSource* channel(int16_t pin);
void _timer_tick(void); void _timer_tick(void);
float board_voltage(void) { return _board_voltage; } float board_voltage(void) { return _board_voltage; }
float servorail_voltage(void) { return _servorail_voltage; } float servorail_voltage(void) { return _servorail_voltage; }
uint16_t power_status_flags(void) { return _power_flags; } uint16_t power_status_flags(void) { return _power_flags; }
private: private:
int _adc_fd; int _adc_fd;
int _battery_handle; int _battery_handle;
int _servorail_handle; int _servorail_handle;
int _system_power_handle; int _system_power_handle;
uint64_t _battery_timestamp; uint64_t _battery_timestamp;
uint64_t _servorail_timestamp; uint64_t _servorail_timestamp;
VRBRAIN::VRBRAINAnalogSource* _channels[VRBRAIN_ANALOG_MAX_CHANNELS]; VRBRAIN::VRBRAINAnalogSource* _channels[VRBRAIN_ANALOG_MAX_CHANNELS];
// what pin is currently held low to stop a sonar from reading // what pin is currently held low to stop a sonar from reading
uint8_t _current_stop_pin_i; uint8_t _current_stop_pin_i;
uint32_t _stop_pin_change_time; uint32_t _stop_pin_change_time;
uint32_t _last_run; uint32_t _last_run;
float _board_voltage; float _board_voltage;
float _servorail_voltage; float _servorail_voltage;
uint16_t _power_flags; uint16_t _power_flags;
void next_stop_pin(void); void next_stop_pin(void);
}; };

478
libraries/AP_HAL_VRBRAIN/GPIO.cpp
View File

@ -1,277 +1,201 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "GPIO.h" #include "GPIO.h"
#include <sys/types.h> #include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
/* VRBRAIN headers */ /* VRBRAIN headers */
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <drivers/drv_buzzer.h> #include <drivers/drv_tone_alarm.h>
#include <drivers/drv_gpio.h> #include <drivers/drv_gpio.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <board_config.h> #include <board_config.h>
#define LOW 0 #define LOW 0
#define HIGH 1 #define HIGH 1
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
using namespace VRBRAIN; using namespace VRBRAIN;
VRBRAINGPIO::VRBRAINGPIO() VRBRAINGPIO::VRBRAINGPIO()
{} {}
void VRBRAINGPIO::init() void VRBRAINGPIO::init()
{ {
_led_fd = open(LED_DEVICE_PATH, O_RDWR); _led_fd = open(LED0_DEVICE_PATH, O_RDWR);
if (_led_fd == -1) { if (_led_fd == -1) {
AP_HAL::panic("Unable to open " LED_DEVICE_PATH); AP_HAL::panic("Unable to open " LED0_DEVICE_PATH);
} }
if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) { if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n"); hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
} }
if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) { if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED RED\n"); hal.console->printf("GPIO: Unable to setup GPIO LED RED\n");
} }
if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) { if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n");
} }
#if defined(LED_EXT1)
if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) { _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n"); if (_tone_alarm_fd == -1) {
} AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
#endif }
#if defined(LED_EXT2)
if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) { _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n"); if (_gpio_fmu_fd == -1) {
} AP_HAL::panic("Unable to open GPIO");
#endif }
#if defined(LED_EXT3) #ifdef GPIO_SERVO_1
if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) { if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_1) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n"); hal.console->printf("GPIO: Unable to setup GPIO_1\n");
} }
#endif #endif
#ifdef GPIO_SERVO_2
#if defined(BUZZER_EXT) if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_2) != 0) {
_buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR); hal.console->printf("GPIO: Unable to setup GPIO_2\n");
if (_buzzer_fd == -1) { }
AP_HAL::panic("Unable to open " BUZZER_DEVICE_PATH); #endif
} #ifdef GPIO_SERVO_3
if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) { if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_3) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n"); hal.console->printf("GPIO: Unable to setup GPIO_3\n");
} }
#endif #endif
}
#if defined(GPIO_GPIO0_OUTPUT)
stm32_configgpio(GPIO_GPIO0_OUTPUT); void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
#endif {
switch (pin) {
#if defined(GPIO_GPIO1_OUTPUT) }
stm32_configgpio(GPIO_GPIO1_OUTPUT); }
#endif
} int8_t VRBRAINGPIO::analogPinToDigitalPin(uint8_t pin)
{
void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output) return -1;
{ }
switch (pin) {
}
} uint8_t VRBRAINGPIO::read(uint8_t pin) {
switch (pin) {
int8_t VRBRAINGPIO::analogPinToDigitalPin(uint8_t pin)
{ #ifdef GPIO_SERVO_3
return -1; case EXTERNAL_RELAY1_PIN: {
} uint32_t relays = 0;
ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
return (relays & GPIO_SERVO_3)?HIGH:LOW;
uint8_t VRBRAINGPIO::read(uint8_t pin) }
{ #endif
switch (pin) { }
case EXTERNAL_RELAY1_PIN: return LOW;
#if defined(GPIO_GPIO0_OUTPUT) }
return (stm32_gpioread(GPIO_GPIO0_OUTPUT))?HIGH:LOW;
#endif void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
break; {
case EXTERNAL_RELAY2_PIN: switch (pin) {
#if defined(GPIO_GPIO1_OUTPUT)
return (stm32_gpioread(GPIO_GPIO1_OUTPUT))?HIGH:LOW; case HAL_GPIO_A_LED_PIN: // Arming LED
#endif if (value == LOW) {
break; ioctl(_led_fd, LED_OFF, LED_GREEN);
} } else {
return LOW; ioctl(_led_fd, LED_ON, LED_GREEN);
} }
break;
void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
{ case HAL_GPIO_B_LED_PIN: // not used yet
switch (pin) { if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_BLUE);
case HAL_GPIO_A_LED_PIN: // Arming LED } else {
if (value == LOW) { ioctl(_led_fd, LED_ON, LED_BLUE);
ioctl(_led_fd, LED_OFF, LED_GREEN); }
} else { break;
ioctl(_led_fd, LED_ON, LED_GREEN);
} case HAL_GPIO_C_LED_PIN: // GPS LED
break; if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_RED);
case HAL_GPIO_B_LED_PIN: // not used yet } else {
if (value == LOW) { ioctl(_led_fd, LED_ON, LED_RED);
ioctl(_led_fd, LED_OFF, LED_BLUE); }
} else { break;
ioctl(_led_fd, LED_ON, LED_BLUE);
} #ifdef GPIO_SERVO_1
break; case EXTERNAL_LED_GPS:
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_1);
case HAL_GPIO_C_LED_PIN: // GPS LED break;
if (value == LOW) { #endif
ioctl(_led_fd, LED_OFF, LED_RED);
} else { #ifdef GPIO_SERVO_2
ioctl(_led_fd, LED_ON, LED_RED); case EXTERNAL_LED_ARMED:
} ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_2);
break; break;
#endif
case EXTERNAL_LED_GPS:
#if defined(LED_EXT1) #ifdef GPIO_SERVO_3
if (value == LOW) { case EXTERNAL_RELAY1_PIN:
ioctl(_led_fd, LED_OFF, LED_EXT1); ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_3);
} else { break;
ioctl(_led_fd, LED_ON, LED_EXT1); #endif
}
#endif
break; }
}
case EXTERNAL_LED_ARMED:
#if defined(LED_EXT2) void VRBRAINGPIO::toggle(uint8_t pin)
if (value == LOW) { {
ioctl(_led_fd, LED_OFF, LED_EXT2); write(pin, !read(pin));
} else { }
ioctl(_led_fd, LED_ON, LED_EXT2);
} /* Alternative interface: */
#endif AP_HAL::DigitalSource* VRBRAINGPIO::channel(uint16_t n) {
break; return new VRBRAINDigitalSource(0);
}
case EXTERNAL_LED_MOTOR1:
break; /* Interrupt interface: */
bool VRBRAINGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode)
case EXTERNAL_LED_MOTOR2: {
break; return true;
}
case BUZZER_PIN:
#if defined(BUZZER_EXT) /*
if (value == LOW) { return true when USB connected
ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT); */
} else { bool VRBRAINGPIO::usb_connected(void)
ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT); {
} /*
#endif we use a combination of voltage on the USB connector and the
break; open of the /dev/ttyACM0 character device. This copes with
systems where the VBUS may go high even with no USB connected
case EXTERNAL_RELAY1_PIN: (such as AUAV-X2)
#if defined(GPIO_GPIO0_OUTPUT) */
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, (value==HIGH)); return stm32_gpioread(GPIO_OTGFS_VBUS) && _usb_connected;
#endif }
break;
case EXTERNAL_RELAY2_PIN:
#if defined(GPIO_GPIO1_OUTPUT) VRBRAINDigitalSource::VRBRAINDigitalSource(uint8_t v) :
stm32_gpiowrite(GPIO_GPIO1_OUTPUT, (value==HIGH)); _v(v)
#endif {}
break;
void VRBRAINDigitalSource::mode(uint8_t output)
} {}
}
uint8_t VRBRAINDigitalSource::read() {
void VRBRAINGPIO::toggle(uint8_t pin) return _v;
{ }
switch (pin) {
void VRBRAINDigitalSource::write(uint8_t value) {
case HAL_GPIO_A_LED_PIN: // Arming LED _v = value;
ioctl(_led_fd, LED_TOGGLE, LED_GREEN); }
break;
void VRBRAINDigitalSource::toggle() {
case HAL_GPIO_B_LED_PIN: // not used yet _v = !_v;
ioctl(_led_fd, LED_TOGGLE, LED_BLUE); }
break;
#endif // CONFIG_HAL_BOARD
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

112
libraries/AP_HAL_VRBRAIN/GPIO.h
View File

@ -1,52 +1,60 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once #pragma once
#include "AP_HAL_VRBRAIN.h" #include "AP_HAL_VRBRAIN.h"
# define HAL_GPIO_A_LED_PIN 25 #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define HAL_GPIO_B_LED_PIN 26 # define HAL_GPIO_A_LED_PIN 25
# define HAL_GPIO_C_LED_PIN 27 # define HAL_GPIO_B_LED_PIN 26
# define EXTERNAL_LED_GPS 28 # define HAL_GPIO_C_LED_PIN 27
# define EXTERNAL_LED_ARMED 29 # define EXTERNAL_LED_GPS 28
# define EXTERNAL_LED_MOTOR1 30 # define EXTERNAL_LED_ARMED 29
# define EXTERNAL_LED_MOTOR2 31 # define EXTERNAL_LED_MOTOR1 30
# define BUZZER_PIN 32 # define EXTERNAL_LED_MOTOR2 31
# define EXTERNAL_RELAY1_PIN 33
# define EXTERNAL_RELAY2_PIN 34 # define EXTERNAL_RELAY1_PIN 33
# define HAL_GPIO_LED_ON HIGH
# define HAL_GPIO_LED_OFF LOW # define HAL_GPIO_LED_ON HIGH
# define HAL_GPIO_LED_OFF LOW
class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO { #endif
public:
VRBRAINGPIO(); class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO {
void init(); public:
void pinMode(uint8_t pin, uint8_t output); VRBRAINGPIO();
int8_t analogPinToDigitalPin(uint8_t pin); void init();
uint8_t read(uint8_t pin); void pinMode(uint8_t pin, uint8_t output);
void write(uint8_t pin, uint8_t value); int8_t analogPinToDigitalPin(uint8_t pin);
void toggle(uint8_t pin); uint8_t read(uint8_t pin);
void write(uint8_t pin, uint8_t value);
/* Alternative interface: */ void toggle(uint8_t pin);
AP_HAL::DigitalSource* channel(uint16_t n);
/* Alternative interface: */
/* Interrupt interface: */ AP_HAL::DigitalSource* channel(uint16_t n);
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
/* Interrupt interface: */
/* return true if USB cable is connected */ bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
bool usb_connected(void);
/* return true if USB cable is connected */
private: bool usb_connected(void);
int _led_fd;
int _buzzer_fd; // used by UART code to avoid a hw bug in the AUAV-X2
}; void set_usb_connected(void) { _usb_connected = true; }
class VRBRAIN::VRBRAINDigitalSource : public AP_HAL::DigitalSource { private:
public: int _led_fd = -1;
VRBRAINDigitalSource(uint8_t v); int _tone_alarm_fd = -1;
void mode(uint8_t output); int _gpio_fmu_fd = -1;
uint8_t read();
void write(uint8_t value); bool _usb_connected = false;
void toggle(); };
private:
uint8_t _v; 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;
};

717
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
View File

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

38
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h
View File

@ -1,18 +1,20 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_HAL_VRBRAIN.h" #include "AP_HAL_VRBRAIN.h"
#include "AP_HAL_VRBRAIN_Namespace.h" #include "AP_HAL_VRBRAIN_Namespace.h"
#include <systemlib/visibility.h> #include <systemlib/visibility.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
class HAL_VRBRAIN : public AP_HAL::HAL { class HAL_VRBRAIN : public AP_HAL::HAL {
public: public:
HAL_VRBRAIN(); HAL_VRBRAIN();
void run(int argc, char* const argv[], Callbacks* callbacks) const override; void run(int argc, char* const argv[], Callbacks* callbacks) const override;
}; };
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN void hal_vrbrain_set_priority(uint8_t priority);
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

143
libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp Normal file
View File

@ -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 <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <stdio.h>
#include <stdarg.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <apps/nsh.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#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, &param);
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

255
libraries/AP_HAL_VRBRAIN/RCInput.cpp
View File

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

69
libraries/AP_HAL_VRBRAIN/RCInput.h
View File

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

835
libraries/AP_HAL_VRBRAIN/RCOutput.cpp
View File

@ -1,219 +1,616 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "RCOutput.h" #include "RCOutput.h"
#include <sys/types.h> #include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_direct.h>
extern const AP_HAL::HAL& hal; #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
using namespace VRBRAIN; #include <drivers/drv_sbus.h>
void VRBRAINRCOutput::init() extern const AP_HAL::HAL& hal;
{
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); using namespace VRBRAIN;
_pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) { /*
AP_HAL::panic("Unable to open " PWM_OUTPUT_DEVICE_PATH); enable RCOUT_DEBUG_LATENCY to measure output latency using a logic
} analyser. AUX6 will go high during the cork/push output.
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) { */
hal.console->printf("RCOutput: Unable to setup IO arming\n"); #define RCOUT_DEBUG_LATENCY 0
}
if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) { void VRBRAINRCOutput::init()
hal.console->printf("RCOutput: Unable to setup IO arming OK\n"); {
} _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_rate_mask = 0; _pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) {
_servo_count = 0; AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH);
}
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) { hal.console->printf("RCOutput: Unable to setup IO arming\n");
hal.console->printf("RCOutput: Unable to get servo count\n"); }
return; 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;
void VRBRAINRCOutput::_init_alt_channels(void) _alt_fd = -1;
{ _servo_count = 0;
_alt_servo_count = 0;
}
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) hal.console->printf("RCOutput: Unable to get servo count\n");
{ return;
// we can't set this per channel yet }
if (freq_hz > 50) {
// we're being asked to set the alt rate for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) { _outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz); }
return;
}
_freq_hz = freq_hz; // _alt_fd = open("/dev/px4fmu", O_RDWR);
} // if (_alt_fd == -1) {
// hal.console->printf("RCOutput: failed to open /dev/px4fmu");
/* work out the new rate mask. The PX4IO board has 3 groups of servos. // return;
// }
Group 0: channels 0 1
Group 1: channels 4 5 6 7
Group 2: channels 2 3 // ensure not to write zeros to disabled channels
_enabled_channels = 0;
Channels within a group must be set to the same rate. for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) {
_period[i] = PWM_IGNORE_THIS_CHANNEL;
For the moment we never set the channels above 8 to more than }
50Hz
*/ // publish actuator vaules on demand
if (freq_hz > 50) { _actuator_direct_pub = NULL;
// we are setting high rates on the given channels
_rate_mask |= chmask & 0xFF; // and armed state
if (_rate_mask & 0x07) { _actuator_armed_pub = NULL;
_rate_mask |= 0x07; }
}
if (_rate_mask & 0x38) {
_rate_mask |= 0x38; void VRBRAINRCOutput::_init_alt_channels(void)
} {
if (_rate_mask & 0xC0) { if (_alt_fd == -1) {
_rate_mask |= 0xC0; return;
} }
} else { if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
// we are setting low rates on the given channels hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
if (chmask & 0x07) { return;
_rate_mask &= ~0x07; }
} if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
if (chmask & 0x38) { hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
_rate_mask &= ~0x38; return;
} }
if (chmask & 0xC0) { if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
_rate_mask &= ~0xC0; hal.console->printf("RCOutput: Unable to get servo count\n");
} }
} }
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); set output frequency on outputs associated with fd
} */
} void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
{
uint16_t VRBRAINRCOutput::get_freq(uint8_t ch) // we can't set this per channel
{ if (freq_hz > 50 || freq_hz == 1) {
if (_rate_mask & (1U<<ch)) { // we're being asked to set the alt rate
return _freq_hz; if (_output_mode == MODE_PWM_ONESHOT) {
} /*
return 50; set a 1Hz update for oneshot. This periodic output will
} never actually trigger, instead we will directly trigger
the pulse after each push()
void VRBRAINRCOutput::enable_ch(uint8_t ch) */
{ freq_hz = 1;
_enabled_channels |= (1U<<ch); }
} //::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz);
if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
void VRBRAINRCOutput::disable_ch(uint8_t ch) hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
{ return;
_enabled_channels &= ~(1U<<ch); }
} _freq_hz = freq_hz;
}
void VRBRAINRCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
{ /* work out the new rate mask. The outputs have 4 groups of servos.
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values)); Group 0: channels 0 1 2
for (uint8_t i=0; i<_servo_count; i++) { Group 1: channels 3 4 5
if ((1UL<<i) & chmask) { Group 2: channels 6 7
pwm_values.values[i] = period_us; Group 8: channels 8 9 10 11
}
pwm_values.channel_count++; Channels within a group must be set to the same rate.
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); For the moment we never set the channels above 8 to more than
if (ret != OK) { 50Hz
hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); */
} if (freq_hz > 50 || freq_hz == 1) {
} // we are setting high rates on the given channels
_rate_mask |= chmask & 0xFF;
void VRBRAINRCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us) if (_rate_mask & 0x07) {
{ _rate_mask |= 0x07;
struct pwm_output_values pwm_values; }
memset(&pwm_values, 0, sizeof(pwm_values)); if (_rate_mask & 0x38) {
for (uint8_t i=0; i<_servo_count; i++) { _rate_mask |= 0x38;
if ((1UL<<i) & chmask) { }
pwm_values.values[i] = period_us; if (_rate_mask & 0xC0) {
} _rate_mask |= 0xC0;
pwm_values.channel_count++; }
} if (_rate_mask & 0xF00) {
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values); _rate_mask |= 0xF00;
if (ret != OK) { }
hal.console->printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); } else {
} // we are setting low rates on the given channels
} if (chmask & 0x07) {
_rate_mask &= ~0x07;
bool VRBRAINRCOutput::force_safety_on(void) }
{ if (chmask & 0x38) {
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); _rate_mask &= ~0x38;
return (ret == OK); }
} if (chmask & 0xC0) {
_rate_mask &= ~0xC0;
void VRBRAINRCOutput::force_safety_off(void) }
{ if (chmask & 0xF00) {
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); _rate_mask &= ~0xF00;
if (ret != OK) { }
hal.console->printf("Failed to force safety off\n"); }
}
} //::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask);
if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us) hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
{ }
if (ch >= _servo_count) { }
return;
} /*
if (!(_enabled_channels & (1U<<ch))) { set output frequency
// not enabled */
return; void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
} {
if (ch >= _max_channel) { if (freq_hz > 50 && _output_mode == MODE_PWM_ONESHOT) {
_max_channel = ch + 1; // rate is irrelevent in oneshot
} return;
if (period_us != _period[ch]) { }
_period[ch] = period_us;
_need_update = true; // re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT
up_pwm_servo_set(ch, period_us); 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;
}
uint16_t VRBRAINRCOutput::read(uint8_t ch) 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");
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) { return;
return 0; }
}
return _period[ch]; // greater than 400 doesn't give enough room at higher periods for
} // the down pulse
if (freq_hz > 400) {
void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len) freq_hz = 400;
{ }
for (uint8_t i=0; i<len; i++) { uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
period_us[i] = read(i); uint32_t alt_mask = chmask >> _servo_count;
} if (primary_mask && _pwm_fd != -1) {
} set_freq_fd(_pwm_fd, primary_mask, freq_hz);
}
void VRBRAINRCOutput::_timer_tick(void) if (alt_mask && _alt_fd != -1) {
{ set_freq_fd(_alt_fd, alt_mask, freq_hz);
uint32_t now = AP_HAL::micros(); }
}
// always send at least at 20Hz, otherwise the IO board may think
// we are dead uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
if (now - _last_output > 50000) { {
_need_update = true; if (_rate_mask & (1U<<ch)) {
} return _freq_hz;
}
if (_need_update && _pwm_fd != -1) { return 50;
_need_update = false; }
perf_begin(_perf_rcout);
::write(_pwm_fd, _period, _max_channel*sizeof(_period[0])); void VRBRAINRCOutput::enable_ch(uint8_t ch)
perf_end(_perf_rcout); {
_last_output = now; if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
} return;
} }
if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
#endif // CONFIG_HAL_BOARD // this is the first enable of an auxiliary channel - setup
// aux channels now. This delayed setup makes it possible to
// use BRD_PWM_COUNT to setup the number of PWM channels.
_init_alt_channels();
}
_enabled_channels |= (1U<<ch);
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
_period[ch] = 0;
}
}
void VRBRAINRCOutput::disable_ch(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
_enabled_channels &= ~(1U<<ch);
_period[ch] = PWM_IGNORE_THIS_CHANNEL;
}
void VRBRAINRCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
{
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (uint8_t i=0; i<_servo_count; i++) {
if ((1UL<<i) & chmask) {
pwm_values.values[i] = period_us;
}
pwm_values.channel_count++;
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
}
}
void VRBRAINRCOutput::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<<i) & chmask) {
pwm_values.values[i] = period_us;
}
pwm_values.channel_count++;
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
hal.console->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<<ch))) {
// not enabled
return;
}
if (ch >= _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<ORB_MULTI_MAX_INSTANCES; i++) {
if (_outputs[i].pwm_sub >= 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<len; i++) {
period_us[i] = read(i);
}
}
uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch)
{
if (ch >= 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<len; i++) {
period_us[i] = read_last_sent(i);
}
}
/*
update actuator armed state
*/
void VRBRAINRCOutput::_arm_actuators(bool arm)
{
if (_armed.armed == arm) {
// already armed;
return;
}
_armed.timestamp = hrt_absolute_time();
_armed.armed = arm;
_armed.ready_to_arm = arm;
_armed.lockdown = false;
_armed.force_failsafe = false;
if (_actuator_armed_pub == NULL) {
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
} else {
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
}
}
/*
publish new outputs to the actuator_direct topic
*/
void VRBRAINRCOutput::_publish_actuators(void)
{
struct actuator_direct_s actuators;
if (_esc_pwm_min == 0 ||
_esc_pwm_max == 0) {
// not initialised yet
return;
}
actuators.nvalues = _max_channel;
if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
}
// don't publish more than 8 actuators for now, as the uavcan ESC
// driver refuses to update any motors if you try to publish more
// than 8
if (actuators.nvalues > 8) {
actuators.nvalues = 8;
}
bool armed = hal.util->get_soft_armed();
actuators.timestamp = hrt_absolute_time();
for (uint8_t i=0; i<actuators.nvalues; i++) {
if (!armed) {
actuators.values[i] = 0;
} else {
actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
}
// actuator values are from -1 to 1
actuators.values[i] = actuators.values[i]*2 - 1;
}
if (_actuator_direct_pub == NULL) {
_actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
} else {
orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
_arm_actuators(true);
}
}
void VRBRAINRCOutput::_send_outputs(void)
{
uint32_t now = AP_HAL::micros();
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<ORB_MULTI_MAX_INSTANCES; i++) {
bool rc_updated = false;
if (_outputs[i].pwm_sub >= 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

117
libraries/AP_HAL_VRBRAIN/RCOutput.h
View File

@ -1,41 +1,76 @@
#pragma once #pragma once
#include "AP_HAL_VRBRAIN.h" #include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_outputs.h>
#define VRBRAIN_NUM_OUTPUT_CHANNELS 16 #include <uORB/topics/actuator_armed.h>
class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput #define VRBRAIN_NUM_OUTPUT_CHANNELS 16
{
public: class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput
void init(); {
void set_freq(uint32_t chmask, uint16_t freq_hz); public:
uint16_t get_freq(uint8_t ch); void init() override;
void enable_ch(uint8_t ch); void set_freq(uint32_t chmask, uint16_t freq_hz) override;
void disable_ch(uint8_t ch); uint16_t get_freq(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us); void enable_ch(uint8_t ch) override;
uint16_t read(uint8_t ch); void disable_ch(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len); void write(uint8_t ch, uint16_t period_us) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us); uint16_t read(uint8_t ch) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us); void read(uint16_t* period_us, uint8_t len) override;
bool force_safety_on(void); uint16_t read_last_sent(uint8_t ch) override;
void force_safety_off(void); 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 _timer_tick(void); void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;
private: void force_safety_off(void) override;
int _pwm_fd; void force_safety_no_wait(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
uint16_t _freq_hz; _esc_pwm_min = min_pwm;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS]; _esc_pwm_max = max_pwm;
volatile uint8_t _max_channel; }
volatile bool _need_update; void cork();
perf_counter_t _perf_rcout; void push();
uint32_t _last_output;
unsigned _servo_count; void set_output_mode(enum output_mode mode) override;
uint32_t _rate_mask; void _timer_tick(void);
uint16_t _enabled_channels; bool enable_sbus_out(uint16_t rate_hz) override;
void _init_alt_channels(void); private:
}; int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
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);
};

709
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
View File

@ -1,321 +1,388 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_HAL_VRBRAIN.h" #include "AP_HAL_VRBRAIN.h"
#include "Scheduler.h" #include "Scheduler.h"
#include <unistd.h> #include <unistd.h>
#include <stdlib.h> #include <stdlib.h>
#include <sched.h> #include <sched.h>
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <nuttx/arch.h> #include <nuttx/arch.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <pthread.h> #include <pthread.h>
#include <poll.h> #include <poll.h>
#include "UARTDriver.h" #include "UARTDriver.h"
#include "AnalogIn.h" #include "AnalogIn.h"
#include "Storage.h" #include "Storage.h"
#include "RCOutput.h" #include "RCOutput.h"
#include "RCInput.h" #include "RCInput.h"
#include <AP_Scheduler/AP_Scheduler.h> #include <AP_Scheduler/AP_Scheduler.h>
using namespace VRBRAIN; using namespace VRBRAIN;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
extern bool _vrbrain_thread_should_exit; extern bool _vrbrain_thread_should_exit;
VRBRAINScheduler::VRBRAINScheduler() : VRBRAINScheduler::VRBRAINScheduler() :
_perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")), _perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")),
_perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")), _perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")),
_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay")) _perf_storage_timer(perf_alloc(PC_ELAPSED, "APM_storage_timers")),
{} _perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
{}
void VRBRAINScheduler::init()
{ void VRBRAINScheduler::init()
_main_task_pid = getpid(); {
_main_task_pid = getpid();
// setup the timer thread - this will call tasks at 1kHz
pthread_attr_t thread_attr; // setup the timer thread - this will call tasks at 1kHz
struct sched_param param; pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048); pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_TIMER_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param); param.sched_priority = APM_TIMER_PRIORITY;
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); (void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_timer_thread, this);
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); // the UART thread runs at a medium priority
pthread_attr_setstacksize(&thread_attr, 2048); pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param); param.sched_priority = APM_UART_PRIORITY;
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); (void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_uart_thread, this);
pthread_create(&_uart_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_uart_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr); // the IO thread runs at lower priority
pthread_attr_setstacksize(&thread_attr, 2048); pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param); param.sched_priority = APM_IO_PRIORITY;
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); (void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_io_thread, this);
} pthread_create(&_io_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_io_thread, this);
/** // the storage thread runs at just above IO priority
delay for a specified number of microseconds using a semaphore wait pthread_attr_init(&thread_attr);
*/ pthread_attr_setstacksize(&thread_attr, 1024);
void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec)
{ param.sched_priority = APM_STORAGE_PRIORITY;
sem_t wait_semaphore; (void)pthread_attr_setschedparam(&thread_attr, &param);
struct hrt_call wait_call; pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
sem_init(&wait_semaphore, 0, 0);
hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore); pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this);
sem_wait(&wait_semaphore); }
}
/**
void VRBRAINScheduler::delay_microseconds(uint16_t usec) delay for a specified number of microseconds using a semaphore wait
{ */
perf_begin(_perf_delay); void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec)
if (usec >= 500) { {
delay_microseconds_semaphore(usec); sem_t wait_semaphore;
perf_end(_perf_delay); struct hrt_call wait_call;
return; sem_init(&wait_semaphore, 0, 0);
} memset(&wait_call, 0, sizeof(wait_call));
uint64_t start = AP_HAL::micros64(); hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore);
uint64_t dt; sem_wait(&wait_semaphore);
while ((dt=(AP_HAL::micros64() - start)) < usec) { }
up_udelay(usec - dt);
} void VRBRAINScheduler::delay_microseconds(uint16_t usec)
perf_end(_perf_delay); {
} perf_begin(_perf_delay);
delay_microseconds_semaphore(usec);
void VRBRAINScheduler::delay(uint16_t ms) perf_end(_perf_delay);
{ }
if (in_timerprocess()) {
::printf("ERROR: delay() from timer process\n"); /*
return; wrapper around sem_post that boosts main thread priority
} */
perf_begin(_perf_delay); static void sem_post_boost(sem_t *sem)
uint64_t start = AP_HAL::micros64(); {
hal_vrbrain_set_priority(APM_MAIN_PRIORITY_BOOST);
while ((AP_HAL::micros64() - start)/1000 < ms && sem_post(sem);
!_vrbrain_thread_should_exit) { }
delay_microseconds_semaphore(1000);
if (_min_delay_cb_ms <= ms) { /*
if (_delay_cb) { return the main thread to normal priority
_delay_cb(); */
} static void set_normal_priority(void *sem)
} {
} hal_vrbrain_set_priority(APM_MAIN_PRIORITY);
perf_end(_perf_delay); }
if (_vrbrain_thread_should_exit) {
exit(1); /*
} 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
void VRBRAINScheduler::register_delay_callback(AP_HAL::Proc proc, the regularity of timing of the main loop as it takes
uint16_t min_time_ms) */
{ void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec)
_delay_cb = proc; {
_min_delay_cb_ms = min_time_ms; sem_t wait_semaphore;
} static struct hrt_call wait_call;
sem_init(&wait_semaphore, 0, 0);
void VRBRAINScheduler::register_timer_process(AP_HAL::MemberProc proc) hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore);
{ sem_wait(&wait_semaphore);
for (uint8_t i = 0; i < _num_timer_procs; i++) { hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, NULL);
if (_timer_proc[i] == proc) { }
return;
} void VRBRAINScheduler::delay(uint16_t ms)
} {
if (in_timerprocess()) {
if (_num_timer_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) { ::printf("ERROR: delay() from timer process\n");
_timer_proc[_num_timer_procs] = proc; return;
_num_timer_procs++; }
} else { perf_begin(_perf_delay);
hal.console->printf("Out of timer processes\n"); uint64_t start = AP_HAL::micros64();
}
} while ((AP_HAL::micros64() - start)/1000 < ms &&
!_vrbrain_thread_should_exit) {
void VRBRAINScheduler::register_io_process(AP_HAL::MemberProc proc) delay_microseconds_semaphore(1000);
{ if (_min_delay_cb_ms <= ms) {
for (uint8_t i = 0; i < _num_io_procs; i++) { if (_delay_cb) {
if (_io_proc[i] == proc) { _delay_cb();
return; }
} }
} }
perf_end(_perf_delay);
if (_num_io_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) { if (_vrbrain_thread_should_exit) {
_io_proc[_num_io_procs] = proc; exit(1);
_num_io_procs++; }
} else { }
hal.console->printf("Out of IO processes\n");
} void VRBRAINScheduler::register_delay_callback(AP_HAL::Proc proc,
} uint16_t min_time_ms)
{
void VRBRAINScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) _delay_cb = proc;
{ _min_delay_cb_ms = min_time_ms;
_failsafe = failsafe; }
}
void VRBRAINScheduler::register_timer_process(AP_HAL::MemberProc proc)
void VRBRAINScheduler::suspend_timer_procs() {
{ for (uint8_t i = 0; i < _num_timer_procs; i++) {
_timer_suspended = true; if (_timer_proc[i] == proc) {
} return;
}
void VRBRAINScheduler::resume_timer_procs() }
{
_timer_suspended = false; if (_num_timer_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) {
if (_timer_event_missed == true) { _timer_proc[_num_timer_procs] = proc;
_run_timers(false); _num_timer_procs++;
_timer_event_missed = false; } else {
} hal.console->printf("Out of timer processes\n");
} }
}
void VRBRAINScheduler::reboot(bool hold_in_bootloader)
{ void VRBRAINScheduler::register_io_process(AP_HAL::MemberProc proc)
systemreset(hold_in_bootloader); {
} for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
void VRBRAINScheduler::_run_timers(bool called_from_timer_thread) return;
{ }
if (_in_timer_proc) { }
return;
} if (_num_io_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) {
_in_timer_proc = true; _io_proc[_num_io_procs] = proc;
_num_io_procs++;
if (!_timer_suspended) { } else {
// now call the timer based drivers hal.console->printf("Out of IO processes\n");
for (int i = 0; i < _num_timer_procs; i++) { }
if (_timer_proc[i]) { }
_timer_proc[i]();
} void VRBRAINScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
} {
} else if (called_from_timer_thread) { _failsafe = failsafe;
_timer_event_missed = true; }
}
void VRBRAINScheduler::suspend_timer_procs()
// and the failsafe, if one is setup {
if (_failsafe) { _timer_suspended = true;
_failsafe(); }
}
void VRBRAINScheduler::resume_timer_procs()
// process analog input {
((VRBRAINAnalogIn *)hal.analogin)->_timer_tick(); _timer_suspended = false;
if (_timer_event_missed == true) {
_in_timer_proc = false; _run_timers(false);
} _timer_event_missed = false;
}
extern bool vrbrain_ran_overtime; }
void *VRBRAINScheduler::_timer_thread(void) void VRBRAINScheduler::reboot(bool hold_in_bootloader)
{ {
uint32_t last_ran_overtime = 0; // disarm motors to ensure they are off during a bootloader upload
while (!_hal_initialized) { hal.rcout->force_safety_on();
poll(NULL, 0, 1); hal.rcout->force_safety_no_wait();
}
while (!_vrbrain_thread_should_exit) { // delay to ensure the async force_saftey operation completes
delay_microseconds_semaphore(1000); delay(500);
// run registered timers px4_systemreset(hold_in_bootloader);
perf_begin(_perf_timers); }
_run_timers(true);
perf_end(_perf_timers); void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
{
// process any pending RC output requests if (_in_timer_proc) {
((VRBRAINRCOutput *)hal.rcout)->_timer_tick(); return;
}
// process any pending RC input requests _in_timer_proc = true;
((VRBRAINRCInput *)hal.rcin)->_timer_tick();
if (!_timer_suspended) {
if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) { // now call the timer based drivers
last_ran_overtime = AP_HAL::millis(); for (int i = 0; i < _num_timer_procs; i++) {
// printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); if (_timer_proc[i]) {
// hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); _timer_proc[i]();
} }
} }
return NULL; } else if (called_from_timer_thread) {
} _timer_event_missed = true;
}
void VRBRAINScheduler::_run_io(void)
{ // and the failsafe, if one is setup
if (_in_io_proc) { if (_failsafe != NULL) {
return; _failsafe();
} }
_in_io_proc = true;
// process analog input
if (!_timer_suspended) { ((VRBRAINAnalogIn *)hal.analogin)->_timer_tick();
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) { _in_timer_proc = false;
if (_io_proc[i]) { }
_io_proc[i]();
} extern bool vrbrain_ran_overtime;
}
} void *VRBRAINScheduler::_timer_thread(void *arg)
{
_in_io_proc = false; VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
} uint32_t last_ran_overtime = 0;
void *VRBRAINScheduler::_uart_thread(void) while (!sched->_hal_initialized) {
{ poll(NULL, 0, 1);
while (!_hal_initialized) { }
poll(NULL, 0, 1); while (!_vrbrain_thread_should_exit) {
} sched->delay_microseconds_semaphore(1000);
while (!_vrbrain_thread_should_exit) {
delay_microseconds_semaphore(1000); // run registered timers
perf_begin(sched->_perf_timers);
// process any pending serial bytes sched->_run_timers(true);
((VRBRAINUARTDriver *)hal.uartA)->_timer_tick(); perf_end(sched->_perf_timers);
((VRBRAINUARTDriver *)hal.uartB)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartC)->_timer_tick(); // process any pending RC output requests
((VRBRAINUARTDriver *)hal.uartD)->_timer_tick(); ((VRBRAINRCOutput *)hal.rcout)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartE)->_timer_tick();
} // process any pending RC input requests
return NULL; ((VRBRAINRCInput *)hal.rcin)->_timer_tick();
}
if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
void *VRBRAINScheduler::_io_thread(void) last_ran_overtime = AP_HAL::millis();
{ #if 0
while (!_hal_initialized) { printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
poll(NULL, 0, 1); hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
} #endif
while (!_vrbrain_thread_should_exit) { }
poll(NULL, 0, 1); }
return NULL;
// process any pending storage writes }
((VRBRAINStorage *)hal.storage)->_timer_tick();
void VRBRAINScheduler::_run_io(void)
// run registered IO processes {
perf_begin(_perf_io_timers); if (_in_io_proc) {
_run_io(); return;
perf_end(_perf_io_timers); }
} _in_io_proc = true;
return NULL;
} if (!_timer_suspended) {
// now call the IO based drivers
bool VRBRAINScheduler::in_timerprocess() for (int i = 0; i < _num_io_procs; i++) {
{ if (_io_proc[i]) {
return getpid() != _main_task_pid; _io_proc[i]();
} }
}
void VRBRAINScheduler::system_initialized() { }
if (_initialized) {
AP_HAL::panic("PANIC: scheduler::system_initialized called" _in_io_proc = false;
"more than once"); }
}
_initialized = true; void *VRBRAINScheduler::_uart_thread(void *arg)
} {
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
#endif
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

180
libraries/AP_HAL_VRBRAIN/Scheduler.h
View File

@ -1,78 +1,102 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_HAL_VRBRAIN_Namespace.h" #include "AP_HAL_VRBRAIN_Namespace.h"
#include <sys/time.h> #include <sys/time.h>
#include <signal.h> #include <signal.h>
#include <pthread.h> #include <pthread.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8 #define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY 180 #define APM_MAIN_PRIORITY_BOOST 241
#define APM_TIMER_PRIORITY 181 #define APM_MAIN_PRIORITY 180
#define APM_UART_PRIORITY 60 #define APM_TIMER_PRIORITY 181
#define APM_IO_PRIORITY 59 #define APM_UART_PRIORITY 60
#define APM_OVERTIME_PRIORITY 10 #define APM_STORAGE_PRIORITY 59
#define APM_STARTUP_PRIORITY 10 #define APM_IO_PRIORITY 58
#define APM_SHELL_PRIORITY 57
/* Scheduler implementation: */ #define APM_OVERTIME_PRIORITY 10
class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler { #define APM_STARTUP_PRIORITY 10
public:
VRBRAINScheduler(); /* how long to boost priority of the main thread for each main
/* AP_HAL::Scheduler methods */ 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
void init(); code to start the AHRS update.
void delay(uint16_t ms);
void delay_microseconds(uint16_t us); Priority boosting of the main thread in delay_microseconds_boost()
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); avoids the problem that drivers in hpwork all happen to run right
void register_timer_process(AP_HAL::MemberProc); at the start of the period where the main vehicle loop is calling
void register_io_process(AP_HAL::MemberProc); wait_for_sample(). That causes main loop timing jitter, which
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); reduces performance. Using the priority boost the main loop
void suspend_timer_procs(); temporarily runs at a priority higher than hpwork and the timer
void resume_timer_procs(); thread, which results in much more consistent loop timing.
void reboot(bool hold_in_bootloader); */
#define APM_MAIN_PRIORITY_BOOST_USEC 150
bool in_timerprocess();
void system_initialized(); #define APM_MAIN_THREAD_STACK_SIZE 8192
void hal_initialized() { _hal_initialized = true; }
/* Scheduler implementation: */
private: class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler {
bool _initialized; public:
volatile bool _hal_initialized; VRBRAINScheduler();
AP_HAL::Proc _delay_cb; /* AP_HAL::Scheduler methods */
uint16_t _min_delay_cb_ms;
AP_HAL::Proc _failsafe; void init();
void delay(uint16_t ms);
volatile bool _timer_suspended; void delay_microseconds(uint16_t us);
void delay_microseconds_boost(uint16_t us);
AP_HAL::MemberProc _timer_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS]; void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
uint8_t _num_timer_procs; void register_timer_process(AP_HAL::MemberProc);
volatile bool _in_timer_proc; void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
AP_HAL::MemberProc _io_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS]; void suspend_timer_procs();
uint8_t _num_io_procs; void resume_timer_procs();
volatile bool _in_io_proc; void reboot(bool hold_in_bootloader);
volatile bool _timer_event_missed; bool in_timerprocess();
void system_initialized();
pid_t _main_task_pid; void hal_initialized() { _hal_initialized = true; }
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx; private:
pthread_t _uart_thread_ctx; bool _initialized;
volatile bool _hal_initialized;
void *_timer_thread(void); AP_HAL::Proc _delay_cb;
void *_io_thread(void); uint16_t _min_delay_cb_ms;
void *_uart_thread(void); AP_HAL::Proc _failsafe;
void _run_timers(bool called_from_timer_thread); volatile bool _timer_suspended;
void _run_io(void);
AP_HAL::MemberProc _timer_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS];
void delay_microseconds_semaphore(uint16_t us); uint8_t _num_timer_procs;
volatile bool _in_timer_proc;
perf_counter_t _perf_timers;
perf_counter_t _perf_io_timers; AP_HAL::MemberProc _io_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS];
perf_counter_t _perf_delay; uint8_t _num_io_procs;
}; volatile bool _in_io_proc;
#endif
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

39
libraries/AP_HAL_VRBRAIN/Semaphores.cpp Normal file
View File

@ -0,0 +1,39 @@
#include <AP_HAL/AP_HAL.h>
#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

20
libraries/AP_HAL_VRBRAIN/Semaphores.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_HAL_VRBRAIN.h"
#include <pthread.h>
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

577
libraries/AP_HAL_VRBRAIN/Storage.cpp
View File

@ -1,268 +1,309 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <assert.h> #include <assert.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>
#include <ctype.h> #include <ctype.h>
#include "Storage.h" #include "Storage.h"
using namespace VRBRAIN; using namespace VRBRAIN;
/* /*
This stores eeprom data in the VRBRAIN MTD interface with a 4k size, and 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. 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 // name the storage file after the sketch so you can use the same sd
// card for ArduCopter and ArduPlane // card for ArduCopter and ArduPlane
#define STORAGE_DIR "/fs/microsd/APM" #define STORAGE_DIR "/fs/microsd/APM"
#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg" #define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak" #define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak"
#define MTD_PARAMS_FILE "/fs/mtd" //#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
#define MTD_SIGNATURE 0x14012014 #define MTD_PARAMS_FILE "/fs/mtd"
#define MTD_SIGNATURE_OFFSET (8192-4) #define MTD_SIGNATURE 0x14012014
#define STORAGE_RENAME_OLD_FILE 0 #define MTD_SIGNATURE_OFFSET (8192-4)
#define STORAGE_RENAME_OLD_FILE 0
extern const AP_HAL::HAL& hal;
extern const AP_HAL::HAL& hal;
VRBRAINStorage::VRBRAINStorage(void) :
_fd(-1), VRBRAINStorage::VRBRAINStorage(void) :
_dirty_mask(0), _fd(-1),
_perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")), _dirty_mask(0),
_perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors")) _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 /*
*/ get signature from bytes at offset MTD_SIGNATURE_OFFSET
uint32_t VRBRAINStorage::_mtd_signature(void) */
{ uint32_t VRBRAINStorage::_mtd_signature(void)
int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY); {
if (mtd_fd == -1) { int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY);
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); 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) { uint32_t v;
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE); 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); bus_lock(true);
} if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
close(mtd_fd); AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
return v; }
} bus_lock(false);
close(mtd_fd);
/* return v;
put signature bytes at offset MTD_SIGNATURE_OFFSET }
*/
void VRBRAINStorage::_mtd_write_signature(void) /*
{ put signature bytes at offset MTD_SIGNATURE_OFFSET
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); */
if (mtd_fd == -1) { void VRBRAINStorage::_mtd_write_signature(void)
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); {
} int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
uint32_t v = MTD_SIGNATURE; if (mtd_fd == -1) {
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) { AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE); }
} uint32_t v = MTD_SIGNATURE;
if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) { if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE); AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
} }
close(mtd_fd); bus_lock(true);
} if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
/* }
upgrade from microSD to MTD (FRAM) bus_lock(false);
*/ close(mtd_fd);
void VRBRAINStorage::_upgrade_to_mtd(void) }
{
// the MTD is completely uninitialised - try to get a /*
// copy from OLD_STORAGE_FILE upgrade from microSD to MTD (FRAM)
int old_fd = open(OLD_STORAGE_FILE, O_RDONLY); */
if (old_fd == -1) { void VRBRAINStorage::_upgrade_to_mtd(void)
::printf("Failed to open %s\n", OLD_STORAGE_FILE); {
return; // the MTD is completely uninitialised - try to get a
} // copy from OLD_STORAGE_FILE
int old_fd = open(OLD_STORAGE_FILE, O_RDONLY);
int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); if (old_fd == -1) {
if (mtd_fd == -1) { ::printf("Failed to open %s\n", OLD_STORAGE_FILE);
AP_HAL::panic("Unable to open MTD for upgrade"); return;
} }
if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY);
close(old_fd); if (mtd_fd == -1) {
close(mtd_fd); AP_HAL::panic("Unable to open MTD for upgrade");
::printf("Failed to read %s\n", OLD_STORAGE_FILE); }
return;
} if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
close(old_fd); close(old_fd);
ssize_t ret; close(mtd_fd);
if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) { ::printf("Failed to read %s\n", OLD_STORAGE_FILE);
::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno); return;
AP_HAL::panic("Unable to write MTD for upgrade"); }
} close(old_fd);
close(mtd_fd); ssize_t ret;
#if STORAGE_RENAME_OLD_FILE bus_lock(true);
rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK); if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
#endif ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
::printf("Upgraded MTD from %s\n", OLD_STORAGE_FILE); AP_HAL::panic("Unable to write MTD for upgrade");
} }
bus_lock(false);
close(mtd_fd);
void VRBRAINStorage::_storage_open(void) #if STORAGE_RENAME_OLD_FILE
{ rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
if (_initialised) { #endif
return; ::printf("Upgraded MTD from %s\n", OLD_STORAGE_FILE);
} }
struct stat st;
_have_mtd = (stat(MTD_PARAMS_FILE, &st) == 0); void VRBRAINStorage::_storage_open(void)
{
// VRBRAIN should always have /fs/mtd_params if (_initialised) {
if (!_have_mtd) { return;
AP_HAL::panic("Failed to find " MTD_PARAMS_FILE); }
}
struct stat st;
/* _have_mtd = (stat(MTD_PARAMS_FILE, &st) == 0);
cope with upgrading from OLD_STORAGE_FILE to MTD
*/ // VRBRAIN should always have /fs/mtd_params
bool good_signature = (_mtd_signature() == MTD_SIGNATURE); if (!_have_mtd) {
if (stat(OLD_STORAGE_FILE, &st) == 0) { AP_HAL::panic("Failed to find " MTD_PARAMS_FILE);
if (good_signature) { }
#if STORAGE_RENAME_OLD_FILE
rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK); /*
#endif cope with upgrading from OLD_STORAGE_FILE to MTD
} else { */
_upgrade_to_mtd(); bool good_signature = (_mtd_signature() == MTD_SIGNATURE);
} if (stat(OLD_STORAGE_FILE, &st) == 0) {
} if (good_signature) {
#if STORAGE_RENAME_OLD_FILE
// we write the signature every time, even if it already is rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
// good, as this gives us a way to detect if the MTD device is #endif
// functional. It is better to panic now than to fail to save } else {
// parameters in flight _upgrade_to_mtd();
_mtd_write_signature(); }
}
_dirty_mask = 0;
int fd = open(MTD_PARAMS_FILE, O_RDONLY); // we write the signature every time, even if it already is
if (fd == -1) { // good, as this gives us a way to detect if the MTD device is
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); // functional. It is better to panic now than to fail to save
} // parameters in flight
const uint16_t chunk_size = 128; _mtd_write_signature();
for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
ssize_t ret = read(fd, &_buffer[ofs], chunk_size); _dirty_mask = 0;
if (ret != chunk_size) { int fd = open(MTD_PARAMS_FILE, O_RDONLY);
::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n", if (fd == -1) {
(unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno); AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
AP_HAL::panic("Failed to read " MTD_PARAMS_FILE); }
} const uint16_t chunk_size = 128;
} for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
close(fd); bus_lock(true);
_initialised = true; ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
} bus_lock(false);
if (ret != chunk_size) {
/* ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
mark some lines as dirty. Note that there is no attempt to avoid (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
the race condition between this code and the _timer_tick() code AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
below, which both update _dirty_mask. If we lose the race then the }
result is that a line is written more than once, but it won't result }
in a line not being written. close(fd);
*/
void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length) #ifdef SAVE_STORAGE_FILE
{ fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
uint16_t end = loc + length; if (fd != -1) {
for (uint8_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT; write(fd, _buffer, sizeof(_buffer));
line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; close(fd);
line++) { ::printf("Saved storage file %s\n", SAVE_STORAGE_FILE);
_dirty_mask |= 1U << line; }
} #endif
} _initialised = true;
}
void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n)
{ /*
if (loc >= sizeof(_buffer)-(n-1)) { mark some lines as dirty. Note that there is no attempt to avoid
return; the race condition between this code and the _timer_tick() code
} below, which both update _dirty_mask. If we lose the race then the
_storage_open(); result is that a line is written more than once, but it won't result
memcpy(dst, &_buffer[loc], n); in a line not being written.
} */
void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length)
void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n) {
{ uint16_t end = loc + length;
if (loc >= sizeof(_buffer)-(n-1)) { for (uint8_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT;
return; line <= end>>VRBRAIN_STORAGE_LINE_SHIFT;
} line++) {
if (memcmp(src, &_buffer[loc], n) != 0) { _dirty_mask |= 1U << line;
_storage_open(); }
memcpy(&_buffer[loc], src, n); }
_mark_dirty(loc, n);
} void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n)
} {
if (loc >= sizeof(_buffer)-(n-1)) {
void VRBRAINStorage::_timer_tick(void) return;
{ }
if (!_initialised || _dirty_mask == 0) { _storage_open();
return; memcpy(dst, &_buffer[loc], n);
} }
perf_begin(_perf_storage);
void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n)
if (_fd == -1) { {
_fd = open(MTD_PARAMS_FILE, O_WRONLY); if (loc >= sizeof(_buffer)-(n-1)) {
if (_fd == -1) { return;
perf_end(_perf_storage); }
perf_count(_perf_errors); if (memcmp(src, &_buffer[loc], n) != 0) {
return; _storage_open();
} memcpy(&_buffer[loc], src, n);
} _mark_dirty(loc, n);
}
// 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; void VRBRAINStorage::bus_lock(bool lock)
for (i=0; i<VRBRAIN_STORAGE_NUM_LINES; i++) { {
if (_dirty_mask & (1<<i)) {
break;
}
}
if (i == VRBRAIN_STORAGE_NUM_LINES) {
// this shouldn't be possible
perf_end(_perf_storage);
perf_count(_perf_errors);
return;
}
uint32_t write_mask = (1U<<i);
// see how many lines to write
for (n=1; (i+n) < VRBRAIN_STORAGE_NUM_LINES &&
n < (VRBRAIN_STORAGE_MAX_WRITE>>VRBRAIN_STORAGE_LINE_SHIFT); n++) {
if (!(_dirty_mask & (1<<(n+i)))) {
break;
} }
// mark that line clean
write_mask |= (1<<(n+i)); void VRBRAINStorage::_timer_tick(void)
} {
if (!_initialised || _dirty_mask == 0) {
/* return;
write the lines. This also updates _dirty_mask. Note that }
because this is a SCHED_FIFO thread it will not be preempted perf_begin(_perf_storage);
by the main task except during blocking calls. This means we
don't need a semaphore around the _dirty_mask updates. if (_fd == -1) {
*/ _fd = open(MTD_PARAMS_FILE, O_WRONLY);
if (lseek(_fd, i<<VRBRAIN_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<VRBRAIN_STORAGE_LINE_SHIFT)) { if (_fd == -1) {
_dirty_mask &= ~write_mask; perf_end(_perf_storage);
if (write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT) != n<<VRBRAIN_STORAGE_LINE_SHIFT) { perf_count(_perf_errors);
// write error - likely EINTR return;
_dirty_mask |= write_mask; }
close(_fd); }
_fd = -1;
perf_count(_perf_errors); // 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;
perf_end(_perf_storage); for (i=0; i<VRBRAIN_STORAGE_NUM_LINES; i++) {
} if (_dirty_mask & (1<<i)) {
break;
#endif // CONFIG_HAL_BOARD }
}
if (i == VRBRAIN_STORAGE_NUM_LINES) {
// this shouldn't be possible
perf_end(_perf_storage);
perf_count(_perf_errors);
return;
}
uint32_t write_mask = (1U<<i);
// see how many lines to write
for (n=1; (i+n) < VRBRAIN_STORAGE_NUM_LINES &&
n < (VRBRAIN_STORAGE_MAX_WRITE>>VRBRAIN_STORAGE_LINE_SHIFT); n++) {
if (!(_dirty_mask & (1<<(n+i)))) {
break;
}
// mark that line clean
write_mask |= (1<<(n+i));
}
/*
write the lines. This also updates _dirty_mask. Note that
because this is a SCHED_FIFO thread it will not be preempted
by the main task except during blocking calls. This means we
don't need a semaphore around the _dirty_mask updates.
*/
if (lseek(_fd, i<<VRBRAIN_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<VRBRAIN_STORAGE_LINE_SHIFT)) {
_dirty_mask &= ~write_mask;
bus_lock(true);
ssize_t ret = write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT);
bus_lock(false);
if (ret != n<<VRBRAIN_STORAGE_LINE_SHIFT) {
// write error - likely EINTR
_dirty_mask |= write_mask;
close(_fd);
_fd = -1;
perf_count(_perf_errors);
}
}
perf_end(_perf_storage);
}
#endif // CONFIG_HAL_BOARD

79
libraries/AP_HAL_VRBRAIN/Storage.h
View File

@ -1,37 +1,42 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h" #include "AP_HAL_VRBRAIN_Namespace.h"
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE #define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE
#define VRBRAIN_STORAGE_MAX_WRITE 512 #define VRBRAIN_STORAGE_MAX_WRITE 512
#define VRBRAIN_STORAGE_LINE_SHIFT 9 #define VRBRAIN_STORAGE_LINE_SHIFT 9
#define VRBRAIN_STORAGE_LINE_SIZE (1<<VRBRAIN_STORAGE_LINE_SHIFT) #define VRBRAIN_STORAGE_LINE_SIZE (1<<VRBRAIN_STORAGE_LINE_SHIFT)
#define VRBRAIN_STORAGE_NUM_LINES (VRBRAIN_STORAGE_SIZE/VRBRAIN_STORAGE_LINE_SIZE) #define VRBRAIN_STORAGE_NUM_LINES (VRBRAIN_STORAGE_SIZE/VRBRAIN_STORAGE_LINE_SIZE)
class VRBRAIN::VRBRAINStorage : public AP_HAL::Storage { class VRBRAIN::VRBRAINStorage : public AP_HAL::Storage {
public: public:
VRBRAINStorage(); VRBRAINStorage();
void init() {} void init() {}
void read_block(void *dst, uint16_t src, size_t n); void read_block(void *dst, uint16_t src, size_t n);
void write_block(uint16_t dst, const void* src, size_t n); void write_block(uint16_t dst, const void* src, size_t n);
void _timer_tick(void); void _timer_tick(void);
private: private:
int _fd; int _fd;
volatile bool _initialised; volatile bool _initialised;
void _storage_create(void); void _storage_create(void);
void _storage_open(void); void _storage_open(void);
void _mark_dirty(uint16_t loc, uint16_t length); void _mark_dirty(uint16_t loc, uint16_t length);
uint8_t _buffer[VRBRAIN_STORAGE_SIZE] __attribute__((aligned(4))); uint8_t _buffer[VRBRAIN_STORAGE_SIZE] __attribute__((aligned(4)));
volatile uint32_t _dirty_mask; volatile uint32_t _dirty_mask;
perf_counter_t _perf_storage; perf_counter_t _perf_storage;
perf_counter_t _perf_errors; perf_counter_t _perf_errors;
bool _have_mtd; bool _have_mtd;
void _upgrade_to_mtd(void); void _upgrade_to_mtd(void);
uint32_t _mtd_signature(void); uint32_t _mtd_signature(void);
void _mtd_write_signature(void); void _mtd_write_signature(void);
};
void bus_lock(bool lock);
};

1081
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

File diff suppressed because it is too large Load Diff

151
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

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

369
libraries/AP_HAL_VRBRAIN/Util.cpp
View File

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

103
libraries/AP_HAL_VRBRAIN/Util.h
View File

@ -1,27 +1,76 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h" #include "AP_HAL_VRBRAIN_Namespace.h"
#include "Semaphores.h"
class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
public: class VRBRAIN::NSHShellStream : public AP_HAL::Stream {
VRBRAINUtil(void); public:
bool run_debug_shell(AP_HAL::BetterStream *stream); size_t write(uint8_t);
size_t write(const uint8_t *buffer, size_t size);
enum safety_state safety_switch_state(void); int16_t read() override;
uint32_t available() override;
/* uint32_t txspace() override;
set system clock in UTC microseconds private:
*/ int shell_stdin = -1;
void set_system_clock(uint64_t time_utc_usec); int shell_stdout = -1;
pthread_t shell_thread_ctx;
/*
get system identifier (STM32 serial number) struct {
*/ int in = -1;
bool get_system_id(char buf[40]); int out = -1;
} child;
uint32_t available_memory(void) override; bool showed_memory_warning = false;
bool showed_armed_warning = false;
private:
int _safety_handle; 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;
};

49
libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp Normal file
View File

@ -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 <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <px4_defines.h>
#include <px4_posix.h>
#include <stdio.h>
#include "systemlib/param/param.h"
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
param_t param_find(const char *name)
{
::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