150 lines
3.3 KiB
C
150 lines
3.3 KiB
C
#include "Vehicle.h"
|
|
#include "fdcan.h"
|
|
#include "CLS.h"
|
|
#include "CLS_BSP.h"
|
|
#include "BSP_GPIO.h"
|
|
#include "cmsis_os2.h"
|
|
|
|
static uint64_t last_unlock_message_time = UINT64_MAX;
|
|
static uint8_t car_can_brightness = 255;
|
|
static float car_can_speed = 0;
|
|
static int car_can_direction = 0;
|
|
|
|
|
|
void Vehicle_Setup_CAN() {
|
|
|
|
|
|
FDCAN_FilterTypeDef sFilterConfig;
|
|
sFilterConfig.IdType = FDCAN_STANDARD_ID;
|
|
sFilterConfig.FilterIndex = 0;
|
|
sFilterConfig.FilterType = CLS_BSP_CAN_FILTER_LIST;
|
|
sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1;
|
|
sFilterConfig.FilterID1 = 0x391;
|
|
sFilterConfig.FilterID2 = 0x395;
|
|
HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);
|
|
|
|
sFilterConfig.FilterIndex = 1;
|
|
sFilterConfig.FilterID1 = 0x351;
|
|
sFilterConfig.FilterID2 = 0x635;
|
|
HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig);
|
|
|
|
}
|
|
|
|
|
|
static void rx_unlock_key(uint8_t * RxData ) {
|
|
if (RxData[1] == 0x04)
|
|
{
|
|
// car was unlocked
|
|
last_unlock_message_time = osKernelGetTickCount();
|
|
}
|
|
else if (RxData[1] ==0x80)
|
|
{
|
|
// car was locked
|
|
if (!BSP_GPIO_K15isSet()) {
|
|
NVIC_SystemReset();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
static void rx_unlock_secure(uint8_t * RxData ) {
|
|
if ((RxData[0] & 0x0F) == 0x01) {
|
|
// car was unlocked
|
|
last_unlock_message_time = osKernelGetTickCount();
|
|
|
|
} else if ((RxData[0] & 0x0F) == 0x02) {
|
|
// car was locked
|
|
if (!BSP_GPIO_K15isSet()) {
|
|
NVIC_SystemReset();
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
static void rx_speed(uint8_t * RxData) {
|
|
// speed signal
|
|
// AA BB XX YY 00 00 00 00
|
|
// Speed (XX*(2^8)+(YY-1))/190
|
|
// direction AA = 0x00 forward, 0x02 backward
|
|
uint16_t speed = (RxData[2] << 8) + RxData[3];
|
|
float speed_kmh = (speed - 1) / 190.0;
|
|
car_can_speed = speed_kmh;
|
|
car_can_direction = RxData[0];
|
|
//ULOG_DEBUG("Speed: %f, Direction: %d", car_can_speed, car_can_direction);
|
|
|
|
}
|
|
|
|
static void rx_brightness(uint8_t* RxData) {
|
|
// scale the brightness to 0 - 255 only using integer math
|
|
car_can_brightness = ((uint32_t)RxData[0] * 255) / 100;
|
|
//ULOG_DEBUG("Brightness: %d", car_can_brightness);
|
|
}
|
|
|
|
void Vehicle_Receive_CAN( FDCAN_RxHeaderTypeDef RxHeader, uint8_t* RxData) {
|
|
|
|
if(RxHeader.Identifier == 0x391) {
|
|
rx_unlock_key(RxData);
|
|
}
|
|
|
|
if (RxHeader.Identifier == 0x395) {
|
|
rx_unlock_secure(RxData);
|
|
}
|
|
|
|
|
|
if (RxHeader.Identifier == 0x351) {
|
|
rx_speed(RxData);
|
|
}
|
|
|
|
// brightness knob in 0 - 100
|
|
if (RxHeader.Identifier == 0x635) {
|
|
rx_brightness(RxData);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
bool Vehicle_gotUnlockMessage() {
|
|
return last_unlock_message_time != UINT64_MAX;
|
|
}
|
|
|
|
|
|
|
|
uint8_t Vehicle_Brightness() {
|
|
return car_can_brightness;
|
|
}
|
|
|
|
float Vehicle_Speed() {
|
|
return car_can_speed;
|
|
}
|
|
|
|
int Vehicle_DirectionIsForward() {
|
|
return car_can_direction == 0;
|
|
}
|
|
|
|
|
|
static bool unlocked = false;
|
|
|
|
|
|
bool Vehicle_UnlockedSignal() {
|
|
|
|
if (unlocked) {
|
|
if(Vehicle_isK15On()) {
|
|
unlocked = false;
|
|
last_unlock_message_time = UINT64_MAX;
|
|
}
|
|
}
|
|
|
|
if (!unlocked) {
|
|
if (Vehicle_gotUnlockMessage()) {
|
|
unlocked = true;
|
|
}
|
|
}
|
|
|
|
return unlocked;
|
|
}
|
|
|
|
|
|
|