aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: support logical/physical CAN bus mapping

Browse Source 
allow for first CAN interface to be CAN2 or CAN3
This commit is contained in:
Andrew Tridgell 2021-03-12 14:08:23 +11:00
parent 2f7ff778be
commit f57b1b9c4b
5 changed files with 183 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -96,21 +96,32 @@ using namespace ChibiOS;
#endif
constexpr CANIface::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES];
uint8_t CANIface::next_interface;
// mapping from logical interface to physical. First physical is 0, first logical is 0
static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST };
// mapping from physical interface back to logical. First physical is 0, first logical is 0
static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST };
#define REG_SET_TIMEOUT 250 // if it takes longer than 250ms for setting a register we have failed
static inline bool driver_initialised(uint8_t iface_index)
{
if (iface_index >= HAL_NUM_CAN_IFACES) {
return false;
}
if (can_ifaces[iface_index] == nullptr) {
return false;
}
return true;
}
static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index)
static inline void handleCANInterrupt(uint8_t phys_index, uint8_t line_index)
{
const int8_t iface_index = can_iface_to_idx[phys_index];
if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) {
return;
}
if (!driver_initialised(iface_index)) {
//Just reset all the interrupts and return
CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N;
@ -161,6 +172,11 @@ CANIface::CANIface(uint8_t index) :
}
}
// constructor suitable for array
CANIface::CANIface() :
CANIface(next_interface++)
{}
void CANIface::handleBusOffInterrupt()
{
_detected_bus_off = true;
@ -548,16 +564,20 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
*/
if (!irq_init_) {
CriticalSectionLocker lock;
if (self_index_ == 0) {
switch (can_interfaces[self_index_]) {
case 0:
nvicEnableVector(FDCAN1_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(FDCAN1_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
}
# if HAL_NUM_CAN_IFACES > 1
else if (self_index_ == 1) {
break;
case 1:
nvicEnableVector(FDCAN2_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(FDCAN2_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
break;
case 2:
nvicEnableVector(FDCAN3_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(FDCAN3_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
break;
}
# endif
irq_init_ = true;
}
@ -679,12 +699,13 @@ void CANIface::clear_rx()
void CANIface::setupMessageRam()
{
#if defined(STM32G4)
memset((void*)SRAMCAN_BASE, 0, 0x350);
MessageRam_.StandardFilterSA = SRAMCAN_BASE;
MessageRam_.ExtendedFilterSA = SRAMCAN_BASE + 0x70;
MessageRam_.RxFIFO0SA = SRAMCAN_BASE + 0xB0;
MessageRam_.RxFIFO1SA = SRAMCAN_BASE + 0x188;
MessageRam_.TxFIFOQSA = SRAMCAN_BASE + 0x278;
const uint32_t base = SRAMCAN_BASE + 0x350 * can_interfaces[self_index_];
memset((void*)base, 0, 0x350);
MessageRam_.StandardFilterSA = base;
MessageRam_.ExtendedFilterSA = base + 0x70;
MessageRam_.RxFIFO0SA = base + 0xB0;
MessageRam_.RxFIFO1SA = base + 0x188;
MessageRam_.TxFIFOQSA = base + 0x278;
can_->TXBC = 0; // fifo mode
#else
@ -997,7 +1018,7 @@ bool CANIface::select(bool &read, bool &write,
}
time = AP_HAL::micros();
}
return true; // Return value doesn't matter as long as it is non-negative
return false;
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
@ -1032,7 +1053,8 @@ void CANIface::get_stats(ExpandingString &str)
*/
extern "C"
{
#ifdef HAL_CAN_IFACE1_ENABLE
// FDCAN1
CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler);
CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler)
{
@ -1048,10 +1070,10 @@ extern "C"
handleCANInterrupt(0, 1);
CH_IRQ_EPILOGUE();
}
#endif
# if HAL_NUM_CAN_IFACES > 1
#ifdef HAL_CAN_IFACE2_ENABLE
// FDCAN2
CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler);
CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler)
{
@ -1067,9 +1089,27 @@ extern "C"
handleCANInterrupt(1, 1);
CH_IRQ_EPILOGUE();
}
#endif
# endif
#ifdef HAL_CAN_IFACE3_ENABLE
// FDCAN3
CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler);
CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler)
{
CH_IRQ_PROLOGUE();
handleCANInterrupt(2, 0);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler);
CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler)
{
CH_IRQ_PROLOGUE();
handleCANInterrupt(2, 1);
CH_IRQ_EPILOGUE();
}
#endif
} // extern "C"
#endif //defined(STM32H7XX) || defined(STM32G4)

13
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -166,6 +166,8 @@ public:
* Common CAN methods *
* ****************************************/
CANIface(uint8_t index);
CANIface();
static uint8_t next_interface;
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const OperatingMode mode) override;
@ -232,14 +234,9 @@ public:
// 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
};
// CAN Peripheral register structure, pointing at base
// register. Indexed by locical interface number
static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
};

10
libraries/AP_HAL_ChibiOS/CANIface.h
View File

@ -161,6 +161,8 @@ public:
* Common CAN methods *
* ****************************************/
CANIface(uint8_t index);
CANIface();
static uint8_t next_interface;
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const OperatingMode mode) override;
@ -233,13 +235,7 @@ public:
void pollErrorFlagsFromISR(void);
// CAN Peripheral register structure
static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = {
reinterpret_cast<bxcan::CanType*>(uintptr_t(0x40006400U))
#if HAL_NUM_CAN_IFACES > 1
,
reinterpret_cast<bxcan::CanType*>(uintptr_t(0x40006800U))
#endif
};
static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
};
#endif //HAL_NUM_CAN_IFACES
#endif //# if defined(STM32H7XX) || defined(STM32G4)

109
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -88,11 +88,20 @@ extern AP_HAL::HAL& hal;
using namespace ChibiOS;
constexpr bxcan::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES];
static inline void handleTxInterrupt(uint8_t iface_index)
uint8_t CANIface::next_interface;
// mapping from logical interface to physical. First physical is 0, first logical is 0
static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST };
// mapping from physical interface back to logical. First physical is 0, first logical is 0
static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST };
static inline void handleTxInterrupt(uint8_t phys_index)
{
if (iface_index >= HAL_NUM_CAN_IFACES) {
const int8_t iface_index = can_iface_to_idx[phys_index];
if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) {
return;
}
uint64_t precise_time = AP_HAL::micros64();
@ -104,9 +113,10 @@ static inline void handleTxInterrupt(uint8_t iface_index)
}
}
static inline void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index)
static inline void handleRxInterrupt(uint8_t phys_index, uint8_t fifo_index)
{
if (iface_index >= HAL_NUM_CAN_IFACES) {
const int8_t iface_index = can_iface_to_idx[phys_index];
if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) {
return;
}
uint64_t precise_time = AP_HAL::micros64();
@ -140,6 +150,11 @@ CANIface::CANIface(uint8_t index) :
}
}
// constructor suitable for array
CANIface::CANIface() :
CANIface(next_interface++)
{}
bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings)
{
if (target_bitrate < 1) {
@ -735,18 +750,27 @@ void CANIface::initOnce(bool enable_irq)
*/
{
CriticalSectionLocker lock;
if (self_index_ == 0) {
switch (can_interfaces[self_index_]) {
case 0:
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
}
# if HAL_NUM_CAN_IFACES > 1
else if (self_index_ == 1) {
break;
#ifdef RCC_APB1ENR_CAN2EN
case 1:
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
break;
#endif
#ifdef RCC_APB1ENR_CAN3EN
case 2:
RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN3RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN3RST;
break;
#endif
}
# endif
}
/*
@ -754,18 +778,27 @@ void CANIface::initOnce(bool enable_irq)
*/
if (!irq_init_ && enable_irq) {
CriticalSectionLocker lock;
if (self_index_ == 0) {
switch (can_interfaces[self_index_]) {
case 0:
nvicEnableVector(CAN1_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN1_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN1_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
}
# if HAL_NUM_CAN_IFACES > 1
else if (self_index_ == 1) {
break;
#ifdef CAN2_TX_IRQn
case 1:
nvicEnableVector(CAN2_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN2_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN2_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
break;
#endif
#ifdef CAN3_TX_IRQn
case 2:
nvicEnableVector(CAN3_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN3_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
nvicEnableVector(CAN3_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY);
break;
#endif
}
# endif
irq_init_ = true;
}
}
@ -935,7 +968,8 @@ void CANIface::get_stats(ExpandingString &str)
*/
extern "C"
{
#ifdef HAL_CAN_IFACE1_ENABLE
// CAN1
CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler)
{
@ -959,21 +993,10 @@ extern "C"
handleRxInterrupt(0, 1);
CH_IRQ_EPILOGUE();
}
#if HAL_NUM_CAN_IFACES > 1
#if !defined(CAN2_TX_IRQ_Handler)
# error "Misconfigured build1"
#endif
#if !defined(CAN2_RX0_IRQ_Handler)
# error "Misconfigured build2"
#endif
#if !defined(CAN2_RX1_IRQ_Handler)
# error "Misconfigured build3"
#endif
#ifdef HAL_CAN_IFACE2_ENABLE
// CAN2
CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler)
{
@ -997,9 +1020,35 @@ extern "C"
handleRxInterrupt(1, 1);
CH_IRQ_EPILOGUE();
}
#endif
# endif
#ifdef HAL_CAN_IFACE3_ENABLE
// CAN3
CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleTxInterrupt(2);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler);
CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(2, 0);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler);
CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(2, 1);
CH_IRQ_EPILOGUE();
}
#endif
} // extern "C"
#endif //!defined(STM32H7XX)

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

@ -615,10 +615,40 @@ def make_line(label):
return line
def enable_can(f, num_ifaces):
def enable_can(f):
'''setup for a CAN enabled board'''
f.write('#define HAL_NUM_CAN_IFACES %d\n' % num_ifaces)
env_vars['HAL_NUM_CAN_IFACES'] = str(num_ifaces)
global mcu_series
if mcu_series.startswith("STM32H7") or mcu_series.startswith("STM32G4"):
prefix = "FDCAN"
cast = "CanType"
else:
prefix = "CAN"
cast = "bxcan::CanType"
# allow for optional CAN_ORDER option giving bus order
can_order_str = get_config('CAN_ORDER', required=False, aslist=True)
if can_order_str:
can_order = [int(s) for s in can_order_str]
else:
can_order = []
for i in range(1,3):
if 'CAN%u' % i in bytype:
can_order.append(i)
base_list = []
for i in can_order:
base_list.append("reinterpret_cast<%s*>(uintptr_t(%s%s_BASE))" % (cast, prefix, i))
f.write("#define HAL_CAN_IFACE%u_ENABLE\n" % i)
can_rev_order = [-1]*3
for i in range(len(can_order)):
can_rev_order[can_order[i]-1] = i
f.write('#define HAL_CAN_INTERFACE_LIST %s\n' % ','.join([str(i-1) for i in can_order]))
f.write('#define HAL_CAN_INTERFACE_REV_LIST %s\n' % ','.join([str(i) for i in can_rev_order]))
f.write('#define HAL_CAN_BASE_LIST %s\n' % ','.join(base_list))
f.write('#define HAL_NUM_CAN_IFACES %d\n' % len(base_list))
env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list))
def has_sdcard_spi():
@ -710,10 +740,7 @@ def write_mcu_config(f):
f.write('#define %s\n' % d[7:])
if have_type_prefix('CAN') and not using_chibios_can:
if 'CAN1' in bytype and 'CAN2' in bytype:
enable_can(f, 2)
else:
enable_can(f, 1)
enable_can(f)
flash_size = get_config('FLASH_SIZE_KB', type=int)
f.write('#define BOARD_FLASH_SIZE %u\n' % flash_size)
env_vars['BOARD_FLASH_SIZE'] = flash_size