From 97b29a333da6c249202b428804aec1a05af00d1f Mon Sep 17 00:00:00 2001 From: LukeMike Date: Sat, 3 Feb 2018 14:46:18 +0100 Subject: [PATCH] VRBRAIN / AP_HAL_VRBRAIN: updated AP_HAL --- .../AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h | 30 +- libraries/AP_HAL_VRBRAIN/AnalogIn.cpp | 6 + libraries/AP_HAL_VRBRAIN/AnalogIn.h | 12 +- libraries/AP_HAL_VRBRAIN/CAN.cpp | 1117 +++++++++++++++++ libraries/AP_HAL_VRBRAIN/CAN.h | 317 +++++ libraries/AP_HAL_VRBRAIN/Device.cpp | 155 +++ libraries/AP_HAL_VRBRAIN/Device.h | 49 + libraries/AP_HAL_VRBRAIN/GPIO.cpp | 26 +- libraries/AP_HAL_VRBRAIN/GPIO.h | 21 +- .../AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp | 49 +- libraries/AP_HAL_VRBRAIN/I2CDevice.cpp | 126 +- libraries/AP_HAL_VRBRAIN/I2CDevice.h | 39 +- libraries/AP_HAL_VRBRAIN/I2CWrapper.h | 18 +- libraries/AP_HAL_VRBRAIN/RCInput.cpp | 163 ++- libraries/AP_HAL_VRBRAIN/RCInput.h | 27 +- libraries/AP_HAL_VRBRAIN/RCOutput.cpp | 308 +++-- libraries/AP_HAL_VRBRAIN/RCOutput.h | 28 +- libraries/AP_HAL_VRBRAIN/SPIDevice.cpp | 329 +++++ libraries/AP_HAL_VRBRAIN/SPIDevice.h | 113 ++ libraries/AP_HAL_VRBRAIN/Scheduler.cpp | 126 +- libraries/AP_HAL_VRBRAIN/Scheduler.h | 14 +- libraries/AP_HAL_VRBRAIN/Semaphores.cpp | 5 + libraries/AP_HAL_VRBRAIN/Storage.cpp | 476 +++---- libraries/AP_HAL_VRBRAIN/Storage.h | 58 +- libraries/AP_HAL_VRBRAIN/UARTDriver.cpp | 116 +- libraries/AP_HAL_VRBRAIN/UARTDriver.h | 16 +- libraries/AP_HAL_VRBRAIN/Util.cpp | 47 +- libraries/AP_HAL_VRBRAIN/Util.h | 8 +- libraries/AP_HAL_VRBRAIN/bxcan.h | 294 +++++ libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp | 7 +- 30 files changed, 3525 insertions(+), 575 deletions(-) create mode 100644 libraries/AP_HAL_VRBRAIN/CAN.cpp create mode 100644 libraries/AP_HAL_VRBRAIN/CAN.h create mode 100644 libraries/AP_HAL_VRBRAIN/Device.cpp create mode 100644 libraries/AP_HAL_VRBRAIN/Device.h create mode 100644 libraries/AP_HAL_VRBRAIN/SPIDevice.cpp create mode 100644 libraries/AP_HAL_VRBRAIN/SPIDevice.h create mode 100644 libraries/AP_HAL_VRBRAIN/bxcan.h diff --git a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h index 779435f1f9..b89e703fb2 100644 --- a/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h +++ b/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h @@ -1,18 +1,20 @@ #pragma once namespace VRBRAIN { - class VRBRAINScheduler; - class VRBRAINUARTDriver; - class VRBRAINStorage; - class VRBRAINRCInput; - class VRBRAINRCOutput; - class VRBRAINAnalogIn; - class VRBRAINAnalogSource; - class VRBRAINUtil; - class VRBRAINGPIO; - class VRBRAINDigitalSource; - class NSHShellStream; - class VRBRAINI2CDriver; - class VRBRAIN_I2C; - class Semaphore; + class VRBRAINScheduler; + class VRBRAINUARTDriver; + class VRBRAINStorage; + class VRBRAINRCInput; + class VRBRAINRCOutput; + class VRBRAINAnalogIn; + class VRBRAINAnalogSource; + class VRBRAINUtil; + class VRBRAINGPIO; + class VRBRAINDigitalSource; + class NSHShellStream; + class VRBRAINI2CDriver; + class VRBRAIN_I2C; + class Semaphore; + class VRBRAINCAN; + class VRBRAINCANManager; } diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp index ab81defa8b..a524b37ab6 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp @@ -52,6 +52,7 @@ static const struct { #else #error "Unknown board type for AnalogIn scaling" #endif + { 0, 0.f }, }; using namespace VRBRAIN; @@ -237,6 +238,11 @@ void VRBRAINAnalogIn::next_stop_pin(void) */ void VRBRAINAnalogIn::_timer_tick(void) { + if (_adc_fd == -1) { + // not initialised yet + return; + } + // read adc at 100Hz uint32_t now = AP_HAL::micros(); uint32_t delta_t = now - _last_run; diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.h b/libraries/AP_HAL_VRBRAIN/AnalogIn.h index eb55bd7910..ad16a9bf9d 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.h +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.h @@ -53,15 +53,15 @@ private: class VRBRAIN::VRBRAINAnalogIn : public AP_HAL::AnalogIn { public: VRBRAINAnalogIn(); - void init(); - AP_HAL::AnalogSource* channel(int16_t pin); + void init() override; + AP_HAL::AnalogSource* channel(int16_t pin) override; void _timer_tick(void); - float board_voltage(void) { return _board_voltage; } - float servorail_voltage(void) { return _servorail_voltage; } - uint16_t power_status_flags(void) { return _power_flags; } + float board_voltage(void) override { return _board_voltage; } + float servorail_voltage(void) override { return _servorail_voltage; } + uint16_t power_status_flags(void) override { return _power_flags; } private: - int _adc_fd; + int _adc_fd = -1; int _battery_handle; int _servorail_handle; int _system_power_handle; diff --git a/libraries/AP_HAL_VRBRAIN/CAN.cpp b/libraries/AP_HAL_VRBRAIN/CAN.cpp new file mode 100644 index 0000000000..3735834b18 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/CAN.cpp @@ -0,0 +1,1117 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + * + * With modifications for Ardupilot CAN driver + * Copyright (C) 2017 Eugene Shamaev + */ + +#include +#include + +#include +#include + +#if HAL_WITH_UAVCAN + +#include +#include +#include "CAN.h" + +#include +#include +#include + +#include + +#include "Scheduler.h" + +/* + * 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; + +#include + +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) { + uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) { + 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) { + uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) { + 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_BoardConfig_CAN::get_can_debug() >= 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_BoardConfig_CAN::get_can_debug() >= 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_BoardConfig_CAN::get_can_debug() >= 1) { + printf("VRBRAINCAN::computeTimings target_bitrate error\n\r"); + } + return -ErrLogic; + } + + if (AP_BoardConfig_CAN::get_can_debug() >= 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) { + 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; + } + + return -ErrFilterNumConfigs; +} + +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_BoardConfig_CAN::get_can_debug() >= 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_BoardConfig_CAN::get_can_debug() >= 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_BoardConfig_CAN::get_can_debug() >= 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) { + + 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) { + + 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) { + 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() +{ + int32_t ret = -1; + + if (initialized_) { + 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), p_uavcan(nullptr) +{ + 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) { + if (!ifaces[i]->isRxBufferEmpty()) { + msk.read |= 1 << i; + } + + if (pending_tx[i] != nullptr) { + 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) { + 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) +{ + int res = -ErrNotImplemented; + static bool initialized_once[CAN_STM32_NUM_IFACES]; + + if (can_number < CAN_STM32_NUM_IFACES) { + res = 0; + + if (AP_BoardConfig_CAN::get_can_debug(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_BoardConfig_CAN::get_can_debug(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_BoardConfig_CAN::get_can_debug(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_BoardConfig_CAN::get_can_debug(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_BoardConfig_CAN::get_can_debug(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; +} + +AP_UAVCAN *VRBRAINCANManager::get_UAVCAN(void) +{ + return p_uavcan; +} + +void VRBRAINCANManager::set_UAVCAN(AP_UAVCAN *uavcan) +{ + p_uavcan = uavcan; +} + +/* + * 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 new file mode 100644 index 0000000000..f69fb2b087 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/CAN.h @@ -0,0 +1,317 @@ +/* + * 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]; + + AP_UAVCAN *p_uavcan; + +public: + VRBRAINCANManager(); + + /** + * This function returns select masks indicating which interfaces are available for read/write. + */ + uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const; + + /** + * Whether there's at least one interface where receive() would return a frame. + */ + bool hasReadableInterfaces() const; + + /** + * Returns zero if OK. + * Returns negative value if failed (e.g. invalid bitrate). + */ + int init(const uint32_t bitrate, const VRBRAINCAN::OperatingMode mode, uint8_t can_number); + + virtual VRBRAINCAN* getIface(uint8_t iface_index) override; + VRBRAINCAN* getIface_out_to_in(uint8_t iface_index); + + virtual uint8_t getNumIfaces() const override + { + return _ifaces_num; + } + + /** + * Whether at least one iface had at least one successful IO since previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); + + bool begin(uint32_t bitrate, uint8_t can_number) override; + + bool is_initialized() override; + void initialized(bool val) override; + + AP_UAVCAN *get_UAVCAN(void) override; + void set_UAVCAN(AP_UAVCAN *uavcan) override; +}; +} diff --git a/libraries/AP_HAL_VRBRAIN/Device.cpp b/libraries/AP_HAL_VRBRAIN/Device.cpp new file mode 100644 index 0000000000..de8dc64a5d --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/Device.cpp @@ -0,0 +1,155 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#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 new file mode 100644 index 0000000000..25a012efcb --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/Device.h @@ -0,0 +1,49 @@ +/* + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published + * by the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#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 index 6b46633226..36415e5025 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.cpp +++ b/libraries/AP_HAL_VRBRAIN/GPIO.cpp @@ -29,6 +29,7 @@ VRBRAINGPIO::VRBRAINGPIO() void VRBRAINGPIO::init() { + _led_fd = open(LED0_DEVICE_PATH, O_RDWR); if (_led_fd == -1) { AP_HAL::panic("Unable to open " LED0_DEVICE_PATH); @@ -37,12 +38,15 @@ void VRBRAINGPIO::init() hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n"); } if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED RED\n"); + hal.console->printf("GPIO: Unable to setup GPIO LED RED\n"); } + if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) { - hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); + hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); } + + _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (_tone_alarm_fd == -1) { AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH); @@ -67,6 +71,11 @@ void VRBRAINGPIO::init() hal.console->printf("GPIO: Unable to setup GPIO_3\n"); } #endif +#ifdef GPIO_SERVO_4 + if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_4) != 0) { + hal.console->printf("GPIO: Unable to setup GPIO_4\n"); + } +#endif } void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output) @@ -92,6 +101,14 @@ uint8_t VRBRAINGPIO::read(uint8_t pin) { } #endif +#ifdef GPIO_SERVO_4 + case EXTERNAL_RELAY2_PIN: { + uint32_t relays = 0; + ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays); + return (relays & GPIO_SERVO_4)?HIGH:LOW; + } +#endif + } return LOW; } @@ -142,6 +159,11 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value) break; #endif +#ifdef GPIO_SERVO_4 + case EXTERNAL_RELAY2_PIN: + ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_4); + break; +#endif } } diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.h b/libraries/AP_HAL_VRBRAIN/GPIO.h index cc1d818638..0a9ac5b2cb 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.h +++ b/libraries/AP_HAL_VRBRAIN/GPIO.h @@ -11,7 +11,8 @@ # define EXTERNAL_LED_MOTOR1 30 # define EXTERNAL_LED_MOTOR2 31 - # define EXTERNAL_RELAY1_PIN 33 + # define EXTERNAL_RELAY1_PIN 34 + # define EXTERNAL_RELAY2_PIN 33 # define HAL_GPIO_LED_ON HIGH # define HAL_GPIO_LED_OFF LOW @@ -20,21 +21,21 @@ class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO { public: VRBRAINGPIO(); - void init(); - void pinMode(uint8_t pin, uint8_t output); - int8_t analogPinToDigitalPin(uint8_t pin); - uint8_t read(uint8_t pin); - void write(uint8_t pin, uint8_t value); - void toggle(uint8_t pin); + void init() override; + void pinMode(uint8_t pin, uint8_t output) override; + int8_t analogPinToDigitalPin(uint8_t pin) override; + uint8_t read(uint8_t pin) override; + void write(uint8_t pin, uint8_t value) override; + void toggle(uint8_t pin) override; /* Alternative interface: */ - AP_HAL::DigitalSource* channel(uint16_t n); + AP_HAL::DigitalSource* channel(uint16_t n) override; /* Interrupt interface: */ - bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode); + bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override; /* return true if USB cable is connected */ - bool usb_connected(void); + bool usb_connected(void) override; // used by UART code to avoid a hw bug in the AUAV-X2 void set_usb_connected(void) { _usb_connected = true; } diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp index 86c7fdeb2f..7993c88210 100644 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp +++ b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp @@ -2,6 +2,10 @@ #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" @@ -14,9 +18,11 @@ #include "Util.h" #include "GPIO.h" #include "I2CDevice.h" +#include "SPIDevice.h" -#include -#include +#if HAL_WITH_UAVCAN +#include "CAN.h" +#endif #include #include @@ -28,8 +34,8 @@ #include using namespace VRBRAIN; +using namespace ap; -static Empty::SPIDeviceManager spiDeviceManager; //static Empty::GPIO gpioDriver; static VRBRAINScheduler schedulerInstance; @@ -41,6 +47,7 @@ static VRBRAINUtil utilInstance; static VRBRAINGPIO gpioDriver; static VRBRAIN::I2CDeviceManager i2c_mgr_instance; +static VRBRAIN::SPIDeviceManager spi_mgr_instance; #if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" @@ -67,14 +74,14 @@ static VRBRAIN::I2CDeviceManager i2c_mgr_instance; #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" #define UARTB_DEFAULT_DEVICE "/dev/ttyS0" #define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/null" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" #define UARTE_DEFAULT_DEVICE "/dev/null" #define UARTF_DEFAULT_DEVICE "/dev/null" #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" #define UARTB_DEFAULT_DEVICE "/dev/ttyS0" #define UARTC_DEFAULT_DEVICE "/dev/ttyS2" -#define UARTD_DEFAULT_DEVICE "/dev/null" +#define UARTD_DEFAULT_DEVICE "/dev/ttyS1" #define UARTE_DEFAULT_DEVICE "/dev/null" #define UARTF_DEFAULT_DEVICE "/dev/null" #elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) @@ -117,7 +124,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() : &uartEDriver, /* uartE */ &uartFDriver, /* uartF */ &i2c_mgr_instance, - &spiDeviceManager, /* spi */ + &spi_mgr_instance, &analogIn, /* analogin */ &storageDriver, /* storage */ &uartADriver, /* console */ @@ -126,7 +133,8 @@ HAL_VRBRAIN::HAL_VRBRAIN() : &rcoutDriver, /* rcoutput */ &schedulerInstance, /* scheduler */ &utilInstance, /* util */ - nullptr) /* no onboard optical flow */ + nullptr, /* no onboard optical flow */ + nullptr) /* CAN */ {} bool _vrbrain_thread_should_exit = false; /**< Daemon exit flag */ @@ -168,12 +176,10 @@ static int main_loop(int argc, char **argv) hal.uartD->begin(57600); hal.uartE->begin(57600); hal.scheduler->init(); - hal.rcin->init(); - hal.rcout->init(); - hal.analogin->init(); - hal.gpio->init(); - + // init the I2C wrapper class + VRBRAIN_I2C::init_lock(); + /* run setup() at low priority to ensure CLI doesn't hang the system, and to allow initial sensor read loops to run @@ -240,6 +246,7 @@ static void usage(void) printf("\t-d2 DEVICE set second terminal device (default %s)\n", UARTC_DEFAULT_DEVICE); printf("\t-d3 DEVICE set 3rd terminal device (default %s)\n", UARTD_DEFAULT_DEVICE); printf("\t-d4 DEVICE set 2nd GPS device (default %s)\n", UARTE_DEFAULT_DEVICE); + printf("\t-d5 DEVICE set uartF (default %s)\n", UARTF_DEFAULT_DEVICE); printf("\n"); } @@ -251,6 +258,7 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const const char *deviceC = UARTC_DEFAULT_DEVICE; const char *deviceD = UARTD_DEFAULT_DEVICE; const char *deviceE = UARTE_DEFAULT_DEVICE; + const char *deviceF = UARTF_DEFAULT_DEVICE; if (argc < 1) { printf("%s: missing command (try '%s start')", @@ -274,8 +282,10 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const uartCDriver.set_device_path(deviceC); uartDDriver.set_device_path(deviceD); uartEDriver.set_device_path(deviceE); - printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\n", - SKETCHNAME, deviceA, deviceC, deviceD, deviceE); + uartFDriver.set_device_path(deviceF); + + printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s uartF=%s\n", + SKETCHNAME, deviceA, deviceC, deviceD, deviceE, deviceF); _vrbrain_thread_should_exit = false; daemon_task = px4_task_spawn_cmd(SKETCHNAME, @@ -346,6 +356,17 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const exit(1); } } + + if (strcmp(argv[i], "-d5") == 0) { + // set uartF + if (argc > i + 1) { + deviceF = strdup(argv[i+1]); + } else { + printf("missing parameter to -d5 DEVICE\n"); + usage(); + exit(1); + } + } } usage(); diff --git a/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp b/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp index d1e09f4f3a..ca9dfd71bd 100644 --- a/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp +++ b/libraries/AP_HAL_VRBRAIN/I2CDevice.cpp @@ -17,48 +17,132 @@ #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 VRBRAIN_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers) { + set_address(address); if (!init_done) { + if (pthread_mutex_lock(&instance_lock) != 0) { + return false; + } init_done = true; // we do late init() so we can setup the device paths + snprintf(devname, sizeof(devname), "AP_I2C_%u", instance); snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance); init_ok = (init() == OK); if (init_ok) { instance++; } + pthread_mutex_unlock(&instance_lock); } if (!init_ok) { return false; } - set_address(address); - bool ret = (transfer(send, send_len, recv, recv_len) == OK); - return ret; + + if (split_transfers) { + /* + splitting the transfer() into two pieces avoids a stop condition + with SCL low which is not supported on some devices (such as + LidarLite blue label) + */ + if (send && send_len) { + if (transfer(send, send_len, nullptr, 0) != OK) { + return false; + } + } + if (recv && recv_len) { + if (transfer(nullptr, 0, recv, recv_len) != OK) { + return false; + } + } + } else { + // combined transfer + if (transfer(send, send_len, recv, recv_len) != OK) { + return false; + } + } + return true; } - + I2CDevice::I2CDevice(uint8_t bus, uint8_t address) : - _bus(bus), + _busnum(bus), + _vrbraindev(_busnum), _address(address) { + set_device_bus(bus); + set_device_address(address); + asprintf(&pname, "I2C:%u:%02x", + (unsigned)bus, (unsigned)address); + perf = perf_alloc(PC_ELAPSED, pname); } I2CDevice::~I2CDevice() { + printf("I2C device bus %u address 0x%02x closed\n", + (unsigned)_busnum, (unsigned)_address); + perf_free(perf); + free(pname); } bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) { - return _bus.do_transfer(_address, send, send_len, recv, recv_len); + perf_begin(perf); + bool ret = _vrbraindev.do_transfer(_address, send, send_len, recv, recv_len, _split_transfers); + perf_end(perf); + return ret; } bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, @@ -67,6 +151,34 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, return false; } + +/* + register a periodic callback +*/ +AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + if (_busnum >= num_buses) { + return nullptr; + } + struct DeviceBus &binfo = businfo[_busnum]; + return binfo.register_periodic_callback(period_usec, cb, this); +} + + +/* + adjust a periodic callback +*/ +bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + if (_busnum >= num_buses) { + return false; + } + + struct DeviceBus &binfo = businfo[_busnum]; + + return binfo.adjust_timer(h, period_usec); +} + AP_HAL::OwnPtr I2CDeviceManager::get_device(uint8_t bus, uint8_t address, uint32_t bus_clock, diff --git a/libraries/AP_HAL_VRBRAIN/I2CDevice.h b/libraries/AP_HAL_VRBRAIN/I2CDevice.h index 8be434ba1d..0c10f5be6e 100644 --- a/libraries/AP_HAL_VRBRAIN/I2CDevice.h +++ b/libraries/AP_HAL_VRBRAIN/I2CDevice.h @@ -21,9 +21,9 @@ #include #include #include -#include -#include +#include "Semaphores.h" #include "I2CWrapper.h" +#include "Device.h" namespace VRBRAIN { @@ -41,7 +41,7 @@ public: void set_address(uint8_t address) override { _address = address; } /* See AP_HAL::I2CDevice::set_retries() */ - void set_retries(uint8_t retries) override { _retries = retries; } + void set_retries(uint8_t retries) override { _vrbraindev.set_retries(retries); } /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ bool set_speed(enum Device::Speed speed) override { return true; } @@ -55,27 +55,30 @@ public: /* See AP_HAL::Device::register_periodic_callback() */ AP_HAL::Device::PeriodicHandle register_periodic_callback( - uint32_t period_usec, AP_HAL::Device::PeriodicCb) override - { - /* Not implemented yet */ - return nullptr; - } + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; /* See AP_HAL::Device::adjust_periodic_callback() */ - bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override - { - /* Not implemented yet */ - return false; + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override { + // if asking for invalid bus number use bus 0 semaphore + return &businfo[_busnum #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN @@ -13,11 +15,23 @@ extern const AP_HAL::HAL& hal; */ class VRBRAIN::VRBRAIN_I2C : public device::I2C { public: - VRBRAIN_I2C(uint8_t bus) : I2C(devname, devpath, bus, 0, 400000UL) { } - bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len); + VRBRAIN_I2C(uint8_t bus); + bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers); + void set_retries(uint8_t retries) { + _retries = retries; + } + + uint8_t map_bus_number(uint8_t bus) const; + + // setup instance_lock + static void init_lock(void) { + pthread_mutex_init(&instance_lock, nullptr); + } + private: static uint8_t instance; + static pthread_mutex_t instance_lock; bool init_done; bool init_ok; char devname[10]; diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.cpp b/libraries/AP_HAL_VRBRAIN/RCInput.cpp index 56535b8b27..e9c190ebe4 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCInput.cpp @@ -8,28 +8,42 @@ #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"); - } - clear_overrides(); - pthread_mutex_init(&rcin_mutex, nullptr); + _perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin"); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + if (_rc_sub == -1) { + AP_HAL::panic("Unable to subscribe to input_rc"); + } + clear_overrides(); + pthread_mutex_init(&rcin_mutex, nullptr); } bool VRBRAINRCInput::new_input() { pthread_mutex_lock(&rcin_mutex); - bool valid = _rcin.timestamp_last_signal != _last_read || _override_valid; + bool valid = _rcin.timestamp_last_signal != _last_read; + if (_rcin.rc_failsafe) { + // don't consider input valid if we are in RC failsafe. + valid = false; + } + if (_override_valid) { + // if we have RC overrides active, then always consider it valid + valid = true; + } _last_read = _rcin.timestamp_last_signal; _override_valid = false; pthread_mutex_unlock(&rcin_mutex); + if (_rcin.input_source != last_input_source) { + gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", input_source_name(_rcin.input_source)); + last_input_source = _rcin.input_source; + } return valid; } @@ -43,78 +57,106 @@ uint8_t VRBRAINRCInput::num_channels() uint16_t VRBRAINRCInput::read(uint8_t ch) { - if (ch >= RC_INPUT_MAX_CHANNELS) { - return 0; - } - pthread_mutex_lock(&rcin_mutex); - if (_override[ch]) { - uint16_t v = _override[ch]; - pthread_mutex_unlock(&rcin_mutex); - return v; - } - if (ch >= _rcin.channel_count) { - pthread_mutex_unlock(&rcin_mutex); - return 0; - } - uint16_t v = _rcin.values[ch]; + if (ch >= RC_INPUT_MAX_CHANNELS) { + return 0; + } + pthread_mutex_lock(&rcin_mutex); + if (_override[ch]) { + uint16_t v = _override[ch]; pthread_mutex_unlock(&rcin_mutex); return v; + } + if (ch >= _rcin.channel_count) { + pthread_mutex_unlock(&rcin_mutex); + return 0; + } + uint16_t v = _rcin.values[ch]; + pthread_mutex_unlock(&rcin_mutex); + return v; } uint8_t VRBRAINRCInput::read(uint16_t* periods, uint8_t len) { - if (len > RC_INPUT_MAX_CHANNELS) { - len = RC_INPUT_MAX_CHANNELS; - } - for (uint8_t i = 0; i < len; i++){ - periods[i] = read(i); - } - return len; + if (len > RC_INPUT_MAX_CHANNELS) { + len = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i = 0; i < len; i++){ + periods[i] = read(i); + } + return len; } bool VRBRAINRCInput::set_overrides(int16_t *overrides, uint8_t len) { - bool res = false; - for (uint8_t i = 0; i < len; i++) { - res |= set_override(i, overrides[i]); - } - return res; + bool res = false; + for (uint8_t i = 0; i < len; i++) { + res |= set_override(i, overrides[i]); + } + return res; } bool VRBRAINRCInput::set_override(uint8_t channel, int16_t override) { - if (override < 0) { - return false; /* -1: no change. */ - } - if (channel >= RC_INPUT_MAX_CHANNELS) { - return false; - } - _override[channel] = override; - if (override != 0) { - _override_valid = true; - return true; - } - return false; + if (override < 0) { + return false; /* -1: no change. */ + } + if (channel >= RC_INPUT_MAX_CHANNELS) { + return false; + } + _override[channel] = override; + if (override != 0) { + _override_valid = true; + return true; + } + return false; } void VRBRAINRCInput::clear_overrides() { - for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { - set_override(i, 0); - } + for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + set_override(i, 0); + } } +const char *VRBRAINRCInput::input_source_name(uint8_t id) const +{ + switch(id) { + case input_rc_s::RC_INPUT_SOURCE_UNKNOWN: return "UNKNOWN"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM: return "PX4FMU_PPM"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM: return "PX4IO_PPM"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM: return "PX4IO_SPEKTRUM"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS: return "PX4IO_SBUS"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24: return "PX4IO_ST24"; + case input_rc_s::RC_INPUT_SOURCE_MAVLINK: return "MAVLINK"; + case input_rc_s::RC_INPUT_SOURCE_QURT: return "QURT"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SPEKTRUM: return "PX4FMU_SPEKTRUM"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS: return "PX4FMU_SBUS"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24: return "PX4FMU_ST24"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD: return "PX4FMU_SUMD"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM: return "PX4FMU_DSM"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD: return "PX4IO_SUMD"; + case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SRXL: return "PX4FMU_SRXL"; + case input_rc_s::RC_INPUT_SOURCE_PX4IO_SRXL: return "PX4IO_SRXL"; + default: return "ERROR"; + } +} + + void VRBRAINRCInput::_timer_tick(void) { - perf_begin(_perf_rcin); - bool rc_updated = false; - if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) { - pthread_mutex_lock(&rcin_mutex); - orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin); - pthread_mutex_unlock(&rcin_mutex); - } - // note, we rely on the vehicle code checking new_input() - // and a timeout for the last valid input to handle failsafe - perf_end(_perf_rcin); + perf_begin(_perf_rcin); + bool rc_updated = false; + if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) { + pthread_mutex_lock(&rcin_mutex); + orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin); + if (_rcin.rssi != 0 || _rssi != -1) { + // always zero means not supported + _rssi = _rcin.rssi; + } + pthread_mutex_unlock(&rcin_mutex); + } + // note, we rely on the vehicle code checking new_input() + // and a timeout for the last valid input to handle failsafe + perf_end(_perf_rcin); } bool VRBRAINRCInput::rc_bind(int dsmMode) @@ -132,6 +174,9 @@ bool VRBRAINRCInput::rc_bind(int dsmMode) + + + return true; } diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.h b/libraries/AP_HAL_VRBRAIN/RCInput.h index bfb9ddc6b9..4a1779116d 100644 --- a/libraries/AP_HAL_VRBRAIN/RCInput.h +++ b/libraries/AP_HAL_VRBRAIN/RCInput.h @@ -12,19 +12,24 @@ class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput { public: - void init(); - bool new_input(); - uint8_t num_channels(); - uint16_t read(uint8_t ch); - uint8_t read(uint16_t* periods, uint8_t len); + void init() override; + bool new_input() override; + uint8_t num_channels() override; + uint16_t read(uint8_t ch) override; + uint8_t read(uint16_t* periods, uint8_t len) override; - bool set_overrides(int16_t *overrides, uint8_t len); - bool set_override(uint8_t channel, int16_t override); - void clear_overrides(); + int16_t get_rssi(void) override { + return _rssi; + } + + + bool set_overrides(int16_t *overrides, uint8_t len) override; + bool set_override(uint8_t channel, int16_t override) override; + void clear_overrides() override; void _timer_tick(void); - bool rc_bind(int dsmMode); + bool rc_bind(int dsmMode) override; private: /* override state */ @@ -35,4 +40,8 @@ private: bool _override_valid; perf_counter_t _perf_rcin; pthread_mutex_t rcin_mutex; + int16_t _rssi = -1; + + uint8_t last_input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + const char *input_source_name(uint8_t id) const; }; diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp index 7c7d050f64..4461b47333 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp @@ -9,11 +9,17 @@ #include #include -#include #include #include #include +#include + +#if HAL_WITH_UAVCAN +#include +#include +#endif + extern const AP_HAL::HAL& hal; using namespace VRBRAIN; @@ -38,7 +44,8 @@ void VRBRAINRCOutput::init() hal.console->printf("RCOutput: Unable to setup IO arming OK\n"); } - _rate_mask = 0; + _rate_mask_main = 0; + _rate_mask_alt = 0; _alt_fd = -1; _servo_count = 0; _alt_servo_count = 0; @@ -53,24 +60,22 @@ void VRBRAINRCOutput::init() } -// _alt_fd = open("/dev/px4fmu", O_RDWR); -// if (_alt_fd == -1) { -// hal.console->printf("RCOutput: failed to open /dev/px4fmu"); -// return; -// } + + + + + + + + + // ensure not to write zeros to disabled channels - _enabled_channels = 0; for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) { _period[i] = PWM_IGNORE_THIS_CHANNEL; + _last_sent[i] = PWM_IGNORE_THIS_CHANNEL; } - - // publish actuator vaules on demand - _actuator_direct_pub = nullptr; - - // and armed state - _actuator_armed_pub = nullptr; } @@ -95,8 +100,14 @@ void VRBRAINRCOutput::_init_alt_channels(void) /* set output frequency on outputs associated with fd */ -void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) +void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask) { + if (_output_mode == MODE_PWM_BRUSHED) { + freq_hz /= 8; // divide by 8 for 8MHz clock + // remember max period + _period_max = 1000000UL/freq_hz; + } + // we can't set this per channel if (freq_hz > 50 || freq_hz == 1) { // we're being asked to set the alt rate @@ -108,7 +119,7 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) */ freq_hz = 1; } - //::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz); + //::printf("SET_UPDATE_RATE %d %u\n", fd, (unsigned)freq_hz); if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) { hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz); return; @@ -118,10 +129,15 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) /* work out the new rate mask. The outputs have 4 groups of servos. + Group 0: channels 0 1 2 3 + Group 1: channels 4 5 + Group 2: channels 6 7 + Group 3: channels 8 9 10 + Group 0: channels 0 1 2 Group 1: channels 3 4 5 Group 2: channels 6 7 - Group 8: channels 8 9 10 11 + Group 3: channels 8 9 10 11 Channels within a group must be set to the same rate. @@ -130,39 +146,72 @@ void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) */ if (freq_hz > 50 || freq_hz == 1) { // we are setting high rates on the given channels - _rate_mask |= chmask & 0xFF; - if (_rate_mask & 0x07) { - _rate_mask |= 0x07; + rate_mask |= chmask & 0xFFF; +#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) + if (rate_mask & 0x0F) { + rate_mask |= 0x0F; } - if (_rate_mask & 0x38) { - _rate_mask |= 0x38; + if (rate_mask & 0x30) { + rate_mask |= 0x30; } - if (_rate_mask & 0xC0) { - _rate_mask |= 0xC0; + if (rate_mask & 0xC0) { + rate_mask |= 0xC0; } - if (_rate_mask & 0xF00) { - _rate_mask |= 0xF00; + if (rate_mask & 0x700) { + rate_mask |= 0x700; } +#else + if (rate_mask & 0x07) { + rate_mask |= 0x07; + } + if (rate_mask & 0x38) { + rate_mask |= 0x38; + } + if (rate_mask & 0xC0) { + rate_mask |= 0xC0; + } + if (rate_mask & 0xF00) { + rate_mask |= 0xF00; + } +#endif } else { // we are setting low rates on the given channels - if (chmask & 0x07) { - _rate_mask &= ~0x07; +#if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) + if (chmask & 0x0F) { + rate_mask &= ~0x0F; } - if (chmask & 0x38) { - _rate_mask &= ~0x38; + if (chmask & 0x30) { + rate_mask &= ~0x30; } if (chmask & 0xC0) { - _rate_mask &= ~0xC0; + rate_mask &= ~0xC0; + } + if (chmask & 0x700) { + rate_mask &= ~0x700; + } +#else + if (chmask & 0x07) { + rate_mask &= ~0x07; + } + if (chmask & 0x38) { + rate_mask &= ~0x38; + } + if (chmask & 0xC0) { + rate_mask &= ~0xC0; } if (chmask & 0xF00) { - _rate_mask &= ~0xF00; + rate_mask &= ~0xF00; } +#endif } - //::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask); - if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) { - hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask); + if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, rate_mask) != 0) { + hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)rate_mask); } + + if (_output_mode == MODE_PWM_BRUSHED) { + ioctl(fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); + } } /* @@ -180,30 +229,32 @@ void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) hal.console->printf("RCOutput: Unable to get servo count\n"); return; } - if (_alt_fd != -1 && ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) { - hal.console->printf("RCOutput: Unable to get alt servo count\n"); - return; - } // greater than 400 doesn't give enough room at higher periods for // the down pulse - if (freq_hz > 400) { + if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) { freq_hz = 400; } uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1); uint32_t alt_mask = chmask >> _servo_count; if (primary_mask && _pwm_fd != -1) { - set_freq_fd(_pwm_fd, primary_mask, freq_hz); + set_freq_fd(_pwm_fd, primary_mask, freq_hz, _rate_mask_main); } if (alt_mask && _alt_fd != -1) { - set_freq_fd(_alt_fd, alt_mask, freq_hz); + set_freq_fd(_alt_fd, alt_mask, freq_hz, _rate_mask_alt); } } uint16_t VRBRAINRCOutput::get_freq(uint8_t ch) { - if (_rate_mask & (1U<= 100) { - if (hal.util->safety_switch_state() == _safety_state_request) { - // current switch state matches request, stop attempting + now - _safety_state_request_last_ms >= 100) { + if (hal.util->safety_switch_state() == _safety_state_request && + _safety_state_request_last_ms != 1) { _safety_state_request_last_ms = 0; } else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) { // current != requested, set it ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); - _safety_state_request_last_ms = AP_HAL::millis(); + _safety_state_request_last_ms = now; } else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) { // current != requested, set it ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - _safety_state_request_last_ms = AP_HAL::millis(); + _safety_state_request_last_ms = now; } } } @@ -320,6 +373,24 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us) if (ch >= _max_channel) { _max_channel = ch + 1; } + + // keep unscaled value + _last_sent[ch] = period_us; + + if (_output_mode == MODE_PWM_BRUSHED) { + // map from the PWM range to 0 t0 100% duty cycle. For 16kHz + // this ends up being 0 to 500 pulse width in units of + // 125usec. + if (period_us <= _esc_pwm_min) { + period_us = 0; + } else if (period_us >= _esc_pwm_max) { + period_us = _period_max; + } else { + uint32_t pdiff = period_us - _esc_pwm_min; + period_us = pdiff*_period_max/(_esc_pwm_max - _esc_pwm_min); + } + } + /* only mark an update is needed if there has been a change, or we are in oneshot mode. In oneshot mode we always need to send the @@ -329,7 +400,7 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us) _output_mode == MODE_PWM_ONESHOT) { _period[ch] = period_us; _need_update = true; - up_pwm_servo_set(ch, period_us); +// up_pwm_servo_set(ch, period_us); } } @@ -364,7 +435,7 @@ uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch) return 0; } - return _period[ch]; + return _last_sent[ch]; } void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len) @@ -386,7 +457,11 @@ void VRBRAINRCOutput::_arm_actuators(bool arm) _armed.timestamp = hrt_absolute_time(); _armed.armed = arm; - _armed.ready_to_arm = arm; + if (arm) { + // this latches ready_to_arm to true once set once. Otherwise + // we have a race condition with px4io safety switch update + _armed.ready_to_arm = true; + } _armed.lockdown = false; _armed.force_failsafe = false; @@ -397,51 +472,6 @@ void VRBRAINRCOutput::_arm_actuators(bool arm) } } -/* - publish new outputs to the actuator_direct topic - */ -void VRBRAINRCOutput::_publish_actuators(void) -{ - struct actuator_direct_s actuators; - - if (_esc_pwm_min == 0 || - _esc_pwm_max == 0) { - // not initialised yet - return; - } - - actuators.nvalues = _max_channel; - if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) { - actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT; - } - // don't publish more than 8 actuators for now, as the uavcan ESC - // driver refuses to update any motors if you try to publish more - // than 8 - if (actuators.nvalues > 8) { - actuators.nvalues = 8; - } - bool armed = hal.util->get_soft_armed(); - actuators.timestamp = hrt_absolute_time(); - for (uint8_t i=0; isafety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { - _arm_actuators(true); - } -} - void VRBRAINRCOutput::_send_outputs(void) { uint32_t now = AP_HAL::micros(); @@ -478,7 +508,7 @@ void VRBRAINRCOutput::_send_outputs(void) } if (to_send > 0) { for (int i=to_send-1; i >= 0; i--) { - if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) { + if (_period[i] == PWM_IGNORE_THIS_CHANNEL) { to_send = i; } else { break; @@ -486,6 +516,8 @@ void VRBRAINRCOutput::_send_outputs(void) } } if (to_send > 0) { + _arm_actuators(true); + ::write(_pwm_fd, _period, to_send*sizeof(_period[0])); } if (_max_channel > _servo_count) { @@ -501,8 +533,35 @@ void VRBRAINRCOutput::_send_outputs(void) } } - // also publish to actuator_direct - _publish_actuators(); +#if HAL_WITH_UAVCAN + if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1) + { + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) + { + AP_UAVCAN *ap_uc = hal.can_mgr[i]->get_UAVCAN(); + if (ap_uc != nullptr) + { + if (ap_uc->rc_out_sem_take()) + { + for (uint8_t j = 0; j < _max_channel; j++) + { + ap_uc->rco_write(_period[j], j); + } + + if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { + ap_uc->rco_arm_actuators(true); + } else { + ap_uc->rco_arm_actuators(false); + } + + ap_uc->rc_out_sem_give(); + } + } + } + } + } +#endif // HAL_WITH_UAVCAN perf_end(_perf_rcout); _last_output = now; @@ -531,21 +590,20 @@ void VRBRAINRCOutput::cork() void VRBRAINRCOutput::push() { - if (!_corking) { - return; - } #if RCOUT_DEBUG_LATENCY hal.gpio->pinMode(55, HAL_GPIO_OUTPUT); hal.gpio->write(55, 0); #endif - _corking = false; - if (_output_mode == MODE_PWM_ONESHOT) { - // run timer immediately in oneshot mode - _send_outputs(); + if (_corking) { + _corking = false; + if (_output_mode == MODE_PWM_ONESHOT) { + // run timer immediately in oneshot mode + _send_outputs(); + } } } -void VRBRAINRCOutput::_timer_tick(void) +void VRBRAINRCOutput::timer_tick(void) { if (_output_mode != MODE_PWM_ONESHOT && !_corking) { /* in oneshot mode the timer does nothing as the outputs are @@ -596,22 +654,48 @@ void VRBRAINRCOutput::set_output_mode(enum output_mode mode) // mean the timer is constantly reset, so no pulses are // produced except when triggered by push() when the main loop // is running - set_freq(_rate_mask, 1); + set_freq_fd(_pwm_fd, _rate_mask_main, 1, _rate_mask_main); + if (_alt_fd != -1) { + set_freq_fd(_alt_fd, _rate_mask_alt, 1, _rate_mask_alt); + } } _output_mode = mode; - if (_output_mode == MODE_PWM_ONESHOT) { - //::printf("enable oneshot\n"); + switch (_output_mode) { + case MODE_PWM_ONESHOT: ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1); if (_alt_fd != -1) { ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1); } - } else { + break; + case MODE_PWM_NORMAL: ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0); if (_alt_fd != -1) { ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0); } + break; + case MODE_PWM_BRUSHED: + // setup an 8MHz clock. This has the effect of scaling all outputs by 8x + ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); + if (_alt_fd != -1) { + ioctl(_alt_fd, PWM_SERVO_SET_UPDATE_CLOCK, 8); + } + break; } } +// set default output update rate +void VRBRAINRCOutput::set_default_rate(uint16_t rate_hz) +{ + if (rate_hz != _default_rate_hz) { + // set servo update rate for first 8 pwm channels + ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); + if (_alt_fd != -1) { + // set servo update rate for auxiliary channels + ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz); + } + _default_rate_hz = rate_hz; + } +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.h b/libraries/AP_HAL_VRBRAIN/RCOutput.h index 638d5ceef8..2c798efe25 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.h +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.h @@ -29,19 +29,28 @@ public: _esc_pwm_min = min_pwm; _esc_pwm_max = max_pwm; } + float scale_esc_to_unity(uint16_t pwm) override { + return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0; + } void cork(); void push(); void set_output_mode(enum output_mode mode) override; - - void _timer_tick(void); + + void timer_tick(void) override; bool enable_px4io_sbus_out(uint16_t rate_hz) override; + // set default output update rate + void set_default_rate(uint16_t rate_hz) override; + private: int _pwm_fd; int _alt_fd; uint16_t _freq_hz; uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS]; + // we keep the last_sent value separately, as we need to keep the unscaled + // value for systems with brushed motors which scale outputs + uint16_t _last_sent[VRBRAIN_NUM_OUTPUT_CHANNELS]; volatile uint8_t _max_channel; volatile bool _need_update; bool _sbus_enabled:1; @@ -50,27 +59,28 @@ private: uint32_t _last_config_us; unsigned _servo_count; unsigned _alt_servo_count; - uint32_t _rate_mask; + uint32_t _rate_mask_main; + uint32_t _rate_mask_alt; uint16_t _enabled_channels; + uint32_t _period_max; struct { int pwm_sub; actuator_outputs_s outputs; } _outputs[ORB_MULTI_MAX_INSTANCES] {}; actuator_armed_s _armed; - orb_advert_t _actuator_direct_pub = nullptr; - orb_advert_t _actuator_armed_pub = nullptr; - uint16_t _esc_pwm_min = 0; - uint16_t _esc_pwm_max = 0; + orb_advert_t _actuator_armed_pub; + uint16_t _esc_pwm_min; + uint16_t _esc_pwm_max; void _init_alt_channels(void); - void _publish_actuators(void); void _arm_actuators(bool arm); - void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz); + void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask); bool _corking; enum output_mode _output_mode = MODE_PWM_NORMAL; void _send_outputs(void); enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE; uint32_t _safety_state_request_last_ms = 0; void force_safety_pending_requests(void); + uint16_t _default_rate_hz = 50; }; diff --git a/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp b/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp new file mode 100644 index 0000000000..70e7be4578 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/SPIDevice.cpp @@ -0,0 +1,329 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#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) { + printf("SPI: Invalid device name: %s\n", name); + 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 new file mode 100644 index 0000000000..6c9f5d5c64 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/SPIDevice.h @@ -0,0 +1,113 @@ +/* + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published + * by the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#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 index 369756b29a..77403538f8 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.cpp +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.cpp @@ -20,7 +20,14 @@ #include "Storage.h" #include "RCOutput.h" #include "RCInput.h" + #include +#include + +#if HAL_WITH_UAVCAN +#include "CAN.h" +#include +#endif using namespace VRBRAIN; @@ -32,7 +39,7 @@ VRBRAINScheduler::VRBRAINScheduler() : _perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")), _perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")), _perf_storage_timer(perf_alloc(PC_ELAPSED, "APM_storage_timers")), - _perf_delay(perf_alloc(PC_ELAPSED, "APM_delay")) + _perf_delay(perf_alloc(PC_ELAPSED, "APM_delay")) {} void VRBRAINScheduler::init() @@ -40,34 +47,34 @@ void VRBRAINScheduler::init() _main_task_pid = getpid(); // setup the timer thread - this will call tasks at 1kHz - pthread_attr_t thread_attr; - struct sched_param param; + pthread_attr_t thread_attr; + struct sched_param param; - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 2048); + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); - param.sched_priority = APM_TIMER_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); + 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); + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); - param.sched_priority = APM_UART_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); + 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); + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); - param.sched_priority = APM_IO_PRIORITY; - (void)pthread_attr_setschedparam(&thread_attr, ¶m); + 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); @@ -83,6 +90,34 @@ void VRBRAINScheduler::init() pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this); } +void VRBRAINScheduler::create_uavcan_thread() +{ +#if HAL_WITH_UAVCAN + pthread_attr_t thread_attr; + struct sched_param param; + + //the UAVCAN thread runs at medium priority + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 8192); + + param.sched_priority = APM_CAN_PRIORITY; + (void) pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) { + if (hal.can_mgr[i]->get_UAVCAN() != nullptr) { + _uavcan_thread_arg *arg = new _uavcan_thread_arg; + arg->sched = this; + arg->uavcan_number = i; + + pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg); + } + } + } +#endif +} + /** delay for a specified number of microseconds using a semaphore wait */ @@ -124,7 +159,7 @@ static void set_normal_priority(void *sem) a variant of delay_microseconds that boosts priority to APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC microseconds when the time completes. This significantly improves - the regularity of timing of the main loop as it takes + the regularity of timing of the main loop as it takes */ void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec) { @@ -143,9 +178,9 @@ void VRBRAINScheduler::delay(uint16_t ms) return; } perf_begin(_perf_delay); - uint64_t start = AP_HAL::micros64(); - - while ((AP_HAL::micros64() - start)/1000 < ms && + uint64_t start = AP_HAL::micros64(); + + while ((AP_HAL::micros64() - start)/1000 < ms && !_vrbrain_thread_should_exit) { delay_microseconds_semaphore(1000); if (_min_delay_cb_ms <= ms) { @@ -161,7 +196,7 @@ void VRBRAINScheduler::delay(uint16_t ms) } void VRBRAINScheduler::register_delay_callback(AP_HAL::Proc proc, - uint16_t min_time_ms) + uint16_t min_time_ms) { _delay_cb = proc; _min_delay_cb_ms = min_time_ms; @@ -227,7 +262,7 @@ void VRBRAINScheduler::reboot(bool hold_in_bootloader) // delay to ensure the async force_saftey operation completes delay(500); - px4_systemreset(hold_in_bootloader); + px4_systemreset(hold_in_bootloader); } void VRBRAINScheduler::_run_timers(bool called_from_timer_thread) @@ -266,6 +301,8 @@ void *VRBRAINScheduler::_timer_thread(void *arg) VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; uint32_t last_ran_overtime = 0; + pthread_setname_np(pthread_self(), "apm_timer"); + while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } @@ -278,7 +315,7 @@ void *VRBRAINScheduler::_timer_thread(void *arg) perf_end(sched->_perf_timers); // process any pending RC output requests - ((VRBRAINRCOutput *)hal.rcout)->_timer_tick(); + hal.rcout->timer_tick(); // process any pending RC input requests ((VRBRAINRCInput *)hal.rcin)->_timer_tick(); @@ -317,6 +354,8 @@ void *VRBRAINScheduler::_uart_thread(void *arg) { VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + pthread_setname_np(pthread_self(), "apm_uart"); + while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } @@ -338,11 +377,13 @@ void *VRBRAINScheduler::_io_thread(void *arg) { VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + pthread_setname_np(pthread_self(), "apm_io"); + while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_vrbrain_thread_should_exit) { - poll(nullptr, 0, 1); + sched->delay_microseconds_semaphore(1000); // run registered IO processes perf_begin(sched->_perf_io_timers); @@ -356,11 +397,13 @@ void *VRBRAINScheduler::_storage_thread(void *arg) { VRBRAINScheduler *sched = (VRBRAINScheduler *)arg; + pthread_setname_np(pthread_self(), "apm_storage"); + while (!sched->_hal_initialized) { poll(nullptr, 0, 1); } while (!_vrbrain_thread_should_exit) { - poll(nullptr, 0, 10); + sched->delay_microseconds_semaphore(10000); // process any pending storage writes perf_begin(sched->_perf_storage_timer); @@ -370,15 +413,46 @@ void *VRBRAINScheduler::_storage_thread(void *arg) return nullptr; } +#if HAL_WITH_UAVCAN +void *VRBRAINScheduler::_uavcan_thread(void *arg) +{ + VRBRAINScheduler *sched = ((_uavcan_thread_arg *) arg)->sched; + uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number; + + char name[15]; + snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number); + pthread_setname_np(pthread_self(), name); + + while (!sched->_hal_initialized) { + poll(nullptr, 0, 1); + } + + while (!_px4_thread_should_exit) { + if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->is_initialized()) { + if (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) { + (((VRBRAINCANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic(); + } else { + sched->delay_microseconds_semaphore(10000); + } + } else { + sched->delay_microseconds_semaphore(10000); + } + } + + return nullptr; +} +#endif + bool VRBRAINScheduler::in_main_thread() const { return getpid() == _main_task_pid; } -void VRBRAINScheduler::system_initialized() { +void VRBRAINScheduler::system_initialized() +{ if (_initialized) { AP_HAL::panic("PANIC: scheduler::system_initialized called" - "more than once"); + "more than once"); } _initialized = true; } diff --git a/libraries/AP_HAL_VRBRAIN/Scheduler.h b/libraries/AP_HAL_VRBRAIN/Scheduler.h index 77bcc7662b..fa0b82a201 100644 --- a/libraries/AP_HAL_VRBRAIN/Scheduler.h +++ b/libraries/AP_HAL_VRBRAIN/Scheduler.h @@ -13,6 +13,9 @@ #define APM_MAIN_PRIORITY_BOOST 241 #define APM_MAIN_PRIORITY 180 #define APM_TIMER_PRIORITY 181 +#define APM_SPI_PRIORITY 242 +#define APM_CAN_PRIORITY 179 +#define APM_I2C_PRIORITY 178 #define APM_UART_PRIORITY 60 #define APM_STORAGE_PRIORITY 59 #define APM_IO_PRIORITY 58 @@ -58,7 +61,9 @@ public: bool in_main_thread() const override; void system_initialized(); void hal_initialized() { _hal_initialized = true; } - + + void create_uavcan_thread() override; + private: bool _initialized; volatile bool _hal_initialized; @@ -83,11 +88,18 @@ private: pthread_t _io_thread_ctx; pthread_t _storage_thread_ctx; pthread_t _uart_thread_ctx; + pthread_t _uavcan_thread_ctx; + + struct _uavcan_thread_arg { + VRBRAINScheduler *sched; + uint8_t uavcan_number; + }; static void *_timer_thread(void *arg); static void *_io_thread(void *arg); static void *_storage_thread(void *arg); static void *_uart_thread(void *arg); + static void *_uavcan_thread(void *arg); void _run_timers(bool called_from_timer_thread); void _run_io(void); diff --git a/libraries/AP_HAL_VRBRAIN/Semaphores.cpp b/libraries/AP_HAL_VRBRAIN/Semaphores.cpp index ca29566930..6ee249fa7b 100644 --- a/libraries/AP_HAL_VRBRAIN/Semaphores.cpp +++ b/libraries/AP_HAL_VRBRAIN/Semaphores.cpp @@ -3,6 +3,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include "Semaphores.h" +#include extern const AP_HAL::HAL& hal; @@ -15,6 +16,10 @@ bool Semaphore::give() bool Semaphore::take(uint32_t timeout_ms) { + if (up_interrupt_context()) { + // don't ever wait on a semaphore in interrupt context + return take_nonblocking(); + } if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { return pthread_mutex_lock(&_lock) == 0; } diff --git a/libraries/AP_HAL_VRBRAIN/Storage.cpp b/libraries/AP_HAL_VRBRAIN/Storage.cpp index 6ffb70cc56..d262d90428 100644 --- a/libraries/AP_HAL_VRBRAIN/Storage.cpp +++ b/libraries/AP_HAL_VRBRAIN/Storage.cpp @@ -9,6 +9,9 @@ #include #include #include +#include + +#include #include "Storage.h" using namespace VRBRAIN; @@ -16,173 +19,48 @@ using namespace VRBRAIN; /* This stores eeprom data in the VRBRAIN MTD interface with a 4k size, and a in-memory buffer. This keeps the latency and devices IOs down. - */ +*/ // name the storage file after the sketch so you can use the same sd // card for ArduCopter and ArduPlane #define STORAGE_DIR "/fs/microsd/APM" -#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg" -#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak" //#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav" #define MTD_PARAMS_FILE "/fs/mtd" -#define MTD_SIGNATURE 0x14012014 -#define MTD_SIGNATURE_OFFSET (8192-4) -#define STORAGE_RENAME_OLD_FILE 0 extern const AP_HAL::HAL& hal; +extern "C" int mtd_main(int, char **); + VRBRAINStorage::VRBRAINStorage(void) : - _fd(-1), - _dirty_mask(0), _perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")), _perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors")) { } -/* - get signature from bytes at offset MTD_SIGNATURE_OFFSET - */ -uint32_t VRBRAINStorage::_mtd_signature(void) +void VRBRAINStorage::_storage_open(void) { - int mtd_fd = open(MTD_PARAMS_FILE, O_RDONLY); - if (mtd_fd == -1) { - AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); - } - uint32_t v; - if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) { - AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE); - } - bus_lock(true); - if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) { - AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE); - } - bus_lock(false); - close(mtd_fd); - return v; -} - -/* - put signature bytes at offset MTD_SIGNATURE_OFFSET - */ -void VRBRAINStorage::_mtd_write_signature(void) -{ - int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); - if (mtd_fd == -1) { - AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); - } - uint32_t v = MTD_SIGNATURE; - if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) { - AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE); - } - bus_lock(true); - if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) { - AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE); - } - bus_lock(false); - close(mtd_fd); -} - -/* - upgrade from microSD to MTD (FRAM) - */ -void VRBRAINStorage::_upgrade_to_mtd(void) -{ - // the MTD is completely uninitialised - try to get a - // copy from OLD_STORAGE_FILE - int old_fd = open(OLD_STORAGE_FILE, O_RDONLY); - if (old_fd == -1) { - ::printf("Failed to open %s\n", OLD_STORAGE_FILE); + if (_initialised) { return; } - int mtd_fd = open(MTD_PARAMS_FILE, O_WRONLY); - if (mtd_fd == -1) { - AP_HAL::panic("Unable to open MTD for upgrade"); - } + _dirty_mask.clearall(); - if (::read(old_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) { - close(old_fd); - close(mtd_fd); - ::printf("Failed to read %s\n", OLD_STORAGE_FILE); - return; - } - close(old_fd); - ssize_t ret; - bus_lock(true); - if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) { - ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno); - AP_HAL::panic("Unable to write MTD for upgrade"); - } - bus_lock(false); - close(mtd_fd); -#if STORAGE_RENAME_OLD_FILE - rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK); + // load from storage backend +#if USE_FLASH_STORAGE + _flash_load(); +#else + _mtd_load(); #endif - ::printf("Upgraded MTD from %s\n", OLD_STORAGE_FILE); -} - - -void VRBRAINStorage::_storage_open(void) -{ - if (_initialised) { - return; - } - - struct stat st; - _have_mtd = (stat(MTD_PARAMS_FILE, &st) == 0); - - // VRBRAIN should always have /fs/mtd_params - if (!_have_mtd) { - AP_HAL::panic("Failed to find " MTD_PARAMS_FILE); - } - - /* - cope with upgrading from OLD_STORAGE_FILE to MTD - */ - bool good_signature = (_mtd_signature() == MTD_SIGNATURE); - if (stat(OLD_STORAGE_FILE, &st) == 0) { - if (good_signature) { -#if STORAGE_RENAME_OLD_FILE - rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK); -#endif - } else { - _upgrade_to_mtd(); - } - } - - // we write the signature every time, even if it already is - // good, as this gives us a way to detect if the MTD device is - // functional. It is better to panic now than to fail to save - // parameters in flight - _mtd_write_signature(); - - _dirty_mask = 0; - int fd = open(MTD_PARAMS_FILE, O_RDONLY); - if (fd == -1) { - AP_HAL::panic("Failed to open " MTD_PARAMS_FILE); - } - const uint16_t chunk_size = 128; - for (uint16_t ofs=0; ofs>VRBRAIN_STORAGE_LINE_SHIFT; + for (uint16_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT; line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; line++) { - _dirty_mask |= 1U << line; + _dirty_mask.set(line); } } void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n) { - if (loc >= sizeof(_buffer)-(n-1)) { - return; - } - _storage_open(); - memcpy(dst, &_buffer[loc], n); + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + _storage_open(); + memcpy(dst, &_buffer[loc], n); } void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n) { - if (loc >= sizeof(_buffer)-(n-1)) { - return; - } - if (memcmp(src, &_buffer[loc], n) != 0) { - _storage_open(); - memcpy(&_buffer[loc], src, n); - _mark_dirty(loc, n); - } -} - -void VRBRAINStorage::bus_lock(bool lock) -{ - - - - - - - - - - - - - - - - + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + if (memcmp(src, &_buffer[loc], n) != 0) { + _storage_open(); + memcpy(&_buffer[loc], src, n); + _mark_dirty(loc, n); + } } void VRBRAINStorage::_timer_tick(void) { - if (!_initialised || _dirty_mask == 0) { - return; - } - perf_begin(_perf_storage); + if (!_initialised || _dirty_mask.empty()) { + return; + } + perf_begin(_perf_storage); - if (_fd == -1) { - _fd = open(MTD_PARAMS_FILE, O_WRONLY); - if (_fd == -1) { - perf_end(_perf_storage); - perf_count(_perf_errors); - return; - } - } +#if !USE_FLASH_STORAGE + if (_fd == -1) { + _fd = open(MTD_PARAMS_FILE, O_WRONLY); + if (_fd == -1) { + perf_end(_perf_storage); + perf_count(_perf_errors); + return; + } + } +#endif - // write out the first dirty set of lines. We don't write more - // than one to keep the latency of this call to a minimum - uint8_t i, n; - for (i=0; i>VRBRAIN_STORAGE_LINE_SHIFT); n++) { - if (!(_dirty_mask & (1<<(n+i)))) { - break; - } - // mark that line clean - write_mask |= (1<<(n+i)); - } + // write out the first dirty line. We don't write more + // than one to keep the latency of this call to a minimum + uint16_t i; + for (i=0; i 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 index 20be9ffbe7..529defc64f 100644 --- a/libraries/AP_HAL_VRBRAIN/Storage.h +++ b/libraries/AP_HAL_VRBRAIN/Storage.h @@ -1,18 +1,35 @@ #pragma once +/* + we can optionally use flash for storage instead of FRAM. That allows + ArduPilot to run on a wider range of boards and reduces board cost + */ +#ifndef USE_FLASH_STORAGE +#define USE_FLASH_STORAGE 0 +#endif + #include #include "AP_HAL_VRBRAIN_Namespace.h" #include +#include +#include #define VRBRAIN_STORAGE_SIZE HAL_STORAGE_SIZE -#define VRBRAIN_STORAGE_MAX_WRITE 512 + +#if USE_FLASH_STORAGE +// when using flash storage we use a small line size to make storage +// compact and minimise the number of erase cycles needed +#define VRBRAIN_STORAGE_LINE_SHIFT 3 +#else #define VRBRAIN_STORAGE_LINE_SHIFT 9 +#endif + #define VRBRAIN_STORAGE_LINE_SIZE (1<delay(1); @@ -85,7 +86,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) /* allocate the write buffer */ - if (txS != 0 && txS != _writebuf.get_size()) { + if (txS != _writebuf.get_size()) { _initialised = false; while (_in_timer) { hal.scheduler->delay(1); @@ -123,13 +124,11 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) if (strcmp(_devpath, "/dev/ttyACM0") == 0) { ((VRBRAINGPIO *)hal.gpio)->set_usb_connected(); } - ::printf("initialised %s OK %u %u\n", _devpath, + ::printf("initialised %s OK %u %u\n", _devpath, (unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size()); } _initialised = true; } - _uart_owner_pid = getpid(); - } void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol) @@ -154,6 +153,44 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol) _flow_control = fcontrol; } +void VRBRAINUARTDriver::configure_parity(uint8_t v) { + if (_fd == -1) { + return; + } + struct termios t; + tcgetattr(_fd, &t); + if (v != 0) { + // enable parity + t.c_cflag |= PARENB; + if (v == 1) { + t.c_cflag |= PARODD; + } else { + t.c_cflag &= ~PARODD; + } + } + else { + // disable parity + t.c_cflag &= ~PARENB; + } + tcsetattr(_fd, TCSANOW, &t); +} + +void VRBRAINUARTDriver::set_stop_bits(int n) { + if (_fd == -1) { + return; + } + struct termios t; + tcgetattr(_fd, &t); + if (n > 1) t.c_cflag |= CSTOPB; + else t.c_cflag &= ~CSTOPB; + tcsetattr(_fd, TCSANOW, &t); +} + +bool VRBRAINUARTDriver::set_unbuffered_writes(bool on) { + _unbuffered_writes = on; + return _unbuffered_writes; +} + void VRBRAINUARTDriver::begin(uint32_t b) { begin(b, 0, 0); @@ -197,9 +234,9 @@ void VRBRAINUARTDriver::end() void VRBRAINUARTDriver::flush() {} bool VRBRAINUARTDriver::is_initialized() -{ +{ try_initialise(); - return _initialised; + return _initialised; } void VRBRAINUARTDriver::set_blocking_writes(bool blocking) @@ -213,7 +250,7 @@ bool VRBRAINUARTDriver::tx_pending() { return false; } return number of bytes available to be read from the buffer */ uint32_t VRBRAINUARTDriver::available() -{ +{ if (!_initialised) { try_initialise(); return 0; @@ -226,7 +263,7 @@ uint32_t VRBRAINUARTDriver::available() return number of bytes that can be added to the write buffer */ uint32_t VRBRAINUARTDriver::txspace() -{ +{ if (!_initialised) { try_initialise(); return 0; @@ -239,62 +276,82 @@ uint32_t VRBRAINUARTDriver::txspace() read one byte from the read buffer */ int16_t VRBRAINUARTDriver::read() -{ - if (_uart_owner_pid != getpid()){ +{ + if (!_semaphore.take_nonblocking()) { return -1; } if (!_initialised) { try_initialise(); + _semaphore.give(); return -1; } uint8_t byte; - if (!_readbuf.read_byte(&byte)) + if (!_readbuf.read_byte(&byte)) { + _semaphore.give(); return -1; + } + _semaphore.give(); return byte; } -/* - write one byte to the buffer +/* + write one byte */ size_t VRBRAINUARTDriver::write(uint8_t c) -{ - if (_uart_owner_pid != getpid()){ - return 0; +{ + if (!_semaphore.take_nonblocking()) { + return -1; } if (!_initialised) { try_initialise(); + _semaphore.give(); return 0; } + if (_unbuffered_writes) { + // write one byte to the file descriptor + return _write_fd(&c, 1); + } + while (_writebuf.space() == 0) { if (_nonblocking_writes) { + _semaphore.give(); return 0; } + _semaphore.give(); hal.scheduler->delay(1); + if (!_semaphore.take_nonblocking()) { + return -1; + } } - return _writebuf.write(&c, 1); + size_t ret = _writebuf.write(&c, 1); + _semaphore.give(); + return ret; } /* - write size bytes to the write buffer + * write size bytes */ size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size) { - if (_uart_owner_pid != getpid()){ - return 0; + if (!_semaphore.take_nonblocking()) { + return -1; } - if (!_initialised) { + if (!_initialised) { try_initialise(); + _semaphore.give(); return 0; } + size_t ret = 0; + if (!_nonblocking_writes) { + _semaphore.give(); /* use the per-byte delay loop in write() above for blocking writes */ - size_t ret = 0; while (size--) { if (write(*buffer++) != 1) break; ret++; @@ -302,7 +359,14 @@ size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size) return ret; } - return _writebuf.write(buffer, size); + if (_unbuffered_writes) { + // write buffer straight to the file descriptor + ret = _write_fd(buffer, size); + } else { + ret = _writebuf.write(buffer, size); + } + _semaphore.give(); + return ret; } /* @@ -322,7 +386,7 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n) // and no data has been removed from the buffer since flow control turned on // and more than .5 seconds elapsed after writing a total of > 5 characters // - + int nwrite = 0; if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) { @@ -336,7 +400,7 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n) if (_os_start_auto_space - nwrite + 1 >= _total_written && (AP_HAL::micros64() - _first_write_time) > 500*1000UL) { // it doesn't look like hw flow control is working - ::printf("disabling flow control on %s _total_written=%u\n", + ::printf("disabling flow control on %s _total_written=%u\n", _devpath, (unsigned)_total_written); set_flow_control(FLOW_CONTROL_DISABLE); } diff --git a/libraries/AP_HAL_VRBRAIN/UARTDriver.h b/libraries/AP_HAL_VRBRAIN/UARTDriver.h index ea4929a70a..a9e3237b6d 100644 --- a/libraries/AP_HAL_VRBRAIN/UARTDriver.h +++ b/libraries/AP_HAL_VRBRAIN/UARTDriver.h @@ -1,9 +1,10 @@ #pragma once #include -#include #include "AP_HAL_VRBRAIN.h" +#include +#include "Semaphores.h" class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver { public: @@ -39,6 +40,10 @@ public: void set_flow_control(enum flow_control flow_control); enum flow_control get_flow_control(void) { return _flow_control; } + void configure_parity(uint8_t v); + void set_stop_bits(int n); + bool set_unbuffered_writes(bool on); + private: const char *_devpath; int _fd; @@ -47,12 +52,12 @@ private: volatile bool _in_timer; bool _nonblocking_writes; + bool _unbuffered_writes; // we use in-task ring buffers to reduce the system call cost // of ::read() and ::write() in the main loop - ByteBuffer _readbuf; - ByteBuffer _writebuf; - + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; perf_counter_t _perf_uart; int _write_fd(const uint8_t *buf, uint16_t n); @@ -68,6 +73,5 @@ private: uint32_t _total_written; enum flow_control _flow_control; - pid_t _uart_owner_pid; - + Semaphore _semaphore; }; diff --git a/libraries/AP_HAL_VRBRAIN/Util.cpp b/libraries/AP_HAL_VRBRAIN/Util.cpp index fe97086373..a61db5ec0a 100644 --- a/libraries/AP_HAL_VRBRAIN/Util.cpp +++ b/libraries/AP_HAL_VRBRAIN/Util.cpp @@ -73,6 +73,10 @@ bool VRBRAINUtil::run_debug_shell(AP_HAL::BetterStream *stream) */ enum VRBRAINUtil::safety_state VRBRAINUtil::safety_switch_state(void) { +#if !HAL_HAVE_SAFETY_SWITCH + return AP_HAL::Util::SAFETY_NONE; +#endif + if (_safety_handle == -1) { _safety_handle = orb_subscribe(ORB_ID(safety)); } @@ -96,7 +100,7 @@ void VRBRAINUtil::set_system_clock(uint64_t time_utc_usec) { timespec ts; ts.tv_sec = time_utc_usec/1000000ULL; - ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000UL; + ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL; clock_settime(CLOCK_REALTIME, &ts); } @@ -202,8 +206,12 @@ void VRBRAINUtil::set_imu_temp(float current) // experimentally tweaked for Pixhawk2 const float kI = 0.3f; const float kP = 200.0f; + float target = (float)(*_heater.target); + + // limit to 65 degrees to prevent damage + target = constrain_float(target, 0, 65); - float err = ((float)*_heater.target) - current; + float err = target - current; _heater.integrator += kI * err; _heater.integrator = constrain_float(_heater.integrator, 0, 70); @@ -226,4 +234,39 @@ void VRBRAINUtil::set_imu_target_temp(int8_t *target) _heater.target = target; } + +extern "C" { + extern void *fat_dma_alloc(size_t); + extern void fat_dma_free(void *, size_t); +} + +/* + allocate DMA-capable memory if possible. Otherwise return normal + memory. +*/ +void *VRBRAINUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) +{ + + + + + + + + return calloc(1, size); + +} +void VRBRAINUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) +{ + + + + + + + + return free(ptr); + +} + #endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_HAL_VRBRAIN/Util.h b/libraries/AP_HAL_VRBRAIN/Util.h index d9d1936fdd..db1afffa3a 100644 --- a/libraries/AP_HAL_VRBRAIN/Util.h +++ b/libraries/AP_HAL_VRBRAIN/Util.h @@ -29,7 +29,7 @@ private: class VRBRAIN::VRBRAINUtil : public AP_HAL::Util { public: - VRBRAINUtil(void); + VRBRAINUtil(void); bool run_debug_shell(AP_HAL::BetterStream *stream); enum safety_state safety_switch_state(void); @@ -60,7 +60,11 @@ public: void set_imu_temp(float current) override; void set_imu_target_temp(int8_t *target) override; - + + // allocate and free DMA-capable memory if possible. Otherwise return normal memory + void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; + void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; + private: int _safety_handle; VRBRAIN::NSHShellStream _shell_stream; diff --git a/libraries/AP_HAL_VRBRAIN/bxcan.h b/libraries/AP_HAL_VRBRAIN/bxcan.h new file mode 100644 index 0000000000..f5ad7cd0f7 --- /dev/null +++ b/libraries/AP_HAL_VRBRAIN/bxcan.h @@ -0,0 +1,294 @@ +/* + * 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/vrbrain_param.cpp b/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp index 12fdfba0ba..e9975dc6d9 100644 --- a/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp +++ b/libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp @@ -16,17 +16,16 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#include -#include /** parameter update topic */ ORB_DEFINE(parameter_update, struct parameter_update_s); -ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); -ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); param_t param_find(const char *name) { +#if 0 + // useful for driver debugging ::printf("VRBRAIN: param_find(%s)\n", name); +#endif return PARAM_INVALID; }