Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 02:33:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

VRBRAIN / AP_HAL_VRBRAIN: updated AP_HAL

Browse Source
This commit is contained in:
LukeMike 2018-02-03 14:46:18 +01:00 committed by Andrew Tridgell
parent 9cd469fc38
commit 97b29a333d
30 changed files with 3525 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h
View File

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

6
libraries/AP_HAL_VRBRAIN/AnalogIn.cpp
View File

@ -52,6 +52,7 @@ static const struct {
#else
#error "Unknown board type for AnalogIn scaling"
#endif
{ 0, 0.f },
};
using namespace VRBRAIN;
@ -237,6 +238,11 @@ void VRBRAINAnalogIn::next_stop_pin(void)
*/
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;

12
libraries/AP_HAL_VRBRAIN/AnalogIn.h
View File

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

1117
libraries/AP_HAL_VRBRAIN/CAN.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

317
libraries/AP_HAL_VRBRAIN/CAN.h Normal file
View File

@ -0,0 +1,317 @@
/*
* 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];
AP_UAVCAN *p_uavcan;
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;
AP_UAVCAN *get_UAVCAN(void) override;
void set_UAVCAN(AP_UAVCAN *uavcan) override;
};
}

155
libraries/AP_HAL_VRBRAIN/Device.cpp Normal file
View File

@ -0,0 +1,155 @@
/*
* 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 Normal file
View File

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

26
libraries/AP_HAL_VRBRAIN/GPIO.cpp
View File

@ -29,6 +29,7 @@ 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);
@ -37,12 +38,15 @@ void VRBRAINGPIO::init()
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");
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");
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);
@ -67,6 +71,11 @@ void VRBRAINGPIO::init()
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)
@ -92,6 +101,14 @@ uint8_t VRBRAINGPIO::read(uint8_t pin) {
}
#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;
}
@ -142,6 +159,11 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
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
}
}

21
libraries/AP_HAL_VRBRAIN/GPIO.h
View File

@ -11,7 +11,8 @@
# define EXTERNAL_LED_MOTOR1 30
# define EXTERNAL_LED_MOTOR2 31
# define EXTERNAL_RELAY1_PIN 33
# define EXTERNAL_RELAY1_PIN 34
# define EXTERNAL_RELAY2_PIN 33
# define HAL_GPIO_LED_ON HIGH
# define HAL_GPIO_LED_OFF LOW
@ -20,21 +21,21 @@
class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO {
public:
VRBRAINGPIO();
void init();
void pinMode(uint8_t pin, uint8_t output);
int8_t analogPinToDigitalPin(uint8_t pin);
uint8_t read(uint8_t pin);
void write(uint8_t pin, uint8_t value);
void toggle(uint8_t pin);
void init() override;
void pinMode(uint8_t pin, uint8_t output) override;
int8_t analogPinToDigitalPin(uint8_t pin) 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);
AP_HAL::DigitalSource* channel(uint16_t n) override;
/* Interrupt interface: */
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override;
/* return true if USB cable is connected */
bool usb_connected(void);
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; }

49
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
View File

@ -2,6 +2,10 @@
#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"
@ -14,9 +18,11 @@
#include "Util.h"
#include "GPIO.h"
#include "I2CDevice.h"
#include "SPIDevice.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#if HAL_WITH_UAVCAN
#include "CAN.h"
#endif
#include <stdlib.h>
#include <systemlib/systemlib.h>
@ -28,8 +34,8 @@
#include <drivers/drv_hrt.h>
using namespace VRBRAIN;
using namespace ap;
static Empty::SPIDeviceManager spiDeviceManager;
//static Empty::GPIO gpioDriver;
static VRBRAINScheduler schedulerInstance;
@ -41,6 +47,7 @@ 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"
@ -67,14 +74,14 @@ static VRBRAIN::I2CDeviceManager i2c_mgr_instance;
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define 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/null"
#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)
@ -117,7 +124,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
&uartEDriver, /* uartE */
&uartFDriver, /* uartF */
&i2c_mgr_instance,
&spiDeviceManager, /* spi */
&spi_mgr_instance,
&analogIn, /* analogin */
&storageDriver, /* storage */
&uartADriver, /* console */
@ -126,7 +133,8 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
&rcoutDriver, /* rcoutput */
&schedulerInstance, /* scheduler */
&utilInstance, /* util */
nullptr) /* no onboard optical flow */
nullptr, /* no onboard optical flow */
nullptr) /* CAN */
{}
bool _vrbrain_thread_should_exit = false; /**< Daemon exit flag */
@ -168,12 +176,10 @@ static int main_loop(int argc, char **argv)
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init();
hal.rcin->init();
hal.rcout->init();
hal.analogin->init();
hal.gpio->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
@ -240,6 +246,7 @@ static void usage(void)
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");
}
@ -251,6 +258,7 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
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')",
@ -274,8 +282,10 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
uartCDriver.set_device_path(deviceC);
uartDDriver.set_device_path(deviceD);
uartEDriver.set_device_path(deviceE);
printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\n",
SKETCHNAME, deviceA, deviceC, deviceD, deviceE);
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,
@ -346,6 +356,17 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
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();

126
libraries/AP_HAL_VRBRAIN/I2CDevice.cpp
View File

@ -17,48 +17,132 @@
#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 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;
}
set_address(address);
bool ret = (transfer(send, send_len, recv, recv_len) == OK);
return ret;
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) :
_bus(bus),
_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)
{
return _bus.do_transfer(_address, send, send_len, recv, 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,
@ -67,6 +151,34 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
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,

39
libraries/AP_HAL_VRBRAIN/I2CDevice.h
View File

@ -21,9 +21,9 @@
#include <AP_HAL/HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include "Semaphores.h"
#include "I2CWrapper.h"
#include "Device.h"
namespace VRBRAIN {
@ -41,7 +41,7 @@ public:
void set_address(uint8_t address) override { _address = address; }
/* See AP_HAL::I2CDevice::set_retries() */
void set_retries(uint8_t retries) override { _retries = 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; }
@ -55,27 +55,30 @@ public:
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
{
/* Not implemented yet */
return nullptr;
}
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
{
/* Not implemented yet */
return false;
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;
}
AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
void set_split_transfers(bool set) override {
_split_transfers = set;
}
private:
// we use an empty semaphore as the underlying I2C class already has a semaphore
Empty::Semaphore semaphore;
VRBRAIN_I2C _bus;
static const uint8_t num_buses = 3;
static DeviceBus businfo[num_buses];
uint8_t _busnum;
VRBRAIN_I2C _vrbraindev;
uint8_t _address;
uint8_t _retries = 0;
perf_counter_t perf;
char *pname;
bool _split_transfers;
};
class I2CDeviceManager : public AP_HAL::I2CDeviceManager {

18
libraries/AP_HAL_VRBRAIN/I2CWrapper.h
View File

@ -1,3 +1,5 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
@ -13,11 +15,23 @@ extern const AP_HAL::HAL& hal;
*/
class VRBRAIN::VRBRAIN_I2C : public device::I2C {
public:
VRBRAIN_I2C(uint8_t bus) : I2C(devname, devpath, bus, 0, 400000UL) { }
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
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];

163
libraries/AP_HAL_VRBRAIN/RCInput.cpp
View File

@ -8,28 +8,42 @@
#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");
}
clear_overrides();
pthread_mutex_init(&rcin_mutex, nullptr);
_perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
_rc_sub = orb_subscribe(ORB_ID(input_rc));
if (_rc_sub == -1) {
AP_HAL::panic("Unable to subscribe to input_rc");
}
clear_overrides();
pthread_mutex_init(&rcin_mutex, nullptr);
}
bool VRBRAINRCInput::new_input()
{
pthread_mutex_lock(&rcin_mutex);
bool valid = _rcin.timestamp_last_signal != _last_read || _override_valid;
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;
}
if (_override_valid) {
// if we have RC overrides active, then always consider it valid
valid = true;
}
_last_read = _rcin.timestamp_last_signal;
_override_valid = false;
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;
}
@ -43,78 +57,106 @@ uint8_t VRBRAINRCInput::num_channels()
uint16_t VRBRAINRCInput::read(uint8_t ch)
{
if (ch >= RC_INPUT_MAX_CHANNELS) {
return 0;
}
pthread_mutex_lock(&rcin_mutex);
if (_override[ch]) {
uint16_t v = _override[ch];
pthread_mutex_unlock(&rcin_mutex);
return v;
}
if (ch >= _rcin.channel_count) {
pthread_mutex_unlock(&rcin_mutex);
return 0;
}
uint16_t v = _rcin.values[ch];
if (ch >= RC_INPUT_MAX_CHANNELS) {
return 0;
}
pthread_mutex_lock(&rcin_mutex);
if (_override[ch]) {
uint16_t v = _override[ch];
pthread_mutex_unlock(&rcin_mutex);
return v;
}
if (ch >= _rcin.channel_count) {
pthread_mutex_unlock(&rcin_mutex);
return 0;
}
uint16_t v = _rcin.values[ch];
pthread_mutex_unlock(&rcin_mutex);
return v;
}
uint8_t VRBRAINRCInput::read(uint16_t* periods, uint8_t len)
{
if (len > RC_INPUT_MAX_CHANNELS) {
len = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < len; i++){
periods[i] = read(i);
}
return len;
if (len > RC_INPUT_MAX_CHANNELS) {
len = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < len; i++){
periods[i] = read(i);
}
return len;
}
bool VRBRAINRCInput::set_overrides(int16_t *overrides, uint8_t len)
{
bool res = false;
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
bool res = false;
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool VRBRAINRCInput::set_override(uint8_t channel, int16_t override) {
if (override < 0) {
return false; /* -1: no change. */
}
if (channel >= RC_INPUT_MAX_CHANNELS) {
return false;
}
_override[channel] = override;
if (override != 0) {
_override_valid = true;
return true;
}
return false;
if (override < 0) {
return false; /* -1: no change. */
}
if (channel >= RC_INPUT_MAX_CHANNELS) {
return false;
}
_override[channel] = override;
if (override != 0) {
_override_valid = true;
return true;
}
return false;
}
void VRBRAINRCInput::clear_overrides()
{
for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
set_override(i, 0);
}
for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
set_override(i, 0);
}
}
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);
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);
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)
@ -132,6 +174,9 @@ bool VRBRAINRCInput::rc_bind(int dsmMode)
return true;
}

27
libraries/AP_HAL_VRBRAIN/RCInput.h
View File

@ -12,19 +12,24 @@
class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput {
public:
void init();
bool new_input();
uint8_t num_channels();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
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;
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
int16_t get_rssi(void) override {
return _rssi;
}
bool set_overrides(int16_t *overrides, uint8_t len) override;
bool set_override(uint8_t channel, int16_t override) override;
void clear_overrides() override;
void _timer_tick(void);
bool rc_bind(int dsmMode);
bool rc_bind(int dsmMode) override;
private:
/* override state */
@ -35,4 +40,8 @@ private:
bool _override_valid;
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;
};

308
libraries/AP_HAL_VRBRAIN/RCOutput.cpp
View File

@ -9,11 +9,17 @@
#include <unistd.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_direct.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#endif
extern const AP_HAL::HAL& hal;
using namespace VRBRAIN;
@ -38,7 +44,8 @@ void VRBRAINRCOutput::init()
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
}
_rate_mask = 0;
_rate_mask_main = 0;
_rate_mask_alt = 0;
_alt_fd = -1;
_servo_count = 0;
_alt_servo_count = 0;
@ -53,24 +60,22 @@ void VRBRAINRCOutput::init()
}
// _alt_fd = open("/dev/px4fmu", O_RDWR);
// if (_alt_fd == -1) {
// hal.console->printf("RCOutput: failed to open /dev/px4fmu");
// return;
// }
// ensure not to write zeros to disabled channels
_enabled_channels = 0;
for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) {
_period[i] = PWM_IGNORE_THIS_CHANNEL;
_last_sent[i] = PWM_IGNORE_THIS_CHANNEL;
}
// publish actuator vaules on demand
_actuator_direct_pub = nullptr;
// and armed state
_actuator_armed_pub = nullptr;
}
@ -95,8 +100,14 @@ void VRBRAINRCOutput::_init_alt_channels(void)
/*
set output frequency on outputs associated with fd
*/
void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
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
@ -108,7 +119,7 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
*/
freq_hz = 1;
}
//::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz);
//::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;
@ -118,10 +129,15 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t 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 8: channels 8 9 10 11
Group 3: channels 8 9 10 11
Channels within a group must be set to the same rate.
@ -130,39 +146,72 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
*/
if (freq_hz > 50 || freq_hz == 1) {
// we are setting high rates on the given channels
_rate_mask |= chmask & 0xFF;
if (_rate_mask & 0x07) {
_rate_mask |= 0x07;
rate_mask |= chmask & 0xFFF;
#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
if (rate_mask & 0x0F) {
rate_mask |= 0x0F;
}
if (_rate_mask & 0x38) {
_rate_mask |= 0x38;
if (rate_mask & 0x30) {
rate_mask |= 0x30;
}
if (_rate_mask & 0xC0) {
_rate_mask |= 0xC0;
if (rate_mask & 0xC0) {
rate_mask |= 0xC0;
}
if (_rate_mask & 0xF00) {
_rate_mask |= 0xF00;
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 (chmask & 0x07) {
_rate_mask &= ~0x07;
#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
if (chmask & 0x0F) {
rate_mask &= ~0x0F;
}
if (chmask & 0x38) {
_rate_mask &= ~0x38;
if (chmask & 0x30) {
rate_mask &= ~0x30;
}
if (chmask & 0xC0) {
_rate_mask &= ~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;
rate_mask &= ~0xF00;
}
#endif
}
//::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask);
if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
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);
}
}
/*
@ -180,30 +229,32 @@ void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
if (_alt_fd != -1 && ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get alt servo count\n");
return;
}
// greater than 400 doesn't give enough room at higher periods for
// the down pulse
if (freq_hz > 400) {
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);
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);
set_freq_fd(_alt_fd, alt_mask, freq_hz, _rate_mask_alt);
}
}
uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
{
if (_rate_mask & (1U<<ch)) {
return _freq_hz;
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;
}
@ -222,6 +273,7 @@ void VRBRAINRCOutput::enable_ch(uint8_t ch)
_enabled_channels |= (1U<<ch);
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
_period[ch] = 0;
_last_sent[ch] = 0;
}
}
@ -283,19 +335,20 @@ void VRBRAINRCOutput::force_safety_off(void)
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 &&
AP_HAL::millis() - _safety_state_request_last_ms >= 100) {
if (hal.util->safety_switch_state() == _safety_state_request) {
// current switch state matches request, stop attempting
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 = AP_HAL::millis();
_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 = AP_HAL::millis();
_safety_state_request_last_ms = now;
}
}
}
@ -320,6 +373,24 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
if (ch >= _max_channel) {
_max_channel = ch + 1;
}
// 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
@ -329,7 +400,7 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
_output_mode == MODE_PWM_ONESHOT) {
_period[ch] = period_us;
_need_update = true;
up_pwm_servo_set(ch, period_us);
// up_pwm_servo_set(ch, period_us);
}
}
@ -364,7 +435,7 @@ uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch)
return 0;
}
return _period[ch];
return _last_sent[ch];
}
void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
@ -386,7 +457,11 @@ void VRBRAINRCOutput::_arm_actuators(bool arm)
_armed.timestamp = hrt_absolute_time();
_armed.armed = arm;
_armed.ready_to_arm = 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;
@ -397,51 +472,6 @@ void VRBRAINRCOutput::_arm_actuators(bool arm)
}
}
/*
publish new outputs to the actuator_direct topic
*/
void VRBRAINRCOutput::_publish_actuators(void)
{
struct actuator_direct_s actuators;
if (_esc_pwm_min == 0 ||
_esc_pwm_max == 0) {
// not initialised yet
return;
}
actuators.nvalues = _max_channel;
if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
}
// don't publish more than 8 actuators for now, as the uavcan ESC
// driver refuses to update any motors if you try to publish more
// than 8
if (actuators.nvalues > 8) {
actuators.nvalues = 8;
}
bool armed = hal.util->get_soft_armed();
actuators.timestamp = hrt_absolute_time();
for (uint8_t i=0; i<actuators.nvalues; i++) {
if (!armed) {
actuators.values[i] = 0;
} else {
actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
}
// actuator values are from -1 to 1
actuators.values[i] = actuators.values[i]*2 - 1;
}
if (_actuator_direct_pub == nullptr) {
_actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
} else {
orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
_arm_actuators(true);
}
}
void VRBRAINRCOutput::_send_outputs(void)
{
uint32_t now = AP_HAL::micros();
@ -478,7 +508,7 @@ void VRBRAINRCOutput::_send_outputs(void)
}
if (to_send > 0) {
for (int i=to_send-1; i >= 0; i--) {
if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) {
if (_period[i] == PWM_IGNORE_THIS_CHANNEL) {
to_send = i;
} else {
break;
@ -486,6 +516,8 @@ void VRBRAINRCOutput::_send_outputs(void)
}
}
if (to_send > 0) {
_arm_actuators(true);
::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
}
if (_max_channel > _servo_count) {
@ -501,8 +533,35 @@ void VRBRAINRCOutput::_send_outputs(void)
}
}
// also publish to actuator_direct
_publish_actuators();
#if HAL_WITH_UAVCAN
if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1)
{
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr)
{
AP_UAVCAN *ap_uc = hal.can_mgr[i]->get_UAVCAN();
if (ap_uc != nullptr)
{
if (ap_uc->rc_out_sem_take())
{
for (uint8_t j = 0; j < _max_channel; j++)
{
ap_uc->rco_write(_period[j], j);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
ap_uc->rco_arm_actuators(true);
} else {
ap_uc->rco_arm_actuators(false);
}
ap_uc->rc_out_sem_give();
}
}
}
}
}
#endif // HAL_WITH_UAVCAN
perf_end(_perf_rcout);
_last_output = now;
@ -531,21 +590,20 @@ void VRBRAINRCOutput::cork()
void VRBRAINRCOutput::push()
{
if (!_corking) {
return;
}
#if RCOUT_DEBUG_LATENCY
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 0);
#endif
_corking = false;
if (_output_mode == MODE_PWM_ONESHOT) {
// run timer immediately in oneshot mode
_send_outputs();
if (_corking) {
_corking = false;
if (_output_mode == MODE_PWM_ONESHOT) {
// run timer immediately in oneshot mode
_send_outputs();
}
}
}
void VRBRAINRCOutput::_timer_tick(void)
void VRBRAINRCOutput::timer_tick(void)
{
if (_output_mode != MODE_PWM_ONESHOT && !_corking) {
/* in oneshot mode the timer does nothing as the outputs are
@ -596,22 +654,48 @@ void VRBRAINRCOutput::set_output_mode(enum output_mode mode)
// mean the timer is constantly reset, so no pulses are
// produced except when triggered by push() when the main loop
// is running
set_freq(_rate_mask, 1);
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;
if (_output_mode == MODE_PWM_ONESHOT) {
//::printf("enable oneshot\n");
switch (_output_mode) {
case MODE_PWM_ONESHOT:
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
}
} else {
break;
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

28
libraries/AP_HAL_VRBRAIN/RCOutput.h
View File

@ -29,19 +29,28 @@ public:
_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(enum output_mode mode) override;
void _timer_tick(void);
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;
@ -50,27 +59,28 @@ private:
uint32_t _last_config_us;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
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_direct_pub = nullptr;
orb_advert_t _actuator_armed_pub = nullptr;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;
orb_advert_t _actuator_armed_pub;
uint16_t _esc_pwm_min;
uint16_t _esc_pwm_max;
void _init_alt_channels(void);
void _publish_actuators(void);
void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
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;
};

329
libraries/AP_HAL_VRBRAIN/SPIDevice.cpp Normal file
View File

@ -0,0 +1,329 @@
/*
* 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) {
printf("SPI: Invalid device name: %s\n", name);
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 Normal file
View File

@ -0,0 +1,113 @@
/*
* 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;
};
}

126
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
View File

@ -20,7 +20,14 @@
#include "Storage.h"
#include "RCOutput.h"
#include "RCInput.h"
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#if HAL_WITH_UAVCAN
#include "CAN.h"
#include <AP_UAVCAN/AP_UAVCAN.h>
#endif
using namespace VRBRAIN;
@ -32,7 +39,7 @@ 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"))
_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
{}
void VRBRAINScheduler::init()
@ -40,34 +47,34 @@ 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_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_TIMER_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
param.sched_priority = APM_TIMER_PRIORITY;
(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);
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
param.sched_priority = APM_UART_PRIORITY;
(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);
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
param.sched_priority = APM_IO_PRIORITY;
(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);
@ -83,6 +90,34 @@ void VRBRAINScheduler::init()
pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this);
}
void VRBRAINScheduler::create_uavcan_thread()
{
#if HAL_WITH_UAVCAN
pthread_attr_t thread_attr;
struct sched_param param;
//the UAVCAN thread runs at medium priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 8192);
param.sched_priority = APM_CAN_PRIORITY;
(void) pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
if (hal.can_mgr[i]->get_UAVCAN() != nullptr) {
_uavcan_thread_arg *arg = new _uavcan_thread_arg;
arg->sched = this;
arg->uavcan_number = i;
pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg);
}
}
}
#endif
}
/**
delay for a specified number of microseconds using a semaphore wait
*/
@ -124,7 +159,7 @@ static void set_normal_priority(void *sem)
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
the regularity of timing of the main loop as it takes
*/
void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec)
{
@ -143,9 +178,9 @@ void VRBRAINScheduler::delay(uint16_t ms)
return;
}
perf_begin(_perf_delay);
uint64_t start = AP_HAL::micros64();
while ((AP_HAL::micros64() - start)/1000 < ms &&
uint64_t start = AP_HAL::micros64();
while ((AP_HAL::micros64() - start)/1000 < ms &&
!_vrbrain_thread_should_exit) {
delay_microseconds_semaphore(1000);
if (_min_delay_cb_ms <= ms) {
@ -161,7 +196,7 @@ void VRBRAINScheduler::delay(uint16_t ms)
}
void VRBRAINScheduler::register_delay_callback(AP_HAL::Proc proc,
uint16_t min_time_ms)
uint16_t min_time_ms)
{
_delay_cb = proc;
_min_delay_cb_ms = min_time_ms;
@ -227,7 +262,7 @@ void VRBRAINScheduler::reboot(bool hold_in_bootloader)
// delay to ensure the async force_saftey operation completes
delay(500);
px4_systemreset(hold_in_bootloader);
px4_systemreset(hold_in_bootloader);
}
void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
@ -266,6 +301,8 @@ 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);
}
@ -278,7 +315,7 @@ void *VRBRAINScheduler::_timer_thread(void *arg)
perf_end(sched->_perf_timers);
// process any pending RC output requests
((VRBRAINRCOutput *)hal.rcout)->_timer_tick();
hal.rcout->timer_tick();
// process any pending RC input requests
((VRBRAINRCInput *)hal.rcin)->_timer_tick();
@ -317,6 +354,8 @@ 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);
}
@ -338,11 +377,13 @@ 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) {
poll(nullptr, 0, 1);
sched->delay_microseconds_semaphore(1000);
// run registered IO processes
perf_begin(sched->_perf_io_timers);
@ -356,11 +397,13 @@ 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) {
poll(nullptr, 0, 10);
sched->delay_microseconds_semaphore(10000);
// process any pending storage writes
perf_begin(sched->_perf_storage_timer);
@ -370,15 +413,46 @@ void *VRBRAINScheduler::_storage_thread(void *arg)
return nullptr;
}
#if HAL_WITH_UAVCAN
void *VRBRAINScheduler::_uavcan_thread(void *arg)
{
VRBRAINScheduler *sched = ((_uavcan_thread_arg *) arg)->sched;
uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number;
char name[15];
snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number);
pthread_setname_np(pthread_self(), name);
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_px4_thread_should_exit) {
if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->is_initialized()) {
if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) {
(((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic();
} else {
sched->delay_microseconds_semaphore(10000);
}
} else {
sched->delay_microseconds_semaphore(10000);
}
}
return nullptr;
}
#endif
bool VRBRAINScheduler::in_main_thread() const
{
return getpid() == _main_task_pid;
}
void VRBRAINScheduler::system_initialized() {
void VRBRAINScheduler::system_initialized()
{
if (_initialized) {
AP_HAL::panic("PANIC: scheduler::system_initialized called"
"more than once");
"more than once");
}
_initialized = true;
}

14
libraries/AP_HAL_VRBRAIN/Scheduler.h
View File

@ -13,6 +13,9 @@
#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
@ -58,7 +61,9 @@ public:
bool in_main_thread() const override;
void system_initialized();
void hal_initialized() { _hal_initialized = true; }
void create_uavcan_thread() override;
private:
bool _initialized;
volatile bool _hal_initialized;
@ -83,11 +88,18 @@ private:
pthread_t _io_thread_ctx;
pthread_t _storage_thread_ctx;
pthread_t _uart_thread_ctx;
pthread_t _uavcan_thread_ctx;
struct _uavcan_thread_arg {
VRBRAINScheduler *sched;
uint8_t uavcan_number;
};
static void *_timer_thread(void *arg);
static void *_io_thread(void *arg);
static void *_storage_thread(void *arg);
static void *_uart_thread(void *arg);
static void *_uavcan_thread(void *arg);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);

5
libraries/AP_HAL_VRBRAIN/Semaphores.cpp
View File

@ -3,6 +3,7 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "Semaphores.h"
#include <nuttx/arch.h>
extern const AP_HAL::HAL& hal;
@ -15,6 +16,10 @@ bool Semaphore::give()
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;
}

476
libraries/AP_HAL_VRBRAIN/Storage.cpp
View File

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

58
libraries/AP_HAL_VRBRAIN/Storage.h
View File

@ -1,18 +1,35 @@
#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
#define VRBRAIN_STORAGE_MAX_WRITE 512
#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();
VRBRAINStorage();
void init() {}
void read_block(void *dst, uint16_t src, size_t n);
@ -21,22 +38,41 @@ public:
void _timer_tick(void);
private:
int _fd;
volatile bool _initialised;
void _storage_create(void);
void _storage_open(void);
void _mark_dirty(uint16_t loc, uint16_t length);
uint8_t _buffer[VRBRAIN_STORAGE_SIZE] __attribute__((aligned(4)));
volatile uint32_t _dirty_mask;
Bitmask _dirty_mask{VRBRAIN_STORAGE_NUM_LINES};
perf_counter_t _perf_storage;
perf_counter_t _perf_errors;
bool _have_mtd;
void _upgrade_to_mtd(void);
uint32_t _mtd_signature(void);
void _mtd_write_signature(void);
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
};

116
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

@ -26,6 +26,7 @@ VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name)
_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)
@ -69,7 +70,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
if (rxS != 0 && rxS != _readbuf.get_size()) {
if (rxS != _readbuf.get_size()) {
_initialised = false;
while (_in_timer) {
hal.scheduler->delay(1);
@ -85,7 +86,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
/*
allocate the write buffer
*/
if (txS != 0 && txS != _writebuf.get_size()) {
if (txS != _writebuf.get_size()) {
_initialised = false;
while (_in_timer) {
hal.scheduler->delay(1);
@ -123,13 +124,11 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
((VRBRAINGPIO *)hal.gpio)->set_usb_connected();
}
::printf("initialised %s OK %u %u\n", _devpath,
::printf("initialised %s OK %u %u\n", _devpath,
(unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size());
}
_initialised = true;
}
_uart_owner_pid = getpid();
}
void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
@ -154,6 +153,44 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
_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);
@ -197,9 +234,9 @@ void VRBRAINUARTDriver::end()
void VRBRAINUARTDriver::flush() {}
bool VRBRAINUARTDriver::is_initialized()
{
{
try_initialise();
return _initialised;
return _initialised;
}
void VRBRAINUARTDriver::set_blocking_writes(bool blocking)
@ -213,7 +250,7 @@ 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;
@ -226,7 +263,7 @@ uint32_t VRBRAINUARTDriver::available()
return number of bytes that can be added to the write buffer
*/
uint32_t VRBRAINUARTDriver::txspace()
{
{
if (!_initialised) {
try_initialise();
return 0;
@ -239,62 +276,82 @@ uint32_t VRBRAINUARTDriver::txspace()
read one byte from the read buffer
*/
int16_t VRBRAINUARTDriver::read()
{
if (_uart_owner_pid != getpid()){
{
if (!_semaphore.take_nonblocking()) {
return -1;
}
if (!_initialised) {
try_initialise();
_semaphore.give();
return -1;
}
uint8_t byte;
if (!_readbuf.read_byte(&byte))
if (!_readbuf.read_byte(&byte)) {
_semaphore.give();
return -1;
}
_semaphore.give();
return byte;
}
/*
write one byte to the buffer
/*
write one byte
*/
size_t VRBRAINUARTDriver::write(uint8_t c)
{
if (_uart_owner_pid != getpid()){
return 0;
{
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;
}
}
return _writebuf.write(&c, 1);
size_t ret = _writebuf.write(&c, 1);
_semaphore.give();
return ret;
}
/*
write size bytes to the write buffer
* write size bytes
*/
size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
{
if (_uart_owner_pid != getpid()){
return 0;
if (!_semaphore.take_nonblocking()) {
return -1;
}
if (!_initialised) {
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
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
@ -302,7 +359,14 @@ size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
return ret;
}
return _writebuf.write(buffer, size);
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;
}
/*
@ -322,7 +386,7 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
// 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) {
@ -336,7 +400,7 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
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",
::printf("disabling flow control on %s _total_written=%u\n",
_devpath, (unsigned)_total_written);
set_flow_control(FLOW_CONTROL_DISABLE);
}

16
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -1,9 +1,10 @@
#pragma once
#include <AP_HAL/utility/RingBuffer.h>
#include <systemlib/perf_counter.h>
#include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h>
#include "Semaphores.h"
class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver {
public:
@ -39,6 +40,10 @@ public:
void set_flow_control(enum flow_control flow_control);
enum flow_control get_flow_control(void) { 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;
@ -47,12 +52,12 @@ private:
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;
ByteBuffer _writebuf;
ByteBuffer _readbuf{0};
ByteBuffer _writebuf{0};
perf_counter_t _perf_uart;
int _write_fd(const uint8_t *buf, uint16_t n);
@ -68,6 +73,5 @@ private:
uint32_t _total_written;
enum flow_control _flow_control;
pid_t _uart_owner_pid;
Semaphore _semaphore;
};

47
libraries/AP_HAL_VRBRAIN/Util.cpp
View File

@ -73,6 +73,10 @@ bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream)
*/
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));
}
@ -96,7 +100,7 @@ void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec)
{
timespec ts;
ts.tv_sec = time_utc_usec/1000000ULL;
ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000UL;
ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL;
clock_settime(CLOCK_REALTIME, &ts);
}
@ -202,8 +206,12 @@ void VRBRAINUtil::set_imu_temp(float current)
// 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 = ((float)*_heater.target) - current;
float err = target - current;
_heater.integrator += kI * err;
_heater.integrator = constrain_float(_heater.integrator, 0, 70);
@ -226,4 +234,39 @@ 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

8
libraries/AP_HAL_VRBRAIN/Util.h
View File

@ -29,7 +29,7 @@ private:
class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
public:
VRBRAINUtil(void);
VRBRAINUtil(void);
bool run_debug_shell(AP_HAL::BetterStream *stream);
enum safety_state safety_switch_state(void);
@ -60,7 +60,11 @@ public:
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;

294
libraries/AP_HAL_VRBRAIN/bxcan.h Normal file
View File

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

7
libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp
View File

@ -16,17 +16,16 @@
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
param_t param_find(const char *name)
{
#if 0
// useful for driver debugging
::printf("VRBRAIN: param_find(%s)\n", name);
#endif
return PARAM_INVALID;
}