Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_VRBRAIN: removed HAL

This commit is contained in:
Andrew Tridgell 2019-01-08 20:59:08 +11:00
parent 38e2c8e0ec
commit 43ed4175b1
35 changed files with 0 additions and 6851 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h
View File

@ -1,8 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "HAL_VRBRAIN_Class.h"
#endif // CONFIG_HAL_BOARD

20
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h
View File

@ -1,20 +0,0 @@
#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;
class VRBRAINCAN;
class VRBRAINCANManager;
}

301
libraries/AP_HAL_VRBRAIN/AnalogIn.cpp
View File

@ -1,301 +0,0 @@
#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_VRBRAIN_V52E) || 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
{ 0, 0.f },
};
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()
{
WITH_SEMAPHORE(_semaphore);
if (_sum_count == 0) {
return _value;
}
_value = _sum_value / _sum_count;
_value_ratiometric = _sum_ratiometric / _sum_count;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
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;
}
WITH_SEMAPHORE(_semaphore);
_pin = pin;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
_latest_value = 0;
_value = 0;
_value_ratiometric = 0;
}
/*
apply a reading in ADC counts
*/
void VRBRAINAnalogSource::_add_value(float v, float vcc5V)
{
WITH_SEMAPHORE(_semaphore);
_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)
{
if (_adc_fd == -1) {
// not initialised yet
return;
}
// read adc at 100Hz
uint32_t now = AP_HAL::micros();
uint32_t delta_t = now - _last_run;
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] == nullptr ||
_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 != nullptr && 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] == nullptr) {
_channels[j] = new VRBRAINAnalogSource(pin, 0.0f);
return _channels[j];
}
}
hal.console->printf("Out of analog channels\n");
return nullptr;
}
#endif // CONFIG_HAL_BOARD

83
libraries/AP_HAL_VRBRAIN/AnalogIn.h
View File

@ -1,83 +0,0 @@
#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_VRBRAIN_V52E) || 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();
HAL_Semaphore _semaphore;
};
class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn {
public:
VRBRAINAnalogIn();
void init() override;
AP_HAL::AnalogSource* channel(int16_t pin) override;
void _timer_tick(void);
float board_voltage(void) override { return _board_voltage; }
float servorail_voltage(void) override { return _servorail_voltage; }
uint16_t power_status_flags(void) override { return _power_flags; }
private:
int _adc_fd = -1;
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);
};

1117
libraries/AP_HAL_VRBRAIN/CAN.cpp
View File

File diff suppressed because it is too large Load Diff

312
libraries/AP_HAL_VRBRAIN/CAN.h
View File

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

155
libraries/AP_HAL_VRBRAIN/Device.cpp
View File

@ -1,155 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Device.h"
#include <arch/board/board.h>
#include "board_config.h"
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/OwnPtr.h>
#include "Scheduler.h"
#include "Semaphores.h"
extern bool _vrbrain_thread_should_exit;
namespace VRBRAIN {
static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
/*
per-bus callback thread
*/
void *DeviceBus::bus_thread(void *arg)
{
struct DeviceBus *binfo = (struct DeviceBus *)arg;
// setup a name for the thread
char name[] = "XXX:X";
switch (binfo->hal_device->bus_type()) {
case AP_HAL::Device::BUS_TYPE_I2C:
snprintf(name, sizeof(name), "I2C:%u",
binfo->hal_device->bus_num());
break;
case AP_HAL::Device::BUS_TYPE_SPI:
snprintf(name, sizeof(name), "SPI:%u",
binfo->hal_device->bus_num());
break;
default:
break;
}
pthread_setname_np(pthread_self(), name);
while (!_vrbrain_thread_should_exit) {
uint64_t now = AP_HAL::micros64();
DeviceBus::callback_info *callback;
// find a callback to run
for (callback = binfo->callbacks; callback; callback = callback->next) {
if (now >= callback->next_usec) {
while (now >= callback->next_usec) {
callback->next_usec += callback->period_usec;
}
// call it with semaphore held
if (binfo->semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
callback->cb();
binfo->semaphore.give();
}
}
}
// work out when next loop is needed
uint64_t next_needed = 0;
now = AP_HAL::micros64();
for (callback = binfo->callbacks; callback; callback = callback->next) {
if (next_needed == 0 ||
callback->next_usec < next_needed) {
next_needed = callback->next_usec;
if (next_needed < now) {
next_needed = now;
}
}
}
// delay for at most 50ms, to handle newly added callbacks
uint32_t delay = 50000;
if (next_needed >= now && next_needed - now < delay) {
delay = next_needed - now;
}
// don't delay for less than 400usec, so one thread doesn't
// completely dominate the CPU
if (delay < 400) {
delay = 400;
}
hal.scheduler->delay_microseconds(delay);
}
return nullptr;
}
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
{
if (!thread_started) {
thread_started = true;
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 1024);
param.sched_priority = thread_priority;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
hal_device = _hal_device;
pthread_create(&thread_ctx, &thread_attr, &DeviceBus::bus_thread, this);
}
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
if (callback == nullptr) {
return nullptr;
}
callback->cb = cb;
callback->period_usec = period_usec;
callback->next_usec = AP_HAL::micros64() + period_usec;
// add to linked list of callbacks on thread
callback->next = callbacks;
callbacks = callback;
return callback;
}
/*
* Adjust the timer for the next call: it needs to be called from the bus
* thread, otherwise it will race with it
*/
bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
{
if (!pthread_equal(pthread_self(), thread_ctx)) {
fprintf(stderr, "can't adjust timer from unknown thread context\n");
return false;
}
DeviceBus::callback_info *callback = static_cast<DeviceBus::callback_info *>(h);
callback->period_usec = period_usec;
callback->next_usec = AP_HAL::micros64() + period_usec;
return true;
}
}

49
libraries/AP_HAL_VRBRAIN/Device.h
View File

@ -1,49 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published
* by the Free Software Foundation, either version 3 of the License,
* or (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <inttypes.h>
#include <AP_HAL/HAL.h>
#include "Semaphores.h"
#include "Scheduler.h"
namespace VRBRAIN {
class DeviceBus {
public:
DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) :
thread_priority(_thread_priority) {}
struct DeviceBus *next;
Semaphore semaphore;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device);
bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
static void *bus_thread(void *arg);
private:
struct callback_info {
struct callback_info *next;
AP_HAL::Device::PeriodicCb cb;
uint32_t period_usec;
uint64_t next_usec;
} *callbacks;
uint8_t thread_priority;
pthread_t thread_ctx;
bool thread_started;
AP_HAL::Device *hal_device;
};
}

209
libraries/AP_HAL_VRBRAIN/GPIO.cpp
View File

@ -1,209 +0,0 @@
#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
#ifdef GPIO_SERVO_4
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_4) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO_4\n");
}
#endif
}
void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
{
switch (pin) {
}
}
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
#ifdef GPIO_SERVO_4
case EXTERNAL_RELAY2_PIN: {
uint32_t relays = 0;
ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
return (relays & GPIO_SERVO_4)?HIGH:LOW;
}
#endif
}
return LOW;
}
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
#ifdef GPIO_SERVO_4
case EXTERNAL_RELAY2_PIN:
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_4);
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);
}
/*
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

56
libraries/AP_HAL_VRBRAIN/GPIO.h
View File

@ -1,56 +0,0 @@
#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 34
# define EXTERNAL_RELAY2_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() override;
void pinMode(uint8_t pin, uint8_t output) override;
uint8_t read(uint8_t pin) override;
void write(uint8_t pin, uint8_t value) override;
void toggle(uint8_t pin) override;
/* Alternative interface: */
AP_HAL::DigitalSource* channel(uint16_t n) override;
/* return true if USB cable is connected */
bool usb_connected(void) override;
// used by UART code to avoid a hw bug in the AUAV-X2
void set_usb_connected(void) { _usb_connected = true; }
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;
};

390
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
View File

@ -1,390 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_HAL/utility/RCOutput_Tap.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include "AP_HAL_VRBRAIN.h"
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "HAL_VRBRAIN_Class.h"
#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 "SPIDevice.h"
#if HAL_WITH_UAVCAN
#include "CAN.h"
#endif
#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;
using namespace ap;
//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;
static VRBRAIN::SPIDeviceManager spi_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_VRBRAIN_V52E)
#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/ttyS1"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
#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 */
nullptr, /* no uartG */
&i2c_mgr_instance,
&spi_mgr_instance,
&analogIn, /* analogin */
&storageDriver, /* storage */
&uartADriver, /* console */
&gpioDriver, /* gpio */
&rcinDriver, /* rcinput */
&rcoutDriver, /* rcoutput */
&schedulerInstance, /* scheduler */
&utilInstance, /* util */
nullptr, /* no onboard optical flow */
nullptr) /* CAN */
{}
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();
// init the I2C wrapper class
VRBRAIN_I2C::init_lock();
/*
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, nullptr);
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("\t-d5 DEVICE set uartF (default %s)\n", UARTF_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;
const char *deviceF = UARTF_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);
uartFDriver.set_device_path(deviceF);
printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s uartF=%s\n",
SKETCHNAME, deviceA, deviceC, deviceD, deviceE, deviceF);
_vrbrain_thread_should_exit = false;
daemon_task = px4_task_spawn_cmd(SKETCHNAME,
SCHED_FIFO,
APM_MAIN_PRIORITY,
APM_MAIN_THREAD_STACK_SIZE,
main_loop,
nullptr);
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);
}
}
if (strcmp(argv[i], "-d5") == 0) {
// set uartF
if (argc > i + 1) {
deviceF = strdup(argv[i+1]);
} else {
printf("missing parameter to -d5 DEVICE\n");
usage();
exit(1);
}
}
}
usage();
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

20
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h
View File

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

192
libraries/AP_HAL_VRBRAIN/I2CDevice.cpp
View File

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

99
libraries/AP_HAL_VRBRAIN/I2CDevice.h
View File

@ -1,99 +0,0 @@
/*
* 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 "Semaphores.h"
#include "I2CWrapper.h"
#include "Device.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 { _vrbraindev.set_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;
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
AP_HAL::Semaphore* get_semaphore() override {
// if asking for invalid bus number use bus 0 semaphore
return &businfo[_busnum<num_buses?_busnum:0].semaphore;
}
void set_split_transfers(bool set) override {
_split_transfers = set;
}
private:
static const uint8_t num_buses = 3;
static DeviceBus businfo[num_buses];
uint8_t _busnum;
VRBRAIN_I2C _vrbraindev;
uint8_t _address;
perf_counter_t perf;
char *pname;
bool _split_transfers;
};
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,
uint32_t bus_clock=400000,
bool use_smbus = false,
uint32_t timeout_ms=4) override;
};
}

42
libraries/AP_HAL_VRBRAIN/I2CWrapper.h
View File

@ -1,42 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <arch/board/board.h>
#include "board_config.h"
#include <drivers/device/i2c.h>
#include "AP_HAL_VRBRAIN.h"
extern const AP_HAL::HAL& hal;
/*
wrapper class for I2C to expose protected functions from PX4Firmware
*/
class VRBRAIN::VRBRAIN_I2C : public device::I2C {
public:
VRBRAIN_I2C(uint8_t bus);
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers);
void set_retries(uint8_t retries) {
_retries = retries;
}
uint8_t map_bus_number(uint8_t bus) const;
// setup instance_lock
static void init_lock(void) {
pthread_mutex_init(&instance_lock, nullptr);
}
private:
static uint8_t instance;
static pthread_mutex_t instance_lock;
bool init_done;
bool init_ok;
char devname[10];
char devpath[14];
};
#endif // CONFIG_HAL_BOARD

142
libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp
View File

@ -1,142 +0,0 @@
/*
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, nullptr);
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

137
libraries/AP_HAL_VRBRAIN/RCInput.cpp
View File

@ -1,137 +0,0 @@
#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>
#include <GCS_MAVLink/GCS.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");
}
pthread_mutex_init(&rcin_mutex, nullptr);
}
bool VRBRAINRCInput::new_input()
{
pthread_mutex_lock(&rcin_mutex);
bool valid = _rcin.timestamp_last_signal != _last_read;
if (_rcin.rc_failsafe) {
// don't consider input valid if we are in RC failsafe.
valid = false;
}
_last_read = _rcin.timestamp_last_signal;
pthread_mutex_unlock(&rcin_mutex);
if (_rcin.input_source != last_input_source) {
gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", input_source_name(_rcin.input_source));
last_input_source = _rcin.input_source;
}
return valid;
}
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 >= MIN(RC_INPUT_MAX_CHANNELS, _rcin.channel_count)) {
return 0;
}
pthread_mutex_lock(&rcin_mutex);
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;
}
const char *VRBRAINRCInput::input_source_name(uint8_t id) const
{
switch(id) {
case input_rc_s::RC_INPUT_SOURCE_UNKNOWN: return "UNKNOWN";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM: return "PX4FMU_PPM";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM: return "PX4IO_PPM";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM: return "PX4IO_SPEKTRUM";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS: return "PX4IO_SBUS";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24: return "PX4IO_ST24";
case input_rc_s::RC_INPUT_SOURCE_MAVLINK: return "MAVLINK";
case input_rc_s::RC_INPUT_SOURCE_QURT: return "QURT";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SPEKTRUM: return "PX4FMU_SPEKTRUM";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS: return "PX4FMU_SBUS";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24: return "PX4FMU_ST24";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD: return "PX4FMU_SUMD";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM: return "PX4FMU_DSM";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD: return "PX4IO_SUMD";
case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SRXL: return "PX4FMU_SRXL";
case input_rc_s::RC_INPUT_SOURCE_PX4IO_SRXL: return "PX4IO_SRXL";
default: return "ERROR";
}
}
void VRBRAINRCInput::_timer_tick(void)
{
perf_begin(_perf_rcin);
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);
if (_rcin.rssi != 0 || _rssi != -1) {
// always zero means not supported
_rssi = _rcin.rssi;
}
pthread_mutex_unlock(&rcin_mutex);
}
// note, we rely on the vehicle code checking new_input()
// and a timeout for the last valid input to handle failsafe
perf_end(_perf_rcin);
}
bool VRBRAINRCInput::rc_bind(int dsmMode)
{
return true;
}
#endif

39
libraries/AP_HAL_VRBRAIN/RCInput.h
View File

@ -1,39 +0,0 @@
#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() override;
bool new_input() override;
uint8_t num_channels() override;
uint16_t read(uint8_t ch) override;
uint8_t read(uint16_t* periods, uint8_t len) override;
int16_t get_rssi(void) override {
return _rssi;
}
void _timer_tick(void);
bool rc_bind(int dsmMode) override;
private:
struct rc_input_values _rcin;
int _rc_sub;
uint64_t _last_read;
perf_counter_t _perf_rcin;
pthread_mutex_t rcin_mutex;
int16_t _rssi = -1;
uint8_t last_input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
const char *input_source_name(uint8_t id) const;
};

682
libraries/AP_HAL_VRBRAIN/RCOutput.cpp
View File

@ -1,682 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "RCOutput.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL& hal;
using namespace VRBRAIN;
/*
enable RCOUT_DEBUG_LATENCY to measure output latency using a logic
analyser. AUX6 will go high during the cork/push output.
*/
#define RCOUT_DEBUG_LATENCY 0
void VRBRAINRCOutput::init()
{
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) {
AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH);
}
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming\n");
}
if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
}
_rate_mask_main = 0;
_rate_mask_alt = 0;
_alt_fd = -1;
_servo_count = 0;
_alt_servo_count = 0;
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
_outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
}
// ensure not to write zeros to disabled channels
for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) {
_period[i] = PWM_IGNORE_THIS_CHANNEL;
_last_sent[i] = PWM_IGNORE_THIS_CHANNEL;
}
}
void VRBRAINRCOutput::_init_alt_channels(void)
{
if (_alt_fd == -1) {
return;
}
if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
return;
}
if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
return;
}
if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
}
}
/*
set output frequency on outputs associated with fd
*/
void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask)
{
if (_output_mode == MODE_PWM_BRUSHED) {
freq_hz /= 8; // divide by 8 for 8MHz clock
// remember max period
_period_max = 1000000UL/freq_hz;
}
// we can't set this per channel
if (freq_hz > 50 || freq_hz == 1) {
// we're being asked to set the alt rate
if (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125) {
/*
set a 1Hz update for oneshot. This periodic output will
never actually trigger, instead we will directly trigger
the pulse after each push()
*/
freq_hz = 1;
}
//::printf("SET_UPDATE_RATE %d %u\n", fd, (unsigned)freq_hz);
if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
return;
}
_freq_hz = freq_hz;
}
/* work out the new rate mask. The outputs have 4 groups of servos.
Group 0: channels 0 1 2 3
Group 1: channels 4 5
Group 2: channels 6 7
Group 3: channels 8 9 10
Group 0: channels 0 1 2
Group 1: channels 3 4 5
Group 2: channels 6 7
Group 3: channels 8 9 10 11
Channels within a group must be set to the same rate.
For the moment we never set the channels above 8 to more than
50Hz
*/
if (freq_hz > 50 || freq_hz == 1) {
// we are setting high rates on the given channels
rate_mask |= chmask & 0xFFF;
#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
if (rate_mask & 0x0F) {
rate_mask |= 0x0F;
}
if (rate_mask & 0x30) {
rate_mask |= 0x30;
}
if (rate_mask & 0xC0) {
rate_mask |= 0xC0;
}
if (rate_mask & 0x700) {
rate_mask |= 0x700;
}
#else
if (rate_mask & 0x07) {
rate_mask |= 0x07;
}
if (rate_mask & 0x38) {
rate_mask |= 0x38;
}
if (rate_mask & 0xC0) {
rate_mask |= 0xC0;
}
if (rate_mask & 0xF00) {
rate_mask |= 0xF00;
}
#endif
} else {
// we are setting low rates on the given channels
#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
if (chmask & 0x0F) {
rate_mask &= ~0x0F;
}
if (chmask & 0x30) {
rate_mask &= ~0x30;
}
if (chmask & 0xC0) {
rate_mask &= ~0xC0;
}
if (chmask & 0x700) {
rate_mask &= ~0x700;
}
#else
if (chmask & 0x07) {
rate_mask &= ~0x07;
}
if (chmask & 0x38) {
rate_mask &= ~0x38;
}
if (chmask & 0xC0) {
rate_mask &= ~0xC0;
}
if (chmask & 0xF00) {
rate_mask &= ~0xF00;
}
#endif
}
if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, rate_mask) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)rate_mask);
}
if (_output_mode == MODE_PWM_BRUSHED) {
ioctl(fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
}
}
/*
set output frequency
*/
void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
if (freq_hz > 50 && (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125)) {
// rate is irrelevent in oneshot
return;
}
// re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT
if (_pwm_fd != -1 && ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
// greater than 400 doesn't give enough room at higher periods for
// the down pulse
if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) {
freq_hz = 400;
}
uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
uint32_t alt_mask = chmask >> _servo_count;
if (primary_mask && _pwm_fd != -1) {
set_freq_fd(_pwm_fd, primary_mask, freq_hz, _rate_mask_main);
}
if (alt_mask && _alt_fd != -1) {
set_freq_fd(_alt_fd, alt_mask, freq_hz, _rate_mask_alt);
}
}
uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
{
if (ch < _servo_count) {
if (_rate_mask_main & (1U<<ch)) {
return _freq_hz;
}
} else if (_alt_fd != -1) {
if (_rate_mask_alt & (1U<<(ch-_servo_count))) {
return _freq_hz;
}
}
return 50;
}
void VRBRAINRCOutput::enable_ch(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
// this is the first enable of an auxiliary channel - setup
// aux channels now. This delayed setup makes it possible to
// use BRD_PWM_COUNT to setup the number of PWM channels.
_init_alt_channels();
}
_enabled_channels |= (1U<<ch);
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
_period[ch] = 0;
_last_sent[ch] = 0;
}
}
void VRBRAINRCOutput::disable_ch(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
_enabled_channels &= ~(1U<<ch);
_period[ch] = PWM_IGNORE_THIS_CHANNEL;
}
void VRBRAINRCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
{
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (uint8_t i=0; i<_servo_count; i++) {
if ((1UL<<i) & chmask) {
pwm_values.values[i] = period_us;
}
pwm_values.channel_count++;
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
}
}
void VRBRAINRCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
{
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (uint8_t i=0; i<_servo_count; i++) {
if ((1UL<<i) & chmask) {
pwm_values.values[i] = period_us;
}
pwm_values.channel_count++;
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
hal.console->printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
}
}
bool VRBRAINRCOutput::force_safety_on(void)
{
_safety_state_request = AP_HAL::Util::SAFETY_DISARMED;
_safety_state_request_last_ms = 1;
return true;
}
void VRBRAINRCOutput::force_safety_off(void)
{
_safety_state_request = AP_HAL::Util::SAFETY_ARMED;
_safety_state_request_last_ms = 1;
}
void VRBRAINRCOutput::force_safety_pending_requests(void)
{
// check if there is a pending saftey_state change. If so (timer != 0) then set it.
uint32_t now = AP_HAL::millis();
if (_safety_state_request_last_ms != 0 &&
now - _safety_state_request_last_ms >= 100) {
if (hal.util->safety_switch_state() == _safety_state_request &&
_safety_state_request_last_ms != 1) {
_safety_state_request_last_ms = 0;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
_safety_state_request_last_ms = now;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
_safety_state_request_last_ms = now;
}
}
}
void VRBRAINRCOutput::force_safety_no_wait(void)
{
if (_safety_state_request_last_ms != 0) {
_safety_state_request_last_ms = 1;
force_safety_pending_requests();
}
}
void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
if (!(_enabled_channels & (1U<<ch))) {
// not enabled
return;
}
if (ch >= _max_channel) {
_max_channel = ch + 1;
}
if (_output_mode == MODE_PWM_ONESHOT125) {
// we treat oneshot125 very simply on HAL_PX4, with 1us
// resolution. Correctly handling it would use a 125 nsec
// step size, to give the full 1000 steps
period_us /= 8;
}
// keep unscaled value
_last_sent[ch] = period_us;
if (_output_mode == MODE_PWM_BRUSHED) {
// map from the PWM range to 0 t0 100% duty cycle. For 16kHz
// this ends up being 0 to 500 pulse width in units of
// 125usec.
if (period_us <= _esc_pwm_min) {
period_us = 0;
} else if (period_us >= _esc_pwm_max) {
period_us = _period_max;
} else {
uint32_t pdiff = period_us - _esc_pwm_min;
period_us = pdiff*_period_max/(_esc_pwm_max - _esc_pwm_min);
}
}
/*
only mark an update is needed if there has been a change, or we
are in oneshot mode. In oneshot mode we always need to send the
output
*/
if (period_us != _period[ch] ||
_output_mode == MODE_PWM_ONESHOT ||
_output_mode == MODE_PWM_ONESHOT125) {
_period[ch] = period_us;
_need_update = true;
// up_pwm_servo_set(ch, period_us);
}
}
uint16_t VRBRAINRCOutput::read(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return 0;
}
// if px4io has given us a value for this channel use that,
// otherwise use the value we last sent. This makes it easier to
// observe the behaviour of failsafe in px4io
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
if (_outputs[i].pwm_sub >= 0 &&
ch < _outputs[i].outputs.noutputs &&
_outputs[i].outputs.output[ch] > 0) {
return _outputs[i].outputs.output[ch];
}
}
return _period[ch];
}
void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len)
{
for (uint8_t i=0; i<len; i++) {
period_us[i] = read(i);
}
}
uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return 0;
}
return _last_sent[ch];
}
void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
{
for (uint8_t i=0; i<len; i++) {
period_us[i] = read_last_sent(i);
}
}
/*
update actuator armed state
*/
void VRBRAINRCOutput::_arm_actuators(bool arm)
{
if (_armed.armed == arm) {
// already armed;
return;
}
_armed.timestamp = hrt_absolute_time();
_armed.armed = arm;
if (arm) {
// this latches ready_to_arm to true once set once. Otherwise
// we have a race condition with px4io safety switch update
_armed.ready_to_arm = true;
}
_armed.lockdown = false;
_armed.force_failsafe = false;
if (_actuator_armed_pub == nullptr) {
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
} else {
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
}
}
void VRBRAINRCOutput::_send_outputs(void)
{
uint32_t now = AP_HAL::micros();
if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) {
// no channels enabled
_arm_actuators(false);
goto update_pwm;
}
// always send at least at 20Hz, otherwise the IO board may think
// we are dead
if (now - _last_output > 50000) {
_need_update = true;
}
// check for PWM count changing. This can happen then the user changes BRD_PWM_COUNT
if (now - _last_config_us > 1000000) {
if (_pwm_fd != -1) {
ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count);
}
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count);
}
_last_config_us = now;
}
if (_need_update && _pwm_fd != -1) {
_need_update = false;
perf_begin(_perf_rcout);
uint8_t to_send = _max_channel<_servo_count?_max_channel:_servo_count;
if (_sbus_enabled) {
to_send = _max_channel;
}
if (to_send > 0) {
for (int i=to_send-1; i >= 0; i--) {
if (_period[i] == PWM_IGNORE_THIS_CHANNEL) {
to_send = i;
} else {
break;
}
}
}
if (to_send > 0) {
_arm_actuators(true);
::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
}
if (_max_channel > _servo_count) {
// maybe send updates to alt_fd
if (_alt_fd != -1 && _alt_servo_count > 0) {
uint8_t n = _max_channel - _servo_count;
if (n > _alt_servo_count) {
n = _alt_servo_count;
}
if (n > 0) {
::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
}
}
}
perf_end(_perf_rcout);
_last_output = now;
}
update_pwm:
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
bool rc_updated = false;
if (_outputs[i].pwm_sub >= 0 &&
orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 &&
rc_updated) {
orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs);
}
}
}
void VRBRAINRCOutput::cork()
{
#if RCOUT_DEBUG_LATENCY
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 1);
#endif
_corking = true;
}
void VRBRAINRCOutput::push()
{
#if RCOUT_DEBUG_LATENCY
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 0);
#endif
if (_corking) {
_corking = false;
if (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125) {
// run timer immediately in oneshot mode
_send_outputs();
}
}
}
void VRBRAINRCOutput::timer_tick(void)
{
if (_output_mode != MODE_PWM_ONESHOT && _output_mode != MODE_PWM_ONESHOT125 && !_corking) {
/* in oneshot mode the timer does nothing as the outputs are
* sent from push() */
_send_outputs();
}
force_safety_pending_requests();
}
/*
enable sbus output
*/
bool VRBRAINRCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
{
int fd = open("/dev/px4io", 0);
if (fd == -1) {
return false;
}
for (uint8_t tries=0; tries<10; tries++) {
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
continue;
}
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate_hz) != 0) {
continue;
}
close(fd);
_sbus_enabled = true;
return true;
}
close(fd);
return false;
}
/*
setup output mode
*/
void VRBRAINRCOutput::set_output_mode(uint16_t mask, enum output_mode mode)
{
if (_output_mode == mode) {
// no change
return;
}
if (mode == MODE_PWM_ONESHOT || mode == MODE_PWM_ONESHOT125) {
// when using oneshot we don't want the regular pulses. The
// best we can do with the current PX4Firmware code is ask for
// 1Hz. This does still produce pulses, but the trigger calls
// mean the timer is constantly reset, so no pulses are
// produced except when triggered by push() when the main loop
// is running
set_freq_fd(_pwm_fd, _rate_mask_main, 1, _rate_mask_main);
if (_alt_fd != -1) {
set_freq_fd(_alt_fd, _rate_mask_alt, 1, _rate_mask_alt);
}
}
_output_mode = mode;
switch (_output_mode) {
case MODE_PWM_ONESHOT:
case MODE_PWM_ONESHOT125:
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
}
break;
case MODE_PWM_DSHOT150:
case MODE_PWM_DSHOT300:
case MODE_PWM_DSHOT600:
case MODE_PWM_DSHOT1200:
// treat as normal PWM for now
hal.console->printf("DShot not supported\n");
FALLTHROUGH;
case MODE_PWM_NORMAL:
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0);
}
break;
case MODE_PWM_BRUSHED:
// setup an 8MHz clock. This has the effect of scaling all outputs by 8x
ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8);
}
break;
}
}
// set default output update rate
void VRBRAINRCOutput::set_default_rate(uint16_t rate_hz)
{
if (rate_hz != _default_rate_hz) {
// set servo update rate for first 8 pwm channels
ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
if (_alt_fd != -1) {
// set servo update rate for auxiliary channels
ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
}
_default_rate_hz = rate_hz;
}
}
#endif // CONFIG_HAL_BOARD

86
libraries/AP_HAL_VRBRAIN/RCOutput.h
View File

@ -1,86 +0,0 @@
#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;
}
float scale_esc_to_unity(uint16_t pwm) override {
return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
}
void cork();
void push();
void set_output_mode(uint16_t mask, enum output_mode mode) override;
void timer_tick(void) override;
bool enable_px4io_sbus_out(uint16_t rate_hz) override;
// set default output update rate
void set_default_rate(uint16_t rate_hz) override;
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
// we keep the last_sent value separately, as we need to keep the unscaled
// value for systems with brushed motors which scale outputs
uint16_t _last_sent[VRBRAIN_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
bool _sbus_enabled:1;
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_main;
uint32_t _rate_mask_alt;
uint16_t _enabled_channels;
uint32_t _period_max;
struct {
int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed;
orb_advert_t _actuator_armed_pub;
uint16_t _esc_pwm_min;
uint16_t _esc_pwm_max;
void _init_alt_channels(void);
void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask);
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
uint32_t _safety_state_request_last_ms = 0;
void force_safety_pending_requests(void);
uint16_t _default_rate_hz = 50;
};

328
libraries/AP_HAL_VRBRAIN/SPIDevice.cpp
View File

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

113
libraries/AP_HAL_VRBRAIN/SPIDevice.h
View File

@ -1,113 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published
* by the Free Software Foundation, either version 3 of the License,
* or (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <inttypes.h>
#include <AP_HAL/HAL.h>
#include <AP_HAL/SPIDevice.h>
#include <drivers/device/spi.h>
#include "Semaphores.h"
#include "Device.h"
#include "Scheduler.h"
namespace VRBRAIN {
class SPIDesc;
class SPIBus : public DeviceBus {
public:
SPIBus(void) :
DeviceBus(APM_SPI_PRIORITY) {}
struct spi_dev_s *dev;
uint8_t bus;
};
struct SPIDesc {
SPIDesc(const char *_name, uint8_t _bus,
enum spi_dev_e _device, enum spi_mode_e _mode,
uint32_t _lowspeed, uint32_t _highspeed)
: name(_name), bus(_bus), device(_device), mode(_mode),
lowspeed(_lowspeed), highspeed(_highspeed)
{
}
const char *name;
uint8_t bus;
enum spi_dev_e device;
enum spi_mode_e mode;
uint32_t lowspeed;
uint32_t highspeed;
};
class SPIDevice : public AP_HAL::SPIDevice {
public:
SPIDevice(SPIBus &_bus, SPIDesc &_device_desc);
virtual ~SPIDevice();
/* See AP_HAL::Device::set_speed() */
bool set_speed(AP_HAL::Device::Speed speed) override;
// low level transfer function
void do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len);
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
/* See AP_HAL::SPIDevice::transfer_fullduplex() */
bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
uint32_t len) override;
/* See AP_HAL::Device::get_semaphore() */
AP_HAL::Semaphore *get_semaphore() override;
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
bool set_chip_select(bool set) override;
private:
SPIBus &bus;
SPIDesc &device_desc;
uint32_t frequency;
perf_counter_t perf;
char *pname;
bool cs_forced;
static void *spi_thread(void *arg);
};
class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
public:
friend class SPIDevice;
static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr)
{
return static_cast<SPIDeviceManager*>(spi_mgr);
}
AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name);
private:
static SPIDesc device_table[];
SPIBus *buses;
};
}

364
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
View File

@ -1,364 +0,0 @@
#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>
#include <AP_BoardConfig/AP_BoardConfig.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, nullptr);
}
void VRBRAINScheduler::delay(uint16_t ms)
{
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 (in_main_thread() && _min_delay_cb_ms <= ms) {
call_delay_cb();
}
}
perf_end(_perf_delay);
if (_vrbrain_thread_should_exit) {
exit(1);
}
}
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::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()
{
if (_in_timer_proc) {
return;
}
_in_timer_proc = true;
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i]) {
_timer_proc[i]();
}
}
// and the failsafe, if one is setup
if (_failsafe != nullptr) {
_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;
pthread_setname_np(pthread_self(), "apm_timer");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// run registered timers
perf_begin(sched->_perf_timers);
sched->_run_timers();
perf_end(sched->_perf_timers);
// process any pending RC output requests
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 nullptr;
}
void VRBRAINScheduler::_run_io(void)
{
if (_in_io_proc) {
return;
}
_in_io_proc = true;
// 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;
pthread_setname_np(pthread_self(), "apm_uart");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// process any pending serial bytes
hal.uartA->_timer_tick();
hal.uartB->_timer_tick();
hal.uartC->_timer_tick();
hal.uartD->_timer_tick();
hal.uartE->_timer_tick();
hal.uartF->_timer_tick();
}
return nullptr;
}
void *VRBRAINScheduler::_io_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
pthread_setname_np(pthread_self(), "apm_io");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(1000);
// run registered IO processes
perf_begin(sched->_perf_io_timers);
sched->_run_io();
perf_end(sched->_perf_io_timers);
}
return nullptr;
}
void *VRBRAINScheduler::_storage_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
pthread_setname_np(pthread_self(), "apm_storage");
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
sched->delay_microseconds_semaphore(10000);
// process any pending storage writes
perf_begin(sched->_perf_storage_timer);
hal.storage->_timer_tick();
perf_end(sched->_perf_storage_timer);
}
return nullptr;
}
bool VRBRAINScheduler::in_main_thread() const
{
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

96
libraries/AP_HAL_VRBRAIN/Scheduler.h
View File

@ -1,96 +0,0 @@
#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_SPI_PRIORITY 242
#define APM_CAN_PRIORITY 179
#define APM_I2C_PRIORITY 178
#define APM_UART_PRIORITY 60
#define APM_STORAGE_PRIORITY 59
#define APM_IO_PRIORITY 58
#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_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
void reboot(bool hold_in_bootloader);
bool in_main_thread() const override;
void system_initialized();
void hal_initialized() { _hal_initialized = true; }
private:
bool _initialized;
volatile bool _hal_initialized;
AP_HAL::Proc _failsafe;
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;
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();
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

44
libraries/AP_HAL_VRBRAIN/Semaphores.cpp
View File

@ -1,44 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "Semaphores.h"
#include <nuttx/arch.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 (up_interrupt_context()) {
// don't ever wait on a semaphore in interrupt context
return take_nonblocking();
}
if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
return pthread_mutex_lock(&_lock) == 0;
}
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

21
libraries/AP_HAL_VRBRAIN/Semaphores.h
View File

@ -1,21 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <stdint.h>
#include <AP_HAL/AP_HAL_Macros.h>
#include <AP_HAL/Semaphores.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include <pthread.h>
class VRBRAIN::Semaphore : public AP_HAL::Semaphore {
public:
Semaphore() {
pthread_mutex_init(&_lock, nullptr);
}
bool give();
bool take(uint32_t timeout_ms);
bool take_nonblocking();
private:
pthread_mutex_t _lock;
};

315
libraries/AP_HAL_VRBRAIN/Storage.cpp
View File

@ -1,315 +0,0 @@
#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 <nuttx/progmem.h>
#include <AP_BoardConfig/AP_BoardConfig.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 SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
#define MTD_PARAMS_FILE "/fs/mtd"
extern const AP_HAL::HAL& hal;
extern "C" int mtd_main(int, char **);
VRBRAINStorage::VRBRAINStorage(void) :
_perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")),
_perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors"))
{
}
void VRBRAINStorage::_storage_open(void)
{
if (_initialised) {
return;
}
_dirty_mask.clearall();
// load from storage backend
#if USE_FLASH_STORAGE
_flash_load();
#else
_mtd_load();
#endif
#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 (uint16_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT;
line <= end>>VRBRAIN_STORAGE_LINE_SHIFT;
line++) {
_dirty_mask.set(line);
}
}
void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n)
{
if (loc >= sizeof(_buffer)-(n-1)) {
return;
}
_storage_open();
memcpy(dst, &_buffer[loc], n);
}
void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n)
{
if (loc >= sizeof(_buffer)-(n-1)) {
return;
}
if (memcmp(src, &_buffer[loc], n) != 0) {
_storage_open();
memcpy(&_buffer[loc], src, n);
_mark_dirty(loc, n);
}
}
void VRBRAINStorage::_timer_tick(void)
{
if (!_initialised || _dirty_mask.empty()) {
return;
}
perf_begin(_perf_storage);
#if !USE_FLASH_STORAGE
if (_fd == -1) {
_fd = open(MTD_PARAMS_FILE, O_WRONLY);
if (_fd == -1) {
perf_end(_perf_storage);
perf_count(_perf_errors);
return;
}
}
#endif
// write out the first dirty line. We don't write more
// than one to keep the latency of this call to a minimum
uint16_t i;
for (i=0; i<VRBRAIN_STORAGE_NUM_LINES; i++) {
if (_dirty_mask.get(i)) {
break;
}
}
if (i == VRBRAIN_STORAGE_NUM_LINES) {
// this shouldn't be possible
perf_end(_perf_storage);
perf_count(_perf_errors);
return;
}
// save to storage backend
#if USE_FLASH_STORAGE
_flash_write(i);
#else
_mtd_write(i);
#endif
perf_end(_perf_storage);
}
#if !USE_FLASH_STORAGE
void VRBRAINStorage::bus_lock(bool lock)
{
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
/*
this is needed on Pixracer where the ms5611 may be on the same
bus as FRAM, and the NuttX ramtron driver does not go via the
PX4 spi bus abstraction. The ramtron driver relies on
SPI_LOCK(). We need to prevent the ms5611 reads which happen in
interrupt context from interfering with the FRAM operations. As
the px4 spi bus abstraction just uses interrupt blocking as the
locking mechanism we need to block interrupts here as well.
*/
if (lock) {
irq_state = irqsave();
} else {
irqrestore(irq_state);
}
#endif
}
/*
write one storage line. This also updates _dirty_mask.
*/
void VRBRAINStorage::_mtd_write(uint16_t line)
{
uint16_t ofs = line * VRBRAIN_STORAGE_LINE_SIZE;
if (lseek(_fd, ofs, SEEK_SET) != ofs) {
return;
}
// mark the line clean
_dirty_mask.clear(line);
bus_lock(true);
ssize_t ret = write(_fd, &_buffer[ofs], VRBRAIN_STORAGE_LINE_SIZE);
bus_lock(false);
if (ret != VRBRAIN_STORAGE_LINE_SIZE) {
// write error - likely EINTR
_dirty_mask.set(line);
close(_fd);
_fd = -1;
perf_count(_perf_errors);
}
}
/*
load all data from mtd
*/
void VRBRAINStorage::_mtd_load(void)
{
if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "start " MTD_PARAMS_FILE)) {
printf("mtd: started OK\n");
if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "readtest " MTD_PARAMS_FILE)) {
printf("mtd: readtest OK\n");
} else {
AP_BoardConfig::sensor_config_error("mtd: failed readtest");
}
} else {
AP_BoardConfig::sensor_config_error("mtd: failed start");
}
int fd = open(MTD_PARAMS_FILE, O_RDONLY);
if (fd == -1) {
AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
}
const uint16_t chunk_size = 128;
for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
bus_lock(true);
ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
bus_lock(false);
if (ret != chunk_size) {
::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
(unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
AP_HAL::panic("Failed to read " MTD_PARAMS_FILE);
}
}
close(fd);
}
#else // USE_FLASH_STORAGE
/*
load all data from flash
*/
void VRBRAINStorage::_flash_load(void)
{
_flash_page = up_progmem_npages() - 2;
if (up_progmem_pagesize(_flash_page) != 128*1024U ||
up_progmem_pagesize(_flash_page+1) != 128*1024U) {
AP_HAL::panic("Bad flash page sizes %u %u",
up_progmem_pagesize(_flash_page),
up_progmem_pagesize(_flash_page+1));
}
printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
if (!_flash.init()) {
AP_HAL::panic("unable to init flash storage");
}
}
/*
write one storage line. This also updates _dirty_mask.
*/
void VRBRAINStorage::_flash_write(uint16_t line)
{
if (_flash.write(line*VRBRAIN_STORAGE_LINE_SIZE, VRBRAIN_STORAGE_LINE_SIZE)) {
// mark the line clean
_dirty_mask.clear(line);
} else {
perf_count(_perf_errors);
}
}
/*
callback to write data to flash
*/
bool VRBRAINStorage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
{
size_t base_address = up_progmem_getaddress(_flash_page+sector);
bool ret = up_progmem_write(base_address+offset, data, length) == length;
if (!ret && _flash_erase_ok()) {
// we are getting flash write errors while disarmed. Try
// re-writing all of flash
uint32_t now = AP_HAL::millis();
if (now - _last_re_init_ms > 5000) {
_last_re_init_ms = now;
bool ok = _flash.re_initialise();
printf("Storage: failed at %u:%u for %u - re-init %u\n",
sector, offset, length, (unsigned)ok);
}
}
return ret;
}
/*
callback to read data from flash
*/
bool VRBRAINStorage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
{
size_t base_address = up_progmem_getaddress(_flash_page+sector);
const uint8_t *b = ((const uint8_t *)base_address)+offset;
memcpy(data, b, length);
return true;
}
/*
callback to erase flash sector
*/
bool VRBRAINStorage::_flash_erase_sector(uint8_t sector)
{
return up_progmem_erasepage(_flash_page+sector) > 0;
}
/*
callback to check if erase is allowed
*/
bool VRBRAINStorage::_flash_erase_ok(void)
{
// only allow erase while disarmed
return !hal.util->get_soft_armed();
}
#endif // USE_FLASH_STORAGE
#endif // CONFIG_HAL_BOARD

78
libraries/AP_HAL_VRBRAIN/Storage.h
View File

@ -1,78 +0,0 @@
#pragma once
/*
we can optionally use flash for storage instead of FRAM. That allows
ArduPilot to run on a wider range of boards and reduces board cost
*/
#ifndef USE_FLASH_STORAGE
#define USE_FLASH_STORAGE 0
#endif
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include <systemlib/perf_counter.h>
#include <AP_Common/Bitmask.h>
#include <AP_FlashStorage/AP_FlashStorage.h>
#define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE
#if USE_FLASH_STORAGE
// when using flash storage we use a small line size to make storage
// compact and minimise the number of erase cycles needed
#define VRBRAIN_STORAGE_LINE_SHIFT 3
#else
#define VRBRAIN_STORAGE_LINE_SHIFT 9
#endif
#define VRBRAIN_STORAGE_LINE_SIZE (1<<VRBRAIN_STORAGE_LINE_SHIFT)
#define VRBRAIN_STORAGE_NUM_LINES (VRBRAIN_STORAGE_SIZE/VRBRAIN_STORAGE_LINE_SIZE)
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) override;
private:
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)));
Bitmask _dirty_mask{VRBRAIN_STORAGE_NUM_LINES};
perf_counter_t _perf_storage;
perf_counter_t _perf_errors;
uint32_t _last_re_init_ms;
#if !USE_FLASH_STORAGE
int _fd = -1;
void bus_lock(bool lock);
void _mtd_load(void);
void _mtd_write(uint16_t line);
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
irqstate_t irq_state;
#endif
#else // USE_FLASH_STORAGE
bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length);
bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length);
bool _flash_erase_sector(uint8_t sector);
bool _flash_erase_ok(void);
uint8_t _flash_page;
bool _flash_failed;
AP_FlashStorage _flash{_buffer,
128*1024U,
FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_erase_sector, bool, uint8_t),
FUNCTOR_BIND_MEMBER(&VRBRAINStorage::_flash_erase_ok, bool)};
void _flash_load(void);
void _flash_write(uint16_t line);
#endif
};

543
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

@ -1,543 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "UARTDriver.h"
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#include <assert.h>
#include "GPIO.h"
using namespace VRBRAIN;
extern const AP_HAL::HAL& hal;
VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name) :
_devpath(devpath),
_fd(-1),
_baudrate(57600),
_initialised(false),
_in_timer(false),
_unbuffered_writes(false),
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
_os_start_auto_space(-1),
_flow_control(FLOW_CONTROL_DISABLE)
{
}
extern const AP_HAL::HAL& hal;
/*
this UART driver maps to a serial device in /dev
*/
void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (strcmp(_devpath, "/dev/null") == 0) {
// leave uninitialised
return;
}
uint16_t min_tx_buffer = 1024;
uint16_t min_rx_buffer = 512;
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
min_tx_buffer = 4096;
min_rx_buffer = 1024;
}
// on VRBRAIN we have enough memory to have a larger transmit and
// receive buffer for all ports. This means we don't get delays
// while waiting to write GPS config packets
if (txS < min_tx_buffer) {
txS = min_tx_buffer;
}
if (rxS < min_rx_buffer) {
rxS = min_rx_buffer;
}
/*
allocate the read buffer
we allocate buffers before we successfully open the device as we
want to allocate in the early stages of boot, and cause minimum
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
while (_in_timer) {
hal.scheduler->delay(1);
}
if (rxS != _readbuf.get_size()) {
_initialised = false;
_readbuf.set_size(rxS);
}
bool clear_buffers = false;
if (b != 0) {
// clear buffers on baudrate change, but not on the console (which is usually USB)
if (_baudrate != b && hal.console != this) {
clear_buffers = true;
}
_baudrate = b;
}
if (b != 0) {
_baudrate = b;
}
if (clear_buffers) {
_readbuf.clear();
}
/*
allocate the write buffer
*/
while (_in_timer) {
hal.scheduler->delay(1);
}
if (txS != _writebuf.get_size()) {
_initialised = false;
_writebuf.set_size(txS);
}
if (clear_buffers) {
_writebuf.clear();
}
if (_fd == -1) {
_fd = open(_devpath, O_RDWR);
if (_fd == -1) {
return;
}
}
if (_baudrate != 0) {
// set the baud rate
struct termios t;
tcgetattr(_fd, &t);
cfsetspeed(&t, _baudrate);
// disable LF -> CR/LF
t.c_oflag &= ~ONLCR;
tcsetattr(_fd, TCSANOW, &t);
// separately setup IFLOW if we can. We do this as a 2nd call
// as if the port has no RTS pin then the tcsetattr() call
// will fail, and if done as one call then it would fail to
// set the baudrate.
tcgetattr(_fd, &t);
t.c_cflag |= CRTS_IFLOW;
tcsetattr(_fd, TCSANOW, &t);
}
if (_writebuf.get_size() && _readbuf.get_size() && _fd != -1) {
if (!_initialised) {
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
((VRBRAINGPIO *)hal.gpio)->set_usb_connected();
}
::printf("initialised %s OK %u %u\n", _devpath,
(unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size());
}
_initialised = true;
}
}
void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
{
if (_fd == -1) {
return;
}
struct termios t;
tcgetattr(_fd, &t);
// we already enabled CRTS_IFLOW above, just enable output flow control
if (fcontrol != FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
if (fcontrol == FLOW_CONTROL_AUTO) {
// reset flow control auto state machine
_total_written = 0;
_first_write_time = 0;
}
_flow_control = fcontrol;
}
void VRBRAINUARTDriver::configure_parity(uint8_t v) {
if (_fd == -1) {
return;
}
struct termios t;
tcgetattr(_fd, &t);
if (v != 0) {
// enable parity
t.c_cflag |= PARENB;
if (v == 1) {
t.c_cflag |= PARODD;
} else {
t.c_cflag &= ~PARODD;
}
}
else {
// disable parity
t.c_cflag &= ~PARENB;
}
tcsetattr(_fd, TCSANOW, &t);
}
void VRBRAINUARTDriver::set_stop_bits(int n) {
if (_fd == -1) {
return;
}
struct termios t;
tcgetattr(_fd, &t);
if (n > 1) t.c_cflag |= CSTOPB;
else t.c_cflag &= ~CSTOPB;
tcsetattr(_fd, TCSANOW, &t);
}
bool VRBRAINUARTDriver::set_unbuffered_writes(bool on) {
_unbuffered_writes = on;
return _unbuffered_writes;
}
void VRBRAINUARTDriver::begin(uint32_t b)
{
begin(b, 0, 0);
}
/*
try to initialise the UART. This is used to cope with the way NuttX
handles /dev/ttyACM0 (the USB port). The port appears in /dev on
boot, but cannot be opened until a USB cable is connected and the
host starts the CDCACM communication.
*/
void VRBRAINUARTDriver::try_initialise(void)
{
if (_initialised) {
return;
}
if ((AP_HAL::millis() - _last_initialise_attempt_ms) < 2000) {
return;
}
_last_initialise_attempt_ms = AP_HAL::millis();
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_armed()) {
begin(0);
}
}
void VRBRAINUARTDriver::end()
{
_initialised = false;
while (_in_timer) hal.scheduler->delay(1);
if (_fd != -1) {
close(_fd);
_fd = -1;
}
_readbuf.set_size(0);
_writebuf.set_size(0);
}
void VRBRAINUARTDriver::flush() {}
bool VRBRAINUARTDriver::is_initialized()
{
try_initialise();
return _initialised;
}
void VRBRAINUARTDriver::set_blocking_writes(bool blocking)
{
_nonblocking_writes = !blocking;
}
bool VRBRAINUARTDriver::tx_pending() { return false; }
/*
return number of bytes available to be read from the buffer
*/
uint32_t VRBRAINUARTDriver::available()
{
if (!_initialised) {
try_initialise();
return 0;
}
return _readbuf.available();
}
/*
return number of bytes that can be added to the write buffer
*/
uint32_t VRBRAINUARTDriver::txspace()
{
if (!_initialised) {
try_initialise();
return 0;
}
return _writebuf.space();
}
/*
read one byte from the read buffer
*/
int16_t VRBRAINUARTDriver::read()
{
if (!_semaphore.take_nonblocking()) {
return -1;
}
if (!_initialised) {
try_initialise();
_semaphore.give();
return -1;
}
uint8_t byte;
if (!_readbuf.read_byte(&byte)) {
_semaphore.give();
return -1;
}
_semaphore.give();
return byte;
}
/*
write one byte
*/
size_t VRBRAINUARTDriver::write(uint8_t c)
{
if (!_semaphore.take_nonblocking()) {
return -1;
}
if (!_initialised) {
try_initialise();
_semaphore.give();
return 0;
}
if (_unbuffered_writes) {
// write one byte to the file descriptor
return _write_fd(&c, 1);
}
while (_writebuf.space() == 0) {
if (_nonblocking_writes) {
_semaphore.give();
return 0;
}
_semaphore.give();
hal.scheduler->delay(1);
if (!_semaphore.take_nonblocking()) {
return -1;
}
}
size_t ret = _writebuf.write(&c, 1);
_semaphore.give();
return ret;
}
/*
* write size bytes
*/
size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!_semaphore.take_nonblocking()) {
return -1;
}
if (!_initialised) {
try_initialise();
_semaphore.give();
return 0;
}
size_t ret = 0;
if (!_nonblocking_writes) {
_semaphore.give();
/*
use the per-byte delay loop in write() above for blocking writes
*/
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
return ret;
}
if (_unbuffered_writes) {
// write buffer straight to the file descriptor
ret = _write_fd(buffer, size);
} else {
ret = _writebuf.write(buffer, size);
}
_semaphore.give();
return ret;
}
/*
try writing n bytes, handling an unresponsive port
*/
int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
{
int ret = 0;
// the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0
// FIONWRITE is also used for auto flow control detection
// Assume output flow control is not working if:
// port is configured for auto flow control
// and this is not the first write since flow control turned on
// and no data has been removed from the buffer since flow control turned on
// and more than .5 seconds elapsed after writing a total of > 5 characters
//
int nwrite = 0;
if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
if (_flow_control == FLOW_CONTROL_AUTO) {
if (_first_write_time == 0) {
if (_total_written == 0) {
// save the remaining buffer bytes for comparison next write
_os_start_auto_space = nwrite;
}
} else {
if (_os_start_auto_space - nwrite + 1 >= _total_written &&
(AP_HAL::micros64() - _first_write_time) > 500*1000UL) {
// it doesn't look like hw flow control is working
::printf("disabling flow control on %s _total_written=%u\n",
_devpath, (unsigned)_total_written);
set_flow_control(FLOW_CONTROL_DISABLE);
}
}
}
if (nwrite > n) {
nwrite = n;
}
if (nwrite > 0) {
ret = ::write(_fd, buf, nwrite);
}
}
if (ret > 0) {
_last_write_time = AP_HAL::micros64();
_total_written += ret;
if (! _first_write_time && _total_written > 5) {
_first_write_time = _last_write_time;
}
return ret;
}
if (AP_HAL::micros64() - _last_write_time > 2000 &&
_flow_control == FLOW_CONTROL_DISABLE) {
_last_write_time = AP_HAL::micros64();
// we haven't done a successful write for 2ms, which means the
// port is running at less than 500 bytes/sec. Start
// discarding bytes, even if this is a blocking port. This
// prevents the ttyACM0 port blocking startup if the endpoint
// is not connected
return n;
}
return ret;
}
/*
try reading n bytes, handling an unresponsive port
*/
int VRBRAINUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{
int ret = 0;
// the FIONREAD check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0
int nread = 0;
if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) {
if (nread > n) {
nread = n;
}
if (nread > 0) {
ret = ::read(_fd, buf, nread);
}
}
if (ret > 0) {
_total_read += ret;
}
return ret;
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread. Doing it this way reduces the system call
overhead in the main task enormously.
*/
void VRBRAINUARTDriver::_timer_tick(void)
{
int ret;
uint32_t n;
if (!_initialised) return;
// don't try IO on a disconnected USB port
if (strcmp(_devpath, "/dev/ttyACM0") == 0 && !hal.gpio->usb_connected()) {
return;
}
_in_timer = true;
// write any pending bytes
n = _writebuf.available();
if (n > 0) {
ByteBuffer::IoVec vec[2];
perf_begin(_perf_uart);
const auto n_vec = _writebuf.peekiovec(vec, n);
for (int i = 0; i < n_vec; i++) {
ret = _write_fd(vec[i].data, (uint16_t)vec[i].len);
if (ret < 0) {
break;
}
_writebuf.advance(ret);
/* We wrote less than we asked for, stop */
if ((unsigned)ret != vec[i].len) {
break;
}
}
perf_end(_perf_uart);
}
// try to fill the read buffer
ByteBuffer::IoVec vec[2];
perf_begin(_perf_uart);
const auto n_vec = _readbuf.reserve(vec, _readbuf.space());
for (int i = 0; i < n_vec; i++) {
ret = _read_fd(vec[i].data, vec[i].len);
if (ret < 0) {
break;
}
_readbuf.commit((unsigned)ret);
/* stop reading as we read less than we asked for */
if ((unsigned)ret < vec[i].len) {
break;
}
}
perf_end(_perf_uart);
_in_timer = false;
}
#endif

77
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -1,77 +0,0 @@
#pragma once
#include <AP_HAL/utility/RingBuffer.h>
#include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h>
#include "Semaphores.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) override { return _flow_control; }
void configure_parity(uint8_t v);
void set_stop_bits(int n);
bool set_unbuffered_writes(bool on);
private:
const char *_devpath;
int _fd;
uint32_t _baudrate;
volatile bool _initialised;
volatile bool _in_timer;
bool _nonblocking_writes;
bool _unbuffered_writes;
// we use in-task ring buffers to reduce the system call cost
// of ::read() and ::write() in the main loop
ByteBuffer _readbuf{0};
ByteBuffer _writebuf{0};
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;
Semaphore _semaphore;
};

267
libraries/AP_HAL_VRBRAIN/Util.cpp
View File

@ -1,267 +0,0 @@
#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, nullptr);
// 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 !HAL_HAVE_SAFETY_SWITCH
return AP_HAL::Util::SAFETY_NONE;
#endif
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;
}
/*
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_VRBRAIN_V52E)
const char *board_type = "VRBRAINv52E";
#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]);
buf[39] = 0;
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 nullptr;
}
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 target = (float)(*_heater.target);
// limit to 65 degrees to prevent damage
target = constrain_float(target, 0, 65);
float err = target - current;
_heater.integrator += kI * err;
_heater.integrator = constrain_float(_heater.integrator, 0, 70);
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;
}
extern "C" {
extern void *fat_dma_alloc(size_t);
extern void fat_dma_free(void *, size_t);
}
/*
allocate DMA-capable memory if possible. Otherwise return normal
memory.
*/
void *VRBRAINUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type)
{
return calloc(1, size);
}
void VRBRAINUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
{
return free(ptr);
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

72
libraries/AP_HAL_VRBRAIN/Util.h
View File

@ -1,72 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "Semaphores.h"
class VRBRAIN::NSHShellStream : public AP_HAL::BetterStream {
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);
/*
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::BetterStream *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;
void set_imu_temp(float current) override;
void set_imu_target_temp(int8_t *target) override;
// allocate and free DMA-capable memory if possible. Otherwise return normal memory
void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override;
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
private:
int _safety_handle;
VRBRAIN::NSHShellStream _shell_stream;
struct {
int8_t *target;
float integrator;
uint16_t count;
float sum;
uint32_t last_update_ms;
int fd = -1;
} _heater;
};

294
libraries/AP_HAL_VRBRAIN/bxcan.h
View File

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

53
libraries/AP_HAL_VRBRAIN/system.cpp
View File

@ -1,53 +0,0 @@
#include <stdarg.h>
#include <stdio.h>
#include <drivers/drv_hrt.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/system.h>
extern const AP_HAL::HAL& hal;
extern bool _vrbrain_thread_should_exit;
namespace AP_HAL {
void init()
{
}
void panic(const char *errormsg, ...)
{
va_list ap;
va_start(ap, errormsg);
vdprintf(1, errormsg, ap);
va_end(ap);
write(1, "\n", 1);
hal.scheduler->delay_microseconds(10000);
_vrbrain_thread_should_exit = true;
exit(1);
}
uint32_t micros()
{
return micros64() & 0xFFFFFFFF;
}
uint32_t millis()
{
return millis64() & 0xFFFFFFFF;
}
uint64_t micros64()
{
return hrt_absolute_time();
}
uint64_t millis64()
{
return micros64() / 1000;
}
} // namespace AP_HAL

47
libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp
View File

@ -1,47 +0,0 @@
/*
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"
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
param_t param_find(const char *name)
{
#if 0
// useful for driver debugging
::printf("VRBRAIN: param_find(%s)\n", name);
#endif
return PARAM_INVALID;
}
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