Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

ChibiOS: move CAN driver code into HAL

Browse Source
This commit is contained in:
Siddharth Purohit 2018-11-16 17:11:26 +08:00 committed by Andrew Tridgell
parent 504cfd8989
commit 8f964b08b0
10 changed files with 2896 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_ChibiOS/CAN.h
View File

@ -26,9 +26,9 @@
#include <uavcan/uavcan.hpp> #include <uavcan/uavcan.hpp>
#include <uavcan/time.hpp> #include <uavcan/time.hpp>
#include <uavcan_stm32/thread.hpp> #include "CANThread.h"
#include <uavcan_stm32/clock.hpp> #include "CANClock.h"
#include <uavcan_stm32/can.hpp> #include "CANIface.h"
#define MAX_NUMBER_OF_CAN_INTERFACES 2 #define MAX_NUMBER_OF_CAN_INTERFACES 2
#define MAX_NUMBER_OF_CAN_DRIVERS 2 #define MAX_NUMBER_OF_CAN_DRIVERS 2
@ -66,7 +66,7 @@ public:
private: private:
bool initialized_; bool initialized_;
uavcan_stm32::CanInitHelper<CAN_STM32_RX_QUEUE_SIZE> can_helper; ChibiOS_CAN::CanInitHelper<CAN_STM32_RX_QUEUE_SIZE> can_helper;
}; };
} }

408
libraries/AP_HAL_ChibiOS/CANClock.cpp Normal file
View File

@ -0,0 +1,408 @@
/*
* 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 "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::instance()
{
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

161
libraries/AP_HAL_ChibiOS/CANClock.h Normal file
View File

@ -0,0 +1,161 @@
/*
* 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 <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)
{
clock::adjustUtc(adjustment);
}
public:
virtual uavcan::MonotonicTime getMonotonic() const
{
return clock::getMonotonic();
}
virtual uavcan::UtcTime getUtc() const
{
return clock::getUtc();
}
/**
* Calls clock::init() as needed.
* This function is thread safe.
*/
static SystemClock& instance();
};
}

442
libraries/AP_HAL_ChibiOS/CANIface.h Normal file
View File

@ -0,0 +1,442 @@
/*
* 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 "CANThread.h"
#include "CANIface.h"
#include "bxcan.hpp"
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)
{ }
};
/**
* Single CAN iface.
* The application shall not use this directly.
*/
class CanIface : public uavcan::ICanIface, uavcan::Noncopyable {
friend class ::SLCANRouter;
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
{
return len_;
}
uavcan::uint32_t getOverflowCount() const
{
return overflow_cnt_;
}
};
struct Timings {
uavcan::uint16_t prescaler;
uavcan::uint8_t sjw;
uavcan::uint8_t bs1;
uavcan::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 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_;
bool had_activity_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags);
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs);
virtual uavcan::uint16_t getNumFilters() const
{
return NumFilters;
}
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
bool waitMsrINakBitStateChange(bool target_state);
public:
enum { MaxRxQueueCapacity = 254 };
enum OperatingMode {
NormalMode,
SilentMode
};
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;
/**
* 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);
}
};
/**
* 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);
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);
virtual uavcan::uint8_t getNumIfaces() const
{
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"

99
libraries/AP_HAL_ChibiOS/CANInternal.h Normal file
View File

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

13
libraries/AP_HAL_ChibiOS/CAN.cpp → libraries/AP_HAL_ChibiOS/CANManager.cpp
View File

@ -11,7 +11,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
* Based on stm32 can driver by Pavel Kirienko * Based on stm32 can driver by Pavel Kirienko
*/ */
@ -20,14 +20,13 @@
#if HAL_WITH_UAVCAN #if HAL_WITH_UAVCAN
#include <uavcan_stm32/../../src/internal.hpp> #include "CANInternal.h"
#include "CANClock.h"
using namespace ChibiOS;
using namespace uavcan_stm32;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
using namespace ChibiOS;
namespace uavcan_stm32 { namespace ChibiOS_CAN {
uint64_t clock::getUtcUSecFromCanInterrupt() uint64_t clock::getUtcUSecFromCanInterrupt()
{ {
@ -43,7 +42,7 @@ uavcan::MonotonicTime clock::getMonotonic()
bool CANManager::begin(uint32_t bitrate, uint8_t can_number) bool CANManager::begin(uint32_t bitrate, uint8_t can_number)
{ {
return (can_helper.init(bitrate, CanIface::OperatingMode::NormalMode, can_number) == 0); return (can_helper.init(bitrate, ChibiOS_CAN::CanIface::OperatingMode::NormalMode, can_number) == 0);
} }
bool CANManager::is_initialized() bool CANManager::is_initialized()

117
libraries/AP_HAL_ChibiOS/CANThread.cpp Normal file
View File

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

91
libraries/AP_HAL_ChibiOS/CANThread.h Normal file
View File

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

1244
libraries/AP_HAL_ChibiOS/CanIface.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

324
libraries/AP_HAL_ChibiOS/bxcan.hpp Normal file
View File

@ -0,0 +1,324 @@
/*
* 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 <uavcan_stm32/build_config.hpp>
#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 bxcan
{
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[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 */
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