Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: use HAL CAN Drivers instead of ChibiOS's

This commit is contained in:
Siddharth Purohit 2020-07-31 19:07:18 +05:30 committed by Peter Barker
parent 9aa0970eed
commit 5ab6916a41
2 changed files with 22 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
Tools/AP_Periph/can.cpp
View File

@ -52,6 +52,7 @@
#include "../AP_Bootloader/app_comms.h"
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_HAL_ChibiOS/CANIface.h>
#include "i2c.h"
#include <utility>
@ -79,6 +80,8 @@ static uint8_t transfer_id;
#define CAN_PROBE_CONTINUOUS 0
#endif
static ChibiOS::CANIface can_iface(0);
/*
* Variables used for dynamic node ID allocation.
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
@ -780,13 +783,12 @@ static void processTx(void)
{
static uint8_t fail_count;
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
CANTxFrame txmsg {};
txmsg.DLC = txf->data_len;
memcpy(txmsg.data8, txf->data, 8);
txmsg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
txmsg.IDE = 1;
txmsg.RTR = 0;
if (canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE) == MSG_OK) {
AP_HAL::CANFrame txmsg {};
txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout
if (can_iface.send(txmsg, AP_HAL::micros64() + 1000000, 0) > 0) {
canardPopTxQueue(&canard);
fail_count = 0;
} else {
@ -802,41 +804,25 @@ static void processTx(void)
}
}
static ObjectBuffer<CANRxFrame> rxbuffer{32};
static void can_rxfull_cb(CANDriver *canp, uint32_t flags)
{
CANRxFrame rxmsg;
chSysLockFromISR();
while (canTryReceiveI(canp, CAN_ANY_MAILBOX, &rxmsg) == false) {
rxbuffer.push_force(rxmsg);
}
chSysUnlockFromISR();
}
static void processRx(void)
{
CANRxFrame rxmsg;
AP_HAL::CANFrame rxmsg;
while (true) {
bool have_msg;
chSysLock();
have_msg = rxbuffer.pop(rxmsg);
chSysUnlock();
if (!have_msg) {
bool read_select = true;
bool write_select = false;
can_iface.select(read_select, write_select, nullptr, 0);
if (!read_select) {
break;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
const uint64_t timestamp = AP_HAL::micros64();
memcpy(rx_frame.data, rxmsg.data8, 8);
rx_frame.data_len = rxmsg.DLC;
if(rxmsg.IDE) {
rx_frame.id = CANARD_CAN_FRAME_EFF | rxmsg.EID;
} else {
rx_frame.id = rxmsg.SID;
}
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
can_iface.receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id;
canardHandleRxFrame(&canard, &rx_frame, timestamp);
}
}
@ -1030,26 +1016,7 @@ void AP_Periph_FW::can_start()
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION;
node_status.uptime_sec = AP_HAL::millis() / 1000U;
static CANConfig cancfg = {
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
0
};
// calculate optimal CAN timings given PCLK1 and baudrate
CanardSTM32CANTimings timings {};
canardSTM32ComputeCANTimings(STM32_PCLK1, unsigned(g.can_baudrate), &timings);
cancfg.btr = CAN_BTR_SJW(0) |
CAN_BTR_TS2(timings.bit_segment_2-1) |
CAN_BTR_TS1(timings.bit_segment_1-1) |
CAN_BTR_BRP(timings.bit_rate_prescaler-1);
if (g.can_node >= 0 && g.can_node < 128) {
PreferredNodeID = g.can_node;
}
CAND1.rxfull_cb = can_rxfull_cb;
canStart(&CAND1, &cancfg);
can_iface.init(1000000, AP_HAL::CANIface::NormalMode);
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, NULL);

1
Tools/AP_Periph/wscript
View File

@ -24,6 +24,7 @@ def build(bld):
'AP_Param',
'StorageManager',
'AP_FlashStorage',
'AP_RAMTRON',
'AP_GPS',
'AP_SerialManager',
'AP_RTC',