mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-11 02:13:57 -04:00
AP_HAL_ChibiOS: create UAVCAN agnostic CANIface driver
HAL_ChibiOS
This commit is contained in:
Siddharth Purohit
Andrew Tridgell
parent
15abca8ba4
commit
cdf8e369f1
@ -26,6 +26,6 @@ namespace ChibiOS {
|
|||||||
class Shared_DMA;
|
class Shared_DMA;
|
||||||
class SoftSigReader;
|
class SoftSigReader;
|
||||||
class SoftSigReaderInt;
|
class SoftSigReaderInt;
|
||||||
class CANManager;
|
class CANIface;
|
||||||
class Flash;
|
class Flash;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
File diff suppressed because it is too large
Load Diff
@ -41,104 +41,57 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_HAL_ChibiOS.h"
|
#include "AP_HAL_ChibiOS.h"
|
||||||
|
#include "EventSource.h"
|
||||||
|
|
||||||
#if HAL_WITH_UAVCAN
|
#if HAL_NUM_CAN_IFACES
|
||||||
|
|
||||||
#include "CANThread.h"
|
#ifndef HAL_CAN_RX_STORAGE_SIZE
|
||||||
#include "fdcan.hpp"
|
#define HAL_CAN_RX_QUEUE_SIZE 128
|
||||||
|
#endif
|
||||||
|
|
||||||
class SLCANRouter;
|
static_assert(HAL_CAN_RX_QUEUE_SIZE <= 254, "Invalid CAN Rx queue size");
|
||||||
|
|
||||||
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.
|
* 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
|
static constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
|
||||||
friend class ::SLCANRouter;
|
static constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
|
||||||
static SLCANRouter _slcan_router;
|
static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
|
||||||
#endif
|
static constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
|
||||||
class RxQueue
|
static constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
|
||||||
{
|
|
||||||
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)
|
* CANx register sets
|
||||||
: buf_(buf)
|
*/
|
||||||
, capacity_(capacity)
|
typedef FDCAN_GlobalTypeDef CanType;
|
||||||
, in_(0)
|
|
||||||
, out_(0)
|
|
||||||
, len_(0)
|
|
||||||
, overflow_cnt_(0)
|
|
||||||
{ }
|
|
||||||
|
|
||||||
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
|
struct CriticalSectionLocker {
|
||||||
void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
|
CriticalSectionLocker()
|
||||||
|
|
||||||
void reset();
|
|
||||||
|
|
||||||
unsigned getLength() const
|
|
||||||
{
|
{
|
||||||
return len_;
|
chSysSuspend();
|
||||||
}
|
}
|
||||||
|
~CriticalSectionLocker()
|
||||||
uavcan::uint32_t getOverflowCount() const
|
|
||||||
{
|
{
|
||||||
return overflow_cnt_;
|
chSysEnable();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
struct MessageRAM {
|
struct MessageRAM {
|
||||||
uavcan::uint32_t StandardFilterSA;
|
uint32_t StandardFilterSA;
|
||||||
uavcan::uint32_t ExtendedFilterSA;
|
uint32_t ExtendedFilterSA;
|
||||||
uavcan::uint32_t RxFIFO0SA;
|
uint32_t RxFIFO0SA;
|
||||||
uavcan::uint32_t RxFIFO1SA;
|
uint32_t RxFIFO1SA;
|
||||||
uavcan::uint32_t TxFIFOQSA;
|
uint32_t TxFIFOQSA;
|
||||||
uavcan::uint32_t EndAddress;
|
uint32_t EndAddress;
|
||||||
} MessageRam_;
|
} MessageRam_;
|
||||||
|
|
||||||
struct Timings {
|
struct Timings {
|
||||||
uavcan::uint16_t prescaler;
|
uint16_t prescaler;
|
||||||
uavcan::uint8_t sjw;
|
uint8_t sjw;
|
||||||
uavcan::uint8_t bs1;
|
uint8_t bs1;
|
||||||
uavcan::uint8_t bs2;
|
uint8_t bs2;
|
||||||
|
|
||||||
Timings()
|
Timings()
|
||||||
: prescaler(0)
|
: 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 };
|
enum { NumTxMailboxes = 32 };
|
||||||
|
|
||||||
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
|
|
||||||
static uint32_t FDCANMessageRAMOffset_;
|
static uint32_t FDCANMessageRAMOffset_;
|
||||||
RxQueue rx_queue_;
|
|
||||||
fdcan::CanType* const can_;
|
CanType* can_;
|
||||||
uavcan::uint64_t error_cnt_;
|
ObjectBuffer<CanRxItem> rx_queue_;
|
||||||
uavcan::uint32_t served_aborts_cnt_;
|
CanTxItem pending_tx_[NumTxMailboxes];
|
||||||
BusEvent& update_event_;
|
uint8_t peak_tx_mailbox_index_;
|
||||||
TxItem pending_tx_[NumTxMailboxes];
|
bool irq_init_;
|
||||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
bool initialised_;
|
||||||
const uavcan::uint8_t self_index_;
|
|
||||||
bool had_activity_;
|
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,
|
bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
|
||||||
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;
|
|
||||||
|
|
||||||
void setupMessageRam(void);
|
void setupMessageRam(void);
|
||||||
|
|
||||||
static uint32_t FDCAN2MessageRAMOffset_;
|
bool readRxFIFO(uint8_t fifo_index);
|
||||||
public:
|
|
||||||
enum { MaxRxQueueCapacity = 254 };
|
|
||||||
|
|
||||||
enum OperatingMode {
|
void discardTimedOutTxMailboxes(uint64_t current_time);
|
||||||
NormalMode,
|
|
||||||
SilentMode
|
|
||||||
};
|
|
||||||
|
|
||||||
CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
|
bool canAcceptNewTxFrame() const;
|
||||||
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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;
|
bool isRxBufferEmpty() const;
|
||||||
|
|
||||||
/**
|
bool recover_from_busoff();
|
||||||
* 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();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
void pollErrorFlags();
|
||||||
* 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 checkAvailable(bool& read, bool& write,
|
||||||
* Number of times the driver exercised library's requirement to abort transmission on first error.
|
const AP_HAL::CANFrame* pending_tx) const;
|
||||||
* 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_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
// Reset the error states like Bus Off Error
|
||||||
* Returns the number of frames pending in the RX queue.
|
void clearErrors();
|
||||||
* This is intended for debug use only.
|
|
||||||
*/
|
|
||||||
unsigned getRxQueueLength() const;
|
|
||||||
|
|
||||||
/**
|
static uint32_t FDCAN2MessageRAMOffset_;
|
||||||
* 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 bool clock_init_;
|
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,
|
struct {
|
||||||
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
|
uint32_t tx_requests;
|
||||||
uavcan::MonotonicTime blocking_deadline) override;
|
uint32_t tx_rejected;
|
||||||
|
uint32_t tx_success;
|
||||||
static void initOnce();
|
uint32_t tx_timedout;
|
||||||
static void initOnce(uavcan::uint8_t can_number, bool enable_irqs);
|
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:
|
public:
|
||||||
template <unsigned RxQueueCapacity>
|
/******************************************
|
||||||
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
|
* Common CAN methods *
|
||||||
: update_event_(*this)
|
* ****************************************/
|
||||||
, if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
|
CANIface(uint8_t index);
|
||||||
#if UAVCAN_STM32_NUM_IFACES > 1
|
|
||||||
, if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
|
// 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
|
#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:
|
#endif //HAL_NUM_CAN_IFACES
|
||||||
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
|
|
||||||
|
|||||||
@ -41,98 +41,47 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_HAL_ChibiOS.h"
|
#include "AP_HAL_ChibiOS.h"
|
||||||
|
|
||||||
#if HAL_WITH_UAVCAN
|
|
||||||
# if defined(STM32H7XX)
|
# if defined(STM32H7XX)
|
||||||
#include "CANFDIface.h"
|
#include "CANFDIface.h"
|
||||||
#else
|
# else
|
||||||
#include "CANThread.h"
|
#if HAL_NUM_CAN_IFACES
|
||||||
#include "CANIface.h"
|
|
||||||
#include "bxcan.hpp"
|
#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 {
|
static_assert(HAL_CAN_RX_QUEUE_SIZE <= 254, "Invalid CAN Rx queue size");
|
||||||
/**
|
|
||||||
* 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.
|
* Single CAN iface.
|
||||||
* The application shall not use this directly.
|
* 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
|
struct CriticalSectionLocker {
|
||||||
friend class ::SLCANRouter;
|
CriticalSectionLocker()
|
||||||
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
|
|
||||||
{
|
{
|
||||||
return len_;
|
chSysSuspend();
|
||||||
}
|
}
|
||||||
|
~CriticalSectionLocker()
|
||||||
uavcan::uint32_t getOverflowCount() const
|
|
||||||
{
|
{
|
||||||
return overflow_cnt_;
|
chSysEnable();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Timings {
|
struct Timings {
|
||||||
uavcan::uint16_t prescaler;
|
uint16_t prescaler;
|
||||||
uavcan::uint8_t sjw;
|
uint8_t sjw;
|
||||||
uavcan::uint8_t bs1;
|
uint8_t bs1;
|
||||||
uavcan::uint8_t bs2;
|
uint8_t bs2;
|
||||||
|
|
||||||
Timings()
|
Timings()
|
||||||
: prescaler(0)
|
: 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 { NumTxMailboxes = 3 };
|
||||||
enum { NumFilters = 14 };
|
enum { NumFilters = 14 };
|
||||||
|
static const uint32_t TSR_ABRQx[NumTxMailboxes];
|
||||||
|
|
||||||
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
|
ChibiOS::bxcan::CanType* can_;
|
||||||
|
uint64_t error_cnt_;
|
||||||
RxQueue rx_queue_;
|
ObjectBuffer<CanRxItem> rx_queue_;
|
||||||
bxcan::CanType* const can_;
|
CanTxItem pending_tx_[NumTxMailboxes];
|
||||||
uavcan::uint64_t error_cnt_;
|
bool irq_init_;
|
||||||
uavcan::uint32_t served_aborts_cnt_;
|
bool initialised_;
|
||||||
BusEvent& update_event_;
|
|
||||||
TxItem pending_tx_[NumTxMailboxes];
|
|
||||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
|
||||||
const uavcan::uint8_t self_index_;
|
|
||||||
bool had_activity_;
|
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,
|
bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
|
||||||
uavcan::CanIOFlags flags) override;
|
|
||||||
|
|
||||||
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
|
void setupMessageRam(void);
|
||||||
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override;
|
|
||||||
|
|
||||||
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
|
bool readRxFIFO(uint8_t fifo_index);
|
||||||
uavcan::uint16_t num_configs) override;
|
|
||||||
|
|
||||||
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;
|
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;
|
||||||
|
|
||||||
enum OperatingMode {
|
// setup event handle for waiting on events
|
||||||
NormalMode,
|
bool set_event_handle(AP_HAL::EventHandle* handle) override;
|
||||||
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
|
|
||||||
};
|
};
|
||||||
|
#endif //HAL_NUM_CAN_IFACES
|
||||||
/**
|
#endif //# if defined(STM32H7XX)
|
||||||
* 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
|
|
||||||
|
|||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
@ -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
|
|
||||||
File diff suppressed because it is too large
Load Diff
@ -103,6 +103,9 @@ static ChibiOS::Flash flashDriver;
|
|||||||
static Empty::Flash flashDriver;
|
static Empty::Flash flashDriver;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if HAL_NUM_CAN_IFACES > 0
|
||||||
|
static ChibiOS::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
|
||||||
|
#endif
|
||||||
|
|
||||||
#if HAL_WITH_IO_MCU
|
#if HAL_WITH_IO_MCU
|
||||||
HAL_UART_IO_DRIVER;
|
HAL_UART_IO_DRIVER;
|
||||||
@ -133,7 +136,11 @@ HAL_ChibiOS::HAL_ChibiOS() :
|
|||||||
&opticalFlowDriver,
|
&opticalFlowDriver,
|
||||||
&flashDriver,
|
&flashDriver,
|
||||||
&dspDriver,
|
&dspDriver,
|
||||||
|
#if HAL_NUM_CAN_IFACES
|
||||||
|
(AP_HAL::CANIface**)canDrivers
|
||||||
|
#else
|
||||||
nullptr
|
nullptr
|
||||||
|
#endif
|
||||||
)
|
)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
|||||||
@ -35,5 +35,7 @@ public:
|
|||||||
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
|
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
|
||||||
};
|
};
|
||||||
void hal_chibios_set_priority(uint8_t priority);
|
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);
|
thread_t* get_main_thread(void);
|
||||||
|
|||||||
@ -25,7 +25,7 @@
|
|||||||
#include <AP_HAL_ChibiOS/Storage.h>
|
#include <AP_HAL_ChibiOS/Storage.h>
|
||||||
#include <AP_HAL_ChibiOS/RCOutput.h>
|
#include <AP_HAL_ChibiOS/RCOutput.h>
|
||||||
#include <AP_HAL_ChibiOS/RCInput.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>
|
#include <AP_InternalError/AP_InternalError.h>
|
||||||
|
|
||||||
#if CH_CFG_USE_DYNAMIC == TRUE
|
#if CH_CFG_USE_DYNAMIC == TRUE
|
||||||
|
|||||||
@ -42,51 +42,37 @@
|
|||||||
|
|
||||||
#include "AP_HAL_ChibiOS.h"
|
#include "AP_HAL_ChibiOS.h"
|
||||||
|
|
||||||
#if HAL_WITH_UAVCAN
|
#if HAL_NUM_CAN_IFACES
|
||||||
|
|
||||||
# if !defined(STM32H7XX)
|
# if !defined(STM32H7XX)
|
||||||
|
|
||||||
#include <uavcan/uavcan.hpp>
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#ifndef UAVCAN_CPP_VERSION
|
namespace ChibiOS
|
||||||
# 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
|
namespace bxcan
|
||||||
{
|
{
|
||||||
|
|
||||||
struct TxMailboxType
|
struct TxMailboxType {
|
||||||
{
|
|
||||||
volatile uint32_t TIR;
|
volatile uint32_t TIR;
|
||||||
volatile uint32_t TDTR;
|
volatile uint32_t TDTR;
|
||||||
volatile uint32_t TDLR;
|
volatile uint32_t TDLR;
|
||||||
volatile uint32_t TDHR;
|
volatile uint32_t TDHR;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct RxMailboxType
|
struct RxMailboxType {
|
||||||
{
|
|
||||||
volatile uint32_t RIR;
|
volatile uint32_t RIR;
|
||||||
volatile uint32_t RDTR;
|
volatile uint32_t RDTR;
|
||||||
volatile uint32_t RDLR;
|
volatile uint32_t RDLR;
|
||||||
volatile uint32_t RDHR;
|
volatile uint32_t RDHR;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct FilterRegisterType
|
struct FilterRegisterType {
|
||||||
{
|
|
||||||
volatile uint32_t FR1;
|
volatile uint32_t FR1;
|
||||||
volatile uint32_t FR2;
|
volatile uint32_t FR2;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct CanType
|
struct CanType {
|
||||||
{
|
|
||||||
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
|
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 MSR; /*!< CAN master status register, Address offset: 0x04 */
|
||||||
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
|
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 */
|
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 */
|
/* CAN master control register */
|
||||||
|
|
||||||
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
|
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 //!defined(STM32H7XX)
|
||||||
#endif //HAL_WITH_UAVCAN
|
#endif //HAL_NUM_CAN_IFACES
|
||||||
|
|||||||
@ -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
|
|
||||||
@ -588,10 +588,10 @@ def make_line(label):
|
|||||||
return line
|
return line
|
||||||
|
|
||||||
|
|
||||||
def enable_can(f):
|
def enable_can(f, num_ifaces):
|
||||||
'''setup for a CAN enabled board'''
|
'''setup for a CAN enabled board'''
|
||||||
f.write('#define HAL_WITH_UAVCAN 1\n')
|
f.write('#define HAL_NUM_CAN_IFACES %d\n' % num_ifaces)
|
||||||
env_vars['HAL_WITH_UAVCAN'] = '1'
|
env_vars['HAL_NUM_CAN_IFACES'] = str(num_ifaces)
|
||||||
|
|
||||||
|
|
||||||
def has_sdcard_spi():
|
def has_sdcard_spi():
|
||||||
@ -646,7 +646,10 @@ def write_mcu_config(f):
|
|||||||
if 'OTG2' in bytype:
|
if 'OTG2' in bytype:
|
||||||
f.write('#define STM32_USB_USE_OTG2 TRUE\n')
|
f.write('#define STM32_USB_USE_OTG2 TRUE\n')
|
||||||
if have_type_prefix('CAN') and 'AP_PERIPH' not in env_vars:
|
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):
|
if get_config('PROCESS_STACK', required=False):
|
||||||
env_vars['PROCESS_STACK'] = get_config('PROCESS_STACK')
|
env_vars['PROCESS_STACK'] = get_config('PROCESS_STACK')
|
||||||
@ -1195,7 +1198,7 @@ def write_UART_config(f):
|
|||||||
if OTG2_index is not None:
|
if OTG2_index is not None:
|
||||||
f.write('#define HAL_OTG2_UART_INDEX %d\n' % OTG2_index)
|
f.write('#define HAL_OTG2_UART_INDEX %d\n' % OTG2_index)
|
||||||
f.write('''
|
f.write('''
|
||||||
#if HAL_WITH_UAVCAN
|
#if HAL_NUM_CAN_IFACES
|
||||||
#ifndef HAL_OTG2_PROTOCOL
|
#ifndef HAL_OTG2_PROTOCOL
|
||||||
#define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
|
#define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user