diff --git a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h deleted file mode 100644 index 5f04e83687..0000000000 --- a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "HAL_VRBRAIN_Class.h" - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h deleted file mode 100644 index b89e703fb2..0000000000 --- a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -namespace VRBRAIN { - class VRBRAINScheduler; - class VRBRAINUARTDriver; - class VRBRAINStorage; - class VRBRAINRCInput; - class VRBRAINRCOutput; - class VRBRAINAnalogIn; - class VRBRAINAnalogSource; - class VRBRAINUtil; - class VRBRAINGPIO; - class VRBRAINDigitalSource; - class NSHShellStream; - class VRBRAINI2CDriver; - class VRBRAIN_I2C; - class Semaphore; - class VRBRAINCAN; - class VRBRAINCANManager; -} diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp deleted file mode 100644 index 168e621428..0000000000 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ /dev/null @@ -1,301 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "AnalogIn.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "GPIO.h" - -#define ANLOGIN_DEBUGGING 0 - -// base voltage scaling for 12 bit 3.3V ADC -#define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f) - -#if ANLOGIN_DEBUGGING - # define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) -#else - # define Debug(fmt, args ...) -#endif - -extern const AP_HAL::HAL& hal; - -/* - scaling table between ADC count and actual input voltage, to account - for voltage dividers on the board. - */ -static const struct { - uint8_t pin; - float scaling; -} pin_scaling[] = { -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52E) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) - { 10, 3.3f/4096 }, - { 11, 3.3f/4096 }, -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - { 10, 3.3f/4096 }, -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - { 1, 3.3f/4096 }, - { 2, 3.3f/4096 }, - { 3, 3.3f/4096 }, - { 10, 3.3f/4096 }, -#else -#error "Unknown board type for AnalogIn scaling" -#endif - { 0, 0.f }, -}; - -using namespace VRBRAIN; - -VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) : - _pin(pin), - _stop_pin(-1), - _settle_time_ms(0), - _value(initial_value), - _value_ratiometric(initial_value), - _latest_value(initial_value), - _sum_count(0), - _sum_value(0), - _sum_ratiometric(0) -{ -} - -void VRBRAINAnalogSource::set_stop_pin(uint8_t p) -{ - _stop_pin = p; -} - -float VRBRAINAnalogSource::read_average() -{ - WITH_SEMAPHORE(_semaphore); - - if (_sum_count == 0) { - return _value; - } - _value = _sum_value / _sum_count; - _value_ratiometric = _sum_ratiometric / _sum_count; - _sum_value = 0; - _sum_ratiometric = 0; - _sum_count = 0; - - return _value; -} - -float VRBRAINAnalogSource::read_latest() -{ - return _latest_value; -} - -/* - return scaling from ADC count to Volts - */ -float VRBRAINAnalogSource::_pin_scaler(void) -{ - float scaling = VRBRAIN_VOLTAGE_SCALING; - uint8_t num_scalings = ARRAY_SIZE(pin_scaling); - for (uint8_t i=0; i_stop_pin != -1) { - // found another stop pin - _stop_pin_change_time = AP_HAL::millis(); - _current_stop_pin_i = idx; - - // set that pin high - hal.gpio->pinMode(c->_stop_pin, 1); - hal.gpio->write(c->_stop_pin, 1); - - // set all others low - for (uint8_t j=0; j_stop_pin != -1 && j != idx) { - hal.gpio->pinMode(c2->_stop_pin, 1); - hal.gpio->write(c2->_stop_pin, 0); - } - } - break; - } - } -} - -/* - called at 1kHz - */ -void VRBRAINAnalogIn::_timer_tick(void) -{ - if (_adc_fd == -1) { - // not initialised yet - return; - } - - // read adc at 100Hz - uint32_t now = AP_HAL::micros(); - uint32_t delta_t = now - _last_run; - if (delta_t < 10000) { - return; - } - _last_run = now; - - struct adc_msg_s buf_adc[VRBRAIN_ANALOG_MAX_CHANNELS]; - - // cope with initial setup of stop pin - if (_channels[_current_stop_pin_i] == nullptr || - _channels[_current_stop_pin_i]->_stop_pin == -1) { - next_stop_pin(); - } - - /* read all channels available */ - int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc)); - if (ret > 0) { - // match the incoming channels to the currently active pins - for (uint8_t i=0; i_pin) { - // add a value if either there is no stop pin, or - // the stop pin has been settling for enough time - if (c->_stop_pin == -1 || - (_current_stop_pin_i == j && - AP_HAL::millis() - _stop_pin_change_time > c->_settle_time_ms)) { - c->_add_value(buf_adc[i].am_data, _board_voltage); - if (c->_stop_pin != -1 && _current_stop_pin_i == j) { - next_stop_pin(); - } - } - } - } - } - } - -} - -AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin) -{ - for (uint8_t j=0; jprintf("Out of analog channels\n"); - return nullptr; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.h b/libraries/AP_HAL_VRBRAIN/AnalogIn.h deleted file mode 100644 index 3d46af7a9b..0000000000 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.h +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include - -#define VRBRAIN_ANALOG_MAX_CHANNELS 16 - - -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52E) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN -1 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) -#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10 -#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 1 -#endif - -class VRBRAIN::VRBRAINAnalogSource : public AP_HAL::AnalogSource { -public: - friend class VRBRAIN::VRBRAINAnalogIn; - VRBRAINAnalogSource(int16_t pin, float initial_value); - float read_average(); - float read_latest(); - void set_pin(uint8_t p); - float voltage_average(); - float voltage_latest(); - float voltage_average_ratiometric(); - - // implement stop pins - void set_stop_pin(uint8_t p); - void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; } - -private: - // what pin it is attached to - int16_t _pin; - int16_t _stop_pin; - uint16_t _settle_time_ms; - - // what value it has - float _value; - float _value_ratiometric; - float _latest_value; - uint8_t _sum_count; - float _sum_value; - float _sum_ratiometric; - void _add_value(float v, float vcc5V); - float _pin_scaler(); - HAL_Semaphore _semaphore; -}; - -class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn { -public: - VRBRAINAnalogIn(); - void init() override; - AP_HAL::AnalogSource* channel(int16_t pin) override; - void _timer_tick(void); - float board_voltage(void) override { return _board_voltage; } - float servorail_voltage(void) override { return _servorail_voltage; } - uint16_t power_status_flags(void) override { return _power_flags; } - -private: - int _adc_fd = -1; - int _battery_handle; - int _servorail_handle; - int _system_power_handle; - uint64_t _battery_timestamp; - uint64_t _servorail_timestamp; - VRBRAIN::VRBRAINAnalogSource* _channels[VRBRAIN_ANALOG_MAX_CHANNELS]; - - // what pin is currently held low to stop a sonar from reading - uint8_t _current_stop_pin_i; - uint32_t _stop_pin_change_time; - - uint32_t _last_run; - float _board_voltage; - float _servorail_voltage; - uint16_t _power_flags; - - void next_stop_pin(void); -}; diff --git a/libraries/AP_HAL_VRBRAIN/CAN.cpp b/libraries/AP_HAL_VRBRAIN/CAN.cpp deleted file mode 100644 index 23270da41b..0000000000 --- a/libraries/AP_HAL_VRBRAIN/CAN.cpp +++ /dev/null @@ -1,1117 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - * - * With modifications for Ardupilot CAN driver - * Copyright (C) 2017 Eugene Shamaev - */ - -#include -#include - -#if HAL_WITH_UAVCAN - -#include -#include -#include "CAN.h" - -#include -#include -#include - -#include - -#include "Scheduler.h" - -#include - -/* - * FOR INVESTIGATION: - * AP_HAL::micros64() was called for monotonic time counter - * pavel-kirienko: This will work as long as we don't need to synchronize the autopilot's own clock with an external - * time base, e.g. a GNSS time provided by an external GNSS receiver. Libuavcan's STM32 driver supports automatic time - * synchronization only if it has a dedicated hardware timer to work with. - */ - -extern const AP_HAL::HAL& hal; - -extern "C" { - static int can1_irq(const int irq, void*); -#if CAN_STM32_NUM_IFACES > 1 - static int can2_irq(const int irq, void*); -#endif -} - -using namespace VRBRAIN; - -uint64_t clock::getUtcUSecFromCanInterrupt() -{ - return AP_HAL::micros64(); -} - -uavcan::MonotonicTime clock::getMonotonic() -{ - return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64()); -} - -BusEvent::BusEvent(VRBRAINCANManager& can_driver) : - _signal(0) -{ - sem_init(&_wait_semaphore, 0, 0); -} - -BusEvent::~BusEvent() -{ -} - -bool BusEvent::wait(uavcan::MonotonicDuration duration) -{ - struct hrt_call wait_call; - - irqstate_t irs = irqsave(); - if (_signal) { - _signal = 0; - irqrestore(irs); - return true; - } - - sem_init(&_wait_semaphore, 0, 0); - irqrestore(irs); - - hrt_call_after(&wait_call, duration.toUSec(), (hrt_callout) signalFromCallOut, this); - sem_wait(&_wait_semaphore); - - hrt_cancel(&wait_call); - - irs = irqsave(); - if (_signal) { - _signal = 0; - irqrestore(irs); - - return true; - } - irqrestore(irs); - - return false; -} - -void BusEvent::signalFromCallOut(BusEvent *sem) -{ - sem_post(&sem->_wait_semaphore); -} - -void BusEvent::signalFromInterrupt() -{ - _signal++; - sem_post(&_wait_semaphore); -} - -static void handleTxInterrupt(uint8_t iface_index) -{ - if (iface_index >= CAN_STM32_NUM_IFACES) { - return; - } - uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (hal.can_mgr[i] == nullptr) { - continue; - } - VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index); - if (iface != nullptr) { - iface->handleTxInterrupt(utc_usec); - } - } -} - -static void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index) -{ - if (iface_index >= CAN_STM32_NUM_IFACES) { - return; - } - uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - - for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { - if (hal.can_mgr[i] == nullptr) { - continue; - } - VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index); - if (iface != nullptr) { - iface->handleRxInterrupt(fifo_index, utc_usec); - } - } -} - -const uint32_t VRBRAINCAN::TSR_ABRQx[VRBRAINCAN::NumTxMailboxes] = { bxcan::TSR_ABRQ0, bxcan::TSR_ABRQ1, bxcan::TSR_ABRQ2 }; - -int VRBRAINCAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings) -{ - if (target_bitrate < 1) { - return -ErrInvalidBitRate; - } - - /* - * Hardware configuration - */ - const uint32_t pclk = STM32_PCLK1_FREQUENCY; - - static const uint8_t MaxBS1 = 16; - static const uint8_t MaxBS2 = 8; - - /* - * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG - * CAN in Automation, 2003 - * - * According to the source, optimal quanta per bit are: - * Bitrate Optimal Maximum - * 1000 kbps 8 10 - * 500 kbps 16 17 - * 250 kbps 16 17 - * 125 kbps 16 17 - */ - const uint8_t max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; - - if (max_quanta_per_bit > (MaxBS1 + MaxBS2)) { - if (AP::can().get_debug_level(self_index_) >= 1) { - printf("VRBRAINCAN::computeTimings max_quanta_per_bit problem\n\r"); - } - } - - static const uint16_t MaxSamplePointLocation = 900; - - /* - * Computing (prescaler * BS): - * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual - * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified - * let: - * BS = 1 + BS1 + BS2 -- Number of time quanta per bit - * PRESCALER_BS = PRESCALER * BS - * ==> - * PRESCALER_BS = PCLK / BITRATE - */ - const uint32_t prescaler_bs = pclk / target_bitrate; - - /* - * Searching for such prescaler value so that the number of quanta per bit is highest. - */ - uint8_t bs1_bs2_sum = uint8_t(max_quanta_per_bit - 1); - - while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { - if (bs1_bs2_sum <= 2) { - return -ErrInvalidBitRate; // No solution - } - bs1_bs2_sum--; - } - - const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) { - return -ErrInvalidBitRate; // No solution - } - - /* - * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. - * We need to find the values so that the sample point is as close as possible to the optimal value. - * - * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) - * {{bs2 -> (1 + bs1)/7}} - * - * Hence: - * bs2 = (1 + bs1) / 7 - * bs1 = (7 * bs1_bs2_sum - 1) / 8 - * - * Sample point location can be computed as follows: - * Sample point location = (1 + bs1) / (1 + bs1 + bs2) - * - * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: - * - With rounding to nearest - * - With rounding to zero - */ - struct BsPair { - uint8_t bs1; - uint8_t bs2; - uint16_t sample_point_permill; - - BsPair() : - bs1(0), bs2(0), sample_point_permill(0) - { - } - - BsPair(uint8_t bs1_bs2_sum, uint8_t arg_bs1) : - bs1(arg_bs1), bs2(uint8_t(bs1_bs2_sum - bs1)), sample_point_permill( - uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) - { - if (bs1_bs2_sum <= arg_bs1) { - if (AP::can().get_debug_level(self_index_) >= 1) { - AP_HAL::panic("VRBRAINCAN::computeTimings bs1_bs2_sum <= arg_bs1"); - } - } - } - - bool isValid() const - { - return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); - } - }; - - // First attempt with rounding to nearest - BsPair solution(bs1_bs2_sum, uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); - - if (solution.sample_point_permill > MaxSamplePointLocation || !solution.isValid()) { - // Second attempt with rounding to zero - solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8)); - - if (!solution.isValid()) - { - printf("VRBRAINCAN::computeTimings second solution invalid\n\r"); - return -ErrLogic; - } - } - - /* - * Final validation - */ - if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { - if (AP::can().get_debug_level(self_index_) >= 1) { - printf("VRBRAINCAN::computeTimings target_bitrate error\n\r"); - } - return -ErrLogic; - } - - if (AP::can().get_debug_level(self_index_) >= 2) { - printf("VRBRAINCAN::computeTimings Timings: quanta/bit: %d, sample point location: %.1f%%\n\r", - int(1 + solution.bs1 + solution.bs2), double(solution.sample_point_permill / 10.0)); - } - - out_timings.prescaler = uint16_t(prescaler - 1U); - out_timings.sjw = 0; // Which means one - out_timings.bs1 = uint8_t(solution.bs1 - 1); - out_timings.bs2 = uint8_t(solution.bs2 - 1); - - return 0; -} - -int16_t VRBRAINCAN::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) -{ - if (frame.isErrorFrame() || frame.dlc > 8) { - return -ErrUnsupportedFrame; - } - - /* - * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because - * it is possible that the highest-priority frame between select() and send() could have been - * replaced with a lower priority one due to TX timeout. But we don't do this check because: - * - * - It is a highly unlikely scenario. - * - * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new - * frame can only have higher priority, which doesn't break the logic. - * - * - If high-priority frames are timing out in the TX queue, there's probably a lot of other - * issues to take care of before this one becomes relevant. - * - * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. - */ - CriticalSectionLocker lock; - - /* - * Seeking for an empty slot - */ - uint8_t txmailbox = 0xFF; - if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) { - txmailbox = 0; - } else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) { - txmailbox = 1; - } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) { - txmailbox = 2; - } else { - return 0; // No transmission for you. - } - - peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics - - /* - * Setting up the mailbox - */ - bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox]; - if (frame.isExtended()) { - mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE; - } else { - mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21); - } - - if (frame.isRemoteTransmissionRequest()) { - mb.TIR |= bxcan::TIR_RTR; - } - - mb.TDTR = frame.dlc; - - mb.TDHR = (uint32_t(frame.data[7]) << 24) | (uint32_t(frame.data[6]) << 16) | (uint32_t(frame.data[5]) << 8) - | (uint32_t(frame.data[4]) << 0); - mb.TDLR = (uint32_t(frame.data[3]) << 24) | (uint32_t(frame.data[2]) << 16) | (uint32_t(frame.data[1]) << 8) - | (uint32_t(frame.data[0]) << 0); - - mb.TIR |= bxcan::TIR_TXRQ; // Go. - - /* - * Registering the pending transmission so we can track its deadline and loopback it as needed - */ - TxItem& txi = pending_tx_[txmailbox]; - txi.deadline = tx_deadline; - txi.frame = frame; - txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; - txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; - txi.pending = true; - return 1; -} - -int16_t VRBRAINCAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) -{ - out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps - uint64_t utc_usec = 0; - { - CriticalSectionLocker lock; - if (rx_queue_.available() == 0) { - return 0; - } - - CanRxItem frm; - rx_queue_.pop(frm); - out_frame = frm.frame; - utc_usec = frm.utc_usec; - out_flags = frm.flags; - } - out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec); - return 1; -} - -int16_t VRBRAINCAN::configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) -{ - if (num_configs > NumFilters) { - return -ErrFilterNumConfigs; - } - - CriticalSectionLocker lock; - - can_->FMR |= bxcan::FMR_FINIT; - - // Slave (CAN2) gets half of the filters - can_->FMR = (can_->FMR & ~0x00003F00) | static_cast(NumFilters) << 8; - - can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters - can_->FM1R = 0; // Identifier Mask mode - can_->FS1R = 0x7ffffff; // Single 32-bit for all - - const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters; - - if (num_configs == 0) { - can_->FilterRegister[filter_start_index].FR1 = 0; - can_->FilterRegister[filter_start_index].FR2 = 0; - can_->FA1R = 1 << filter_start_index; - } else { - for (uint8_t i = 0; i < NumFilters; i++) { - if (i < num_configs) { - uint32_t id = 0; - uint32_t mask = 0; - - const uavcan::CanFilterConfig* const cfg = filter_configs + i; - - if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { - id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3; - mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3; - id |= bxcan::RIR_IDE; - } else { - id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; - mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; - } - - if (cfg->id & uavcan::CanFrame::FlagRTR) { - id |= bxcan::RIR_RTR; - } - - if (cfg->mask & uavcan::CanFrame::FlagEFF) { - mask |= bxcan::RIR_IDE; - } - - if (cfg->mask & uavcan::CanFrame::FlagRTR) { - mask |= bxcan::RIR_RTR; - } - - can_->FilterRegister[filter_start_index + i].FR1 = id; - can_->FilterRegister[filter_start_index + i].FR2 = mask; - - can_->FA1R |= (1 << (filter_start_index + i)); - } else { - can_->FA1R &= ~(1 << (filter_start_index + i)); - } - } - } - - can_->FMR &= ~bxcan::FMR_FINIT; - - return 0; -} - -bool VRBRAINCAN::waitMsrINakBitStateChange(bool target_state) -{ - const unsigned Timeout = 1000; - for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) { - const bool state = (can_->MSR & bxcan::MSR_INAK) != 0; - if (state == target_state) { - return true; - } - hal.scheduler->delay_microseconds(1000); - } - return false; -} - -int VRBRAINCAN::init(const uint32_t bitrate, const OperatingMode mode) -{ - /* We need to silence the controller in the first order, otherwise it may interfere with the following operations. */ - - { - CriticalSectionLocker lock; - - can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode - can_->MCR |= bxcan::MCR_INRQ; // Request init - - can_->IER = 0; // Disable CAN interrupts while initialization is in progress - } - - if (!waitMsrINakBitStateChange(true)) { - if (AP::can().get_debug_level(self_index_) >= 1) { - printf("VRBRAINCAN::init MSR INAK not set\n\r"); - } - can_->MCR = bxcan::MCR_RESET; - return -ErrMsrInakNotSet; - } - - /* - * Object state - CAN interrupts are disabled, so it's safe to modify it now - */ - rx_queue_.clear(); - error_cnt_ = 0; - served_aborts_cnt_ = 0; - uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem()); - peak_tx_mailbox_index_ = 0; - had_activity_ = false; - - /* - * CAN timings for this bitrate - */ - Timings timings; - const int timings_res = computeTimings(bitrate, timings); - if (timings_res < 0) { - can_->MCR = bxcan::MCR_RESET; - return timings_res; - } - if (AP::can().get_debug_level(self_index_) >= 2) { - printf("VRBRAINCAN::init Timings: presc=%u sjw=%u bs1=%u bs2=%u\n\r", unsigned(timings.prescaler), - unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); - } - - /* - * Hardware initialization (the hardware has already confirmed initialization mode, see above) - */ - can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648 - - can_->BTR = ((timings.sjw & 3U) << 24) | ((timings.bs1 & 15U) << 16) | ((timings.bs2 & 7U) << 20) - | (timings.prescaler & 1023U) | ((mode == SilentMode) ? bxcan::BTR_SILM : 0); - - can_->IER = bxcan::IER_TMEIE | // TX mailbox empty - bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty - bxcan::IER_FMPIE1; // RX FIFO 1 is not empty - - can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode - - if (!waitMsrINakBitStateChange(false)) { - if (AP::can().get_debug_level(self_index_) >= 1) { - printf("VRBRAINCAN::init MSR INAK not cleared\n\r"); - } - can_->MCR = bxcan::MCR_RESET; - return -ErrMsrInakNotCleared; - } - - /* - * Default filter configuration - */ - if (self_index_ == 0) { - can_->FMR |= bxcan::FMR_FINIT; - - can_->FMR &= 0xFFFFC0F1; - can_->FMR |= static_cast(NumFilters) << 8; // Slave (CAN2) gets half of the filters - - can_->FFA1R = 0; // All assigned to FIFO0 by default - can_->FM1R = 0; // Indentifier Mask mode - -#if CAN_STM32_NUM_IFACES > 1 - can_->FS1R = 0x7ffffff; // Single 32-bit for all - can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything - can_->FilterRegister[0].FR2 = 0; - can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything - can_->FilterRegister[NumFilters].FR2 = 0; - can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface -#else - can_->FS1R = 0x1fff; - can_->FilterRegister[0].FR1 = 0; - can_->FilterRegister[0].FR2 = 0; - can_->FA1R = 1; -#endif - - can_->FMR &= ~bxcan::FMR_FINIT; - } - - return 0; -} - -void VRBRAINCAN::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const uint64_t utc_usec) -{ - if (mailbox_index >= NumTxMailboxes) { - return; - } - - had_activity_ = had_activity_ || txok; - - TxItem& txi = pending_tx_[mailbox_index]; - - if (txi.loopback && txok && txi.pending) { - CanRxItem frm; - frm.frame = txi.frame; - frm.flags = uavcan::CanIOFlagLoopback; - frm.utc_usec = utc_usec; - rx_queue_.push(frm); - } - - txi.pending = false; -} - -void VRBRAINCAN::handleTxInterrupt(const uint64_t utc_usec) -{ - // TXOK == false means that there was a hardware failure - if (can_->TSR & bxcan::TSR_RQCP0) { - const bool txok = can_->TSR & bxcan::TSR_TXOK0; - can_->TSR = bxcan::TSR_RQCP0; - handleTxMailboxInterrupt(0, txok, utc_usec); - } - if (can_->TSR & bxcan::TSR_RQCP1) { - const bool txok = can_->TSR & bxcan::TSR_TXOK1; - can_->TSR = bxcan::TSR_RQCP1; - handleTxMailboxInterrupt(1, txok, utc_usec); - } - if (can_->TSR & bxcan::TSR_RQCP2) { - const bool txok = can_->TSR & bxcan::TSR_TXOK2; - can_->TSR = bxcan::TSR_RQCP2; - handleTxMailboxInterrupt(2, txok, utc_usec); - } - - if(update_event_ != nullptr) { - update_event_->signalFromInterrupt(); - } - - pollErrorFlagsFromISR(); -} - -void VRBRAINCAN::handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec) -{ - if (fifo_index >= 2) { - return; - } - - volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R; - if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) { - return; - } - - /* - * Register overflow as a hardware error - */ - if ((*rfr_reg & bxcan::RFR_FOVR) != 0) { - error_cnt_++; - } - - /* - * Read the frame contents - */ - uavcan::CanFrame frame; - const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index]; - - if ((rf.RIR & bxcan::RIR_IDE) == 0) { - frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21); - } else { - frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3); - frame.id |= uavcan::CanFrame::FlagEFF; - } - - if ((rf.RIR & bxcan::RIR_RTR) != 0) { - frame.id |= uavcan::CanFrame::FlagRTR; - } - - frame.dlc = rf.RDTR & 15; - - frame.data[0] = uint8_t(0xFF & (rf.RDLR >> 0)); - frame.data[1] = uint8_t(0xFF & (rf.RDLR >> 8)); - frame.data[2] = uint8_t(0xFF & (rf.RDLR >> 16)); - frame.data[3] = uint8_t(0xFF & (rf.RDLR >> 24)); - frame.data[4] = uint8_t(0xFF & (rf.RDHR >> 0)); - frame.data[5] = uint8_t(0xFF & (rf.RDHR >> 8)); - frame.data[6] = uint8_t(0xFF & (rf.RDHR >> 16)); - frame.data[7] = uint8_t(0xFF & (rf.RDHR >> 24)); - - *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read - - /* - * Store with timeout into the FIFO buffer and signal update event - */ - CanRxItem frm; - frm.frame = frame; - frm.flags = 0; - frm.utc_usec = utc_usec; - rx_queue_.push(frm); - - had_activity_ = true; - if(update_event_ != nullptr) { - update_event_->signalFromInterrupt(); - } - - pollErrorFlagsFromISR(); -} - -void VRBRAINCAN::pollErrorFlagsFromISR() -{ - const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT); - if (lec == 0) { - return; - } - can_->ESR = 0; - error_cnt_++; - - // Serving abort requests - for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please. - TxItem& txi = pending_tx_[i]; - if (txi.pending && txi.abort_on_error) { - can_->TSR = TSR_ABRQx[i]; - txi.pending = false; - served_aborts_cnt_++; - } - } -} - -void VRBRAINCAN::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) -{ - CriticalSectionLocker lock; - for (int i = 0; i < NumTxMailboxes; i++) { - TxItem& txi = pending_tx_[i]; - if (txi.pending && txi.deadline < current_time) { - can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission - txi.pending = false; - error_cnt_++; - } - } -} - -bool VRBRAINCAN::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const -{ - /* - * We can accept more frames only if the following conditions are satisfied: - * - There is at least one TX mailbox free (obvious enough); - * - The priority of the new frame is higher than priority of all TX mailboxes. - */ - { - static const uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2; - const uint32_t tme = can_->TSR & TME; - - if (tme == TME) { // All TX mailboxes are free (as in freedom). - return true; - } - - if (tme == 0) { // All TX mailboxes are busy transmitting. - return false; - } - } - - /* - * The second condition requires a critical section. - */ - CriticalSectionLocker lock; - - for (int mbx = 0; mbx < NumTxMailboxes; mbx++) { - if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame)) { - return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. - } - } - - return true; // This new frame will be added to a free TX mailbox in the next @ref send(). -} - -bool VRBRAINCAN::isRxBufferEmpty() const -{ - CriticalSectionLocker lock; - return rx_queue_.available() == 0; -} - -uint64_t VRBRAINCAN::getErrorCount() const -{ - CriticalSectionLocker lock; - return error_cnt_; - //TODO: + rx_queue_.getOverflowCount(); -} - -unsigned VRBRAINCAN::getRxQueueLength() const -{ - CriticalSectionLocker lock; - return rx_queue_.available(); -} - -bool VRBRAINCAN::hadActivity() -{ - CriticalSectionLocker lock; - const bool ret = had_activity_; - had_activity_ = false; - return ret; -} - -bool VRBRAINCAN::begin(uint32_t bitrate) -{ - if (init(bitrate, OperatingMode::NormalMode) == 0) { - bitrate_ = bitrate; - initialized_ = true; - } else { - initialized_ = false; - } - return initialized_; -} - -void VRBRAINCAN::reset() -{ - if (initialized_ && bitrate_ != 0) { - init(bitrate_, OperatingMode::NormalMode); - } -} - -bool VRBRAINCAN::is_initialized() -{ - return initialized_; -} - -int32_t VRBRAINCAN::available() -{ - if (initialized_) { - return getRxQueueLength(); - } else { - return -1; - } -} - -int32_t VRBRAINCAN::tx_pending() -{ - if (!initialized_) { - return -1; - } - - int32_t ret = 0; - { - CriticalSectionLocker lock; - - for (int mbx = 0; mbx < NumTxMailboxes; mbx++) { - if (pending_tx_[mbx].pending) { - ret++; - } - } - } - return ret; -} - -/* - * CanDriver - */ - -VRBRAINCANManager::VRBRAINCANManager() : - update_event_(*this), if0_(bxcan::Can[0], nullptr, 0, CAN_STM32_RX_QUEUE_SIZE), if1_( - bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false) -{ - uavcan::StaticAssert<(CAN_STM32_RX_QUEUE_SIZE <= VRBRAINCAN::MaxRxQueueCapacity)>::check(); - - for(uint8_t i = 0; i < CAN_STM32_NUM_IFACES; i++) { - _ifaces_out_to_in[i] = UINT8_MAX; - } -} - -uavcan::CanSelectMasks VRBRAINCANManager::makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const -{ - uavcan::CanSelectMasks msk; - - for (uint8_t i = 0; i < _ifaces_num; i++) { - if (ifaces[i] == nullptr) { - continue; - } - if (!ifaces[i]->isRxBufferEmpty()) { - msk.read |= 1 << i; - } - - if (pending_tx[i] == nullptr) { - continue; - } - - if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) { - msk.write |= 1 << i; - } - } - - return msk; -} - -bool VRBRAINCANManager::hasReadableInterfaces() const -{ - bool ret = false; - - for (uint8_t i = 0; i < _ifaces_num; i++) { - if (ifaces[i] != nullptr) { - ret |= !ifaces[i]->isRxBufferEmpty(); - } - } - - return ret; -} - -int16_t VRBRAINCANManager::select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], const uavcan::MonotonicTime blocking_deadline) -{ - const uavcan::CanSelectMasks in_masks = inout_masks; - const uavcan::MonotonicTime time = clock::getMonotonic(); - - for (uint8_t i = 0; i < _ifaces_num; i++) { - if (ifaces[i] == nullptr) { - continue; - } - ifaces[i]->discardTimedOutTxMailboxes(time); - { - CriticalSectionLocker cs_locker; - ifaces[i]->pollErrorFlagsFromISR(); - } - } - - inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - if ((inout_masks.read & in_masks.read) != 0 || (inout_masks.write & in_masks.write) != 0) { - return 1; - } - - (void) update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates - inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set - return 1; // Return value doesn't matter as long as it is non-negative -} - -void VRBRAINCANManager::initOnce(uint8_t can_number) -{ - { - CriticalSectionLocker lock; - if (can_number == 0) { - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN); - } -#if CAN_STM32_NUM_IFACES > 1 - if (can_number == 1) { - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN); - } -#endif - } - - if (can_number == 0) { -#if defined(GPIO_CAN1_RX) && defined(GPIO_CAN1_TX) - stm32_configgpio(GPIO_CAN1_RX); - stm32_configgpio(GPIO_CAN1_TX); -#else -# error "Need to define GPIO_CAN1_RX/TX" -#endif - } -#if CAN_STM32_NUM_IFACES > 1 - if (can_number == 1) { -#if defined(GPIO_CAN2_RX) && defined(GPIO_CAN2_TX) - stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio(GPIO_CAN2_TX); -#else -# error "Need to define GPIO_CAN2_RX/TX" -#endif // defined(GPIO_CAN2_RX) && defined(GPIO_CAN2_TX) - } -#endif // CAN_STM32_NUM_IFACES > 1 - - /* - * IRQ - */ - if (can_number == 0) { -#if defined(STM32_IRQ_CAN1TX) && defined(STM32_IRQ_CAN1RX0) && defined(STM32_IRQ_CAN1RX1) - CAN_IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq); - CAN_IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq); - CAN_IRQ_ATTACH(STM32_IRQ_CAN1RX1, can1_irq); -#else -# error "Need to define STM32_IRQ_CAN1TX/RX0/RX1" -#endif - } - -#if CAN_STM32_NUM_IFACES > 1 - if (can_number == 1) { -#if defined(STM32_IRQ_CAN2TX) && defined(STM32_IRQ_CAN2RX0) && defined(STM32_IRQ_CAN2RX1) - CAN_IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq); - CAN_IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq); - CAN_IRQ_ATTACH(STM32_IRQ_CAN2RX1, can2_irq); -#else -# error "Need to define STM32_IRQ_CAN2TX/RX0/RX1" -#endif // defined(STM32_IRQ_CAN2TX) && defined(STM32_IRQ_CAN2RX0) && defined(STM32_IRQ_CAN2RX1) - } -#endif // CAN_STM32_NUM_IFACES > 1 -} - -int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingMode mode, uint8_t can_number) -{ - static bool initialized_once[CAN_STM32_NUM_IFACES]; - - if (can_number >= CAN_STM32_NUM_IFACES) { - return -ErrNotImplemented; - } - - int res = 0; - - if (AP::can().get_debug_level(can_number) >= 2) { - printf("VRBRAINCANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast(bitrate), - static_cast(mode), static_cast(can_number)); - } - - // If this outside physical interface was never inited - do this and add it to in/out conversion tables - if (!initialized_once[can_number]) { - initialized_once[can_number] = true; - _ifaces_num++; - _ifaces_out_to_in[can_number] = _ifaces_num - 1; - - if (AP::can().get_debug_level(can_number) >= 2) { - printf("VRBRAINCANManager::init First initialization bus %d\n\r", static_cast(can_number)); - } - - initOnce(can_number); - } - - /* - * CAN1 - */ - if (can_number == 0) { - if (AP::can().get_debug_level(0) >= 2) { - printf("VRBRAINCANManager::init Initing iface 0...\n\r"); - } - ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first, - } - -#if CAN_STM32_NUM_IFACES > 1 - /* - * CAN2 - */ - if (can_number == 1) { - if (AP::can().get_debug_level(1) >= 2) { - printf("VRBRAINCANManager::init Initing iface 1...\n\r"); - } - ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here. - } -#endif - - ifaces[_ifaces_out_to_in[can_number]]->set_update_event(&update_event_); - res = ifaces[_ifaces_out_to_in[can_number]]->init(bitrate, mode); - if (res < 0) { - ifaces[_ifaces_out_to_in[can_number]] = nullptr; - return res; - } - - if (AP::can().get_debug_level(can_number) >= 2) { - printf("VRBRAINCANManager::init CAN drv init OK, res = %d\n\r", res); - } - - return res; -} - -VRBRAINCAN* VRBRAINCANManager::getIface(uint8_t iface_index) -{ - if (iface_index < _ifaces_num) { - return ifaces[iface_index]; - } - - return nullptr; -} - -VRBRAINCAN* VRBRAINCANManager::getIface_out_to_in(uint8_t iface_index) -{ - // Find which internal interface corresponds to required outside physical interface - if (iface_index < CAN_STM32_NUM_IFACES) { - if (_ifaces_out_to_in[iface_index] != UINT8_MAX) { - return ifaces[_ifaces_out_to_in[iface_index]]; - } - } - - return nullptr; -} - -bool VRBRAINCANManager::hadActivity() -{ - bool ret = false; - - // Go through all interfaces that are present in this manager - for (uint8_t i = 0; i < _ifaces_num; i++) - { - if (ifaces[i] != nullptr) { - ret |= ifaces[i]->hadActivity(); - } - } - - return ret; -} - -bool VRBRAINCANManager::begin(uint32_t bitrate, uint8_t can_number) -{ - // Try to init outside physical interface 'can_number' - if (init(bitrate, VRBRAINCAN::OperatingMode::NormalMode, can_number) >= 0) { - return true; - } - - return false; -} - -bool VRBRAINCANManager::is_initialized() -{ - return initialized_; -} - -void VRBRAINCANManager::initialized(bool val) -{ - initialized_ = val; -} - -/* - * Interrupt handlers - */ -extern "C" { - static int can1_irq(const int irq, void*) - { - if (irq == STM32_IRQ_CAN1TX) { - handleTxInterrupt(0); - } else if (irq == STM32_IRQ_CAN1RX0) { - handleRxInterrupt(0, 0); - } else if (irq == STM32_IRQ_CAN1RX1) { - handleRxInterrupt(0, 1); - } else { - printf("can1_irq unhandled"); - } - return 0; - } - -#if CAN_STM32_NUM_IFACES > 1 - static int can2_irq(const int irq, void*) - { - if (irq == STM32_IRQ_CAN2TX) { - handleTxInterrupt(1); - } else if (irq == STM32_IRQ_CAN2RX0) { - handleRxInterrupt(1, 0); - } else if (irq == STM32_IRQ_CAN2RX1) { - handleRxInterrupt(1, 1); - } else { - printf("can2_irq unhandled"); - } - return 0; - } -#endif - -} // extern "C" - -#endif diff --git a/libraries/AP_HAL_VRBRAIN/CAN.h b/libraries/AP_HAL_VRBRAIN/CAN.h deleted file mode 100644 index 230b72ffe1..0000000000 --- a/libraries/AP_HAL_VRBRAIN/CAN.h +++ /dev/null @@ -1,312 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - * - * With modifications for Ardupilot CAN driver - * Copyright (C) 2017 Eugene Shamaev - */ - -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include -#include -#include - -#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 rx_queue_; - bxcan::CanType* const can_; - uint64_t error_cnt_; - uint32_t served_aborts_cnt_; - BusEvent* update_event_; - TxItem pending_tx_[NumTxMailboxes]; - uint8_t peak_tx_mailbox_index_; - const uint8_t self_index_; - - bool had_activity_; - - uint32_t bitrate_; - - int computeTimings(uint32_t target_bitrate, Timings& out_timings); - - virtual int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) override; - - virtual int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override; - - virtual int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override; - - virtual uint16_t getNumFilters() const override - { - return NumFilters; - } - - void handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, uint64_t utc_usec); - - bool waitMsrINakBitStateChange(bool target_state); - - bool initialized_; -public: - enum { - MaxRxQueueCapacity = 254 - }; - - enum OperatingMode { - NormalMode, SilentMode - }; - - VRBRAINCAN(bxcan::CanType* can, BusEvent* update_event, uint8_t self_index, uint8_t rx_queue_capacity) : - rx_queue_(rx_queue_capacity), can_(can), error_cnt_(0), served_aborts_cnt_(0), update_event_( - update_event), peak_tx_mailbox_index_(0), self_index_(self_index), had_activity_(false), bitrate_( - 0), initialized_(false) - { - UAVCAN_ASSERT(self_index_ < CAN_STM32_NUM_IFACES); - } - - /** - * Initializes the hardware CAN controller. - * Assumes: - * - Iface clock is enabled - * - Iface has been resetted via RCC - * - Caller will configure NVIC by itself - */ - int init(const uint32_t bitrate, const OperatingMode mode); - - void set_update_event(BusEvent* update_event) - { - update_event_ = update_event; - } - - void handleTxInterrupt(uint64_t utc_usec); - void handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec); - - /** - * This method is used to count errors and abort transmission on error if necessary. - * This functionality used to be implemented in the SCE interrupt handler, but that approach was - * generating too much processing overhead, especially on disconnected interfaces. - * - * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. - */ - void pollErrorFlagsFromISR(); - - void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); - - bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; - bool isRxBufferEmpty() const; - - /** - * Total number of hardware failures and other kinds of errors (e.g. queue overruns). - * May increase continuously if the interface is not connected to the bus. - */ - virtual uint64_t getErrorCount() const override; - - /** - * Number of times the driver exercised library's requirement to abort transmission on first error. - * This is an atomic read, it doesn't require a critical section. - * See @ref uavcan::CanIOFlagAbortOnError. - */ - uint32_t getVoluntaryTxAbortCount() const - { - return served_aborts_cnt_; - } - - /** - * Returns the number of frames pending in the RX queue. - * This is intended for debug use only. - */ - unsigned getRxQueueLength() const; - - /** - * Whether this iface had at least one successful IO since the previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); - - /** - * Peak number of TX mailboxes used concurrently since initialization. - * Range is [1, 3]. - * Value of 3 suggests that priority inversion could be taking place. - */ - uint8_t getPeakNumTxMailboxesUsed() const - { - return uint8_t(peak_tx_mailbox_index_ + 1); - } - - bool begin(uint32_t bitrate) override; - void end() override - { - } - - void reset() override; - - int32_t tx_pending() override; - int32_t available() override; - - bool is_initialized() override; -}; - -class VRBRAINCANManager: public AP_HAL::CANManager { - BusEvent update_event_; - VRBRAINCAN if0_; - VRBRAINCAN if1_; - - virtual int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override; - - void initOnce(uint8_t can_number); - - bool initialized_; - - VRBRAINCAN* ifaces[CAN_STM32_NUM_IFACES]; - uint8_t _ifaces_num; - uint8_t _ifaces_out_to_in[CAN_STM32_NUM_IFACES]; - -public: - VRBRAINCANManager(); - - /** - * This function returns select masks indicating which interfaces are available for read/write. - */ - uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const; - - /** - * Whether there's at least one interface where receive() would return a frame. - */ - bool hasReadableInterfaces() const; - - /** - * Returns zero if OK. - * Returns negative value if failed (e.g. invalid bitrate). - */ - int init(const uint32_t bitrate, const VRBRAINCAN::OperatingMode mode, uint8_t can_number); - - virtual VRBRAINCAN* getIface(uint8_t iface_index) override; - VRBRAINCAN* getIface_out_to_in(uint8_t iface_index); - - virtual uint8_t getNumIfaces() const override - { - return _ifaces_num; - } - - /** - * Whether at least one iface had at least one successful IO since previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); - - bool begin(uint32_t bitrate, uint8_t can_number) override; - - bool is_initialized() override; - void initialized(bool val) override; -}; -} diff --git a/libraries/AP_HAL_VRBRAIN/Device.cpp b/libraries/AP_HAL_VRBRAIN/Device.cpp deleted file mode 100644 index de8dc64a5d..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Device.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ -#include "Device.h" - -#include -#include "board_config.h" -#include -#include -#include -#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, ¶m); - 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(h); - - callback->period_usec = period_usec; - callback->next_usec = AP_HAL::micros64() + period_usec; - - return true; -} - -} diff --git a/libraries/AP_HAL_VRBRAIN/Device.h b/libraries/AP_HAL_VRBRAIN/Device.h deleted file mode 100644 index 25a012efcb..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Device.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published - * by the Free Software Foundation, either version 3 of the License, - * or (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ -#pragma once - -#include -#include -#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; -}; - -} diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.cpp b/libraries/AP_HAL_VRBRAIN/GPIO.cpp deleted file mode 100644 index 43d5ce9264..0000000000 --- a/libraries/AP_HAL_VRBRAIN/GPIO.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "GPIO.h" - -#include -#include -#include -#include - -/* VRBRAIN headers */ -#include -#include -#include - -#include -#include - -#define LOW 0 -#define HIGH 1 - -extern const AP_HAL::HAL& hal; - -using namespace VRBRAIN; - -VRBRAINGPIO::VRBRAINGPIO() -{} - -void VRBRAINGPIO::init() -{ - - _led_fd = open(LED0_DEVICE_PATH, O_RDWR); - if (_led_fd == -1) { - AP_HAL::panic("Unable to open " LED0_DEVICE_PATH); - } - if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n"); - } - if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED RED\n"); - } - - if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); - } - - - - _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - if (_tone_alarm_fd == -1) { - AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH); - } - - _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0); - if (_gpio_fmu_fd == -1) { - AP_HAL::panic("Unable to open GPIO"); - } -#ifdef GPIO_SERVO_1 - if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_1) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO_1\n"); - } -#endif -#ifdef GPIO_SERVO_2 - if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_2) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO_2\n"); - } -#endif -#ifdef GPIO_SERVO_3 - if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_3) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO_3\n"); - } -#endif -#ifdef GPIO_SERVO_4 - if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_4) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO_4\n"); - } -#endif -} - -void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output) -{ - switch (pin) { - } -} - -uint8_t VRBRAINGPIO::read(uint8_t pin) { - switch (pin) { - -#ifdef GPIO_SERVO_3 - case EXTERNAL_RELAY1_PIN: { - uint32_t relays = 0; - ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays); - return (relays & GPIO_SERVO_3)?HIGH:LOW; - } -#endif - -#ifdef GPIO_SERVO_4 - case EXTERNAL_RELAY2_PIN: { - uint32_t relays = 0; - ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays); - return (relays & GPIO_SERVO_4)?HIGH:LOW; - } -#endif - - } - return LOW; -} - -void VRBRAINGPIO::write(uint8_t pin, uint8_t value) -{ - switch (pin) { - - case HAL_GPIO_A_LED_PIN: // Arming LED - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_GREEN); - } else { - ioctl(_led_fd, LED_ON, LED_GREEN); - } - break; - - case HAL_GPIO_B_LED_PIN: // not used yet - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_BLUE); - } else { - ioctl(_led_fd, LED_ON, LED_BLUE); - } - break; - - case HAL_GPIO_C_LED_PIN: // GPS LED - if (value == LOW) { - ioctl(_led_fd, LED_OFF, LED_RED); - } else { - ioctl(_led_fd, LED_ON, LED_RED); - } - break; - -#ifdef GPIO_SERVO_1 - case EXTERNAL_LED_GPS: - ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_1); - break; -#endif - -#ifdef GPIO_SERVO_2 - case EXTERNAL_LED_ARMED: - ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_2); - break; -#endif - -#ifdef GPIO_SERVO_3 - case EXTERNAL_RELAY1_PIN: - ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_3); - break; -#endif - -#ifdef GPIO_SERVO_4 - case EXTERNAL_RELAY2_PIN: - ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_4); - break; -#endif - - } -} - -void VRBRAINGPIO::toggle(uint8_t pin) -{ - write(pin, !read(pin)); -} - -/* Alternative interface: */ -AP_HAL::DigitalSource* VRBRAINGPIO::channel(uint16_t n) { - return new VRBRAINDigitalSource(0); -} - -/* - return true when USB connected - */ -bool VRBRAINGPIO::usb_connected(void) -{ - /* - we use a combination of voltage on the USB connector and the - open of the /dev/ttyACM0 character device. This copes with - systems where the VBUS may go high even with no USB connected - (such as AUAV-X2) - */ - return stm32_gpioread(GPIO_OTGFS_VBUS) && _usb_connected; -} - - -VRBRAINDigitalSource::VRBRAINDigitalSource(uint8_t v) : - _v(v) -{} - -void VRBRAINDigitalSource::mode(uint8_t output) -{} - -uint8_t VRBRAINDigitalSource::read() { - return _v; -} - -void VRBRAINDigitalSource::write(uint8_t value) { - _v = value; -} - -void VRBRAINDigitalSource::toggle() { - _v = !_v; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.h b/libraries/AP_HAL_VRBRAIN/GPIO.h deleted file mode 100644 index f6422383cd..0000000000 --- a/libraries/AP_HAL_VRBRAIN/GPIO.h +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - # define HAL_GPIO_A_LED_PIN 25 - # define HAL_GPIO_B_LED_PIN 26 - # define HAL_GPIO_C_LED_PIN 27 - # define EXTERNAL_LED_GPS 28 - # define EXTERNAL_LED_ARMED 29 - # define EXTERNAL_LED_MOTOR1 30 - # define EXTERNAL_LED_MOTOR2 31 - - # define EXTERNAL_RELAY1_PIN 34 - # define EXTERNAL_RELAY2_PIN 33 - - # define HAL_GPIO_LED_ON HIGH - # define HAL_GPIO_LED_OFF LOW -#endif - -class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO { -public: - VRBRAINGPIO(); - void init() override; - void pinMode(uint8_t pin, uint8_t output) override; - uint8_t read(uint8_t pin) override; - void write(uint8_t pin, uint8_t value) override; - void toggle(uint8_t pin) override; - - /* Alternative interface: */ - AP_HAL::DigitalSource* channel(uint16_t n) override; - - /* return true if USB cable is connected */ - bool usb_connected(void) override; - - // used by UART code to avoid a hw bug in the AUAV-X2 - void set_usb_connected(void) { _usb_connected = true; } - -private: - int _led_fd = -1; - int _tone_alarm_fd = -1; - int _gpio_fmu_fd = -1; - - bool _usb_connected = false; -}; - -class VRBRAIN::VRBRAINDigitalSource : public AP_HAL::DigitalSource { -public: - VRBRAINDigitalSource(uint8_t v); - void mode(uint8_t output); - uint8_t read(); - void write(uint8_t value); - void toggle(); -private: - uint8_t _v; -}; diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp deleted file mode 100644 index 10196b3d25..0000000000 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp +++ /dev/null @@ -1,390 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include -#include - -#include "AP_HAL_VRBRAIN.h" -#include "AP_HAL_VRBRAIN_Namespace.h" -#include "HAL_VRBRAIN_Class.h" -#include "Scheduler.h" -#include "UARTDriver.h" -#include "Storage.h" -#include "RCInput.h" -#include "RCOutput.h" -#include "AnalogIn.h" -#include "Util.h" -#include "GPIO.h" -#include "I2CDevice.h" -#include "SPIDevice.h" - -#if HAL_WITH_UAVCAN -#include "CAN.h" -#endif - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace VRBRAIN; -using namespace ap; - -//static Empty::GPIO gpioDriver; - -static VRBRAINScheduler schedulerInstance; -static VRBRAINStorage storageDriver; -static VRBRAINRCInput rcinDriver; -static VRBRAINRCOutput rcoutDriver; -static VRBRAINAnalogIn analogIn; -static VRBRAINUtil utilInstance; -static VRBRAINGPIO gpioDriver; - -static VRBRAIN::I2CDeviceManager i2c_mgr_instance; -static VRBRAIN::SPIDeviceManager spi_mgr_instance; - -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/null" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52E) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#else -#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" -#define UARTB_DEFAULT_DEVICE "/dev/ttyS0" -#define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" -#define UARTE_DEFAULT_DEVICE "/dev/null" -#define UARTF_DEFAULT_DEVICE "/dev/null" -#endif - -// 3 UART drivers, for GPS plus two mavlink-enabled devices -static VRBRAINUARTDriver uartADriver(UARTA_DEFAULT_DEVICE, "APM_uartA"); -static VRBRAINUARTDriver uartBDriver(UARTB_DEFAULT_DEVICE, "APM_uartB"); -static VRBRAINUARTDriver uartCDriver(UARTC_DEFAULT_DEVICE, "APM_uartC"); -static VRBRAINUARTDriver uartDDriver(UARTD_DEFAULT_DEVICE, "APM_uartD"); -static VRBRAINUARTDriver uartEDriver(UARTE_DEFAULT_DEVICE, "APM_uartE"); -static VRBRAINUARTDriver uartFDriver(UARTF_DEFAULT_DEVICE, "APM_uartF"); - -HAL_VRBRAIN::HAL_VRBRAIN() : - AP_HAL::HAL( - &uartADriver, /* uartA */ - &uartBDriver, /* uartB */ - &uartCDriver, /* uartC */ - &uartDDriver, /* uartD */ - &uartEDriver, /* uartE */ - &uartFDriver, /* uartF */ - nullptr, /* no uartG */ - &i2c_mgr_instance, - &spi_mgr_instance, - &analogIn, /* analogin */ - &storageDriver, /* storage */ - &uartADriver, /* console */ - &gpioDriver, /* gpio */ - &rcinDriver, /* rcinput */ - &rcoutDriver, /* rcoutput */ - &schedulerInstance, /* scheduler */ - &utilInstance, /* util */ - nullptr, /* no onboard optical flow */ - nullptr) /* CAN */ -{} - -bool _vrbrain_thread_should_exit = false; /**< Daemon exit flag */ -static bool thread_running = false; /**< Daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ -bool vrbrain_ran_overtime; - -extern const AP_HAL::HAL& hal; - -/* - set the priority of the main APM task - */ -void hal_vrbrain_set_priority(uint8_t priority) -{ - struct sched_param param; - param.sched_priority = priority; - sched_setscheduler(daemon_task, SCHED_FIFO, ¶m); -} - -/* - this is called when loop() takes more than 1 second to run. If that - happens then something is blocking for a long time in the main - sketch - probably waiting on a low priority driver. Set the priority - of the APM task low to let the driver run. - */ -static void loop_overtime(void *) -{ - hal_vrbrain_set_priority(APM_OVERTIME_PRIORITY); - vrbrain_ran_overtime = true; -} - -static AP_HAL::HAL::Callbacks* g_callbacks; - -static int main_loop(int argc, char **argv) -{ - hal.uartA->begin(115200); - hal.uartB->begin(38400); - hal.uartC->begin(57600); - hal.uartD->begin(57600); - hal.uartE->begin(57600); - hal.scheduler->init(); - - // init the I2C wrapper class - VRBRAIN_I2C::init_lock(); - - /* - run setup() at low priority to ensure CLI doesn't hang the - system, and to allow initial sensor read loops to run - */ - hal_vrbrain_set_priority(APM_STARTUP_PRIORITY); - - schedulerInstance.hal_initialized(); - - g_callbacks->setup(); - hal.scheduler->system_initialized(); - - perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop"); - perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun"); - struct hrt_call loop_overtime_call; - - thread_running = true; - - /* - switch to high priority for main loop - */ - hal_vrbrain_set_priority(APM_MAIN_PRIORITY); - - while (!_vrbrain_thread_should_exit) { - perf_begin(perf_loop); - - /* - this ensures a tight loop waiting on a lower priority driver - will eventually give up some time for the driver to run. It - will only ever be called if a loop() call runs for more than - 0.1 second - */ - hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, nullptr); - - g_callbacks->loop(); - - if (vrbrain_ran_overtime) { - /* - we ran over 1s in loop(), and our priority was lowered - to let a driver run. Set it back to high priority now. - */ - hal_vrbrain_set_priority(APM_MAIN_PRIORITY); - perf_count(perf_overrun); - vrbrain_ran_overtime = false; - } - - perf_end(perf_loop); - - /* - give up 250 microseconds of time, to ensure drivers get a - chance to run. This relies on the accurate semaphore wait - using hrt in semaphore.cpp - */ - hal.scheduler->delay_microseconds(250); - } - thread_running = false; - return 0; -} - -static void usage(void) -{ - printf("Usage: %s [options] {start,stop,status}\n", SKETCHNAME); - printf("Options:\n"); - printf("\t-d DEVICE set terminal device (default %s)\n", UARTA_DEFAULT_DEVICE); - printf("\t-d2 DEVICE set second terminal device (default %s)\n", UARTC_DEFAULT_DEVICE); - printf("\t-d3 DEVICE set 3rd terminal device (default %s)\n", UARTD_DEFAULT_DEVICE); - printf("\t-d4 DEVICE set 2nd GPS device (default %s)\n", UARTE_DEFAULT_DEVICE); - printf("\t-d5 DEVICE set uartF (default %s)\n", UARTF_DEFAULT_DEVICE); - printf("\n"); -} - - -void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const -{ - int i; - const char *deviceA = UARTA_DEFAULT_DEVICE; - const char *deviceC = UARTC_DEFAULT_DEVICE; - const char *deviceD = UARTD_DEFAULT_DEVICE; - const char *deviceE = UARTE_DEFAULT_DEVICE; - const char *deviceF = UARTF_DEFAULT_DEVICE; - - if (argc < 1) { - printf("%s: missing command (try '%s start')", - SKETCHNAME, SKETCHNAME); - usage(); - exit(1); - } - - assert(callbacks); - g_callbacks = callbacks; - - for (i=0; i i + 1) { - deviceA = strdup(argv[i+1]); - } else { - printf("missing parameter to -d DEVICE\n"); - usage(); - exit(1); - } - } - - if (strcmp(argv[i], "-d2") == 0) { - // set uartC terminal device - if (argc > i + 1) { - deviceC = strdup(argv[i+1]); - } else { - printf("missing parameter to -d2 DEVICE\n"); - usage(); - exit(1); - } - } - - if (strcmp(argv[i], "-d3") == 0) { - // set uartD terminal device - if (argc > i + 1) { - deviceD = strdup(argv[i+1]); - } else { - printf("missing parameter to -d3 DEVICE\n"); - usage(); - exit(1); - } - } - - if (strcmp(argv[i], "-d4") == 0) { - // set uartE 2nd GPS device - if (argc > i + 1) { - deviceE = strdup(argv[i+1]); - } else { - printf("missing parameter to -d4 DEVICE\n"); - usage(); - exit(1); - } - } - - if (strcmp(argv[i], "-d5") == 0) { - // set uartF - if (argc > i + 1) { - deviceF = strdup(argv[i+1]); - } else { - printf("missing parameter to -d5 DEVICE\n"); - usage(); - exit(1); - } - } - } - - usage(); - exit(1); -} - -const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_VRBRAIN hal_vrbrain; - return hal_vrbrain; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h deleted file mode 100644 index 9a8a11d972..0000000000 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_HAL_VRBRAIN.h" -#include "AP_HAL_VRBRAIN_Namespace.h" -#include -#include - -class HAL_VRBRAIN : public AP_HAL::HAL { -public: - HAL_VRBRAIN(); - void run(int argc, char* const argv[], Callbacks* callbacks) const override; -}; - -void hal_vrbrain_set_priority(uint8_t priority); - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp b/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp deleted file mode 100644 index ca9dfd71bd..0000000000 --- a/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp +++ /dev/null @@ -1,192 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ -#include "I2CDevice.h" - -#include - -#include "Util.h" -#include "Scheduler.h" - -namespace VRBRAIN { - -uint8_t VRBRAIN::VRBRAIN_I2C::instance; -pthread_mutex_t VRBRAIN::VRBRAIN_I2C::instance_lock; - -DeviceBus I2CDevice::businfo[I2CDevice::num_buses]; - -/* - constructor for I2C wrapper class - */ -VRBRAIN_I2C::VRBRAIN_I2C(uint8_t bus) : - I2C(devname, devpath, map_bus_number(bus), 0, 100000UL) -{} - -/* - map ArduPilot bus numbers to VRBRAIN bus numbers - */ -uint8_t VRBRAIN_I2C::map_bus_number(uint8_t bus) const -{ - switch (bus) { - case 0: - // map to internal bus -#ifdef PX4_I2C_BUS_ONBOARD - return PX4_I2C_BUS_ONBOARD; -#else - return 0; -#endif - - case 1: - // map to expansion bus -#ifdef PX4_I2C_BUS_EXPANSION - return PX4_I2C_BUS_EXPANSION; -#else - return 1; -#endif - case 2: - // map to expansion bus 2 -#ifdef PX4_I2C_BUS_EXPANSION1 - return PX4_I2C_BUS_EXPANSION1; -#else - return 2; -#endif - } - // default to bus 1 - return 1; -} - -/* - implement wrapper for VRBRAIN I2C driver - */ -bool VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers) -{ - set_address(address); - if (!init_done) { - if (pthread_mutex_lock(&instance_lock) != 0) { - return false; - } - init_done = true; - // we do late init() so we can setup the device paths - - snprintf(devname, sizeof(devname), "AP_I2C_%u", instance); - snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance); - init_ok = (init() == OK); - if (init_ok) { - instance++; - } - pthread_mutex_unlock(&instance_lock); - } - if (!init_ok) { - return false; - } - - if (split_transfers) { - /* - splitting the transfer() into two pieces avoids a stop condition - with SCL low which is not supported on some devices (such as - LidarLite blue label) - */ - if (send && send_len) { - if (transfer(send, send_len, nullptr, 0) != OK) { - return false; - } - } - if (recv && recv_len) { - if (transfer(nullptr, 0, recv, recv_len) != OK) { - return false; - } - } - } else { - // combined transfer - if (transfer(send, send_len, recv, recv_len) != OK) { - return false; - } - } - return true; -} - -I2CDevice::I2CDevice(uint8_t bus, uint8_t address) : - _busnum(bus), - _vrbraindev(_busnum), - _address(address) -{ - set_device_bus(bus); - set_device_address(address); - asprintf(&pname, "I2C:%u:%02x", - (unsigned)bus, (unsigned)address); - perf = perf_alloc(PC_ELAPSED, pname); -} - -I2CDevice::~I2CDevice() -{ - printf("I2C device bus %u address 0x%02x closed\n", - (unsigned)_busnum, (unsigned)_address); - perf_free(perf); - free(pname); -} - -bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, - uint8_t *recv, uint32_t recv_len) -{ - perf_begin(perf); - bool ret = _vrbraindev.do_transfer(_address, send, send_len, recv, recv_len, _split_transfers); - perf_end(perf); - return ret; -} - -bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, - uint32_t recv_len, uint8_t times) -{ - return false; -} - - -/* - register a periodic callback -*/ -AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) -{ - if (_busnum >= num_buses) { - return nullptr; - } - struct DeviceBus &binfo = businfo[_busnum]; - return binfo.register_periodic_callback(period_usec, cb, this); -} - - -/* - adjust a periodic callback -*/ -bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) -{ - if (_busnum >= num_buses) { - return false; - } - - struct DeviceBus &binfo = businfo[_busnum]; - - return binfo.adjust_timer(h, period_usec); -} - -AP_HAL::OwnPtr -I2CDeviceManager::get_device(uint8_t bus, uint8_t address, - uint32_t bus_clock, - bool use_smbus, - uint32_t timeout_ms) -{ - auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address)); - return dev; -} - -} diff --git a/libraries/AP_HAL_VRBRAIN/I2CDevice.h b/libraries/AP_HAL_VRBRAIN/I2CDevice.h deleted file mode 100644 index 0c10f5be6e..0000000000 --- a/libraries/AP_HAL_VRBRAIN/I2CDevice.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright (C) 2015-2016 Intel Corporation. All rights reserved. - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ -#pragma once - -#include - -#include -#include -#include -#include "Semaphores.h" -#include "I2CWrapper.h" -#include "Device.h" - -namespace VRBRAIN { - -class I2CDevice : public AP_HAL::I2CDevice { -public: - static I2CDevice *from(AP_HAL::I2CDevice *dev) - { - return static_cast(dev); - } - - I2CDevice(uint8_t bus, uint8_t address); - ~I2CDevice(); - - /* See AP_HAL::I2CDevice::set_address() */ - void set_address(uint8_t address) override { _address = address; } - - /* See AP_HAL::I2CDevice::set_retries() */ - void set_retries(uint8_t retries) override { _vrbraindev.set_retries(retries); } - - /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ - bool set_speed(enum Device::Speed speed) override { return true; } - - /* See AP_HAL::Device::transfer() */ - bool transfer(const uint8_t *send, uint32_t send_len, - uint8_t *recv, uint32_t recv_len) override; - - bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, - uint32_t recv_len, uint8_t times) override; - - /* See AP_HAL::Device::register_periodic_callback() */ - AP_HAL::Device::PeriodicHandle register_periodic_callback( - uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; - - /* See AP_HAL::Device::adjust_periodic_callback() */ - bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; - - AP_HAL::Semaphore* get_semaphore() override { - // if asking for invalid bus number use bus 0 semaphore - return &businfo[_busnum(i2c_mgr); - } - - AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address, - uint32_t bus_clock=400000, - bool use_smbus = false, - uint32_t timeout_ms=4) override; -}; - -} diff --git a/libraries/AP_HAL_VRBRAIN/I2CWrapper.h b/libraries/AP_HAL_VRBRAIN/I2CWrapper.h deleted file mode 100644 index 2a8b1d14fa..0000000000 --- a/libraries/AP_HAL_VRBRAIN/I2CWrapper.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include "board_config.h" -#include -#include "AP_HAL_VRBRAIN.h" - -extern const AP_HAL::HAL& hal; -/* - wrapper class for I2C to expose protected functions from PX4Firmware - */ -class VRBRAIN::VRBRAIN_I2C : public device::I2C { -public: - VRBRAIN_I2C(uint8_t bus); - bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers); - - void set_retries(uint8_t retries) { - _retries = retries; - } - - uint8_t map_bus_number(uint8_t bus) const; - - // setup instance_lock - static void init_lock(void) { - pthread_mutex_init(&instance_lock, nullptr); - } - -private: - static uint8_t instance; - static pthread_mutex_t instance_lock; - bool init_done; - bool init_ok; - char devname[10]; - char devpath[14]; -}; - - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp b/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp deleted file mode 100644 index 01d1f8a4ca..0000000000 --- a/libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - implementation of NSH shell as a stream, for use in nsh over MAVLink - - See GCS_serial_control.cpp for usage - */ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include -#include -#include -#include -#include -#include -#include "Scheduler.h" - -extern const AP_HAL::HAL& hal; - -#include "Util.h" -using namespace VRBRAIN; - -void NSHShellStream::shell_thread(void *arg) -{ - NSHShellStream *nsh = (NSHShellStream *)arg; - close(0); - close(1); - close(2); - dup2(nsh->child.in, 0); - dup2(nsh->child.out, 1); - dup2(nsh->child.out, 2); - - nsh_consolemain(0, nullptr); - - nsh->shell_stdin = -1; - nsh->shell_stdout = -1; - nsh->child.in = -1; - nsh->child.out = -1; -} - -void NSHShellStream::start_shell(void) -{ - if (hal.util->available_memory() < 8192) { - if (!showed_memory_warning) { - showed_memory_warning = true; - hal.console->printf("Not enough memory for shell\n"); - } - return; - } - if (hal.util->get_soft_armed()) { - if (!showed_armed_warning) { - showed_armed_warning = true; - hal.console->printf("Disarm to start nsh shell\n"); - } - // don't allow shell start when armed - return; - } - - int p1[2], p2[2]; - - if (pipe(p1) != 0) { - return; - } - if (pipe(p2) != 0) { - return; - } - shell_stdin = p1[0]; - shell_stdout = p2[1]; - child.in = p2[0]; - child.out = p1[1]; - - pthread_attr_t thread_attr; - struct sched_param param; - - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 2048); - - param.sched_priority = APM_SHELL_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&shell_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::NSHShellStream::shell_thread, this); -} - -size_t NSHShellStream::write(uint8_t b) -{ - if (shell_stdout == -1) { - start_shell(); - } - if (shell_stdout != -1) { - return ::write(shell_stdout, &b, 1); - } - return 0; -} - -size_t NSHShellStream::write(const uint8_t *buffer, size_t size) -{ - size_t ret = 0; - while (size > 0) { - if (write(*buffer++) != 1) { - break; - } - ret++; - size--; - } - return ret; -} - -int16_t NSHShellStream::read() -{ - if (shell_stdin == -1) { - start_shell(); - } - if (shell_stdin != -1) { - uint8_t b; - if (::read(shell_stdin, &b, 1) == 1) { - return b; - } - } - return -1; -} - -uint32_t NSHShellStream::available() -{ - uint32_t ret = 0; - if (ioctl(shell_stdin, FIONREAD, (unsigned long)&ret) == OK) { - return ret; - } - return 0; -} - -uint32_t NSHShellStream::txspace() -{ - uint32_t ret = 0; - if (ioctl(shell_stdout, FIONWRITE, (unsigned long)&ret) == OK) { - return ret; - } - return 0; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.cpp b/libraries/AP_HAL_VRBRAIN/RCInput.cpp deleted file mode 100644 index 55297bba01..0000000000 --- a/libraries/AP_HAL_VRBRAIN/RCInput.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "RCInput.h" -#include -#include -#include -#include -#include - -#include - -using namespace VRBRAIN; - -extern const AP_HAL::HAL& hal; - -void VRBRAINRCInput::init() -{ - _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin"); - _rc_sub = orb_subscribe(ORB_ID(input_rc)); - if (_rc_sub == -1) { - AP_HAL::panic("Unable to subscribe to input_rc"); - } - pthread_mutex_init(&rcin_mutex, nullptr); -} - -bool VRBRAINRCInput::new_input() -{ - pthread_mutex_lock(&rcin_mutex); - bool valid = _rcin.timestamp_last_signal != _last_read; - if (_rcin.rc_failsafe) { - // don't consider input valid if we are in RC failsafe. - valid = false; - } - _last_read = _rcin.timestamp_last_signal; - pthread_mutex_unlock(&rcin_mutex); - if (_rcin.input_source != last_input_source) { - gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", input_source_name(_rcin.input_source)); - last_input_source = _rcin.input_source; - } - return valid; -} - -uint8_t VRBRAINRCInput::num_channels() -{ - pthread_mutex_lock(&rcin_mutex); - uint8_t n = _rcin.channel_count; - pthread_mutex_unlock(&rcin_mutex); - return n; -} - -uint16_t VRBRAINRCInput::read(uint8_t ch) -{ - if (ch >= MIN(RC_INPUT_MAX_CHANNELS, _rcin.channel_count)) { - return 0; - } - pthread_mutex_lock(&rcin_mutex); - uint16_t v = _rcin.values[ch]; - pthread_mutex_unlock(&rcin_mutex); - return v; -} - -uint8_t VRBRAINRCInput::read(uint16_t* periods, uint8_t len) -{ - if (len > RC_INPUT_MAX_CHANNELS) { - len = RC_INPUT_MAX_CHANNELS; - } - for (uint8_t i = 0; i < len; i++){ - periods[i] = read(i); - } - return len; -} - -const char *VRBRAINRCInput::input_source_name(uint8_t id) const -{ - switch(id) { - case input_rc_s::RC_INPUT_SOURCE_UNKNOWN: return "UNKNOWN"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM: return "PX4FMU_PPM"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM: return "PX4IO_PPM"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM: return "PX4IO_SPEKTRUM"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS: return "PX4IO_SBUS"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24: return "PX4IO_ST24"; - case input_rc_s::RC_INPUT_SOURCE_MAVLINK: return "MAVLINK"; - case input_rc_s::RC_INPUT_SOURCE_QURT: return "QURT"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SPEKTRUM: return "PX4FMU_SPEKTRUM"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS: return "PX4FMU_SBUS"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24: return "PX4FMU_ST24"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD: return "PX4FMU_SUMD"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM: return "PX4FMU_DSM"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD: return "PX4IO_SUMD"; - case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SRXL: return "PX4FMU_SRXL"; - case input_rc_s::RC_INPUT_SOURCE_PX4IO_SRXL: return "PX4IO_SRXL"; - default: return "ERROR"; - } -} - - -void VRBRAINRCInput::_timer_tick(void) -{ - perf_begin(_perf_rcin); - bool rc_updated = false; - if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) { - pthread_mutex_lock(&rcin_mutex); - orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin); - if (_rcin.rssi != 0 || _rssi != -1) { - // always zero means not supported - _rssi = _rcin.rssi; - } - pthread_mutex_unlock(&rcin_mutex); - } - // note, we rely on the vehicle code checking new_input() - // and a timeout for the last valid input to handle failsafe - perf_end(_perf_rcin); -} - -bool VRBRAINRCInput::rc_bind(int dsmMode) -{ - - - - - - - - - - - - - - - - - return true; -} - -#endif diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.h b/libraries/AP_HAL_VRBRAIN/RCInput.h deleted file mode 100644 index 875a82fa95..0000000000 --- a/libraries/AP_HAL_VRBRAIN/RCInput.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include -#include - - -#ifndef RC_INPUT_MAX_CHANNELS -#define RC_INPUT_MAX_CHANNELS 18 -#endif - -class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput { -public: - void init() override; - bool new_input() override; - uint8_t num_channels() override; - uint16_t read(uint8_t ch) override; - uint8_t read(uint16_t* periods, uint8_t len) override; - - int16_t get_rssi(void) override { - return _rssi; - } - - void _timer_tick(void); - - bool rc_bind(int dsmMode) override; - -private: - struct rc_input_values _rcin; - int _rc_sub; - uint64_t _last_read; - perf_counter_t _perf_rcin; - pthread_mutex_t rcin_mutex; - int16_t _rssi = -1; - - uint8_t last_input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; - const char *input_source_name(uint8_t id) const; -}; diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp deleted file mode 100644 index 3843793565..0000000000 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp +++ /dev/null @@ -1,682 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "RCOutput.h" - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -extern const AP_HAL::HAL& hal; - -using namespace VRBRAIN; - -/* - enable RCOUT_DEBUG_LATENCY to measure output latency using a logic - analyser. AUX6 will go high during the cork/push output. - */ -#define RCOUT_DEBUG_LATENCY 0 - -void VRBRAINRCOutput::init() -{ - _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); - _pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR); - if (_pwm_fd == -1) { - AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH); - } - if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) { - hal.console->printf("RCOutput: Unable to setup IO arming\n"); - } - if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) { - hal.console->printf("RCOutput: Unable to setup IO arming OK\n"); - } - - _rate_mask_main = 0; - _rate_mask_alt = 0; - _alt_fd = -1; - _servo_count = 0; - _alt_servo_count = 0; - - if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) { - hal.console->printf("RCOutput: Unable to get servo count\n"); - return; - } - - for (uint8_t i=0; iprintf("RCOutput: Unable to setup alt IO arming\n"); - return; - } - if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) { - hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n"); - return; - } - if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) { - hal.console->printf("RCOutput: Unable to get servo count\n"); - } -} - -/* - set output frequency on outputs associated with fd - */ -void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask) -{ - if (_output_mode == MODE_PWM_BRUSHED) { - freq_hz /= 8; // divide by 8 for 8MHz clock - // remember max period - _period_max = 1000000UL/freq_hz; - } - - // we can't set this per channel - if (freq_hz > 50 || freq_hz == 1) { - // we're being asked to set the alt rate - if (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125) { - /* - set a 1Hz update for oneshot. This periodic output will - never actually trigger, instead we will directly trigger - the pulse after each push() - */ - freq_hz = 1; - } - //::printf("SET_UPDATE_RATE %d %u\n", fd, (unsigned)freq_hz); - if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) { - hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz); - return; - } - _freq_hz = freq_hz; - } - - /* work out the new rate mask. The outputs have 4 groups of servos. - - Group 0: channels 0 1 2 3 - Group 1: channels 4 5 - Group 2: channels 6 7 - Group 3: channels 8 9 10 - - Group 0: channels 0 1 2 - Group 1: channels 3 4 5 - Group 2: channels 6 7 - Group 3: channels 8 9 10 11 - - Channels within a group must be set to the same rate. - - For the moment we never set the channels above 8 to more than - 50Hz - */ - if (freq_hz > 50 || freq_hz == 1) { - // we are setting high rates on the given channels - rate_mask |= chmask & 0xFFF; -#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - if (rate_mask & 0x0F) { - rate_mask |= 0x0F; - } - if (rate_mask & 0x30) { - rate_mask |= 0x30; - } - if (rate_mask & 0xC0) { - rate_mask |= 0xC0; - } - if (rate_mask & 0x700) { - rate_mask |= 0x700; - } -#else - if (rate_mask & 0x07) { - rate_mask |= 0x07; - } - if (rate_mask & 0x38) { - rate_mask |= 0x38; - } - if (rate_mask & 0xC0) { - rate_mask |= 0xC0; - } - if (rate_mask & 0xF00) { - rate_mask |= 0xF00; - } -#endif - } else { - // we are setting low rates on the given channels -#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - if (chmask & 0x0F) { - rate_mask &= ~0x0F; - } - if (chmask & 0x30) { - rate_mask &= ~0x30; - } - if (chmask & 0xC0) { - rate_mask &= ~0xC0; - } - if (chmask & 0x700) { - rate_mask &= ~0x700; - } -#else - if (chmask & 0x07) { - rate_mask &= ~0x07; - } - if (chmask & 0x38) { - rate_mask &= ~0x38; - } - if (chmask & 0xC0) { - rate_mask &= ~0xC0; - } - if (chmask & 0xF00) { - rate_mask &= ~0xF00; - } -#endif - } - - if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, rate_mask) != 0) { - hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)rate_mask); - } - - if (_output_mode == MODE_PWM_BRUSHED) { - ioctl(fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); - } -} - -/* - set output frequency - */ -void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) -{ - if (freq_hz > 50 && (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125)) { - // rate is irrelevent in oneshot - return; - } - - // re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT - if (_pwm_fd != -1 && ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) { - hal.console->printf("RCOutput: Unable to get servo count\n"); - return; - } - - // greater than 400 doesn't give enough room at higher periods for - // the down pulse - if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) { - freq_hz = 400; - } - uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1); - uint32_t alt_mask = chmask >> _servo_count; - if (primary_mask && _pwm_fd != -1) { - set_freq_fd(_pwm_fd, primary_mask, freq_hz, _rate_mask_main); - } - if (alt_mask && _alt_fd != -1) { - set_freq_fd(_alt_fd, alt_mask, freq_hz, _rate_mask_alt); - } -} - -uint16_t VRBRAINRCOutput::get_freq(uint8_t ch) -{ - if (ch < _servo_count) { - if (_rate_mask_main & (1U<= VRBRAIN_NUM_OUTPUT_CHANNELS) { - return; - } - if (ch >= 8 && !(_enabled_channels & (1U<= VRBRAIN_NUM_OUTPUT_CHANNELS) { - return; - } - - _enabled_channels &= ~(1U<printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); - } -} - -void VRBRAINRCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us) -{ - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - for (uint8_t i=0; i<_servo_count; i++) { - if ((1UL<printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us); - } -} - -bool VRBRAINRCOutput::force_safety_on(void) -{ - _safety_state_request = AP_HAL::Util::SAFETY_DISARMED; - _safety_state_request_last_ms = 1; - return true; -} - -void VRBRAINRCOutput::force_safety_off(void) -{ - _safety_state_request = AP_HAL::Util::SAFETY_ARMED; - _safety_state_request_last_ms = 1; -} - -void VRBRAINRCOutput::force_safety_pending_requests(void) -{ - // check if there is a pending saftey_state change. If so (timer != 0) then set it. - uint32_t now = AP_HAL::millis(); - if (_safety_state_request_last_ms != 0 && - now - _safety_state_request_last_ms >= 100) { - if (hal.util->safety_switch_state() == _safety_state_request && - _safety_state_request_last_ms != 1) { - _safety_state_request_last_ms = 0; - } else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) { - // current != requested, set it - ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); - _safety_state_request_last_ms = now; - } else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) { - // current != requested, set it - ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - _safety_state_request_last_ms = now; - } - } -} - -void VRBRAINRCOutput::force_safety_no_wait(void) -{ - if (_safety_state_request_last_ms != 0) { - _safety_state_request_last_ms = 1; - force_safety_pending_requests(); - } -} - -void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us) -{ - if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) { - return; - } - if (!(_enabled_channels & (1U<= _max_channel) { - _max_channel = ch + 1; - } - - if (_output_mode == MODE_PWM_ONESHOT125) { - // we treat oneshot125 very simply on HAL_PX4, with 1us - // resolution. Correctly handling it would use a 125 nsec - // step size, to give the full 1000 steps - period_us /= 8; - } - - // keep unscaled value - _last_sent[ch] = period_us; - - if (_output_mode == MODE_PWM_BRUSHED) { - // map from the PWM range to 0 t0 100% duty cycle. For 16kHz - // this ends up being 0 to 500 pulse width in units of - // 125usec. - if (period_us <= _esc_pwm_min) { - period_us = 0; - } else if (period_us >= _esc_pwm_max) { - period_us = _period_max; - } else { - uint32_t pdiff = period_us - _esc_pwm_min; - period_us = pdiff*_period_max/(_esc_pwm_max - _esc_pwm_min); - } - } - - /* - only mark an update is needed if there has been a change, or we - are in oneshot mode. In oneshot mode we always need to send the - output - */ - if (period_us != _period[ch] || - _output_mode == MODE_PWM_ONESHOT || - _output_mode == MODE_PWM_ONESHOT125) { - _period[ch] = period_us; - _need_update = true; -// up_pwm_servo_set(ch, period_us); - } -} - -uint16_t VRBRAINRCOutput::read(uint8_t ch) -{ - if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) { - return 0; - } - // if px4io has given us a value for this channel use that, - // otherwise use the value we last sent. This makes it easier to - // observe the behaviour of failsafe in px4io - for (uint8_t i=0; i= 0 && - ch < _outputs[i].outputs.noutputs && - _outputs[i].outputs.output[ch] > 0) { - return _outputs[i].outputs.output[ch]; - } - } - return _period[ch]; -} - -void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len) -{ - for (uint8_t i=0; i= VRBRAIN_NUM_OUTPUT_CHANNELS) { - return 0; - } - - return _last_sent[ch]; -} - -void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len) -{ - for (uint8_t i=0; i 50000) { - _need_update = true; - } - - // check for PWM count changing. This can happen then the user changes BRD_PWM_COUNT - if (now - _last_config_us > 1000000) { - if (_pwm_fd != -1) { - ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count); - } - if (_alt_fd != -1) { - ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count); - } - _last_config_us = now; - } - - if (_need_update && _pwm_fd != -1) { - _need_update = false; - perf_begin(_perf_rcout); - uint8_t to_send = _max_channel<_servo_count?_max_channel:_servo_count; - if (_sbus_enabled) { - to_send = _max_channel; - } - if (to_send > 0) { - for (int i=to_send-1; i >= 0; i--) { - if (_period[i] == PWM_IGNORE_THIS_CHANNEL) { - to_send = i; - } else { - break; - } - } - } - if (to_send > 0) { - _arm_actuators(true); - - ::write(_pwm_fd, _period, to_send*sizeof(_period[0])); - } - if (_max_channel > _servo_count) { - // maybe send updates to alt_fd - if (_alt_fd != -1 && _alt_servo_count > 0) { - uint8_t n = _max_channel - _servo_count; - if (n > _alt_servo_count) { - n = _alt_servo_count; - } - if (n > 0) { - ::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0])); - } - } - } - - perf_end(_perf_rcout); - _last_output = now; - } - -update_pwm: - for (uint8_t i=0; i= 0 && - orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 && - rc_updated) { - orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs); - } - } - -} - -void VRBRAINRCOutput::cork() -{ -#if RCOUT_DEBUG_LATENCY - hal.gpio->pinMode(55, HAL_GPIO_OUTPUT); - hal.gpio->write(55, 1); -#endif - _corking = true; -} - -void VRBRAINRCOutput::push() -{ -#if RCOUT_DEBUG_LATENCY - hal.gpio->pinMode(55, HAL_GPIO_OUTPUT); - hal.gpio->write(55, 0); -#endif - if (_corking) { - _corking = false; - if (_output_mode == MODE_PWM_ONESHOT || _output_mode == MODE_PWM_ONESHOT125) { - // run timer immediately in oneshot mode - _send_outputs(); - } - } -} - -void VRBRAINRCOutput::timer_tick(void) -{ - if (_output_mode != MODE_PWM_ONESHOT && _output_mode != MODE_PWM_ONESHOT125 && !_corking) { - /* in oneshot mode the timer does nothing as the outputs are - * sent from push() */ - _send_outputs(); - } - - force_safety_pending_requests(); -} - -/* - enable sbus output - */ -bool VRBRAINRCOutput::enable_px4io_sbus_out(uint16_t rate_hz) -{ - int fd = open("/dev/px4io", 0); - if (fd == -1) { - return false; - } - for (uint8_t tries=0; tries<10; tries++) { - if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) { - continue; - } - if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate_hz) != 0) { - continue; - } - close(fd); - _sbus_enabled = true; - return true; - } - close(fd); - return false; -} - -/* - setup output mode - */ -void VRBRAINRCOutput::set_output_mode(uint16_t mask, enum output_mode mode) -{ - if (_output_mode == mode) { - // no change - return; - } - if (mode == MODE_PWM_ONESHOT || mode == MODE_PWM_ONESHOT125) { - // when using oneshot we don't want the regular pulses. The - // best we can do with the current PX4Firmware code is ask for - // 1Hz. This does still produce pulses, but the trigger calls - // mean the timer is constantly reset, so no pulses are - // produced except when triggered by push() when the main loop - // is running - set_freq_fd(_pwm_fd, _rate_mask_main, 1, _rate_mask_main); - if (_alt_fd != -1) { - set_freq_fd(_alt_fd, _rate_mask_alt, 1, _rate_mask_alt); - } - } - _output_mode = mode; - switch (_output_mode) { - case MODE_PWM_ONESHOT: - case MODE_PWM_ONESHOT125: - ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1); - if (_alt_fd != -1) { - ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1); - } - break; - case MODE_PWM_DSHOT150: - case MODE_PWM_DSHOT300: - case MODE_PWM_DSHOT600: - case MODE_PWM_DSHOT1200: - // treat as normal PWM for now - hal.console->printf("DShot not supported\n"); - FALLTHROUGH; - case MODE_PWM_NORMAL: - ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0); - if (_alt_fd != -1) { - ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0); - } - break; - case MODE_PWM_BRUSHED: - // setup an 8MHz clock. This has the effect of scaling all outputs by 8x - ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); - if (_alt_fd != -1) { - ioctl(_alt_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); - } - break; - } -} - - -// set default output update rate -void VRBRAINRCOutput::set_default_rate(uint16_t rate_hz) -{ - if (rate_hz != _default_rate_hz) { - // set servo update rate for first 8 pwm channels - ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); - if (_alt_fd != -1) { - // set servo update rate for auxiliary channels - ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); - } - _default_rate_hz = rate_hz; - } -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.h b/libraries/AP_HAL_VRBRAIN/RCOutput.h deleted file mode 100644 index 8a2bd34e89..0000000000 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.h +++ /dev/null @@ -1,86 +0,0 @@ -#pragma once - -#include "AP_HAL_VRBRAIN.h" -#include -#include -#include - -#define VRBRAIN_NUM_OUTPUT_CHANNELS 16 - -class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput -{ -public: - void init() override; - void set_freq(uint32_t chmask, uint16_t freq_hz) override; - uint16_t get_freq(uint8_t ch) override; - void enable_ch(uint8_t ch) override; - void disable_ch(uint8_t ch) override; - void write(uint8_t ch, uint16_t period_us) override; - uint16_t read(uint8_t ch) override; - void read(uint16_t* period_us, uint8_t len) override; - uint16_t read_last_sent(uint8_t ch) override; - void read_last_sent(uint16_t* period_us, uint8_t len) override; - void set_safety_pwm(uint32_t chmask, uint16_t period_us) override; - void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override; - bool force_safety_on(void) override; - void force_safety_off(void) override; - void force_safety_no_wait(void) override; - void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override { - _esc_pwm_min = min_pwm; - _esc_pwm_max = max_pwm; - } - float scale_esc_to_unity(uint16_t pwm) override { - return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0; - } - void cork(); - void push(); - - void set_output_mode(uint16_t mask, enum output_mode mode) override; - - void timer_tick(void) override; - bool enable_px4io_sbus_out(uint16_t rate_hz) override; - - // set default output update rate - void set_default_rate(uint16_t rate_hz) override; - -private: - int _pwm_fd; - int _alt_fd; - uint16_t _freq_hz; - uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS]; - // we keep the last_sent value separately, as we need to keep the unscaled - // value for systems with brushed motors which scale outputs - uint16_t _last_sent[VRBRAIN_NUM_OUTPUT_CHANNELS]; - volatile uint8_t _max_channel; - volatile bool _need_update; - bool _sbus_enabled:1; - perf_counter_t _perf_rcout; - uint32_t _last_output; - uint32_t _last_config_us; - unsigned _servo_count; - unsigned _alt_servo_count; - uint32_t _rate_mask_main; - uint32_t _rate_mask_alt; - uint16_t _enabled_channels; - uint32_t _period_max; - struct { - int pwm_sub; - actuator_outputs_s outputs; - } _outputs[ORB_MULTI_MAX_INSTANCES] {}; - actuator_armed_s _armed; - - orb_advert_t _actuator_armed_pub; - uint16_t _esc_pwm_min; - uint16_t _esc_pwm_max; - - void _init_alt_channels(void); - void _arm_actuators(bool arm); - void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask); - bool _corking; - enum output_mode _output_mode = MODE_PWM_NORMAL; - void _send_outputs(void); - enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE; - uint32_t _safety_state_request_last_ms = 0; - void force_safety_pending_requests(void); - uint16_t _default_rate_hz = 50; -}; diff --git a/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp b/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp deleted file mode 100644 index ca1b6c419a..0000000000 --- a/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp +++ /dev/null @@ -1,328 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ -#include "SPIDevice.h" - -#include -#include "board_config.h" -#include -#include -#include -#include -#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 -SPIDeviceManager::get_device(const char *name) -{ - /* Find the bus description in the table */ - uint8_t i; - for (i = 0; device_table[i].name; i++) { - if (strcmp(device_table[i].name, name) == 0) { - break; - } - } - if (device_table[i].name == nullptr) { - return AP_HAL::OwnPtr(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(new SPIDevice(*busp, desc)); -} - -} diff --git a/libraries/AP_HAL_VRBRAIN/SPIDevice.h b/libraries/AP_HAL_VRBRAIN/SPIDevice.h deleted file mode 100644 index 6c9f5d5c64..0000000000 --- a/libraries/AP_HAL_VRBRAIN/SPIDevice.h +++ /dev/null @@ -1,113 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published - * by the Free Software Foundation, either version 3 of the License, - * or (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ -#pragma once - -#include -#include -#include -#include -#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(spi_mgr); - } - - AP_HAL::OwnPtr get_device(const char *name); - -private: - static SPIDesc device_table[]; - SPIBus *buses; -}; - -} diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp deleted file mode 100644 index c2eb53d35c..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ /dev/null @@ -1,364 +0,0 @@ -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_HAL_VRBRAIN.h" -#include "Scheduler.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "UARTDriver.h" -#include "AnalogIn.h" -#include "Storage.h" -#include "RCOutput.h" -#include "RCInput.h" - -#include -#include - -using namespace VRBRAIN; - -extern const AP_HAL::HAL& hal; - -extern bool _vrbrain_thread_should_exit; - -VRBRAINScheduler::VRBRAINScheduler() : - _perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")), - _perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")), - _perf_storage_timer(perf_alloc(PC_ELAPSED, "APM_storage_timers")), - _perf_delay(perf_alloc(PC_ELAPSED, "APM_delay")) -{} - -void VRBRAINScheduler::init() -{ - _main_task_pid = getpid(); - - // setup the timer thread - this will call tasks at 1kHz - pthread_attr_t thread_attr; - struct sched_param param; - - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 2048); - - param.sched_priority = APM_TIMER_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_timer_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_timer_thread, this); - - // the UART thread runs at a medium priority - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 2048); - - param.sched_priority = APM_UART_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_uart_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_uart_thread, this); - - // the IO thread runs at lower priority - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 2048); - - param.sched_priority = APM_IO_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_io_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_io_thread, this); - - // the storage thread runs at just above IO priority - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 1024); - - param.sched_priority = APM_STORAGE_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this); -} - -/** - delay for a specified number of microseconds using a semaphore wait - */ -void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec) -{ - sem_t wait_semaphore; - struct hrt_call wait_call; - sem_init(&wait_semaphore, 0, 0); - memset(&wait_call, 0, sizeof(wait_call)); - hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore); - sem_wait(&wait_semaphore); -} - -void VRBRAINScheduler::delay_microseconds(uint16_t usec) -{ - perf_begin(_perf_delay); - delay_microseconds_semaphore(usec); - perf_end(_perf_delay); -} - -/* - wrapper around sem_post that boosts main thread priority - */ -static void sem_post_boost(sem_t *sem) -{ - hal_vrbrain_set_priority(APM_MAIN_PRIORITY_BOOST); - sem_post(sem); -} - -/* - return the main thread to normal priority - */ -static void set_normal_priority(void *sem) -{ - hal_vrbrain_set_priority(APM_MAIN_PRIORITY); -} - -/* - a variant of delay_microseconds that boosts priority to - APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC - microseconds when the time completes. This significantly improves - the regularity of timing of the main loop as it takes - */ -void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec) -{ - sem_t wait_semaphore; - static struct hrt_call wait_call; - sem_init(&wait_semaphore, 0, 0); - hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore); - sem_wait(&wait_semaphore); - hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, nullptr); -} - -void VRBRAINScheduler::delay(uint16_t ms) -{ - perf_begin(_perf_delay); - uint64_t start = AP_HAL::micros64(); - - while ((AP_HAL::micros64() - start)/1000 < ms && - !_vrbrain_thread_should_exit) { - delay_microseconds_semaphore(1000); - if (in_main_thread() && _min_delay_cb_ms <= ms) { - call_delay_cb(); - } - } - perf_end(_perf_delay); - if (_vrbrain_thread_should_exit) { - exit(1); - } -} - -void VRBRAINScheduler::register_timer_process(AP_HAL::MemberProc proc) -{ - for (uint8_t i = 0; i < _num_timer_procs; i++) { - if (_timer_proc[i] == proc) { - return; - } - } - - if (_num_timer_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) { - _timer_proc[_num_timer_procs] = proc; - _num_timer_procs++; - } else { - hal.console->printf("Out of timer processes\n"); - } -} - -void VRBRAINScheduler::register_io_process(AP_HAL::MemberProc proc) -{ - for (uint8_t i = 0; i < _num_io_procs; i++) { - if (_io_proc[i] == proc) { - return; - } - } - - if (_num_io_procs < VRBRAIN_SCHEDULER_MAX_TIMER_PROCS) { - _io_proc[_num_io_procs] = proc; - _num_io_procs++; - } else { - hal.console->printf("Out of IO processes\n"); - } -} - -void VRBRAINScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) -{ - _failsafe = failsafe; -} - -void VRBRAINScheduler::reboot(bool hold_in_bootloader) -{ - // disarm motors to ensure they are off during a bootloader upload - hal.rcout->force_safety_on(); - hal.rcout->force_safety_no_wait(); - - // delay to ensure the async force_saftey operation completes - delay(500); - - px4_systemreset(hold_in_bootloader); -} - -void VRBRAINScheduler::_run_timers() -{ - if (_in_timer_proc) { - return; - } - _in_timer_proc = true; - - // now call the timer based drivers - for (int i = 0; i < _num_timer_procs; i++) { - if (_timer_proc[i]) { - _timer_proc[i](); - } - } - - // and the failsafe, if one is setup - if (_failsafe != nullptr) { - _failsafe(); - } - - // process analog input - ((VRBRAINAnalogIn *)hal.analogin)->_timer_tick(); - - _in_timer_proc = false; -} - -extern bool vrbrain_ran_overtime; - -void *VRBRAINScheduler::_timer_thread(void *arg) -{ - VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; - uint32_t last_ran_overtime = 0; - - pthread_setname_np(pthread_self(), "apm_timer"); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - while (!_vrbrain_thread_should_exit) { - sched->delay_microseconds_semaphore(1000); - - // run registered timers - perf_begin(sched->_perf_timers); - sched->_run_timers(); - perf_end(sched->_perf_timers); - - // process any pending RC output requests - hal.rcout->timer_tick(); - - // process any pending RC input requests - ((VRBRAINRCInput *)hal.rcin)->_timer_tick(); - - if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) { - last_ran_overtime = AP_HAL::millis(); -#if 0 - printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); - hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task); -#endif - } - } - return nullptr; -} - -void VRBRAINScheduler::_run_io(void) -{ - if (_in_io_proc) { - return; - } - _in_io_proc = true; - - // now call the IO based drivers - for (int i = 0; i < _num_io_procs; i++) { - if (_io_proc[i]) { - _io_proc[i](); - } - } - - _in_io_proc = false; -} - -void *VRBRAINScheduler::_uart_thread(void *arg) -{ - VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; - - pthread_setname_np(pthread_self(), "apm_uart"); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - while (!_vrbrain_thread_should_exit) { - sched->delay_microseconds_semaphore(1000); - - // process any pending serial bytes - hal.uartA->_timer_tick(); - hal.uartB->_timer_tick(); - hal.uartC->_timer_tick(); - hal.uartD->_timer_tick(); - hal.uartE->_timer_tick(); - hal.uartF->_timer_tick(); - } - return nullptr; -} - -void *VRBRAINScheduler::_io_thread(void *arg) -{ - VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; - - pthread_setname_np(pthread_self(), "apm_io"); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - while (!_vrbrain_thread_should_exit) { - sched->delay_microseconds_semaphore(1000); - - // run registered IO processes - perf_begin(sched->_perf_io_timers); - sched->_run_io(); - perf_end(sched->_perf_io_timers); - } - return nullptr; -} - -void *VRBRAINScheduler::_storage_thread(void *arg) -{ - VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; - - pthread_setname_np(pthread_self(), "apm_storage"); - - while (!sched->_hal_initialized) { - poll(nullptr, 0, 1); - } - while (!_vrbrain_thread_should_exit) { - sched->delay_microseconds_semaphore(10000); - - // process any pending storage writes - perf_begin(sched->_perf_storage_timer); - hal.storage->_timer_tick(); - perf_end(sched->_perf_storage_timer); - } - return nullptr; -} - -bool VRBRAINScheduler::in_main_thread() const -{ - return getpid() == _main_task_pid; -} - -void VRBRAINScheduler::system_initialized() -{ - if (_initialized) { - AP_HAL::panic("PANIC: scheduler::system_initialized called" - "more than once"); - } - _initialized = true; -} - -#endif diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h deleted file mode 100644 index 136e2cceb8..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "AP_HAL_VRBRAIN_Namespace.h" -#include -#include -#include -#include - -#define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8 - -#define APM_MAIN_PRIORITY_BOOST 241 -#define APM_MAIN_PRIORITY 180 -#define APM_TIMER_PRIORITY 181 -#define APM_SPI_PRIORITY 242 -#define APM_CAN_PRIORITY 179 -#define APM_I2C_PRIORITY 178 -#define APM_UART_PRIORITY 60 -#define APM_STORAGE_PRIORITY 59 -#define APM_IO_PRIORITY 58 -#define APM_SHELL_PRIORITY 57 -#define APM_OVERTIME_PRIORITY 10 -#define APM_STARTUP_PRIORITY 10 - -/* how long to boost priority of the main thread for each main - loop. This needs to be long enough for all interrupt-level drivers - (mostly SPI drivers) to run, and for the main loop of the vehicle - code to start the AHRS update. - - Priority boosting of the main thread in delay_microseconds_boost() - avoids the problem that drivers in hpwork all happen to run right - at the start of the period where the main vehicle loop is calling - wait_for_sample(). That causes main loop timing jitter, which - reduces performance. Using the priority boost the main loop - temporarily runs at a priority higher than hpwork and the timer - thread, which results in much more consistent loop timing. -*/ -#define APM_MAIN_PRIORITY_BOOST_USEC 150 - -#define APM_MAIN_THREAD_STACK_SIZE 8192 - -/* Scheduler implementation: */ -class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler { -public: - VRBRAINScheduler(); - /* AP_HAL::Scheduler methods */ - - void init(); - void delay(uint16_t ms); - void delay_microseconds(uint16_t us); - void delay_microseconds_boost(uint16_t us); - void register_timer_process(AP_HAL::MemberProc); - void register_io_process(AP_HAL::MemberProc); - void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); - void reboot(bool hold_in_bootloader); - - bool in_main_thread() const override; - void system_initialized(); - void hal_initialized() { _hal_initialized = true; } - -private: - bool _initialized; - volatile bool _hal_initialized; - AP_HAL::Proc _failsafe; - - AP_HAL::MemberProc _timer_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS]; - uint8_t _num_timer_procs; - volatile bool _in_timer_proc; - - AP_HAL::MemberProc _io_proc[VRBRAIN_SCHEDULER_MAX_TIMER_PROCS]; - uint8_t _num_io_procs; - volatile bool _in_io_proc; - - pid_t _main_task_pid; - pthread_t _timer_thread_ctx; - pthread_t _io_thread_ctx; - pthread_t _storage_thread_ctx; - pthread_t _uart_thread_ctx; - - static void *_timer_thread(void *arg); - static void *_io_thread(void *arg); - static void *_storage_thread(void *arg); - static void *_uart_thread(void *arg); - - void _run_timers(); - void _run_io(void); - - void delay_microseconds_semaphore(uint16_t us); - - perf_counter_t _perf_timers; - perf_counter_t _perf_io_timers; - perf_counter_t _perf_storage_timer; - perf_counter_t _perf_delay; -}; -#endif diff --git a/libraries/AP_HAL_VRBRAIN/Semaphores.cpp b/libraries/AP_HAL_VRBRAIN/Semaphores.cpp deleted file mode 100644 index 6ee249fa7b..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Semaphores.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "Semaphores.h" -#include - -extern const AP_HAL::HAL& hal; - -using namespace VRBRAIN; - -bool Semaphore::give() -{ - return pthread_mutex_unlock(&_lock) == 0; -} - -bool Semaphore::take(uint32_t timeout_ms) -{ - if (up_interrupt_context()) { - // don't ever wait on a semaphore in interrupt context - return take_nonblocking(); - } - if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { - return pthread_mutex_lock(&_lock) == 0; - } - if (take_nonblocking()) { - return true; - } - uint64_t start = AP_HAL::micros64(); - do { - hal.scheduler->delay_microseconds(200); - if (take_nonblocking()) { - return true; - } - } while ((AP_HAL::micros64() - start) < timeout_ms*1000); - return false; -} - -bool Semaphore::take_nonblocking() -{ - return pthread_mutex_trylock(&_lock) == 0; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/Semaphores.h b/libraries/AP_HAL_VRBRAIN/Semaphores.h deleted file mode 100644 index 3be0a2338e..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Semaphores.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include "AP_HAL_VRBRAIN_Namespace.h" -#include - -class VRBRAIN::Semaphore : public AP_HAL::Semaphore { -public: - Semaphore() { - pthread_mutex_init(&_lock, nullptr); - } - bool give(); - bool take(uint32_t timeout_ms); - bool take_nonblocking(); -private: - pthread_mutex_t _lock; -}; - diff --git a/libraries/AP_HAL_VRBRAIN/Storage.cpp b/libraries/AP_HAL_VRBRAIN/Storage.cpp deleted file mode 100644 index d262d90428..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Storage.cpp +++ /dev/null @@ -1,315 +0,0 @@ -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "Storage.h" -using namespace VRBRAIN; - -/* - This stores eeprom data in the VRBRAIN MTD interface with a 4k size, and - a in-memory buffer. This keeps the latency and devices IOs down. -*/ - -// name the storage file after the sketch so you can use the same sd -// card for ArduCopter and ArduPlane -#define STORAGE_DIR "/fs/microsd/APM" -//#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav" -#define MTD_PARAMS_FILE "/fs/mtd" - -extern const AP_HAL::HAL& hal; - -extern "C" int mtd_main(int, char **); - -VRBRAINStorage::VRBRAINStorage(void) : - _perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")), - _perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors")) -{ -} - -void VRBRAINStorage::_storage_open(void) -{ - if (_initialised) { - return; - } - - _dirty_mask.clearall(); - - // load from storage backend -#if USE_FLASH_STORAGE - _flash_load(); -#else - _mtd_load(); -#endif - -#ifdef SAVE_STORAGE_FILE - fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644); - if (fd != -1) { - write(fd, _buffer, sizeof(_buffer)); - close(fd); - ::printf("Saved storage file %s\n", SAVE_STORAGE_FILE); - } -#endif - _initialised = true; -} - -/* - mark some lines as dirty. Note that there is no attempt to avoid - the race condition between this code and the _timer_tick() code - below, which both update _dirty_mask. If we lose the race then the - result is that a line is written more than once, but it won't result - in a line not being written. -*/ -void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length) -{ - uint16_t end = loc + length; - for (uint16_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT; - line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; - line++) { - _dirty_mask.set(line); - } -} - -void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n) -{ - if (loc >= sizeof(_buffer)-(n-1)) { - return; - } - _storage_open(); - memcpy(dst, &_buffer[loc], n); -} - -void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n) -{ - if (loc >= sizeof(_buffer)-(n-1)) { - return; - } - if (memcmp(src, &_buffer[loc], n) != 0) { - _storage_open(); - memcpy(&_buffer[loc], src, n); - _mark_dirty(loc, n); - } -} - -void VRBRAINStorage::_timer_tick(void) -{ - if (!_initialised || _dirty_mask.empty()) { - return; - } - perf_begin(_perf_storage); - -#if !USE_FLASH_STORAGE - if (_fd == -1) { - _fd = open(MTD_PARAMS_FILE, O_WRONLY); - if (_fd == -1) { - perf_end(_perf_storage); - perf_count(_perf_errors); - return; - } - } -#endif - - // write out the first dirty line. We don't write more - // than one to keep the latency of this call to a minimum - uint16_t i; - for (i=0; i 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 diff --git a/libraries/AP_HAL_VRBRAIN/Storage.h b/libraries/AP_HAL_VRBRAIN/Storage.h deleted file mode 100644 index 64adf47d47..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Storage.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -/* - we can optionally use flash for storage instead of FRAM. That allows - ArduPilot to run on a wider range of boards and reduces board cost - */ -#ifndef USE_FLASH_STORAGE -#define USE_FLASH_STORAGE 0 -#endif - -#include -#include "AP_HAL_VRBRAIN_Namespace.h" -#include -#include -#include - -#define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE - -#if USE_FLASH_STORAGE -// when using flash storage we use a small line size to make storage -// compact and minimise the number of erase cycles needed -#define VRBRAIN_STORAGE_LINE_SHIFT 3 -#else -#define VRBRAIN_STORAGE_LINE_SHIFT 9 -#endif - -#define VRBRAIN_STORAGE_LINE_SIZE (1< - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include "UARTDriver.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "GPIO.h" - -using namespace VRBRAIN; - -extern const AP_HAL::HAL& hal; - -VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name) : - _devpath(devpath), - _fd(-1), - _baudrate(57600), - _initialised(false), - _in_timer(false), - _unbuffered_writes(false), - _perf_uart(perf_alloc(PC_ELAPSED, perf_name)), - _os_start_auto_space(-1), - _flow_control(FLOW_CONTROL_DISABLE) -{ -} - - -extern const AP_HAL::HAL& hal; - -/* - this UART driver maps to a serial device in /dev - */ - -void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) -{ - if (strcmp(_devpath, "/dev/null") == 0) { - // leave uninitialised - return; - } - - uint16_t min_tx_buffer = 1024; - uint16_t min_rx_buffer = 512; - if (strcmp(_devpath, "/dev/ttyACM0") == 0) { - min_tx_buffer = 4096; - min_rx_buffer = 1024; - } - // on VRBRAIN we have enough memory to have a larger transmit and - // receive buffer for all ports. This means we don't get delays - // while waiting to write GPS config packets - if (txS < min_tx_buffer) { - txS = min_tx_buffer; - } - if (rxS < min_rx_buffer) { - rxS = min_rx_buffer; - } - - /* - allocate the read buffer - we allocate buffers before we successfully open the device as we - want to allocate in the early stages of boot, and cause minimum - thrashing of the heap once we are up. The ttyACM0 driver may not - connect for some time after boot - */ - while (_in_timer) { - hal.scheduler->delay(1); - } - if (rxS != _readbuf.get_size()) { - _initialised = false; - _readbuf.set_size(rxS); - } - - bool clear_buffers = false; - if (b != 0) { - // clear buffers on baudrate change, but not on the console (which is usually USB) - if (_baudrate != b && hal.console != this) { - clear_buffers = true; - } - _baudrate = b; - } - - if (b != 0) { - _baudrate = b; - } - - if (clear_buffers) { - _readbuf.clear(); - } - - /* - allocate the write buffer - */ - while (_in_timer) { - hal.scheduler->delay(1); - } - if (txS != _writebuf.get_size()) { - _initialised = false; - _writebuf.set_size(txS); - } - - if (clear_buffers) { - _writebuf.clear(); - } - - if (_fd == -1) { - _fd = open(_devpath, O_RDWR); - if (_fd == -1) { - return; - } - } - - if (_baudrate != 0) { - // set the baud rate - struct termios t; - tcgetattr(_fd, &t); - cfsetspeed(&t, _baudrate); - // disable LF -> CR/LF - t.c_oflag &= ~ONLCR; - tcsetattr(_fd, TCSANOW, &t); - - // separately setup IFLOW if we can. We do this as a 2nd call - // as if the port has no RTS pin then the tcsetattr() call - // will fail, and if done as one call then it would fail to - // set the baudrate. - tcgetattr(_fd, &t); - t.c_cflag |= CRTS_IFLOW; - tcsetattr(_fd, TCSANOW, &t); - } - - if (_writebuf.get_size() && _readbuf.get_size() && _fd != -1) { - if (!_initialised) { - if (strcmp(_devpath, "/dev/ttyACM0") == 0) { - ((VRBRAINGPIO *)hal.gpio)->set_usb_connected(); - } - ::printf("initialised %s OK %u %u\n", _devpath, - (unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size()); - } - _initialised = true; - } -} - -void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol) -{ - if (_fd == -1) { - return; - } - struct termios t; - tcgetattr(_fd, &t); - // we already enabled CRTS_IFLOW above, just enable output flow control - if (fcontrol != FLOW_CONTROL_DISABLE) { - t.c_cflag |= CRTSCTS; - } else { - t.c_cflag &= ~CRTSCTS; - } - tcsetattr(_fd, TCSANOW, &t); - if (fcontrol == FLOW_CONTROL_AUTO) { - // reset flow control auto state machine - _total_written = 0; - _first_write_time = 0; - } - _flow_control = fcontrol; -} - -void VRBRAINUARTDriver::configure_parity(uint8_t v) { - if (_fd == -1) { - return; - } - struct termios t; - tcgetattr(_fd, &t); - if (v != 0) { - // enable parity - t.c_cflag |= PARENB; - if (v == 1) { - t.c_cflag |= PARODD; - } else { - t.c_cflag &= ~PARODD; - } - } - else { - // disable parity - t.c_cflag &= ~PARENB; - } - tcsetattr(_fd, TCSANOW, &t); -} - -void VRBRAINUARTDriver::set_stop_bits(int n) { - if (_fd == -1) { - return; - } - struct termios t; - tcgetattr(_fd, &t); - if (n > 1) t.c_cflag |= CSTOPB; - else t.c_cflag &= ~CSTOPB; - tcsetattr(_fd, TCSANOW, &t); -} - -bool VRBRAINUARTDriver::set_unbuffered_writes(bool on) { - _unbuffered_writes = on; - return _unbuffered_writes; -} - -void VRBRAINUARTDriver::begin(uint32_t b) -{ - begin(b, 0, 0); -} - - -/* - try to initialise the UART. This is used to cope with the way NuttX - handles /dev/ttyACM0 (the USB port). The port appears in /dev on - boot, but cannot be opened until a USB cable is connected and the - host starts the CDCACM communication. - */ -void VRBRAINUARTDriver::try_initialise(void) -{ - if (_initialised) { - return; - } - if ((AP_HAL::millis() - _last_initialise_attempt_ms) < 2000) { - return; - } - _last_initialise_attempt_ms = AP_HAL::millis(); - if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED || !hal.util->get_soft_armed()) { - begin(0); - } -} - - -void VRBRAINUARTDriver::end() -{ - _initialised = false; - while (_in_timer) hal.scheduler->delay(1); - if (_fd != -1) { - close(_fd); - _fd = -1; - } - - _readbuf.set_size(0); - _writebuf.set_size(0); -} - -void VRBRAINUARTDriver::flush() {} - -bool VRBRAINUARTDriver::is_initialized() -{ - try_initialise(); - return _initialised; -} - -void VRBRAINUARTDriver::set_blocking_writes(bool blocking) -{ - _nonblocking_writes = !blocking; -} - -bool VRBRAINUARTDriver::tx_pending() { return false; } - -/* - return number of bytes available to be read from the buffer - */ -uint32_t VRBRAINUARTDriver::available() -{ - if (!_initialised) { - try_initialise(); - return 0; - } - - return _readbuf.available(); -} - -/* - return number of bytes that can be added to the write buffer - */ -uint32_t VRBRAINUARTDriver::txspace() -{ - if (!_initialised) { - try_initialise(); - return 0; - } - - return _writebuf.space(); -} - -/* - read one byte from the read buffer - */ -int16_t VRBRAINUARTDriver::read() -{ - if (!_semaphore.take_nonblocking()) { - return -1; - } - if (!_initialised) { - try_initialise(); - _semaphore.give(); - return -1; - } - - uint8_t byte; - if (!_readbuf.read_byte(&byte)) { - _semaphore.give(); - return -1; - } - - _semaphore.give(); - return byte; -} - -/* - write one byte - */ -size_t VRBRAINUARTDriver::write(uint8_t c) -{ - if (!_semaphore.take_nonblocking()) { - return -1; - } - if (!_initialised) { - try_initialise(); - _semaphore.give(); - return 0; - } - - if (_unbuffered_writes) { - // write one byte to the file descriptor - return _write_fd(&c, 1); - } - - while (_writebuf.space() == 0) { - if (_nonblocking_writes) { - _semaphore.give(); - return 0; - } - _semaphore.give(); - hal.scheduler->delay(1); - if (!_semaphore.take_nonblocking()) { - return -1; - } - } - size_t ret = _writebuf.write(&c, 1); - _semaphore.give(); - return ret; -} - -/* - * write size bytes - */ -size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size) -{ - if (!_semaphore.take_nonblocking()) { - return -1; - } - if (!_initialised) { - try_initialise(); - _semaphore.give(); - return 0; - } - - size_t ret = 0; - - if (!_nonblocking_writes) { - _semaphore.give(); - /* - use the per-byte delay loop in write() above for blocking writes - */ - while (size--) { - if (write(*buffer++) != 1) break; - ret++; - } - return ret; - } - - if (_unbuffered_writes) { - // write buffer straight to the file descriptor - ret = _write_fd(buffer, size); - } else { - ret = _writebuf.write(buffer, size); - } - _semaphore.give(); - return ret; -} - -/* - try writing n bytes, handling an unresponsive port - */ -int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n) -{ - int ret = 0; - - // the FIONWRITE check is to cope with broken O_NONBLOCK behaviour - // in NuttX on ttyACM0 - - // FIONWRITE is also used for auto flow control detection - // Assume output flow control is not working if: - // port is configured for auto flow control - // and this is not the first write since flow control turned on - // and no data has been removed from the buffer since flow control turned on - // and more than .5 seconds elapsed after writing a total of > 5 characters - // - - int nwrite = 0; - - if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) { - if (_flow_control == FLOW_CONTROL_AUTO) { - if (_first_write_time == 0) { - if (_total_written == 0) { - // save the remaining buffer bytes for comparison next write - _os_start_auto_space = nwrite; - } - } else { - if (_os_start_auto_space - nwrite + 1 >= _total_written && - (AP_HAL::micros64() - _first_write_time) > 500*1000UL) { - // it doesn't look like hw flow control is working - ::printf("disabling flow control on %s _total_written=%u\n", - _devpath, (unsigned)_total_written); - set_flow_control(FLOW_CONTROL_DISABLE); - } - } - } - if (nwrite > n) { - nwrite = n; - } - if (nwrite > 0) { - ret = ::write(_fd, buf, nwrite); - } - } - - if (ret > 0) { - _last_write_time = AP_HAL::micros64(); - _total_written += ret; - if (! _first_write_time && _total_written > 5) { - _first_write_time = _last_write_time; - } - return ret; - } - - if (AP_HAL::micros64() - _last_write_time > 2000 && - _flow_control == FLOW_CONTROL_DISABLE) { - _last_write_time = AP_HAL::micros64(); - - // we haven't done a successful write for 2ms, which means the - // port is running at less than 500 bytes/sec. Start - // discarding bytes, even if this is a blocking port. This - // prevents the ttyACM0 port blocking startup if the endpoint - // is not connected - return n; - } - return ret; -} - -/* - try reading n bytes, handling an unresponsive port - */ -int VRBRAINUARTDriver::_read_fd(uint8_t *buf, uint16_t n) -{ - int ret = 0; - - // the FIONREAD check is to cope with broken O_NONBLOCK behaviour - // in NuttX on ttyACM0 - int nread = 0; - if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) { - if (nread > n) { - nread = n; - } - if (nread > 0) { - ret = ::read(_fd, buf, nread); - } - } - if (ret > 0) { - _total_read += ret; - } - return ret; -} - - -/* - push any pending bytes to/from the serial port. This is called at - 1kHz in the timer thread. Doing it this way reduces the system call - overhead in the main task enormously. - */ -void VRBRAINUARTDriver::_timer_tick(void) -{ - int ret; - uint32_t n; - - if (!_initialised) return; - - // don't try IO on a disconnected USB port - if (strcmp(_devpath, "/dev/ttyACM0") == 0 && !hal.gpio->usb_connected()) { - return; - } - - _in_timer = true; - - // write any pending bytes - n = _writebuf.available(); - if (n > 0) { - ByteBuffer::IoVec vec[2]; - perf_begin(_perf_uart); - const auto n_vec = _writebuf.peekiovec(vec, n); - for (int i = 0; i < n_vec; i++) { - ret = _write_fd(vec[i].data, (uint16_t)vec[i].len); - if (ret < 0) { - break; - } - _writebuf.advance(ret); - - /* We wrote less than we asked for, stop */ - if ((unsigned)ret != vec[i].len) { - break; - } - } - perf_end(_perf_uart); - } - - // try to fill the read buffer - ByteBuffer::IoVec vec[2]; - - perf_begin(_perf_uart); - const auto n_vec = _readbuf.reserve(vec, _readbuf.space()); - for (int i = 0; i < n_vec; i++) { - ret = _read_fd(vec[i].data, vec[i].len); - if (ret < 0) { - break; - } - _readbuf.commit((unsigned)ret); - - /* stop reading as we read less than we asked for */ - if ((unsigned)ret < vec[i].len) { - break; - } - } - perf_end(_perf_uart); - - _in_timer = false; -} - -#endif diff --git a/libraries/AP_HAL_VRBRAIN/UARTDriver.h b/libraries/AP_HAL_VRBRAIN/UARTDriver.h deleted file mode 100644 index 8b7c0ea148..0000000000 --- a/libraries/AP_HAL_VRBRAIN/UARTDriver.h +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include - -#include "AP_HAL_VRBRAIN.h" -#include -#include "Semaphores.h" - -class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver { -public: - VRBRAINUARTDriver(const char *devpath, const char *perf_name); - /* VRBRAIN implementations of UARTDriver virtual methods */ - void begin(uint32_t b); - void begin(uint32_t b, uint16_t rxS, uint16_t txS); - void end(); - void flush(); - bool is_initialized(); - void set_blocking_writes(bool blocking); - bool tx_pending(); - - /* VRBRAIN implementations of Stream virtual methods */ - uint32_t available() override; - uint32_t txspace() override; - int16_t read() override; - - /* VRBRAIN implementations of Print virtual methods */ - size_t write(uint8_t c); - size_t write(const uint8_t *buffer, size_t size); - - void set_device_path(const char *path) { - _devpath = path; - } - - void _timer_tick(void); - - int _get_fd(void) { - return _fd; - } - - void set_flow_control(enum flow_control flow_control); - enum flow_control get_flow_control(void) override { return _flow_control; } - - void configure_parity(uint8_t v); - void set_stop_bits(int n); - bool set_unbuffered_writes(bool on); - -private: - const char *_devpath; - int _fd; - uint32_t _baudrate; - volatile bool _initialised; - volatile bool _in_timer; - - bool _nonblocking_writes; - bool _unbuffered_writes; - - // we use in-task ring buffers to reduce the system call cost - // of ::read() and ::write() in the main loop - ByteBuffer _readbuf{0}; - ByteBuffer _writebuf{0}; - perf_counter_t _perf_uart; - - int _write_fd(const uint8_t *buf, uint16_t n); - int _read_fd(uint8_t *buf, uint16_t n); - uint64_t _first_write_time; - uint64_t _last_write_time; - - void try_initialise(void); - uint32_t _last_initialise_attempt_ms; - - uint32_t _os_start_auto_space; - uint32_t _total_read; - uint32_t _total_written; - enum flow_control _flow_control; - - Semaphore _semaphore; -}; diff --git a/libraries/AP_HAL_VRBRAIN/Util.cpp b/libraries/AP_HAL_VRBRAIN/Util.cpp deleted file mode 100644 index 7e399564c7..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Util.cpp +++ /dev/null @@ -1,267 +0,0 @@ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include -#include -#include -#include -#include -#include "UARTDriver.h" -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -#include "Util.h" -using namespace VRBRAIN; - -extern bool _vrbrain_thread_should_exit; - -/* - constructor - */ -VRBRAINUtil::VRBRAINUtil(void) : Util() -{ - _safety_handle = orb_subscribe(ORB_ID(safety)); -} - - -/* - start an instance of nsh - */ -bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream) -{ - VRBRAINUARTDriver *uart = (VRBRAINUARTDriver *)stream; - int fd; - - // trigger exit in the other threads. This stops use of the - // various driver handles, and especially the px4io handle, - // which otherwise would cause a crash if px4io is stopped in - // the shell - _vrbrain_thread_should_exit = true; - - // take control of stream fd - fd = uart->_get_fd(); - - // mark it blocking (nsh expects a blocking fd) - unsigned v; - v = fcntl(fd, F_GETFL, 0); - fcntl(fd, F_SETFL, v & ~O_NONBLOCK); - - // setup the UART on stdin/stdout/stderr - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); - - nsh_consolemain(0, nullptr); - - // this shouldn't happen - hal.console->printf("shell exited\n"); - return true; -} - -/* - return state of safety switch - */ -enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void) -{ -#if !HAL_HAVE_SAFETY_SWITCH - return AP_HAL::Util::SAFETY_NONE; -#endif - - if (_safety_handle == -1) { - _safety_handle = orb_subscribe(ORB_ID(safety)); - } - if (_safety_handle == -1) { - return AP_HAL::Util::SAFETY_NONE; - } - struct safety_s safety; - if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) { - return AP_HAL::Util::SAFETY_NONE; - } - if (!safety.safety_switch_available) { - return AP_HAL::Util::SAFETY_NONE; - } - if (safety.safety_off) { - return AP_HAL::Util::SAFETY_ARMED; - } - return AP_HAL::Util::SAFETY_DISARMED; -} - -/* - display VRBRAIN system identifer - board type and serial number - */ -bool VRBRAINUtil::get_system_id(char buf[40]) -{ - uint8_t serialid[12]; - memset(serialid, 0, sizeof(serialid)); - get_board_serial(serialid); -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) - const char *board_type = "VRBRAINv45"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) - const char *board_type = "VRBRAINv51"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) - const char *board_type = "VRBRAINv52"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52E) - const char *board_type = "VRBRAINv52E"; -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - const char *board_type = "VRUBRAINv51"; -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - const char *board_type = "VRUBRAINv52"; -#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) - const char *board_type = "VRCOREv10"; -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) - const char *board_type = "VRBRAINv54"; -#endif - // this format is chosen to match the human_readable_serial() - // function in auth.c - snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", - board_type, - (unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3], - (unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7], - (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]); - buf[39] = 0; - return true; -} - -/** - how much free memory do we have in bytes. -*/ -uint32_t VRBRAINUtil::available_memory(void) -{ - return mallinfo().fordblks; -} - -/* - AP_HAL wrapper around PX4 perf counters - */ -VRBRAINUtil::perf_counter_t VRBRAINUtil::perf_alloc(VRBRAINUtil::perf_counter_type t, const char *name) -{ - ::perf_counter_type vrbrain_t; - switch (t) { - case VRBRAINUtil::PC_COUNT: - vrbrain_t = ::PC_COUNT; - break; - case VRBRAINUtil::PC_ELAPSED: - vrbrain_t = ::PC_ELAPSED; - break; - case VRBRAINUtil::PC_INTERVAL: - vrbrain_t = ::PC_INTERVAL; - break; - default: - return nullptr; - } - return (perf_counter_t)::perf_alloc(vrbrain_t, name); -} - -void VRBRAINUtil::perf_begin(perf_counter_t h) -{ - ::perf_begin((::perf_counter_t)h); -} - -void VRBRAINUtil::perf_end(perf_counter_t h) -{ - ::perf_end((::perf_counter_t)h); -} - -void VRBRAINUtil::perf_count(perf_counter_t h) -{ - ::perf_count((::perf_counter_t)h); -} - -void VRBRAINUtil::set_imu_temp(float current) -{ - if (!_heater.target || *_heater.target == -1) { - return; - } - - // average over temperatures to remove noise - _heater.count++; - _heater.sum += current; - - // update once a second - uint32_t now = AP_HAL::millis(); - if (now - _heater.last_update_ms < 1000) { - return; - } - _heater.last_update_ms = now; - - current = _heater.sum / _heater.count; - _heater.sum = 0; - _heater.count = 0; - - // experimentally tweaked for Pixhawk2 - const float kI = 0.3f; - const float kP = 200.0f; - float target = (float)(*_heater.target); - - // limit to 65 degrees to prevent damage - target = constrain_float(target, 0, 65); - - float err = target - current; - - _heater.integrator += kI * err; - _heater.integrator = constrain_float(_heater.integrator, 0, 70); - - float output = constrain_float(kP * err + _heater.integrator, 0, 100); - - // hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err); - - if (_heater.fd == -1) { - _heater.fd = open("/dev/px4io", O_RDWR); - } - if (_heater.fd != -1) { - ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output); - } - -} - -void VRBRAINUtil::set_imu_target_temp(int8_t *target) -{ - _heater.target = target; -} - - -extern "C" { - extern void *fat_dma_alloc(size_t); - extern void fat_dma_free(void *, size_t); -} - -/* - allocate DMA-capable memory if possible. Otherwise return normal - memory. -*/ -void *VRBRAINUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) -{ - - - - - - - - return calloc(1, size); - -} -void VRBRAINUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) -{ - - - - - - - - return free(ptr); - -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_HAL_VRBRAIN/Util.h b/libraries/AP_HAL_VRBRAIN/Util.h deleted file mode 100644 index 3325ecc9ca..0000000000 --- a/libraries/AP_HAL_VRBRAIN/Util.h +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -#include -#include "AP_HAL_VRBRAIN_Namespace.h" -#include "Semaphores.h" - -class VRBRAIN::NSHShellStream : public AP_HAL::BetterStream { -public: - size_t write(uint8_t); - size_t write(const uint8_t *buffer, size_t size); - int16_t read() override; - uint32_t available() override; - uint32_t txspace() override; -private: - int shell_stdin = -1; - int shell_stdout = -1; - pthread_t shell_thread_ctx; - - struct { - int in = -1; - int out = -1; - } child; - bool showed_memory_warning = false; - bool showed_armed_warning = false; - - void start_shell(void); - static void shell_thread(void *arg); -}; - -class VRBRAIN::VRBRAINUtil : public AP_HAL::Util { -public: - VRBRAINUtil(void); - bool run_debug_shell(AP_HAL::BetterStream *stream); - - enum safety_state safety_switch_state(void); - - /* - get system identifier (STM32 serial number) - */ - bool get_system_id(char buf[40]); - - uint32_t available_memory(void) override; - - /* - return a stream for access to nsh shell - */ - AP_HAL::BetterStream *get_shell_stream() { return &_shell_stream; } - perf_counter_t perf_alloc(perf_counter_type t, const char *name) override; - void perf_begin(perf_counter_t ) override; - void perf_end(perf_counter_t) override; - void perf_count(perf_counter_t) override; - - void set_imu_temp(float current) override; - void set_imu_target_temp(int8_t *target) override; - - // allocate and free DMA-capable memory if possible. Otherwise return normal memory - void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; - void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; - -private: - int _safety_handle; - VRBRAIN::NSHShellStream _shell_stream; - - struct { - int8_t *target; - float integrator; - uint16_t count; - float sum; - uint32_t last_update_ms; - int fd = -1; - } _heater; -}; diff --git a/libraries/AP_HAL_VRBRAIN/bxcan.h b/libraries/AP_HAL_VRBRAIN/bxcan.h deleted file mode 100644 index f5ad7cd0f7..0000000000 --- a/libraries/AP_HAL_VRBRAIN/bxcan.h +++ /dev/null @@ -1,294 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - * Bit definitions were copied from NuttX STM32 CAN driver. - * - * With modifications for Ardupilot CAN driver - * Copyright (C) 2017 Eugene Shamaev - */ - -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(STM32_CAN1_BASE), reinterpret_cast(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 diff --git a/libraries/AP_HAL_VRBRAIN/system.cpp b/libraries/AP_HAL_VRBRAIN/system.cpp deleted file mode 100644 index 7b9981fbb7..0000000000 --- a/libraries/AP_HAL_VRBRAIN/system.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#include -#include - -#include - -#include -#include - -extern const AP_HAL::HAL& hal; - -extern bool _vrbrain_thread_should_exit; - -namespace AP_HAL { - -void init() -{ -} - -void panic(const char *errormsg, ...) -{ - va_list ap; - - va_start(ap, errormsg); - vdprintf(1, errormsg, ap); - va_end(ap); - write(1, "\n", 1); - - hal.scheduler->delay_microseconds(10000); - _vrbrain_thread_should_exit = true; - exit(1); -} - -uint32_t micros() -{ - return micros64() & 0xFFFFFFFF; -} - -uint32_t millis() -{ - return millis64() & 0xFFFFFFFF; -} - -uint64_t micros64() -{ - return hrt_absolute_time(); -} - -uint64_t millis64() -{ - return micros64() / 1000; -} - -} // namespace AP_HAL diff --git a/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp b/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp deleted file mode 100644 index e9975dc6d9..0000000000 --- a/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - This replaces the PX4Firmware parameter system with dummy - functions. The ArduPilot parameter system uses different formatting - for FRAM and we need to ensure that the PX4 parameter system doesn't - try to access FRAM in an invalid manner - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include -#include - -#include "systemlib/param/param.h" - -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" - -/** parameter update topic */ -ORB_DEFINE(parameter_update, struct parameter_update_s); - -param_t param_find(const char *name) -{ -#if 0 - // useful for driver debugging - ::printf("VRBRAIN: param_find(%s)\n", name); -#endif - return PARAM_INVALID; -} - -int param_get(param_t param, void *val) -{ - return -1; -} - -int param_set(param_t param, const void *val) -{ - return -1; -} - -int -param_set_no_notification(param_t param, const void *val) -{ - return -1; -} -#endif // CONFIG_HAL_BOARD