108 lines
2.7 KiB
C
108 lines
2.7 KiB
C
#include "vehicle.h"
|
|
|
|
#include "main.h"
|
|
#include "state.h"
|
|
|
|
#include "stm32g4xx_hal_def.h"
|
|
#include "stm32g4xx_hal_fdcan.h"
|
|
|
|
FDCAN_HandleTypeDef* can;
|
|
|
|
void handle_as_mission_fb(FDCAN_RxHeaderTypeDef* header, uint8_t* data);
|
|
|
|
void vehicle_init(FDCAN_HandleTypeDef* handle) {
|
|
can = handle;
|
|
|
|
// RX Filter
|
|
FDCAN_FilterTypeDef filter;
|
|
filter.IdType = FDCAN_STANDARD_ID;
|
|
filter.FilterIndex = 0;
|
|
filter.FilterType = FDCAN_FILTER_MASK;
|
|
filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
|
|
filter.FilterID1 = CAN_ID_AS_MISSION_FB;
|
|
filter.FilterID2 = filter.FilterID1;
|
|
if (HAL_FDCAN_ConfigFilter(can, &filter) != HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
|
|
// Global filter: Filter all remote frames with STD & EXT ID, reject non
|
|
// matching frames with STD & EXT ID
|
|
|
|
// TODO: What does that mean?
|
|
if (HAL_FDCAN_ConfigGlobalFilter(can, FDCAN_REJECT, FDCAN_REJECT,
|
|
FDCAN_FILTER_REMOTE,
|
|
FDCAN_FILTER_REMOTE) != HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
|
|
// Start FDCAN module
|
|
if (HAL_FDCAN_Start(can) != HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
|
|
// Enable RX interrupt
|
|
if (HAL_FDCAN_ActivateNotification(can, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) !=
|
|
HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
}
|
|
|
|
void vehicle_send_mission_select(Mission mission) {
|
|
// TODO: Automatically resend this until it's acknowledged
|
|
FDCAN_TxHeaderTypeDef header;
|
|
header.Identifier = CAN_ID_MISSION_SELECT;
|
|
header.IdType = FDCAN_STANDARD_ID;
|
|
header.TxFrameType = FDCAN_DATA_FRAME;
|
|
header.DataLength = FDCAN_DLC_BYTES_1;
|
|
header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
|
|
header.BitRateSwitch = FDCAN_BRS_OFF;
|
|
header.FDFormat = FDCAN_CLASSIC_CAN;
|
|
header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
|
|
header.MessageMarker = 0;
|
|
|
|
uint8_t data[1];
|
|
data[0] = mission;
|
|
if (HAL_FDCAN_AddMessageToTxFifoQ(can, &header, data) != HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
}
|
|
|
|
/* BEGIN CAN MESSAGE HANDLERS { */
|
|
|
|
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* handle,
|
|
uint32_t interrupt_flags) {
|
|
if (!(interrupt_flags & FDCAN_IT_RX_FIFO0_NEW_MESSAGE)) {
|
|
return;
|
|
}
|
|
|
|
static FDCAN_RxHeaderTypeDef header;
|
|
static uint8_t data[64];
|
|
|
|
if (HAL_FDCAN_GetRxMessage(can, FDCAN_RX_FIFO0, &header, data) != HAL_OK) {
|
|
Error_Handler();
|
|
}
|
|
|
|
if (header.IdType != FDCAN_STANDARD_ID) {
|
|
return;
|
|
}
|
|
|
|
switch (header.Identifier) {
|
|
case CAN_ID_AS_MISSION_FB:
|
|
handle_as_mission_fb(&header, data);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void handle_as_mission_fb(FDCAN_RxHeaderTypeDef* header, uint8_t* data) {
|
|
Mission mission_fb = data[0];
|
|
if (mission_fb == MISSION_NONE) {
|
|
return;
|
|
}
|
|
|
|
stw_state.view_state.ami.current_mission = mission_fb;
|
|
|
|
stw_state.view = VIEW_AMI;
|
|
}
|
|
|
|
/* } END CAN MESSAGE HANDLERS */
|