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

AP_HAL_ChibiOS: create UAVCAN agnostic CANIface driver

Browse Source 
HAL_ChibiOS
This commit is contained in:
Siddharth Purohit 2020-05-31 17:47:00 +05:30 committed by Andrew Tridgell
parent 15abca8ba4
commit cdf8e369f1
20 changed files with 1257 additions and 3486 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h
View File

@ -26,6 +26,6 @@ namespace ChibiOS {
class Shared_DMA;
class SoftSigReader;
class SoftSigReaderInt;
class CANManager;
class CANIface;
class Flash;
}

77
libraries/AP_HAL_ChibiOS/CAN.h
View File

@ -1,77 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#define CAN_STM32_LOG(fmt, ...) hal.console->printf("CANManager: " fmt "\n", ##__VA_ARGS__)
#include <uavcan/uavcan.hpp>
#include <uavcan/time.hpp>
#include "CANThread.h"
#include "CANClock.h"
#if defined(STM32H7XX)
#include "CANFDIface.h"
#else
#include "CANIface.h"
#endif
#define MAX_NUMBER_OF_CAN_INTERFACES 2
#define MAX_NUMBER_OF_CAN_DRIVERS 2
#define CAN_STM32_RX_QUEUE_SIZE 64
namespace ChibiOS {
/**
* Generic CAN driver.
*/
class CANManager: public AP_HAL::CANManager {
public:
CANManager()
: can_helper(), AP_HAL::CANManager(&can_helper.driver) { }
/**
* 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();
static CANManager *from(AP_HAL::CANManager *can)
{
return static_cast<CANManager*>(can);
}
bool begin(uint32_t bitrate, uint8_t can_number) override;
/*
Test if CAN manager is ready and initialized
return false - CAN manager not initialized
true - CAN manager is initialized
*/
bool is_initialized() override;
void initialized(bool val) override;
private:
bool initialized_;
ChibiOS_CAN::CanInitHelper<CAN_STM32_RX_QUEUE_SIZE> can_helper;
};
}
#endif

412
libraries/AP_HAL_ChibiOS/CANClock.cpp
View File

@ -1,412 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Modified for Ardupilot by Siddharth Bharat Purohit
*/
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#include "CANClock.h"
#include "CANThread.h"
#include "CANInternal.h"
#ifndef UAVCAN_STM32_TIMER_NUMBER
#define UAVCAN_STM32_TIMER_NUMBER 0
#endif
#if UAVCAN_STM32_TIMER_NUMBER
#include <cassert>
#include <cmath>
/*
* Timer instance
*/
# if (CH_KERNEL_MAJOR == 2)
# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
# define TIMX_INPUT_CLOCK STM32_TIMCLK1
# endif
# if (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4)
# define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER)
# define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER)
# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER)
# define TIMX_INPUT_CLOCK STM32_TIMCLK1
# else
# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
# endif
# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
# else
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
# endif
# if (TIMX_INPUT_CLOCK % 1000000) != 0
# error "No way, timer clock must be divisible to 1e6. FIXME!"
# endif
extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
namespace ChibiOS_CAN {
namespace clock {
namespace {
const uavcan::uint32_t USecPerOverflow = 65536;
Mutex mutex;
bool initialized = false;
bool utc_set = false;
bool utc_locked = false;
uavcan::uint32_t utc_jump_cnt = 0;
UtcSyncParams utc_sync_params;
float utc_prev_adj = 0;
float utc_rel_rate_ppm = 0;
float utc_rel_rate_error_integral = 0;
uavcan::int32_t utc_accumulated_correction_nsec = 0;
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
uavcan::MonotonicTime prev_utc_adj_at;
uavcan::uint64_t time_mono = 0;
uavcan::uint64_t time_utc = 0;
}
void init()
{
CriticalSectionLocker lock;
if (initialized) {
return;
}
initialized = true;
// Power-on and reset
TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK;
TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
// Enable IRQ
nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
# if (TIMX_INPUT_CLOCK % 1000000) != 0
# error "No way, timer clock must be divisible to 1e6. FIXME!"
# endif
// Start the timer
TIMX->ARR = 0xFFFF;
TIMX->PSC = (TIMX_INPUT_CLOCK / 1000000) - 1; // 1 tick == 1 microsecond
TIMX->CR1 = TIM_CR1_URS;
TIMX->SR = 0;
TIMX->EGR = TIM_EGR_UG; // Reload immediately
TIMX->DIER = TIM_DIER_UIE;
TIMX->CR1 = TIM_CR1_CEN; // Start
}
void setUtc(uavcan::UtcTime time)
{
MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized);
{
CriticalSectionLocker locker;
time_utc = time.toUSec();
}
utc_set = true;
utc_locked = false;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
static uavcan::uint64_t sampleUtcFromCriticalSection()
{
UAVCAN_ASSERT(initialized);
UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
volatile uavcan::uint64_t time = time_utc;
volatile uavcan::uint32_t cnt = TIMX->CNT;
if (TIMX->SR & TIM_SR_UIF) {
cnt = TIMX->CNT;
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
}
return time + cnt;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return utc_set ? sampleUtcFromCriticalSection() : 0;
}
uavcan::MonotonicTime getMonotonic()
{
uavcan::uint64_t usec = 0;
// Scope Critical section
{
CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono;
volatile uavcan::uint32_t cnt = TIMX->CNT;
if (TIMX->SR & TIM_SR_UIF) {
cnt = TIMX->CNT;
time += USecPerOverflow;
}
usec = time + cnt;
# ifndef NDEBUG
static uavcan::uint64_t prev_usec = 0; // Self-test
UAVCAN_ASSERT(prev_usec <= usec);
(void)prev_usec;
prev_usec = usec;
# endif
} // End Scope Critical section
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
{
if (utc_set) {
uavcan::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleUtcFromCriticalSection();
}
return uavcan::UtcTime::fromUSec(usec);
}
return uavcan::UtcTime();
}
static float lowpass(float xold, float xnew, float corner, float dt)
{
const float tau = 1.F / corner;
return (dt * xnew + tau * xold) / (dt + tau);
}
static void updateRatePID(uavcan::UtcDuration adjustment)
{
const uavcan::MonotonicTime ts = getMonotonic();
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
prev_utc_adj_at = ts;
const float adj_usec = float(adjustment.toUSec());
/*
* Target relative rate in PPM
* Positive to go faster
*/
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
/*
* Current relative rate in PPM
* Positive if the local clock is faster
*/
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
utc_prev_adj = adj_usec;
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
if (dt > 10) {
utc_rel_rate_error_integral = 0;
}
else {
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
utc_rel_rate_error_integral =
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
utc_rel_rate_error_integral =
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
}
/*
* Rate controller
*/
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
// total_rate_correction_ppm);
}
void adjustUtc(uavcan::UtcDuration adjustment)
{
MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized);
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
const uavcan::int64_t adj_usec = adjustment.toUSec();
{
CriticalSectionLocker locker;
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
time_utc = 1;
}
else {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
}
}
utc_set = true;
utc_locked = false;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
else {
updateRatePID(adjustment);
if (!utc_locked) {
utc_locked =
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
(std::abs(utc_prev_adj) < utc_sync_params.lock_thres_offset.toUSec());
}
}
}
float getUtcRateCorrectionPPM()
{
MutexLocker mlocker(mutex);
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
return 1e6F * rate_correction_mult;
}
uavcan::uint32_t getUtcJumpCount()
{
MutexLocker mlocker(mutex);
return utc_jump_cnt;
}
bool isUtcLocked()
{
MutexLocker mlocker(mutex);
return utc_locked;
}
UtcSyncParams getUtcSyncParams()
{
MutexLocker mlocker(mutex);
return utc_sync_params;
}
void setUtcSyncParams(const UtcSyncParams& params)
{
MutexLocker mlocker(mutex);
// Add some sanity check
utc_sync_params = params;
}
} // namespace clock
SystemClock& SystemClock::get_singleton()
{
static union SystemClockStorage {
uavcan::uint8_t buffer[sizeof(SystemClock)];
long long _aligner_1;
long double _aligner_2;
} storage;
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
if (!clock::initialized) {
MutexLocker mlocker(clock::mutex);
clock::init();
new (ptr)SystemClock();
}
return *ptr;
}
} // namespace uavcan_stm32
/**
* Timer interrupt handler
*/
extern "C"
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
TIMX->SR = 0;
using namespace uavcan_stm32::clock;
UAVCAN_ASSERT(initialized);
time_mono += USecPerOverflow;
if (utc_set) {
time_utc += USecPerOverflow;
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
utc_accumulated_correction_nsec %= 1000;
}
// Correction decay - 1 nsec per 65536 usec
if (utc_correction_nsec_per_overflow > 0) {
utc_correction_nsec_per_overflow--;
}
else if (utc_correction_nsec_per_overflow < 0) {
utc_correction_nsec_per_overflow++;
}
else {
; // Zero
}
}
UAVCAN_STM32_IRQ_EPILOGUE();
}
#endif
#endif //HAL_WITH_UAVCAN

165
libraries/AP_HAL_ChibiOS/CANClock.h
View File

@ -1,165 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Modified for Ardupilot by Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#include <uavcan/driver/system_clock.hpp>
namespace ChibiOS_CAN {
namespace clock {
/**
* Starts the clock.
* Can be called multiple times, only the first call will be effective.
*/
void init();
/**
* Returns current monotonic time since the moment when clock::init() was called.
* This function is thread safe.
*/
uavcan::MonotonicTime getMonotonic();
/**
* Sets the driver's notion of the system UTC. It should be called
* at startup and any time the system clock is updated from an
* external source that is not the UAVCAN Timesync master.
* This function is thread safe.
*/
void setUtc(uavcan::UtcTime time);
/**
* Returns UTC time if it has been set, otherwise returns zero time.
* This function is thread safe.
*/
uavcan::UtcTime getUtc();
/**
* Performs UTC phase and frequency adjustment.
* The UTC time will be zero until first adjustment has been performed.
* This function is thread safe.
*/
void adjustUtc(uavcan::UtcDuration adjustment);
/**
* UTC clock synchronization parameters
*/
struct UtcSyncParams {
float offset_p; ///< PPM per one usec error
float rate_i; ///< PPM per one PPM error for second
float rate_error_corner_freq;
float max_rate_correction_ppm;
float lock_thres_rate_ppm;
uavcan::UtcDuration lock_thres_offset;
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
UtcSyncParams()
: offset_p(0.01F)
, rate_i(0.02F)
, rate_error_corner_freq(0.01F)
, max_rate_correction_ppm(300.0F)
, lock_thres_rate_ppm(2.0F)
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
, min_jump(uavcan::UtcDuration::fromMSec(10))
{ }
};
/**
* Clock rate error.
* Positive if the hardware timer is slower than reference time.
* This function is thread safe.
*/
float getUtcRateCorrectionPPM();
/**
* Number of non-gradual adjustments performed so far.
* Ideally should be zero.
* This function is thread safe.
*/
uavcan::uint32_t getUtcJumpCount();
/**
* Whether UTC is synchronized and locked.
* This function is thread safe.
*/
bool isUtcLocked();
/**
* UTC sync params get/set.
* Both functions are thread safe.
*/
UtcSyncParams getUtcSyncParams();
void setUtcSyncParams(const UtcSyncParams& params);
}
/**
* Adapter for uavcan::ISystemClock.
*/
class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable {
SystemClock() { }
virtual void adjustUtc(uavcan::UtcDuration adjustment) override
{
clock::adjustUtc(adjustment);
}
public:
virtual uavcan::MonotonicTime getMonotonic() const override
{
return clock::getMonotonic();
}
virtual uavcan::UtcTime getUtc() const override
{
return clock::getUtc();
}
/**
* Calls clock::init() as needed.
* This function is thread safe.
*/
static SystemClock& get_singleton();
};
}
#endif //HAL_WITH_UAVCAN

917
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

File diff suppressed because it is too large Load Diff

498
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -41,104 +41,57 @@
#pragma once
#include "AP_HAL_ChibiOS.h"
#include "EventSource.h"
#if HAL_WITH_UAVCAN
#if HAL_NUM_CAN_IFACES
#include "CANThread.h"
#include "fdcan.hpp"
#ifndef HAL_CAN_RX_STORAGE_SIZE
#define HAL_CAN_RX_QUEUE_SIZE 128
#endif
class SLCANRouter;
namespace ChibiOS_CAN
{
/**
* Driver error codes.
* These values can be returned from driver functions negated.
*/
//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use
static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented
static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported
static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error
static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported
/**
* RX queue item.
* The application shall not use this directly.
*/
struct CanRxItem {
uavcan::uint64_t utc_usec;
uavcan::CanFrame frame;
uavcan::CanIOFlags flags;
CanRxItem()
: utc_usec(0)
, flags(0)
{ }
};
static_assert(HAL_CAN_RX_QUEUE_SIZE <= 254, "Invalid CAN Rx queue size");
/**
* Single CAN iface.
* The application shall not use this directly.
*/
class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
class ChibiOS::CANIface : public AP_HAL::CANIface
{
#if AP_UAVCAN_SLCAN_ENABLED
friend class ::SLCANRouter;
static SLCANRouter _slcan_router;
#endif
class RxQueue
{
CanRxItem* const buf_;
const uavcan::uint8_t capacity_;
uavcan::uint8_t in_;
uavcan::uint8_t out_;
uavcan::uint8_t len_;
uavcan::uint32_t overflow_cnt_;
void registerOverflow();
static constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
static constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
static constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
static constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
public:
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
: buf_(buf)
, capacity_(capacity)
, in_(0)
, out_(0)
, len_(0)
, overflow_cnt_(0)
{ }
/**
* CANx register sets
*/
typedef FDCAN_GlobalTypeDef CanType;
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
void reset();
unsigned getLength() const
struct CriticalSectionLocker {
CriticalSectionLocker()
{
return len_;
chSysSuspend();
}
uavcan::uint32_t getOverflowCount() const
~CriticalSectionLocker()
{
return overflow_cnt_;
chSysEnable();
}
};
struct MessageRAM {
uavcan::uint32_t StandardFilterSA;
uavcan::uint32_t ExtendedFilterSA;
uavcan::uint32_t RxFIFO0SA;
uavcan::uint32_t RxFIFO1SA;
uavcan::uint32_t TxFIFOQSA;
uavcan::uint32_t EndAddress;
uint32_t StandardFilterSA;
uint32_t ExtendedFilterSA;
uint32_t RxFIFO0SA;
uint32_t RxFIFO1SA;
uint32_t TxFIFOQSA;
uint32_t EndAddress;
} MessageRam_;
struct Timings {
uavcan::uint16_t prescaler;
uavcan::uint8_t sjw;
uavcan::uint8_t bs1;
uavcan::uint8_t bs2;
uint16_t prescaler;
uint8_t sjw;
uint8_t bs1;
uint8_t bs2;
Timings()
: prescaler(0)
@ -148,307 +101,140 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
{ }
};
struct TxItem {
uavcan::MonotonicTime deadline;
uavcan::CanFrame frame;
bool loopback;
bool abort_on_error;
uint8_t index;
TxItem() :
loopback(false),
abort_on_error(false)
{ }
};
enum { NumTxMailboxes = 32 };
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
static uint32_t FDCANMessageRAMOffset_;
RxQueue rx_queue_;
fdcan::CanType* const can_;
uavcan::uint64_t error_cnt_;
uavcan::uint32_t served_aborts_cnt_;
BusEvent& update_event_;
TxItem pending_tx_[NumTxMailboxes];
uavcan::uint8_t peak_tx_mailbox_index_;
const uavcan::uint8_t self_index_;
CanType* can_;
ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes];
uint8_t peak_tx_mailbox_index_;
bool irq_init_;
bool initialised_;
bool had_activity_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
AP_HAL::EventHandle* event_handle_;
static ChibiOS::EventSource evt_src_;
const uint8_t self_index_;
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags) override;
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override;
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs) override;
virtual uavcan::uint16_t getNumFilters() const override;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
void setupMessageRam(void);
static uint32_t FDCAN2MessageRAMOffset_;
public:
enum { MaxRxQueueCapacity = 254 };
bool readRxFIFO(uint8_t fifo_index);
enum OperatingMode {
NormalMode,
SilentMode
};
void discardTimedOutTxMailboxes(uint64_t current_time);
CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity);
bool canAcceptNewTxFrame() const;
/**
* 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 uavcan::uint32_t bitrate, const OperatingMode mode);
void handleTxCompleteInterrupt(uavcan::uint64_t utc_usec);
void handleRxInterrupt(uavcan::uint8_t fifo_index);
bool readRxFIFO(uavcan::uint8_t fifo_index);
/**
* 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;
/**
* Number of RX frames lost due to queue overflow.
* This is an atomic read, it doesn't require a critical section.
*/
uavcan::uint32_t getRxQueueOverflowCount() const
{
return rx_queue_.getOverflowCount();
}
bool recover_from_busoff();
/**
* 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 uavcan::uint64_t getErrorCount() const override;
void pollErrorFlags();
/**
* 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.
*/
uavcan::uint32_t getVoluntaryTxAbortCount() const
{
return served_aborts_cnt_;
}
void checkAvailable(bool& read, bool& write,
const AP_HAL::CANFrame* pending_tx) const;
/**
* Returns the number of frames pending in the RX queue.
* This is intended for debug use only.
*/
unsigned getRxQueueLength() const;
// Reset the error states like Bus Off Error
void clearErrors();
/**
* 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.
*/
uavcan::uint8_t getPeakNumTxMailboxesUsed() const
{
return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
}
fdcan::CanType* can_reg(void)
{
return can_;
}
#if AP_UAVCAN_SLCAN_ENABLED
static SLCANRouter &slcan_router() { return _slcan_router; }
#endif
};
/**
* CAN driver, incorporates all available CAN ifaces.
* Please avoid direct use, prefer @ref CanInitHelper instead.
*/
class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
{
BusEvent update_event_;
static uint32_t FDCAN2MessageRAMOffset_;
static bool clock_init_;
CanIface if0_;
#if UAVCAN_STM32_NUM_IFACES > 1
CanIface if1_;
#endif
bool initialized_by_me_[UAVCAN_STM32_NUM_IFACES];
uavcan::uint8_t num_ifaces_;
uavcan::uint8_t if_int_to_gl_index_[UAVCAN_STM32_NUM_IFACES];
bool _detected_bus_off;
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override;
static void initOnce();
static void initOnce(uavcan::uint8_t can_number, bool enable_irqs);
struct {
uint32_t tx_requests;
uint32_t tx_rejected;
uint32_t tx_success;
uint32_t tx_timedout;
uint32_t tx_abort;
uint32_t rx_received;
uint32_t rx_overflow;
uint32_t rx_errors;
uint32_t num_busoff_err;
uint32_t num_events;
} stats;
public:
template <unsigned RxQueueCapacity>
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
: update_event_(*this)
, if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_STM32_NUM_IFACES > 1
, if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
/******************************************
* Common CAN methods *
* ****************************************/
CANIface(uint8_t index);
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const OperatingMode mode) override;
// Put frame into Tx FIFO returns negative on error, 0 on buffer full,
// 1 on successfully pushing a frame into FIFO
int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
CanIOFlags flags) override;
// Receive frame from Rx Buffer, returns negative on error, 0 on nothing available,
// 1 on successfully poping a frame
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CanIOFlags& out_flags) override;
// Set Filters to ignore frames not to be handled by us
bool configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override;
// returns true if busoff state was detected and not handled yet
bool is_busoff() const override
{
return _detected_bus_off;
}
// Clear the Rx buffer
void clear_rx() override;
// Get number of Filter configurations
uint16_t getNumFilters() const override;
// Get total number of Errors discovered
uint32_t getErrorCount() const override;
// returns true if init was successfully called
bool is_initialized() const override
{
return initialised_;
}
/******************************************
* Select Method *
* ****************************************/
// wait until selected event is available, false when timed out waiting else true
bool select(bool &read, bool &write,
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
// setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override;
/************************************
* Methods used inside interrupt *
************************************/
void handleTxCompleteInterrupt(uint64_t timestamp_us);
void handleRxInterrupt(uint8_t fifo_index);
void handleBusOffInterrupt();
// handle if any error occured, and do the needful such as,
// droping the frame, and counting errors
void pollErrorFlagsFromISR(void);
// CAN Peripheral register structure
static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = {
reinterpret_cast<CanType*>(FDCAN1_BASE)
#if HAL_NUM_CAN_IFACES > 1
,
reinterpret_cast<CanType*>(FDCAN2_BASE)
#endif
{
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
}
/**
* This function returns select masks indicating which interfaces are available for read/write.
*/
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const;
BusEvent* getUpdateEvent()
{
return &update_event_;
}
/**
* 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 uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number);
virtual CanIface* getIface(uavcan::uint8_t iface_index) override;
virtual uavcan::uint8_t getNumIfaces() const override
{
return num_ifaces_;
}
/**
* 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();
};
};
/**
* Helper class.
* Normally only this class should be used by the application.
* 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns.
*/
template <unsigned RxQueueCapacity = 128>
class CanInitHelper
{
CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
public:
enum { BitRateAutoDetect = 0 };
CanDriver driver;
CanInitHelper() :
driver(queue_storage_)
{ }
/**
* This overload simply configures the provided bitrate.
* Auto bit rate detection will not be performed.
* Bitrate value must be positive.
* @return Negative value on error; non-negative on success. Refer to constants Err*.
*/
int init(uavcan::uint32_t bitrate)
{
return driver.init(bitrate, CanIface::NormalMode);
}
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number)
{
return driver.init(bitrate, mode, can_number);
}
/**
* This function can either initialize the driver at a fixed bit rate, or it can perform
* automatic bit rate detection. For theory please refer to the CiA application note #801.
*
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
* The callable entity will be invoked without arguments.
* @ref getRecommendedListeningDelay().
*
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
* If auto detection was used, the function will update the argument
* with established bit rate. In case of an error the value will be undefined.
*
* @return Negative value on error; non-negative on success. Refer to constants Err*.
*/
template <typename DelayCallable>
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
{
if (inout_bitrate > 0) {
return driver.init(inout_bitrate, CanIface::NormalMode);
} else {
static const uavcan::uint32_t StandardBitRates[] = {
1000000,
500000,
250000,
125000
};
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
inout_bitrate = StandardBitRates[br];
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
delay_callable();
if (res >= 0) {
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
if (!driver.getIface(iface)->isRxBufferEmpty()) {
// Re-initializing in normal mode
return driver.init(inout_bitrate, CanIface::NormalMode);
}
}
}
}
return -ErrBitRateNotDetected;
}
}
/**
* Use this value for listening delay during automatic bit rate detection.
*/
static uavcan::MonotonicDuration getRecommendedListeningDelay()
{
return uavcan::MonotonicDuration::fromMSec(1050);
}
};
}
#include "CANSerialRouter.h"
#endif //HAL_WITH_UAVCAN
#endif //HAL_NUM_CAN_IFACES

513
libraries/AP_HAL_ChibiOS/CANIface.h
View File

@ -41,98 +41,47 @@
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
# if defined(STM32H7XX)
#include "CANFDIface.h"
#else
#include "CANThread.h"
#include "CANIface.h"
# else
#if HAL_NUM_CAN_IFACES
#include "bxcan.hpp"
#include "EventSource.h"
class SLCANRouter;
#ifndef HAL_CAN_RX_STORAGE_SIZE
#define HAL_CAN_RX_QUEUE_SIZE 128
#endif
namespace ChibiOS_CAN {
/**
* Driver error codes.
* These values can be returned from driver functions negated.
*/
//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use
static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented
static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported
static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error
static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported
/**
* RX queue item.
* The application shall not use this directly.
*/
struct CanRxItem {
uavcan::uint64_t utc_usec;
uavcan::CanFrame frame;
uavcan::CanIOFlags flags;
CanRxItem()
: utc_usec(0)
, flags(0)
{ }
};
static_assert(HAL_CAN_RX_QUEUE_SIZE <= 254, "Invalid CAN Rx queue size");
/**
* Single CAN iface.
* The application shall not use this directly.
*/
class CanIface : public uavcan::ICanIface, uavcan::Noncopyable {
class ChibiOS::CANIface : public AP_HAL::CANIface
{
static constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
static constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
static constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
static constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
#if AP_UAVCAN_SLCAN_ENABLED
friend class ::SLCANRouter;
static SLCANRouter _slcan_router;
#endif
class RxQueue {
CanRxItem* const buf_;
const uavcan::uint8_t capacity_;
uavcan::uint8_t in_;
uavcan::uint8_t out_;
uavcan::uint8_t len_;
uavcan::uint32_t overflow_cnt_;
void registerOverflow();
public:
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
: buf_(buf)
, capacity_(capacity)
, in_(0)
, out_(0)
, len_(0)
, overflow_cnt_(0)
{ }
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
void reset();
unsigned getLength() const
struct CriticalSectionLocker {
CriticalSectionLocker()
{
return len_;
chSysSuspend();
}
uavcan::uint32_t getOverflowCount() const
~CriticalSectionLocker()
{
return overflow_cnt_;
chSysEnable();
}
};
struct Timings {
uavcan::uint16_t prescaler;
uavcan::uint8_t sjw;
uavcan::uint8_t bs1;
uavcan::uint8_t bs2;
uint16_t prescaler;
uint8_t sjw;
uint8_t bs1;
uint8_t bs2;
Timings()
: prescaler(0)
@ -142,317 +91,141 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable {
{ }
};
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];
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
RxQueue rx_queue_;
bxcan::CanType* const can_;
uavcan::uint64_t error_cnt_;
uavcan::uint32_t served_aborts_cnt_;
BusEvent& update_event_;
TxItem pending_tx_[NumTxMailboxes];
uavcan::uint8_t peak_tx_mailbox_index_;
const uavcan::uint8_t self_index_;
ChibiOS::bxcan::CanType* can_;
uint64_t error_cnt_;
ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes];
bool irq_init_;
bool initialised_;
bool had_activity_;
AP_HAL::EventHandle* event_handle_;
static ChibiOS::EventSource evt_src_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
const uint8_t self_index_;
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags) override;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override;
void setupMessageRam(void);
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs) override;
bool readRxFIFO(uint8_t fifo_index);
virtual uavcan::uint16_t getNumFilters() const override
void discardTimedOutTxMailboxes(uint64_t current_time);
bool canAcceptNewTxFrame(const AP_HAL::CANFrame& frame) const;
bool isRxBufferEmpty() const;
bool recover_from_busoff();
void pollErrorFlags();
void checkAvailable(bool& read, bool& write,
const AP_HAL::CANFrame* pending_tx) const;
bool waitMsrINakBitStateChange(bool target_state);
void handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const uint64_t timestamp_us);
void initOnce(bool enable_irq);
struct {
uint32_t tx_requests;
uint32_t tx_rejected;
uint32_t tx_success;
uint32_t tx_timedout;
uint32_t tx_loopback;
uint32_t tx_abort;
uint32_t rx_received;
uint32_t rx_overflow;
uint32_t rx_errors;
uint32_t num_busoff_err;
uint32_t num_events;
} stats;
public:
/******************************************
* Common CAN methods *
* ****************************************/
CANIface(uint8_t index);
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const OperatingMode mode) override;
// Put frame into Tx FIFO returns negative on error, 0 on buffer full,
// 1 on successfully pushing a frame into FIFO
int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
CanIOFlags flags) override;
// Receive frame from Rx Buffer, returns negative on error, 0 on nothing available,
// 1 on successfully poping a frame
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CanIOFlags& out_flags) override;
// Set Filters to ignore frames not to be handled by us
bool configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override;
// In BxCAN the Busoff error is cleared automatically,
// so always return false
bool is_busoff() const override
{
return false;
}
void clear_rx() override;
// Get number of Filter configurations
uint16_t getNumFilters() const override
{
return NumFilters;
}
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
// Get total number of Errors discovered
uint32_t getErrorCount() const override;
bool waitMsrINakBitStateChange(bool target_state);
// returns true if init was successfully called
bool is_initialized() const override
{
return initialised_;
}
public:
enum { MaxRxQueueCapacity = 254 };
/******************************************
* Select Method *
* ****************************************/
// wait until selected event is available, false when timed out waiting else true
bool select(bool &read, bool &write,
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
// setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override;
enum OperatingMode {
NormalMode,
SilentMode
// fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override;
/************************************
* Methods used inside interrupt *
************************************/
void handleTxInterrupt(uint64_t timestamp_us);
void handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us);
// handle if any error occured, and do the needful such as,
// droping the frame, and counting errors
void pollErrorFlagsFromISR(void);
// CAN Peripheral register structure
static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = {
reinterpret_cast<bxcan::CanType*>(0x40006400)
#if HAL_NUM_CAN_IFACES > 1
,
reinterpret_cast<bxcan::CanType*>(0x40006800)
#endif
};
CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
: rx_queue_(rx_queue_buffer, 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)
{
UAVCAN_ASSERT(self_index_ < UAVCAN_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 uavcan::uint32_t bitrate, const OperatingMode mode);
void handleTxInterrupt(uavcan::uint64_t utc_usec);
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::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;
/**
* Number of RX frames lost due to queue overflow.
* This is an atomic read, it doesn't require a critical section.
*/
uavcan::uint32_t getRxQueueOverflowCount() const
{
return rx_queue_.getOverflowCount();
}
/**
* 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 uavcan::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.
*/
uavcan::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.
*/
uavcan::uint8_t getPeakNumTxMailboxesUsed() const
{
return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
}
#if AP_UAVCAN_SLCAN_ENABLED
static SLCANRouter &slcan_router() { return _slcan_router; }
#endif
};
/**
* CAN driver, incorporates all available CAN ifaces.
* Please avoid direct use, prefer @ref CanInitHelper instead.
*/
class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable {
BusEvent update_event_;
CanIface if0_;
#if UAVCAN_STM32_NUM_IFACES > 1
CanIface if1_;
#endif
bool initialized_by_me_[UAVCAN_STM32_NUM_IFACES];
uavcan::uint8_t num_ifaces_;
uavcan::uint8_t if_int_to_gl_index_[UAVCAN_STM32_NUM_IFACES];
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override;
static void initOnce();
static void initOnce(uavcan::uint8_t can_number, bool enable_irqs);
public:
template <unsigned RxQueueCapacity>
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
: update_event_(*this)
, if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_STM32_NUM_IFACES > 1
, if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
#endif
{
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
}
/**
* This function returns select masks indicating which interfaces are available for read/write.
*/
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const;
BusEvent* getUpdateEvent()
{
return &update_event_;
}
/**
* 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 uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number);
virtual CanIface* getIface(uavcan::uint8_t iface_index) override;
virtual uavcan::uint8_t getNumIfaces() const override
{
return num_ifaces_;
}
/**
* 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();
};
/**
* Helper class.
* Normally only this class should be used by the application.
* 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns.
*/
template <unsigned RxQueueCapacity = 128>
class CanInitHelper {
CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
public:
enum { BitRateAutoDetect = 0 };
CanDriver driver;
CanInitHelper() :
driver(queue_storage_)
{ }
/**
* This overload simply configures the provided bitrate.
* Auto bit rate detection will not be performed.
* Bitrate value must be positive.
* @return Negative value on error; non-negative on success. Refer to constants Err*.
*/
int init(uavcan::uint32_t bitrate)
{
return driver.init(bitrate, CanIface::NormalMode);
}
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number)
{
return driver.init(bitrate, mode, can_number);
}
/**
* This function can either initialize the driver at a fixed bit rate, or it can perform
* automatic bit rate detection. For theory please refer to the CiA application note #801.
*
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
* The callable entity will be invoked without arguments.
* @ref getRecommendedListeningDelay().
*
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
* If auto detection was used, the function will update the argument
* with established bit rate. In case of an error the value will be undefined.
*
* @return Negative value on error; non-negative on success. Refer to constants Err*.
*/
template <typename DelayCallable>
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
{
if (inout_bitrate > 0) {
return driver.init(inout_bitrate, CanIface::NormalMode);
}
else {
static const uavcan::uint32_t StandardBitRates[] = {
1000000,
500000,
250000,
125000
};
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
inout_bitrate = StandardBitRates[br];
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
delay_callable();
if (res >= 0) {
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
if (!driver.getIface(iface)->isRxBufferEmpty()) {
// Re-initializing in normal mode
return driver.init(inout_bitrate, CanIface::NormalMode);
}
}
}
}
return -ErrBitRateNotDetected;
}
}
/**
* Use this value for listening delay during automatic bit rate detection.
*/
static uavcan::MonotonicDuration getRecommendedListeningDelay()
{
return uavcan::MonotonicDuration::fromMSec(1050);
}
};
}
#include "CANSerialRouter.h"
#endif // defined(STM32H7XX)
#endif //HAL_WITH_UAVCAN
#endif //HAL_NUM_CAN_IFACES
#endif //# if defined(STM32H7XX)

105
libraries/AP_HAL_ChibiOS/CANInternal.h
View File

@ -1,105 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#include <hal.h>
#include <ch.h>
/**
* Debug output
*/
#ifndef UAVCAN_STM32_LOG
# if 0
# define UAVCAN_STM32_LOG(fmt, ...) syslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__)
# else
# define UAVCAN_STM32_LOG(...) ((void)0)
# endif
#endif
/**
* IRQ handler macros
*/
# define UAVCAN_STM32_IRQ_HANDLER(id) CH_IRQ_HANDLER(id)
# define UAVCAN_STM32_IRQ_PROLOGUE() CH_IRQ_PROLOGUE()
# define UAVCAN_STM32_IRQ_EPILOGUE() CH_IRQ_EPILOGUE()
/**
* Priority mask for timer and CAN interrupts.
*/
# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
# if (CH_KERNEL_MAJOR == 2)
# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY)
# else // ChibiOS 3+
# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_MAX_KERNEL_PRIORITY
# endif
# endif
/**
* Glue macros
*/
#define UAVCAN_STM32_GLUE2_(A, B) A##B
#define UAVCAN_STM32_GLUE2(A, B) UAVCAN_STM32_GLUE2_(A, B)
#define UAVCAN_STM32_GLUE3_(A, B, C) A##B##C
#define UAVCAN_STM32_GLUE3(A, B, C) UAVCAN_STM32_GLUE3_(A, B, C)
namespace ChibiOS_CAN {
struct CriticalSectionLocker {
CriticalSectionLocker()
{
chSysSuspend();
}
~CriticalSectionLocker()
{
chSysEnable();
}
};
namespace clock {
uint64_t getUtcUSecFromCanInterrupt();
}
}
#endif //HAL_WITH_UAVCAN

58
libraries/AP_HAL_ChibiOS/CANManager.cpp
View File

@ -1,58 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
* Based on stm32 can driver by Pavel Kirienko
*/
#include "CAN.h"
#if HAL_WITH_UAVCAN
#include "CANInternal.h"
#include "CANClock.h"
extern const AP_HAL::HAL& hal;
using namespace ChibiOS;
namespace ChibiOS_CAN {
uint64_t clock::getUtcUSecFromCanInterrupt()
{
return AP_HAL::micros64();
}
uavcan::MonotonicTime clock::getMonotonic()
{
return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64());
}
}
bool CANManager::begin(uint32_t bitrate, uint8_t can_number)
{
return (can_helper.init(bitrate, ChibiOS_CAN::CanIface::OperatingMode::NormalMode, can_number) == 0);
}
bool CANManager::is_initialized()
{
return initialized_;
}
void CANManager::initialized(bool val)
{
initialized_ = val;
}
#endif //HAL_WITH_UAVCAN

165
libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
View File

@ -1,165 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Siddharth Bharat Purohit
*/
#include "CANSerialRouter.h"
#if AP_UAVCAN_SLCAN_ENABLED
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
extern const AP_HAL::HAL& hal;
void SLCANRouter::init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event)
{
_can_if = can_if;
_update_event = update_event;
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&SLCANRouter::timer, void));
}
void SLCANRouter::run()
{
_port = AP::can().get_slcan_serial();
if (_port == nullptr) {
return;
}
if (_slcan_if.init(921600, SLCAN::CAN::OperatingMode::NormalMode, _port) < 0) {
return;
}
_slcan_rt_timeout = AP::can().get_slcan_timeout();
if (!_thread_started) {
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCANRouter::can2slcan_router_trampoline, void), "C2SRouter", 512, AP_HAL::Scheduler::PRIORITY_CAN, 0);
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCANRouter::slcan2can_router_trampoline, void), "S2CRouter", 512, AP_HAL::Scheduler::PRIORITY_CAN, 0);
_thread_started = true;
_thread_suspended = false;
}
else if (_thread_suspended) { //wake up threads
chSysLock();
if (_c2s_thd_ref && _s2c_thd_ref) {
chThdResumeS(&_c2s_thd_ref, MSG_OK);
chThdResumeS(&_s2c_thd_ref, MSG_OK);
_thread_suspended = false;
}
chSysUnlock();
}
}
void SLCANRouter::timer()
{
if ((!_thread_started || _thread_suspended) && (AP::can().get_slcan_serial() != nullptr)) {
run();
AP::can().reset_slcan_serial();
_last_active_time = AP_HAL::millis();
}
if (!_slcan_if.closed()) {
_last_active_time = AP_HAL::millis();
}
if (_thread_suspended) {
return;
}
if (AP_HAL::millis() - _last_active_time > (_slcan_rt_timeout * 1000) && _slcan_rt_timeout != 0) {
chSysLock();
_port->lock_port(0, 0);
_port->flush();
_thread_suspended = true;
chSysUnlock();
}
}
void SLCANRouter::route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec)
{
if (_can_if != can_if) {
return;
}
CanRouteItem it;
it.frame = frame;
it.utc_usec = timestamp_usec;
if (_slcan_tx_queue.space() == 0) {
_slcan_tx_queue.pop();
}
_slcan_tx_queue.push(it);
}
void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec)
{
CanRouteItem it;
it.frame = frame;
it.utc_usec = timestamp_usec;
if (_can_tx_queue.space() == 0) {
_can_tx_queue.pop();
}
_can_tx_queue.push(it);
}
void SLCANRouter::slcan2can_router_trampoline(void)
{
CanRouteItem it;
while (true) {
chSysLock();
_s2c_thd_ref = nullptr;
if (_thread_suspended) {
chThdSuspendS(&_s2c_thd_ref);
_port->flush();
}
chSysUnlock();
_slcan_if.reader();
while (_can_tx_queue.available() && _can_if) {
if (!_can_tx_queue.peek(it)) {
break;
}
if (_can_if->send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) {
if (!_can_tx_queue.pop()) {
break;
}
} else {
break;
}
hal.scheduler->delay_microseconds(100);
}
hal.scheduler->delay_microseconds(100);
}
}
void SLCANRouter::can2slcan_router_trampoline(void)
{
CanRouteItem it;
while (true) {
chSysLock();
_c2s_thd_ref = nullptr;
if (_thread_suspended) {
chThdSuspendS(&_c2s_thd_ref);
}
chSysUnlock();
_update_event->wait(uavcan::MonotonicDuration::fromUSec(1000));
while (_slcan_tx_queue.available()) {
if (!_slcan_tx_queue.peek(it)) {
break;
}
if (_slcan_if.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) {
if (!_slcan_tx_queue.pop()) {
break;
}
} else {
break;
}
hal.scheduler->delay_microseconds(100);
}
hal.scheduler->delay_microseconds(100);
}
}
#endif

68
libraries/AP_HAL_ChibiOS/CANSerialRouter.h
View File

@ -1,68 +0,0 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if AP_UAVCAN_SLCAN_ENABLED
#include "CAN.h"
#include <AP_UAVCAN/AP_UAVCAN_SLCAN.h>
#include "CANThread.h"
#include "CANClock.h"
#include "CANIface.h"
#define SLCAN_ROUTER_QUEUE_SIZE 64
struct CanRouteItem {
uint64_t utc_usec;
uavcan::CanFrame frame;
CanRouteItem() :
utc_usec(0)
{
}
};
class SLCANRouter {
ChibiOS_CAN::CanIface* _can_if;
SLCAN::CAN _slcan_if;
ObjectBuffer<CanRouteItem> _can_tx_queue;
ObjectBuffer<CanRouteItem> _slcan_tx_queue;
ChibiOS_CAN::BusEvent* _update_event;
uint32_t _last_active_time = 0;
uint32_t _slcan_rt_timeout = 0;
bool _thread_started = false;
bool _thread_suspended = false;
HAL_Semaphore router_sem;
thread_reference_t _s2c_thd_ref;
thread_reference_t _c2s_thd_ref;
void timer(void);
AP_HAL::UARTDriver* _port;
public:
SLCANRouter() : _slcan_if(SLCAN_DRIVER_INDEX, SLCAN_RX_QUEUE_SIZE), _can_tx_queue(SLCAN_ROUTER_QUEUE_SIZE), _slcan_tx_queue(SLCAN_ROUTER_QUEUE_SIZE)
{}
void init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event);
void route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec);
void route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec);
void slcan2can_router_trampoline(void);
void can2slcan_router_trampoline(void);
void run(void);
};
#endif // AP_UAVCAN_SLCAN_ENABLED

122
libraries/AP_HAL_ChibiOS/CANThread.cpp
View File

@ -1,122 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Modified for Ardupilot by Siddharth Bharat Purohit
*/
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#include "CANThread.h"
#include "CANClock.h"
#include "CANIface.h"
#include "CANInternal.h"
namespace ChibiOS_CAN {
/*
* BusEvent
*/
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
// set maximum time to allow for 16 bit timers running at 1MHz
static const uavcan::int64_t MaxDelayUSec = 0x000FFFF;
const uavcan::int64_t usec = duration.toUSec();
msg_t ret = msg_t();
if (usec <= 0) {
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout(TIME_IMMEDIATE);
# else // ChibiOS 3+
ret = sem_.wait(TIME_IMMEDIATE);
# endif
}
else {
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec));
# elif (CH_KERNEL_MAJOR >= 5)
ret = sem_.wait((usec > MaxDelayUSec) ? chTimeUS2I(MaxDelayUSec) : chTimeUS2I(usec));
# else // ChibiOS 3+
ret = sem_.wait((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec));
# endif
}
# if (CH_KERNEL_MAJOR == 2)
return ret == RDY_OK;
# else // ChibiOS 3+
return ret == MSG_OK;
# endif
}
void BusEvent::signal()
{
sem_.signal();
}
void BusEvent::signalFromInterrupt()
{
# if (CH_KERNEL_MAJOR == 2)
chSysLockFromIsr();
sem_.signalI();
chSysUnlockFromIsr();
# else // ChibiOS 3+
chSysLockFromISR();
sem_.signalI();
chSysUnlockFromISR();
# endif
}
/*
* Mutex
*/
void Mutex::lock()
{
mtx_.lock();
}
void Mutex::unlock()
{
# if (CH_KERNEL_MAJOR == 2)
chibios_rt::BaseThread::unlockMutex();
# else // ChibiOS 3+
mtx_.unlock();
# endif
}
}
#endif //HAL_WITH_UAVCAN

97
libraries/AP_HAL_ChibiOS/CANThread.h
View File

@ -1,97 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Modified for Ardupilot by Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
# include <ch.hpp>
#include <uavcan/uavcan.hpp>
namespace ChibiOS_CAN {
class CanDriver;
class BusEvent {
chibios_rt::CounterSemaphore sem_;
public:
BusEvent(CanDriver& can_driver)
: sem_(0)
{
(void)can_driver;
}
bool wait(uavcan::MonotonicDuration duration);
void signal();
void signalFromInterrupt();
};
class Mutex {
chibios_rt::Mutex mtx_;
public:
void lock();
void unlock();
};
class MutexLocker {
Mutex& mutex_;
public:
MutexLocker(Mutex& mutex)
: mutex_(mutex)
{
mutex_.lock();
}
~MutexLocker()
{
mutex_.unlock();
}
};
}
#endif //HAL_WITH_UAVCAN

1385
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

File diff suppressed because it is too large Load Diff

7
libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
View File

@ -103,6 +103,9 @@ static ChibiOS::Flash flashDriver;
static Empty::Flash flashDriver;
#endif
#if HAL_NUM_CAN_IFACES > 0
static ChibiOS::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
#endif
#if HAL_WITH_IO_MCU
HAL_UART_IO_DRIVER;
@ -133,7 +136,11 @@ HAL_ChibiOS::HAL_ChibiOS() :
&opticalFlowDriver,
&flashDriver,
&dspDriver,
#if HAL_NUM_CAN_IFACES
(AP_HAL::CANIface**)canDrivers
#else
nullptr
#endif
)
{}

4
libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h
View File

@ -35,5 +35,7 @@ public:
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
};
void hal_chibios_set_priority(uint8_t priority);
#if HAL_NUM_CAN_IFACES
typedef ChibiOS::CANIface HAL_CANIface;
#endif
thread_t* get_main_thread(void);

2
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -25,7 +25,7 @@
#include <AP_HAL_ChibiOS/Storage.h>
#include <AP_HAL_ChibiOS/RCOutput.h>
#include <AP_HAL_ChibiOS/RCInput.h>
#include <AP_HAL_ChibiOS/CAN.h>
#include <AP_HAL_ChibiOS/CANIface.h>
#include <AP_InternalError/AP_InternalError.h>
#if CH_CFG_USE_DYNAMIC == TRUE

43
libraries/AP_HAL_ChibiOS/bxcan.hpp
View File

@ -42,51 +42,37 @@
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#if HAL_NUM_CAN_IFACES
# if !defined(STM32H7XX)
#include <uavcan/uavcan.hpp>
#include <stdint.h>
#ifndef UAVCAN_CPP_VERSION
# error UAVCAN_CPP_VERSION
#endif
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
// #undef'ed at the end of this file
# define constexpr const
#endif
namespace ChibiOS_CAN
namespace ChibiOS
{
namespace bxcan
{
struct TxMailboxType
{
struct TxMailboxType {
volatile uint32_t TIR;
volatile uint32_t TDTR;
volatile uint32_t TDLR;
volatile uint32_t TDHR;
};
struct RxMailboxType
{
struct RxMailboxType {
volatile uint32_t RIR;
volatile uint32_t RDTR;
volatile uint32_t RDLR;
volatile uint32_t RDHR;
};
struct FilterRegisterType
{
struct FilterRegisterType {
volatile uint32_t FR1;
volatile uint32_t FR2;
};
struct CanType
{
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 */
@ -111,18 +97,6 @@ struct CanType
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
};
/**
* CANx register sets
*/
CanType* const Can[UAVCAN_STM32_NUM_IFACES] =
{
reinterpret_cast<CanType*>(0x40006400)
#if UAVCAN_STM32_NUM_IFACES > 1
,
reinterpret_cast<CanType*>(0x40006800)
#endif
};
/* CAN master control register */
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
@ -323,8 +297,5 @@ constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init
}
}
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
# undef constexpr
#endif
#endif //!defined(STM32H7XX)
#endif //HAL_WITH_UAVCAN
#endif //HAL_NUM_CAN_IFACES

90
libraries/AP_HAL_ChibiOS/fdcan.hpp
View File

@ -1,90 +0,0 @@
/*
* The MIT License (MIT)
*
* Copyright (c) 2014 Pavel Kirienko
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#pragma once
#include "AP_HAL_ChibiOS.h"
#if HAL_WITH_UAVCAN
#if defined(STM32H7XX)
#include <uavcan/uavcan.hpp>
#include <stdint.h>
#ifndef UAVCAN_CPP_VERSION
# error UAVCAN_CPP_VERSION
#endif
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
// #undef'ed at the end of this file
# define constexpr const
#endif
namespace ChibiOS_CAN
{
namespace fdcan
{
typedef FDCAN_GlobalTypeDef CanType;
constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
/**
* CANx register sets
*/
CanType* const Can[UAVCAN_STM32_NUM_IFACES] = {
reinterpret_cast<CanType*>(FDCAN1_BASE)
#if UAVCAN_STM32_NUM_IFACES > 1
,
reinterpret_cast<CanType*>(FDCAN2_BASE)
#endif
};
}
}
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
# undef constexpr
#endif
#endif //defined(STM32H7XX)
#endif //HAL_WITH_UAVCAN

13
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

@ -588,10 +588,10 @@ def make_line(label):
return line
def enable_can(f):
def enable_can(f, num_ifaces):
'''setup for a CAN enabled board'''
f.write('#define HAL_WITH_UAVCAN 1\n')
env_vars['HAL_WITH_UAVCAN'] = '1'
f.write('#define HAL_NUM_CAN_IFACES %d\n' % num_ifaces)
env_vars['HAL_NUM_CAN_IFACES'] = str(num_ifaces)
def has_sdcard_spi():
@ -646,7 +646,10 @@ def write_mcu_config(f):
if 'OTG2' in bytype:
f.write('#define STM32_USB_USE_OTG2 TRUE\n')
if have_type_prefix('CAN') and 'AP_PERIPH' not in env_vars:
enable_can(f)
if 'CAN1' in bytype and 'CAN2' in bytype:
enable_can(f, 2)
else:
enable_can(f, 1)
if get_config('PROCESS_STACK', required=False):
env_vars['PROCESS_STACK'] = get_config('PROCESS_STACK')
@ -1195,7 +1198,7 @@ def write_UART_config(f):
if OTG2_index is not None:
f.write('#define HAL_OTG2_UART_INDEX %d\n' % OTG2_index)
f.write('''
#if HAL_WITH_UAVCAN
#if HAL_NUM_CAN_IFACES
#ifndef HAL_OTG2_PROTOCOL
#define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
#endif