237 lines
7.3 KiB
C
237 lines
7.3 KiB
C
#include "cm_iomux.h"
|
||
#include "cm_gpio.h"
|
||
#include "stdio.h"
|
||
#include "stdlib.h"
|
||
#include "stdarg.h"
|
||
#include "cm_os.h"
|
||
#include "cm_mem.h"
|
||
#include "cm_sys.h"
|
||
#include "cm_uart.h"
|
||
|
||
#include "app_common.h"
|
||
#include "app_uart.h"
|
||
#include "control_out.h"
|
||
#include "radar.h"
|
||
#include "local_tts.h"
|
||
|
||
|
||
#define RADAR_DEBUG_ENABLE 0
|
||
|
||
#if RADAR_DEBUG_ENABLE
|
||
#include "app_uart.h"
|
||
#define DEBUG(fmt, args...) app_printf("[RADAR]" fmt, ##args)
|
||
#else
|
||
#define DEBUG(fmt, ...)
|
||
#endif
|
||
|
||
static osThreadId_t os_RADAR_ThreadId;
|
||
radar_data_t radar_data;
|
||
osMutexId_t radar_mutex; // 互斥锁
|
||
uint8_t old_state = 0;
|
||
/******************************************************************************
|
||
* Name: CRC-16/MODBUS
|
||
* Poly: 0x8005 ( x16+x15+x2+1 )
|
||
* Init: 0xFFFF
|
||
* Refin: True
|
||
* Refout: True
|
||
* Xorout: 0x0000
|
||
*****************************************************************************/
|
||
uint16_t crc16_modbus(uint8_t *data, uint16_t length){
|
||
uint8_t i;
|
||
uint16_t crc = 0xFFFF; // Initial value
|
||
while(length--){
|
||
crc ^= *data++;
|
||
for (i = 0; i < 8; ++i){
|
||
if (crc & 1){
|
||
crc = (crc >> 1) ^ 0xA001; // 0xA001 = reverse 0x8005
|
||
}else{
|
||
crc = (crc >> 1);
|
||
}
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
|
||
// 自动刹车或减速判断处理函数
|
||
static void radar_AUTO_BrakeORSpeedCut(uint8_t radar_id , uint16_t Car_Distance){ //距离数据30~4500mm
|
||
// 自动刹车距离
|
||
uint16_t Brake_Distance = ((radar_id == RADAR_ID_Back )? SYS_CONF_BRAKE_LIMIT_B : SYS_CONF_BRAKE_LIMIT);
|
||
// 自动减速距离
|
||
uint16_t Speed_Cut_Distance = ((radar_id == RADAR_ID_Back )? SYS_CONF_SPEED_CUT_LIMIT_B : SYS_CONF_SPEED_CUT_LIMIT);
|
||
|
||
|
||
if((35 < Car_Distance) && Car_Distance < Brake_Distance){//小于自动刹车距离时
|
||
if(!sys_sta.A_brake){
|
||
// local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("刹车",0,0);
|
||
// Time_Event_Blink(Buzzer_Event,100,100,0xffff,NULL);
|
||
}
|
||
sys_sta.A_brake =1;//使能自动刹车
|
||
sys_sta.A_Speed_Cut=1;
|
||
}else if((35 < Car_Distance) && Car_Distance < Speed_Cut_Distance){//小于自动减速距离时
|
||
if(sys_sta.A_brake || !sys_sta.A_Speed_Cut){
|
||
// local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("减速",0,0);
|
||
// Time_Event_Blink(Buzzer_Event,200,500,0xffff,NULL);
|
||
}
|
||
sys_sta.A_brake =0;
|
||
sys_sta.A_Speed_Cut=1;//使能自动减速
|
||
}else{
|
||
if(sys_sta.A_brake || sys_sta.A_Speed_Cut){
|
||
// local_tts_text_play("自动刹车",0,0);
|
||
// Time_Event_Off(Buzzer_Event,NULL);
|
||
}
|
||
sys_sta.A_brake =0;
|
||
sys_sta.A_Speed_Cut=0;
|
||
}
|
||
|
||
|
||
}
|
||
|
||
static uint8_t radar_CMDSend_cnt=0; // 发送命令计数
|
||
static uint8_t radar_CMDReceive_cnt=0; // 接收命令计数
|
||
|
||
//发送读取实时值数据命令
|
||
void radar_Sendcmd(uint8_t radar_id , uint8_t radar_mode){
|
||
uint8_t len=0;
|
||
uint8_t data[8];
|
||
|
||
data[len++] = radar_id;
|
||
data[len++] = 0x03;
|
||
data[len++] = 0x01;
|
||
data[len++] = radar_mode ? 0x01 : 0x00; // 0x01:实时值(120ms) 0x00:处理值(520ms)
|
||
data[len++] = 0x00;
|
||
data[len++] = 0x01;
|
||
|
||
uint16_t crc16 =crc16_modbus(data,len);
|
||
data[len++] = crc16 & 0xff;
|
||
data[len++] = (crc16 >> 8) & 0xff;
|
||
uart0_send_msg(data,len,0);
|
||
radar_CMDSend_cnt++;
|
||
}
|
||
|
||
// 数据接收处理函数
|
||
void radar_CheckData(uint8_t *data, uint16_t data_len){
|
||
if(data_len != 7){ // 数据长度不正确
|
||
return;
|
||
}
|
||
uint16_t crc16 =crc16_modbus(data, 5);
|
||
if((data[0] == RADAR_ID_Front) || (data[0] == RADAR_ID_Back) ||
|
||
(crc16 == (((uint16_t)data[6] << 8 ) | data[5]))){ // 雷达ID匹配
|
||
radar_data.radar_id = data[0];
|
||
radar_data.distance = ((uint16_t)data[3] << 8 ) | data[4];
|
||
radar_AUTO_BrakeORSpeedCut(radar_data.radar_id ,radar_data.distance);
|
||
DEBUG("radar_id:%#02x,distance:%d", radar_data.radar_id, radar_data.distance);
|
||
radar_CMDReceive_cnt++;
|
||
}
|
||
}
|
||
|
||
// 定时处理数和获取数据
|
||
static void RADAR_TaskHandle(void *param){
|
||
uint8_t temp_count =0;
|
||
uint32_t time_count =0;
|
||
while(1){
|
||
|
||
if(old_state != jt808_term_param_item.set_term_param.RadarEN)
|
||
{
|
||
sys_sta.P_Radar_EN = jt808_term_param_item.set_term_param.RadarEN;
|
||
old_state = jt808_term_param_item.set_term_param.RadarEN;
|
||
|
||
if (0==sys_sta.P_Radar_EN)
|
||
{
|
||
local_tts_text_play("雷达关闭",0,0);
|
||
}
|
||
|
||
if (1==sys_sta.P_Radar_EN)
|
||
{
|
||
local_tts_text_play("雷达开启",0,0);
|
||
}
|
||
|
||
if (2==sys_sta.P_Radar_EN)
|
||
{
|
||
local_tts_text_play("雷达临时关闭",0,0);
|
||
}
|
||
|
||
}
|
||
DEBUG("radar task init\r\n");
|
||
|
||
if(0==sys_sta.P_Radar_EN)
|
||
{
|
||
sys_sta.A_Speed_Cut =0; // 清空自动减速状态
|
||
sys_sta.A_brake =0; // 清空自动刹车状态
|
||
DEBUG("radar close\r\n"); // 任务开始
|
||
|
||
continue;
|
||
}
|
||
|
||
DEBUG("radar open\r\n"); // 任务开始
|
||
|
||
osMutexAcquire(radar_mutex, osWaitForever);
|
||
if (1==sys_sta.P_Radar_EN)
|
||
{
|
||
if(radar_CMDSend_cnt != radar_CMDReceive_cnt){ // 发送命令和接收命令计数不一致时，复位自动刹车和减速状态
|
||
temp_count++;
|
||
if(temp_count > 10){ // 连续10次未收到数据，复位自动刹车和减速状态
|
||
temp_count =0;
|
||
radar_CMDSend_cnt =0; // 清空发送命令计数
|
||
radar_CMDReceive_cnt =0; // 清空接收命令计数
|
||
DEBUG("radar anomaly......"); // 雷达异常
|
||
radar_AUTO_BrakeORSpeedCut(radar_data.radar_id , 0); // 复位自动刹车和减速状态
|
||
}
|
||
}
|
||
|
||
|
||
// 进入游客模式开启雷达 //管理员模式优先于游客模式
|
||
if(((0 != sys_sta.O_door_lock) && ((1 == sys_sta.MAG_MODE)&&(1 == sys_sta.PLT_MODE ))) && (1 == sys_sta.P_Radar_EN)){
|
||
// 根据倒车状态确定雷达ID
|
||
// DEBUG("SendCMD:ID=%#02x", (0 == sys_sta.IO_RX_back)?RADAR_ID_Back:RADAR_ID_Front);
|
||
radar_Sendcmd((0 == sys_sta.IO_RX_back)?RADAR_ID_Back:RADAR_ID_Front, RADAR_MODE_Real);
|
||
|
||
// DEBUG("radar 1\r\n"); // 任务结束
|
||
}else{
|
||
sys_sta.A_Speed_Cut =0; // 清空自动减速状态
|
||
sys_sta.A_brake =0; // 清空自动刹车状态
|
||
DEBUG("radar 1\r\n"); // 任务结束
|
||
|
||
}
|
||
|
||
}
|
||
|
||
else if (2==sys_sta.P_Radar_EN)
|
||
{
|
||
|
||
time_count++;
|
||
sys_sta.A_Speed_Cut =0; // 清空自动减速状态
|
||
sys_sta.A_brake =0; // 清空自动刹车状态
|
||
|
||
if(time_count>=400)
|
||
{
|
||
sys_sta.P_Radar_EN = 1;
|
||
time_count=0;
|
||
|
||
|
||
}
|
||
|
||
|
||
DEBUG("radar 2\r\n"); // 任务结束
|
||
|
||
}
|
||
|
||
osMutexRelease(radar_mutex);
|
||
osDelay(140/5); // 140ms
|
||
}
|
||
}
|
||
|
||
// 雷达测离初始化
|
||
void radar_init(void){
|
||
radar_data.radar_id = RADAR_ID_Front;
|
||
radar_data.distance = 0;
|
||
radar_mutex = osMutexNew(NULL);
|
||
osThreadAttr_t radar_task_attr = {
|
||
.name = "uart_tx_task",
|
||
.stack_size = 4096*4,
|
||
.priority= osPriorityBelowNormal
|
||
};
|
||
os_RADAR_ThreadId= osThreadNew(RADAR_TaskHandle, 0, &radar_task_attr);
|
||
}
|