Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Global: Remove '\r' character from all source code files

Browse Source
This commit is contained in:
Murilo Belluzzo 2016-09-27 11:46:23 -03:00 committed by Lucas De Marchi
parent 1fc4a6b19f
commit 0b86532b5c
26 changed files with 4106 additions and 4106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h
View File

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

596
libraries/AP_HAL_VRBRAIN/AnalogIn.cpp
View File

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

402
libraries/AP_HAL_VRBRAIN/GPIO.cpp
View File

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

120
libraries/AP_HAL_VRBRAIN/GPIO.h
View File

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

726
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
View File

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

40
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h
View File

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

156
libraries/AP_HAL_VRBRAIN/I2CDevice.cpp
View File

@ -1,78 +1,78 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "I2CDevice.h"
#include <AP_HAL/AP_HAL.h>
#include "Util.h"
namespace VRBRAIN {
uint8_t VRBRAIN::VRBRAIN_I2C::instance;
/*
implement wrapper for VRBRAIN I2C driver
*/
bool VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
{
if (!init_done) {
init_done = true;
// we do late init() so we can setup the device paths
snprintf(devname, sizeof(devname), "AP_I2C_%u", instance);
snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance);
init_ok = (init() == OK);
if (init_ok) {
instance++;
}
}
if (!init_ok) {
return false;
}
set_address(address);
bool ret = (transfer(send, send_len, recv, recv_len) == OK);
return ret;
}
I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
_bus(bus),
_address(address)
{
}
I2CDevice::~I2CDevice()
{
}
bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
return _bus.do_transfer(_address, send, send_len, recv, recv_len);
}
bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times)
{
return false;
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
{
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address));
return dev;
}
}
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "I2CDevice.h"
#include <AP_HAL/AP_HAL.h>
#include "Util.h"
namespace VRBRAIN {
uint8_t VRBRAIN::VRBRAIN_I2C::instance;
/*
implement wrapper for VRBRAIN I2C driver
*/
bool VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
{
if (!init_done) {
init_done = true;
// we do late init() so we can setup the device paths
snprintf(devname, sizeof(devname), "AP_I2C_%u", instance);
snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance);
init_ok = (init() == OK);
if (init_ok) {
instance++;
}
}
if (!init_ok) {
return false;
}
set_address(address);
bool ret = (transfer(send, send_len, recv, recv_len) == OK);
return ret;
}
I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
_bus(bus),
_address(address)
{
}
I2CDevice::~I2CDevice()
{
}
bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
return _bus.do_transfer(_address, send, send_len, recv, recv_len);
}
bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times)
{
return false;
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
{
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address));
return dev;
}
}

188
libraries/AP_HAL_VRBRAIN/I2CDevice.h
View File

@ -1,94 +1,94 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <inttypes.h>
#include <AP_HAL/HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include "I2CWrapper.h"
namespace VRBRAIN {
class I2CDevice : public AP_HAL::I2CDevice {
public:
static I2CDevice *from(AP_HAL::I2CDevice *dev)
{
return static_cast<I2CDevice*>(dev);
}
I2CDevice(uint8_t bus, uint8_t address);
~I2CDevice();
/* See AP_HAL::I2CDevice::set_address() */
void set_address(uint8_t address) override { _address = address; }
/* See AP_HAL::I2CDevice::set_retries() */
void set_retries(uint8_t retries) override { _retries = retries; }
/* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
bool set_speed(enum Device::Speed speed) override { return true; }
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times) override;
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
{
/* Not implemented yet */
return nullptr;
}
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
{
/* Not implemented yet */
return false;
}
AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
private:
// we use an empty semaphore as the underlying I2C class already has a semaphore
Empty::Semaphore semaphore;
VRBRAIN_I2C _bus;
uint8_t _address;
uint8_t _retries = 0;
};
class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
public:
friend class I2CDevice;
static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
{
return static_cast<I2CDeviceManager*>(i2c_mgr);
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address) override;
};
}
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <inttypes.h>
#include <AP_HAL/HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include "I2CWrapper.h"
namespace VRBRAIN {
class I2CDevice : public AP_HAL::I2CDevice {
public:
static I2CDevice *from(AP_HAL::I2CDevice *dev)
{
return static_cast<I2CDevice*>(dev);
}
I2CDevice(uint8_t bus, uint8_t address);
~I2CDevice();
/* See AP_HAL::I2CDevice::set_address() */
void set_address(uint8_t address) override { _address = address; }
/* See AP_HAL::I2CDevice::set_retries() */
void set_retries(uint8_t retries) override { _retries = retries; }
/* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
bool set_speed(enum Device::Speed speed) override { return true; }
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times) override;
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
{
/* Not implemented yet */
return nullptr;
}
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override
{
/* Not implemented yet */
return false;
}
AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
private:
// we use an empty semaphore as the underlying I2C class already has a semaphore
Empty::Semaphore semaphore;
VRBRAIN_I2C _bus;
uint8_t _address;
uint8_t _retries = 0;
};
class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
public:
friend class I2CDevice;
static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
{
return static_cast<I2CDeviceManager*>(i2c_mgr);
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address) override;
};
}

286
libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp
View File

@ -1,143 +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
/// -*- 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

276
libraries/AP_HAL_VRBRAIN/RCInput.cpp
View File

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

76
libraries/AP_HAL_VRBRAIN/RCInput.h
View File

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

1232
libraries/AP_HAL_VRBRAIN/RCOutput.cpp
View File

File diff suppressed because it is too large Load Diff

152
libraries/AP_HAL_VRBRAIN/RCOutput.h
View File

@ -1,76 +1,76 @@
#pragma once
#include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#define VRBRAIN_NUM_OUTPUT_CHANNELS 16
class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput
{
public:
void init() override;
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
uint16_t read_last_sent(uint8_t ch) override;
void read_last_sent(uint16_t* period_us, uint8_t len) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;
void force_safety_off(void) override;
void force_safety_no_wait(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
_esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm;
}
void cork();
void push();
void set_output_mode(enum output_mode mode) override;
void _timer_tick(void);
bool enable_sbus_out(uint16_t rate_hz) override;
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
bool _sbus_enabled:1;
perf_counter_t _perf_rcout;
uint32_t _last_output;
uint32_t _last_config_us;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
uint16_t _enabled_channels;
struct {
int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = NULL;
orb_advert_t _actuator_armed_pub = NULL;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;
void _init_alt_channels(void);
void _publish_actuators(void);
void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
uint32_t _safety_state_request_last_ms = 0;
void force_safety_pending_requests(void);
};
#pragma once
#include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#define VRBRAIN_NUM_OUTPUT_CHANNELS 16
class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput
{
public:
void init() override;
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
uint16_t read_last_sent(uint8_t ch) override;
void read_last_sent(uint16_t* period_us, uint8_t len) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;
void force_safety_off(void) override;
void force_safety_no_wait(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
_esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm;
}
void cork();
void push();
void set_output_mode(enum output_mode mode) override;
void _timer_tick(void);
bool enable_sbus_out(uint16_t rate_hz) override;
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
bool _sbus_enabled:1;
perf_counter_t _perf_rcout;
uint32_t _last_output;
uint32_t _last_config_us;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
uint16_t _enabled_channels;
struct {
int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = NULL;
orb_advert_t _actuator_armed_pub = NULL;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;
void _init_alt_channels(void);
void _publish_actuators(void);
void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
uint32_t _safety_state_request_last_ms = 0;
void force_safety_pending_requests(void);
};

776
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
View File

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

204
libraries/AP_HAL_VRBRAIN/Scheduler.h
View File

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

78
libraries/AP_HAL_VRBRAIN/Semaphores.cpp
View File

@ -1,39 +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
#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

40
libraries/AP_HAL_VRBRAIN/Semaphores.h
View File

@ -1,20 +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
#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

618
libraries/AP_HAL_VRBRAIN/Storage.cpp
View File

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

84
libraries/AP_HAL_VRBRAIN/Storage.h
View File

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

1088
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

File diff suppressed because it is too large Load Diff

154
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

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

458
libraries/AP_HAL_VRBRAIN/Util.cpp
View File

@ -1,229 +1,229 @@
#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 "UARTDriver.h"
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <systemlib/board_serial.h>
#include <drivers/drv_gpio.h>
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace VRBRAIN;
extern bool _vrbrain_thread_should_exit;
/*
constructor
*/
VRBRAINUtil::VRBRAINUtil(void) : Util()
{
_safety_handle = orb_subscribe(ORB_ID(safety));
}
/*
start an instance of nsh
*/
bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream)
{
VRBRAINUARTDriver *uart = (VRBRAINUARTDriver *)stream;
int fd;
// trigger exit in the other threads. This stops use of the
// various driver handles, and especially the px4io handle,
// which otherwise would cause a crash if px4io is stopped in
// the shell
_vrbrain_thread_should_exit = true;
// take control of stream fd
fd = uart->_get_fd();
// mark it blocking (nsh expects a blocking fd)
unsigned v;
v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v & ~O_NONBLOCK);
// setup the UART on stdin/stdout/stderr
close(0);
close(1);
close(2);
dup2(fd, 0);
dup2(fd, 1);
dup2(fd, 2);
nsh_consolemain(0, NULL);
// this shouldn't happen
hal.console->printf("shell exited\n");
return true;
}
/*
return state of safety switch
*/
enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void)
{
if (_safety_handle == -1) {
_safety_handle = orb_subscribe(ORB_ID(safety));
}
if (_safety_handle == -1) {
return AP_HAL::Util::SAFETY_NONE;
}
struct safety_s safety;
if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) {
return AP_HAL::Util::SAFETY_NONE;
}
if (!safety.safety_switch_available) {
return AP_HAL::Util::SAFETY_NONE;
}
if (safety.safety_off) {
return AP_HAL::Util::SAFETY_ARMED;
}
return AP_HAL::Util::SAFETY_DISARMED;
}
void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec)
{
timespec ts;
ts.tv_sec = time_utc_usec/1.0e6f;
ts.tv_nsec = (time_utc_usec % 1000000) * 1000;
clock_settime(CLOCK_REALTIME, &ts);
}
/*
display VRBRAIN system identifer - board type and serial number
*/
bool VRBRAINUtil::get_system_id(char buf[40])
{
uint8_t serialid[12];
memset(serialid, 0, sizeof(serialid));
get_board_serial(serialid);
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
const char *board_type = "VRBRAINv45";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
const char *board_type = "VRBRAINv51";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
const char *board_type = "VRBRAINv52";
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
const char *board_type = "VRUBRAINv51";
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
const char *board_type = "VRUBRAINv52";
#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
const char *board_type = "VRCOREv10";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
const char *board_type = "VRBRAINv54";
#endif
// this format is chosen to match the human_readable_serial()
// function in auth.c
snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
board_type,
(unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3],
(unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7],
(unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
return true;
}
/**
how much free memory do we have in bytes.
*/
uint32_t VRBRAINUtil::available_memory(void)
{
return mallinfo().fordblks;
}
/*
AP_HAL wrapper around PX4 perf counters
*/
VRBRAINUtil::perf_counter_t VRBRAINUtil::perf_alloc(VRBRAINUtil::perf_counter_type t, const char *name)
{
::perf_counter_type vrbrain_t;
switch (t) {
case VRBRAINUtil::PC_COUNT:
vrbrain_t = ::PC_COUNT;
break;
case VRBRAINUtil::PC_ELAPSED:
vrbrain_t = ::PC_ELAPSED;
break;
case VRBRAINUtil::PC_INTERVAL:
vrbrain_t = ::PC_INTERVAL;
break;
default:
return NULL;
}
return (perf_counter_t)::perf_alloc(vrbrain_t, name);
}
void VRBRAINUtil::perf_begin(perf_counter_t h)
{
::perf_begin((::perf_counter_t)h);
}
void VRBRAINUtil::perf_end(perf_counter_t h)
{
::perf_end((::perf_counter_t)h);
}
void VRBRAINUtil::perf_count(perf_counter_t h)
{
::perf_count((::perf_counter_t)h);
}
void VRBRAINUtil::set_imu_temp(float current)
{
if (!_heater.target || *_heater.target == -1) {
return;
}
// average over temperatures to remove noise
_heater.count++;
_heater.sum += current;
// update once a second
uint32_t now = AP_HAL::millis();
if (now - _heater.last_update_ms < 1000) {
return;
}
_heater.last_update_ms = now;
current = _heater.sum / _heater.count;
_heater.sum = 0;
_heater.count = 0;
// experimentally tweaked for Pixhawk2
const float kI = 0.3f;
const float kP = 200.0f;
float err = ((float)*_heater.target) - current;
_heater.integrator += kI * err;
_heater.integrator = constrain_float(_heater.integrator, 0, 70);
float output = constrain_float(kP * err + _heater.integrator, 0, 100);
// hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err);
if (_heater.fd == -1) {
_heater.fd = open("/dev/px4io", O_RDWR);
}
if (_heater.fd != -1) {
ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output);
}
}
void VRBRAINUtil::set_imu_target_temp(int8_t *target)
{
_heater.target = target;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#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 "UARTDriver.h"
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <systemlib/board_serial.h>
#include <drivers/drv_gpio.h>
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace VRBRAIN;
extern bool _vrbrain_thread_should_exit;
/*
constructor
*/
VRBRAINUtil::VRBRAINUtil(void) : Util()
{
_safety_handle = orb_subscribe(ORB_ID(safety));
}
/*
start an instance of nsh
*/
bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream)
{
VRBRAINUARTDriver *uart = (VRBRAINUARTDriver *)stream;
int fd;
// trigger exit in the other threads. This stops use of the
// various driver handles, and especially the px4io handle,
// which otherwise would cause a crash if px4io is stopped in
// the shell
_vrbrain_thread_should_exit = true;
// take control of stream fd
fd = uart->_get_fd();
// mark it blocking (nsh expects a blocking fd)
unsigned v;
v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v & ~O_NONBLOCK);
// setup the UART on stdin/stdout/stderr
close(0);
close(1);
close(2);
dup2(fd, 0);
dup2(fd, 1);
dup2(fd, 2);
nsh_consolemain(0, NULL);
// this shouldn't happen
hal.console->printf("shell exited\n");
return true;
}
/*
return state of safety switch
*/
enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void)
{
if (_safety_handle == -1) {
_safety_handle = orb_subscribe(ORB_ID(safety));
}
if (_safety_handle == -1) {
return AP_HAL::Util::SAFETY_NONE;
}
struct safety_s safety;
if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) {
return AP_HAL::Util::SAFETY_NONE;
}
if (!safety.safety_switch_available) {
return AP_HAL::Util::SAFETY_NONE;
}
if (safety.safety_off) {
return AP_HAL::Util::SAFETY_ARMED;
}
return AP_HAL::Util::SAFETY_DISARMED;
}
void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec)
{
timespec ts;
ts.tv_sec = time_utc_usec/1.0e6f;
ts.tv_nsec = (time_utc_usec % 1000000) * 1000;
clock_settime(CLOCK_REALTIME, &ts);
}
/*
display VRBRAIN system identifer - board type and serial number
*/
bool VRBRAINUtil::get_system_id(char buf[40])
{
uint8_t serialid[12];
memset(serialid, 0, sizeof(serialid));
get_board_serial(serialid);
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
const char *board_type = "VRBRAINv45";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
const char *board_type = "VRBRAINv51";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
const char *board_type = "VRBRAINv52";
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
const char *board_type = "VRUBRAINv51";
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
const char *board_type = "VRUBRAINv52";
#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
const char *board_type = "VRCOREv10";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
const char *board_type = "VRBRAINv54";
#endif
// this format is chosen to match the human_readable_serial()
// function in auth.c
snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
board_type,
(unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3],
(unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7],
(unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
return true;
}
/**
how much free memory do we have in bytes.
*/
uint32_t VRBRAINUtil::available_memory(void)
{
return mallinfo().fordblks;
}
/*
AP_HAL wrapper around PX4 perf counters
*/
VRBRAINUtil::perf_counter_t VRBRAINUtil::perf_alloc(VRBRAINUtil::perf_counter_type t, const char *name)
{
::perf_counter_type vrbrain_t;
switch (t) {
case VRBRAINUtil::PC_COUNT:
vrbrain_t = ::PC_COUNT;
break;
case VRBRAINUtil::PC_ELAPSED:
vrbrain_t = ::PC_ELAPSED;
break;
case VRBRAINUtil::PC_INTERVAL:
vrbrain_t = ::PC_INTERVAL;
break;
default:
return NULL;
}
return (perf_counter_t)::perf_alloc(vrbrain_t, name);
}
void VRBRAINUtil::perf_begin(perf_counter_t h)
{
::perf_begin((::perf_counter_t)h);
}
void VRBRAINUtil::perf_end(perf_counter_t h)
{
::perf_end((::perf_counter_t)h);
}
void VRBRAINUtil::perf_count(perf_counter_t h)
{
::perf_count((::perf_counter_t)h);
}
void VRBRAINUtil::set_imu_temp(float current)
{
if (!_heater.target || *_heater.target == -1) {
return;
}
// average over temperatures to remove noise
_heater.count++;
_heater.sum += current;
// update once a second
uint32_t now = AP_HAL::millis();
if (now - _heater.last_update_ms < 1000) {
return;
}
_heater.last_update_ms = now;
current = _heater.sum / _heater.count;
_heater.sum = 0;
_heater.count = 0;
// experimentally tweaked for Pixhawk2
const float kI = 0.3f;
const float kP = 200.0f;
float err = ((float)*_heater.target) - current;
_heater.integrator += kI * err;
_heater.integrator = constrain_float(_heater.integrator, 0, 70);
float output = constrain_float(kP * err + _heater.integrator, 0, 100);
// hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err);
if (_heater.fd == -1) {
_heater.fd = open("/dev/px4io", O_RDWR);
}
if (_heater.fd != -1) {
ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output);
}
}
void VRBRAINUtil::set_imu_target_temp(int8_t *target)
{
_heater.target = target;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

152
libraries/AP_HAL_VRBRAIN/Util.h
View File

@ -1,76 +1,76 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "Semaphores.h"
class VRBRAIN::NSHShellStream : public AP_HAL::Stream {
public:
size_t write(uint8_t);
size_t write(const uint8_t *buffer, size_t size);
int16_t read() override;
uint32_t available() override;
uint32_t txspace() override;
private:
int shell_stdin = -1;
int shell_stdout = -1;
pthread_t shell_thread_ctx;
struct {
int in = -1;
int out = -1;
} child;
bool showed_memory_warning = false;
bool showed_armed_warning = false;
void start_shell(void);
static void shell_thread(void *arg);
};
class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
public:
VRBRAINUtil(void);
bool run_debug_shell(AP_HAL::BetterStream *stream);
enum safety_state safety_switch_state(void);
/*
set system clock in UTC microseconds
*/
void set_system_clock(uint64_t time_utc_usec);
/*
get system identifier (STM32 serial number)
*/
bool get_system_id(char buf[40]);
uint32_t available_memory(void) override;
/*
return a stream for access to nsh shell
*/
AP_HAL::Stream *get_shell_stream() { return &_shell_stream; }
perf_counter_t perf_alloc(perf_counter_type t, const char *name) override;
void perf_begin(perf_counter_t ) override;
void perf_end(perf_counter_t) override;
void perf_count(perf_counter_t) override;
// create a new semaphore
AP_HAL::Semaphore *new_semaphore(void) override { return new VRBRAIN::Semaphore; }
void set_imu_temp(float current) override;
void set_imu_target_temp(int8_t *target) override;
private:
int _safety_handle;
VRBRAIN::NSHShellStream _shell_stream;
struct {
int8_t *target;
float integrator;
uint16_t count;
float sum;
uint32_t last_update_ms;
int fd = -1;
} _heater;
};
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "Semaphores.h"
class VRBRAIN::NSHShellStream : public AP_HAL::Stream {
public:
size_t write(uint8_t);
size_t write(const uint8_t *buffer, size_t size);
int16_t read() override;
uint32_t available() override;
uint32_t txspace() override;
private:
int shell_stdin = -1;
int shell_stdout = -1;
pthread_t shell_thread_ctx;
struct {
int in = -1;
int out = -1;
} child;
bool showed_memory_warning = false;
bool showed_armed_warning = false;
void start_shell(void);
static void shell_thread(void *arg);
};
class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
public:
VRBRAINUtil(void);
bool run_debug_shell(AP_HAL::BetterStream *stream);
enum safety_state safety_switch_state(void);
/*
set system clock in UTC microseconds
*/
void set_system_clock(uint64_t time_utc_usec);
/*
get system identifier (STM32 serial number)
*/
bool get_system_id(char buf[40]);
uint32_t available_memory(void) override;
/*
return a stream for access to nsh shell
*/
AP_HAL::Stream *get_shell_stream() { return &_shell_stream; }
perf_counter_t perf_alloc(perf_counter_type t, const char *name) override;
void perf_begin(perf_counter_t ) override;
void perf_end(perf_counter_t) override;
void perf_count(perf_counter_t) override;
// create a new semaphore
AP_HAL::Semaphore *new_semaphore(void) override { return new VRBRAIN::Semaphore; }
void set_imu_temp(float current) override;
void set_imu_target_temp(int8_t *target) override;
private:
int _safety_handle;
VRBRAIN::NSHShellStream _shell_stream;
struct {
int8_t *target;
float integrator;
uint16_t count;
float sum;
uint32_t last_update_ms;
int fd = -1;
} _heater;
};

98
libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp
View File

@ -1,49 +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
/// -*- 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

10
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -111,12 +111,12 @@ char *FlightAxis::soap_request(const char *action, const char *fmt, ...)
sock.set_blocking(false);
char *req;
asprintf(&req, R"(POST / HTTP/1.1
soapaction: '%s'
content-length: %u
asprintf(&req, R"(POST / HTTP/1.1
soapaction: '%s'
content-length: %u
content-type: text/xml;charset='UTF-8'
Connection: Keep-Alive
Connection: Keep-Alive
%s)",
action,
(unsigned)strlen(req1), req1);