Dummy-Robot/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp
2022-02-09 11:52:03 +08:00

251 lines
8.1 KiB
C++

/*
*
* Zero-config node ID negotiation
* -------------------------------
*
* A heartbeat message is a message with a 8 byte unique serial number as payload.
* A regular message is any message that is not a heartbeat message.
*
* All nodes MUST obey these four rules:
*
* a) At a given point in time, a node MUST consider a node ID taken (by others)
* if any of the following is true:
* - the node received a (not self-emitted) heartbeat message with that node ID
* within the last second
* - the node attempted and failed at sending a heartbeat message with that
* node ID within the last second (failed in the sense of not ACK'd)
*
* b) At a given point in time, a node MUST NOT consider a node ID self-assigned
* if, within the last second, it did not succeed in sending a heartbeat
* message with that node ID.
*
* c) At a given point in time, a node MUST NOT send any heartbeat message with
* a node ID that is taken.
*
* d) At a given point in time, a node MUST NOT send any regular message with
* a node ID that is not self-assigned.
*
* Hardware allocation
* -------------------
* RX FIFO0:
* - filter bank 0: heartbeat messages
*/
#include "common_inc.h"
#include <stm32f4xx_hal.h>
#include <cmsis_os.h>
// defined in can.c
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
CAN_context can1Ctx;
CAN_context can2Ctx;
static CAN_context* ctxs = nullptr;
static CAN_RxHeaderTypeDef headerRx;
static uint8_t data[8];
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
{
if (hcan->Instance == CAN1)
return &can1Ctx;
else if (hcan->Instance == CAN2)
return &can2Ctx;
else
return nullptr;
}
bool StartCanServer(CAN_TypeDef* hcan)
{
if (hcan == CAN1)
{
ctxs = &can1Ctx;
ctxs->handle = &hcan1;
} else if (hcan == CAN2)
{
ctxs = &can2Ctx;
ctxs->handle = &hcan2;
} else
return false; // fail if none of the above checks matched
HAL_StatusTypeDef status;
ctxs->node_id = 0;
ctxs->serial_number = serialNumber;
osSemaphoreDef(sem_send_heartbeat);
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
//// Set up filter
CAN_FilterTypeDef sFilterConfig = {
.FilterIdHigh = 0x0000,
.FilterIdLow = 0x0000,
.FilterMaskIdHigh = 0x0000,
.FilterMaskIdLow = 0x0000,
.FilterFIFOAssignment = CAN_RX_FIFO0,
.FilterBank = 0,
.FilterMode = CAN_FILTERMODE_IDMASK,
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
.FilterActivation = ENABLE,
.SlaveStartFilterBank = 0
};
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
if (status != HAL_OK)
return false;
status = HAL_CAN_Start(ctxs->handle);
if (status != HAL_OK)
return false;
status = HAL_CAN_ActivateNotification(ctxs->handle,
CAN_IT_TX_MAILBOX_EMPTY |
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
/* we probably only want this */
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
CAN_IT_ERROR);
if (status != HAL_OK)
return false;
return true;
}
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
// CAN_context* ctx = get_can_ctx(hcan);
// if (!ctx) return;
// ctx->tx_msg_cnt++;
if (hcan->Instance == CAN1)
osSemaphoreRelease(sem_can1_tx);
else if (hcan->Instance == CAN2)
osSemaphoreRelease(sem_can2_tx);
}
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
if (!get_can_ctx(hcan))
return;
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
}
void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
{
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 0); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 1); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 2); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 0); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 1); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 2); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
{
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
ctx->received_msg_cnt++;
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
if (status != HAL_OK)
{
ctx->unexpected_errors++;
return;
}
OnCanMessage(ctx, &headerRx, data);
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
{
//__asm volatile ("bkpt");
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
volatile uint32_t original_error = hcan->ErrorCode;
(void) original_error;
// handle transmit errors in all three mailboxes
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
{
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
{
tx_error(ctx, 0);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
{
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
{
tx_error(ctx, 1);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
{
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
{
tx_error(ctx, 2);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
}
if (hcan->ErrorCode)
ctx->unexpected_errors++;
}
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
{
osStatus semaphore_status;
if (canCtx->handle->Instance == CAN1)
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
else if (canCtx->handle->Instance == CAN2)
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
else
return;
if (semaphore_status == osOK)
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
}