410 lines
15 KiB
C
410 lines
15 KiB
C
#include <stdint.h>
|
||
#include "cm_adc.h"
|
||
#include "cm_gpio.h"
|
||
#include "cm_iomux.h"
|
||
#include "cm_pwm.h"
|
||
|
||
#include "local_tts.h"
|
||
#include "control_out.h"
|
||
#include "radar.h"
|
||
|
||
|
||
#define ADC_DAC_DEBUG_ENABLE 0
|
||
|
||
#if ADC_DAC_DEBUG_ENABLE
|
||
#include "app_uart.h"
|
||
#define DEBUG(fmt, args...) app_printf("[CONTROL_OUT]" fmt, ##args)
|
||
#else
|
||
#define DEBUG(fmt, ...)
|
||
#endif
|
||
|
||
#define PWM0_IOMUX CM_IOMUX_PIN_74, CM_IOMUX_FUNC_FUNCTION1
|
||
#define PWM1_IOMUX CM_IOMUX_PIN_75, CM_IOMUX_FUNC_FUNCTION1
|
||
#define PWM2_IOMUX CM_IOMUX_PIN_21, CM_IOMUX_FUNC_FUNCTION2
|
||
#define PWM3_IOMUX 0, 0
|
||
|
||
osThreadId_t control_out_TaskHandle = NULL;
|
||
|
||
// SYS_CONF_t sys_conf ={
|
||
// .SW_version = 1,
|
||
// .HW_version = 1,
|
||
// .VER_type = 1,
|
||
// .Manager_sLim =100,
|
||
// .Tourist_sLim =50,
|
||
// .Speed_Cut_sLim =20,
|
||
// .Brake_DLimit =1200,
|
||
// .Speed_Cut_DLimit =2000,
|
||
// .Brake_DLimit_B =1200,
|
||
// .Speed_Cut_DLimit_B =2000,
|
||
// };
|
||
sys_sta_t sys_sta;
|
||
|
||
typedef enum {
|
||
ADC_power = 0,
|
||
ADC_acc = 1,
|
||
}adc_chx_t;
|
||
|
||
// 获取电压值 unit:mV
|
||
int32_t get_voltage(adc_chx_t chx){
|
||
int32_t voltage = 0;
|
||
if(chx == ADC_acc){
|
||
cm_gpio_set_level(CM_GPIO_NUM_20, 0);
|
||
}else if(chx == ADC_power){
|
||
cm_gpio_set_level(CM_GPIO_NUM_20, 1);
|
||
}else{
|
||
return 0;
|
||
}
|
||
osDelay(1);//5ms
|
||
if(0 != cm_adc_read(CM_ADC_0,&voltage)){
|
||
DEBUG("ADC read fail\r\n");
|
||
voltage = 0;
|
||
}
|
||
if(chx == ADC_acc){
|
||
voltage = voltage * ((ACC_RES_UP+ACC_RES_DOWN) / ACC_RES_DOWN); //转化为电压值
|
||
cm_gpio_set_level(CM_GPIO_NUM_20, 1);
|
||
}else if(chx == ADC_power){
|
||
voltage = voltage * ((VIN_RES_UP+VIN_RES_DOWN) / VIN_RES_DOWN); //转化为电压值
|
||
cm_gpio_set_level(CM_GPIO_NUM_20, 0);
|
||
}else{
|
||
return 0;
|
||
}
|
||
// cm_gpio_set_level(CM_GPIO_NUM_20, 1); //恢复默认状态
|
||
// DEBUG("CH%d voltage:%d\n",chx ,voltage);
|
||
return voltage;
|
||
}
|
||
|
||
uint32_t ACC_Dmin =(((ACC_OUT_Voltage_MIN*ACC_OUT_RES_DOWN)/(ACC_OUT_RES_UP+ACC_OUT_RES_DOWN))*(ACC_OUT_PERIOD_US/(1800)));
|
||
uint32_t ACC_Dmax =(((ACC_OUT_Voltage_MAX*ACC_OUT_RES_DOWN)/(ACC_OUT_RES_UP+ACC_OUT_RES_DOWN))*(ACC_OUT_PERIOD_US/(1800)));
|
||
|
||
// 获取输入油门百分比（unit:%)(踏板信号)
|
||
uint16_t get_in_acc_percent(void){
|
||
uint16_t acc_voltage =get_voltage(ADC_acc);
|
||
// DEBUG("acc_voltage:%d\n",acc_voltage);
|
||
if(acc_voltage < 900){
|
||
return 0;
|
||
}else
|
||
if(acc_voltage > 3800){
|
||
return 100;
|
||
}else{
|
||
return ((acc_voltage-900)/29);
|
||
// return (100*(acc_voltage - 900)/(3800-900));
|
||
}
|
||
}
|
||
|
||
// 设置输出油门大小
|
||
void set_out_acc_percent(uint8_t percent){//1000000设置频率为10KHz
|
||
if(0 != cm_pwm_open_ns(CM_PWM_DEV_0,ACC_OUT_PERIOD_US,(ACC_Dmin+((ACC_Dmax-ACC_Dmin)*(percent))/100))){
|
||
DEBUG("pwm0 open error\n");
|
||
}
|
||
}
|
||
|
||
// // 设置输出油门大小
|
||
// void set_out_acc_percent(uint8_t percent){//1000000设置频率为10KHz
|
||
// static uint32_t last_period_h = 0;
|
||
// uint32_t period_h = (ACC_Dmin+((ACC_Dmax-ACC_Dmin)*(percent))/100);
|
||
// if(last_period_h != period_h){
|
||
// last_period_h = period_h;
|
||
// if(0 != cm_pwm_open_ns(CM_PWM_DEV_0,ACC_OUT_PERIOD_US,period_h){
|
||
// DEBUG("pwm0 open error\n");
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
const float coulomp_voltage_tablesSLA[3][21] = {
|
||
{ // 4串
|
||
420.0f, 423.6f, 427.2f, 430.8f, 434.4f, 438.0f, 441.6f, 445.2f, 448.8f, 452.4f,
|
||
456.0f, 459.6f, 463.2f, 466.8f, 470.4f, 474.0f, 477.6f, 481.2f, 484.8f, 488.4f, 492.0f
|
||
},
|
||
{ // 5串
|
||
525.0f, 529.5f, 534.0f, 538.5f, 543.0f, 547.5f, 552.0f, 556.5f, 561.0f, 565.5f,
|
||
570.0f, 574.5f, 579.0f, 583.5f, 588.0f, 592.5f, 597.0f, 601.5f, 606.0f, 610.5f, 615.0f
|
||
},
|
||
{ // 6串
|
||
630.0f, 635.4f, 640.8f, 646.2f, 651.6f, 657.0f, 662.4f, 667.8f, 673.2f, 678.6f,
|
||
684.0f, 689.4f, 694.8f, 700.2f, 705.6f, 711.0f, 716.4f, 721.8f, 727.2f, 732.6f, 738.0f
|
||
}
|
||
};
|
||
|
||
const float coulomp_voltage_tablesLi[3][21] = {
|
||
{ // 4串
|
||
|
||
},
|
||
{ // 5串
|
||
|
||
},
|
||
{ // 6串
|
||
|
||
}
|
||
};
|
||
|
||
|
||
|
||
uint8_t pmu_voltage_to_percent(uint32_t volt){//电压转百分比
|
||
uint8_t i, percent;
|
||
float step, base;
|
||
uint32_t table_index;
|
||
table_index = jt808_term_param_item.set_term_param.BatPack_V;
|
||
const float* voltage_table = coulomp_voltage_tablesSLA[table_index];
|
||
|
||
if((volt < voltage_table[0])){
|
||
return 0;
|
||
}
|
||
if((volt >= voltage_table[20])){
|
||
return 100;
|
||
}
|
||
|
||
for (i = 0; i < 20; i++) {
|
||
if((volt < voltage_table[i+1]) && (volt >= voltage_table[i])){
|
||
step = (voltage_table[i+1] - voltage_table[i]) / 10.0;
|
||
base = voltage_table[i];
|
||
percent = (10 * i) + (volt - base) / step;
|
||
// DEBUG("\n\ni:%d,volt:%d,step:%02.2f,base:%02.2f,percent:%d\n\n",i,volt,step,base,percent);
|
||
return (uint8_t)percent;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
#define _out_UPDETE_TIME 10
|
||
// static uint32_t back_timecount = 0;
|
||
static uint8_t door_lock_last_status = 0;
|
||
static uint8_t S_MODE_last_status = 1;
|
||
static uint8_t P_MODE_last_status = 1;
|
||
|
||
|
||
BAT_Message_t BAT_Message;
|
||
|
||
// 门锁状态更新标志
|
||
bool GRB_Door_lock_updata=0;
|
||
uint16_t Voltage_lag_count;
|
||
uint16_t Voltage_update_count;
|
||
float temp_bat_voltage=0;
|
||
|
||
void control_out_task(void *argument){
|
||
cm_gpio_level_e level = 0;
|
||
BAT_Message.Bat_Voltage = get_voltage(ADC_power) / 100;
|
||
BAT_Message.Rsp_BigBat_Voltage =Swap16(BAT_Message.Bat_Voltage);
|
||
BAT_Message.Bat_Percent = pmu_voltage_to_percent(BAT_Message.Bat_Voltage);
|
||
while(1){
|
||
|
||
uint16_t in_acc_percent= get_in_acc_percent();
|
||
// DEBUG("in_acc_percent:%d\n\n",in_acc_percent);
|
||
|
||
if(BAT_Message.Runing_Time < 0xFFFF){
|
||
BAT_Message.Runing_Time++;
|
||
}
|
||
|
||
temp_bat_voltage += get_voltage(ADC_power) / 100;
|
||
|
||
Voltage_update_count++;
|
||
if(Voltage_update_count >= (1000/_out_UPDETE_TIME)){ // 1s
|
||
BAT_Message.Bat_Voltage = temp_bat_voltage / Voltage_update_count;
|
||
BAT_Message.Rsp_BigBat_Voltage =Swap16(BAT_Message.Bat_Voltage);
|
||
Voltage_update_count=0;
|
||
temp_bat_voltage = 0;
|
||
}
|
||
// in_acc_percent =80;
|
||
if(in_acc_percent >= 80){ // 油门大于80%时，
|
||
if(BAT_Message.Deep_ACC_Time < 0xFFFF){
|
||
BAT_Message.Deep_ACC_Time++;
|
||
}
|
||
Voltage_lag_count++;
|
||
if(Voltage_lag_count >= (2000/_out_UPDETE_TIME)){ // 2s
|
||
Voltage_lag_count=0;
|
||
BAT_Message.Bat_Percent = pmu_voltage_to_percent(BAT_Message.Bat_Voltage);
|
||
}
|
||
if(BAT_Message.Bat_Percent < 12){
|
||
BAT_Message.Bat_Low_Volt_count++;
|
||
}
|
||
}else{
|
||
Voltage_lag_count=0;
|
||
}
|
||
|
||
if(BAT_Message.Bat_Percent < 7){
|
||
BAT_Message.Bat_STA =(BAT_Message.Bat_STA & 0xE0)|Bat_Low_STA;
|
||
}else if(BAT_Message.Bat_Percent < 30){ // 7~30%
|
||
BAT_Message.Bat_STA =(BAT_Message.Bat_STA & 0xE0)|Bat_30_STA;
|
||
}else if(BAT_Message.Bat_Percent < 60){ // 30~60%
|
||
BAT_Message.Bat_STA =(BAT_Message.Bat_STA & 0xE0)|Bat_60_STA;
|
||
}else if(BAT_Message.Bat_Percent < 80){ // 60~80%
|
||
BAT_Message.Bat_STA =(BAT_Message.Bat_STA & 0xE0)|Bat_80_STA;
|
||
}else{
|
||
BAT_Message.Bat_STA =(BAT_Message.Bat_STA & 0xE0)|Bat_100_STA;
|
||
}
|
||
// DEBUG("Bat_Percent:%d,Bat_Voltage:%d,Bat_STA:%X\r\n",BAT_Message.Bat_Percent,BAT_Message.Bat_Voltage,BAT_Message.Bat_STA);
|
||
|
||
// DEBUG("ADC_acc:%dmV\r\n",get_voltage(ADC_acc));
|
||
// DEBUG("ADC_power:%dmV\r\n\n",get_voltage(ADC_power));
|
||
// 系统模式状态
|
||
if(0 ==cm_gpio_get_level(IO_MANAGER_MODE, &level)){ // 更新系统模式
|
||
sys_sta.MAG_MODE = level; // 1:游客模式,0:管理模式
|
||
// DEBUG("IO_SYS_MODE:%d\r\n",level);
|
||
}
|
||
// 倒车状态
|
||
if(0 ==cm_gpio_get_level(IO_RX_BACK, &level)){ // 更新倒车模式
|
||
sys_sta.IO_RX_back = level; // 0:倒车模式,1:正常模式
|
||
// DEBUG("IO_RX_back:%d\r\n",level);
|
||
}
|
||
// 刹车状态
|
||
if(1 == sys_sta.A_brake){ // 自动刹车触发
|
||
cm_gpio_set_direction(IO_TX_BRAKE, CM_GPIO_DIRECTION_OUTPUT);
|
||
cm_gpio_set_level(IO_TX_BRAKE, 0); // 打开刹车
|
||
// DEBUG("auto brake\r\n");
|
||
}else{
|
||
cm_gpio_set_direction(IO_TX_BRAKE, CM_GPIO_DIRECTION_INPUT);
|
||
cm_gpio_set_pull(IO_TX_BRAKE, CM_GPIO_PULL_UP); // 关闭刹车
|
||
cm_gpio_set_level(IO_TX_BRAKE, 1);
|
||
// DEBUG("close brake\r\n");
|
||
}
|
||
if(0 ==cm_gpio_get_level(IO_TX_BRAKE, &level)){ // 更新刹车状态
|
||
// if(level != sys_sta.IO_TX_brake && level == 0){
|
||
// local_tts_text_play("刹车",0,0);
|
||
// }
|
||
sys_sta.IO_TX_brake = level; // 0:刹车模式,1:正常模式
|
||
// DEBUG("IO_TX_brake:%d\r\n",level);
|
||
// if(level == 0){
|
||
// sys_sta.O_door_lock = 1; // 电门锁打开
|
||
// sys_sta.PLT_MODE = 1; //
|
||
// }else{
|
||
// sys_sta.O_door_lock = 0; // 电门锁打开
|
||
// sys_sta.PLT_MODE = 1; //
|
||
// }
|
||
}
|
||
|
||
// 动作语音提示
|
||
if((door_lock_last_status != sys_sta.O_door_lock) && (1 == sys_sta.MAG_MODE ) && (1 == sys_sta.PLT_MODE)){ // 游客模式下，电门锁状态变化时触发
|
||
door_lock_last_status = sys_sta.O_door_lock;
|
||
if(1 == sys_sta.O_door_lock){ // 电门锁打开
|
||
// DEBUG("\n\ndoor lock open\r\n\n");
|
||
// local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("车辆已开锁",0,0);
|
||
}else{ // 电门锁关闭
|
||
// DEBUG("\n\ndoor lock close\r\n\n");
|
||
// local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("已锁车",0,0);
|
||
// local_tts_text_play("行程结束，祝您生活愉快",0,0);
|
||
}
|
||
}
|
||
if((S_MODE_last_status != sys_sta.MAG_MODE)||(P_MODE_last_status != sys_sta.PLT_MODE)){ // 管理员模式状态变化时触发
|
||
S_MODE_last_status = sys_sta.MAG_MODE;
|
||
P_MODE_last_status = sys_sta.PLT_MODE;
|
||
if((0 == sys_sta.MAG_MODE)||(0 == sys_sta.PLT_MODE)){ // 管理模式
|
||
DEBUG("\n\nMAG_MODE\r\n\n");
|
||
// local_tts_set(5, 7, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("管理员模式",0,1);
|
||
DEBUG("MAGmMODE=%d, PLTMODE=%d\r\n\n",sys_sta.MAG_MODE,sys_sta.PLT_MODE);
|
||
|
||
}else{ // 退出管理员模式
|
||
DEBUG("\n\nPLT_MODE\r\n\n");
|
||
// local_tts_set(10, 7, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
local_tts_text_play("退出管理员模式",0,1);
|
||
}
|
||
}
|
||
|
||
// if((sys_sta.IO_RX_back == 0)&&((1 == sys_sta.O_door_lock) || (0 == sys_sta.MAG_MODE ))){ // 倒车模式
|
||
// back_timecount++;
|
||
// // DEBUG("back_timecount:%d\r\n\n",back_timecount);
|
||
// if(back_timecount > _out_UPDETE_TIME*10){ // 3秒播报倒车提示语一次
|
||
// back_timecount = 0;
|
||
// DEBUG("\n\n>>>>>>>>>>>>back time play.......... \r\n\n");
|
||
// // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
|
||
// local_tts_text_play("请注意倒车！",0,0); // 忙时自动取消倒车提示语
|
||
// }
|
||
// }
|
||
|
||
//油门控制
|
||
if((1 == sys_sta.O_door_lock) || (0 == sys_sta.MAG_MODE )){
|
||
cm_gpio_set_level(OUT_Door_lock, 1); // 打开电门锁
|
||
uint8_t acc_percent =0;
|
||
if((1 == sys_sta.MAG_MODE)&&(1 == sys_sta.PLT_MODE )){ // 游客模式
|
||
|
||
osMutexAcquire(radar_mutex, osWaitForever);
|
||
if(sys_sta.A_Speed_Cut){ // 自动减速
|
||
acc_percent = (in_acc_percent * SYS_CONF_SPEED_CUT_ACC) / 100.0;
|
||
}else{
|
||
acc_percent = (in_acc_percent * SYS_CONF_TOURIST_ACC) / 100.0;
|
||
}
|
||
osMutexRelease(radar_mutex);
|
||
|
||
}else{ // 管理员模式
|
||
acc_percent = (in_acc_percent * SYS_CONF_MANAGER_ACC) / 100.0;
|
||
}
|
||
set_out_acc_percent(acc_percent);
|
||
// DEBUG("in_acc_percent:%d ,acc_percent:%d \r\n",in_acc_percent, acc_percent);
|
||
// DEBUG("SYS_CONF_SPEED_CUT_ACC:%d,SYS_CONF_TOURIST_ACC:%d,SYS_CONF_MANAGER_ACC:%d\r\n",SYS_CONF_SPEED_CUT_ACC,SYS_CONF_TOURIST_ACC,SYS_CONF_MANAGER_ACC);
|
||
// DEBUG("IO_RX_back:%d,IO_TX_brake:%d,PLT_MODE:%d\r\n",sys_sta.IO_RX_back,sys_sta.IO_TX_brake,sys_sta.PLT_MODE);
|
||
}else{
|
||
cm_gpio_set_level(OUT_Door_lock, 0); // 关闭电门锁
|
||
set_out_acc_percent(0);
|
||
}
|
||
|
||
// osDelay(_out_UPDETE_TIME/5);
|
||
}
|
||
}
|
||
|
||
void control_out_init(void){
|
||
cm_gpio_cfg_t cfg = {0};
|
||
|
||
// 手动设置初始值
|
||
sys_sta.O_door_lock = 0; // 电门锁状态:关闭
|
||
sys_sta.PLT_MODE = 1; // 模式:游客模式
|
||
sys_sta.P_Radar_EN = 1; // 雷达使能状态:开启
|
||
// 自动设置
|
||
sys_sta.MAG_MODE = 1; // 系统模式:游客模式
|
||
sys_sta.IO_RX_back = 1; // 倒车模式:正常模式
|
||
sys_sta.IO_TX_brake = 1; // 刹车模式:正常模式
|
||
sys_sta.A_brake = 0; // 自动刹车状态:关闭
|
||
sys_sta.A_Speed_Cut = 0; // 自动减速状态:关闭
|
||
|
||
// SW_ADC
|
||
cfg.direction = CM_GPIO_DIRECTION_OUTPUT;
|
||
cfg.pull = CM_GPIO_PULL_UP;
|
||
cm_iomux_set_pin_func(CM_IOMUX_PIN_86, CM_IOMUX_FUNC_FUNCTION2);//初始化之前一定要先设置引脚复用
|
||
cm_gpio_init(CM_GPIO_NUM_20, &cfg);
|
||
cm_gpio_set_level(CM_GPIO_NUM_20, 1);
|
||
|
||
// 电门锁IO初始化 (输出)
|
||
cfg.direction = CM_GPIO_DIRECTION_OUTPUT;
|
||
cfg.pull = CM_GPIO_PULL_UP;
|
||
cm_iomux_set_pin_func(CM_IOMUX_PIN_87, CM_IOMUX_FUNC_FUNCTION2);
|
||
cm_gpio_init(OUT_Door_lock, &cfg);
|
||
cm_gpio_set_level(OUT_Door_lock, 0);//默认关闭
|
||
|
||
// 管理员模式IO初始化 (输入)
|
||
cfg.direction = CM_GPIO_DIRECTION_INPUT;
|
||
cfg.pull = CM_GPIO_PULL_UP;
|
||
cm_iomux_set_pin_func(CM_IOMUX_PIN_76, CM_IOMUX_FUNC_FUNCTION2);
|
||
cm_gpio_init(IO_MANAGER_MODE, &cfg);
|
||
// cm_gpio_set_level(IO_MANAGER_MODE, 0);
|
||
|
||
// 倒车IO初始化 (输入)
|
||
cfg.direction = CM_GPIO_DIRECTION_INPUT;
|
||
cfg.pull = CM_GPIO_PULL_UP;
|
||
cm_iomux_set_pin_func(CM_IOMUX_PIN_50, CM_IOMUX_FUNC_FUNCTION2);
|
||
cm_gpio_init(IO_RX_BACK, &cfg);
|
||
// cm_gpio_set_level(IO_RX_BACK, 0);
|
||
|
||
// 刹车IO初始化 (输入输出)
|
||
cfg.direction = CM_GPIO_DIRECTION_INPUT;
|
||
cfg.pull = CM_GPIO_PULL_UP;
|
||
cm_iomux_set_pin_func(CM_IOMUX_PIN_51, CM_IOMUX_FUNC_FUNCTION2);
|
||
cm_gpio_init(IO_TX_BRAKE, &cfg);
|
||
// cm_gpio_set_level(IO_TX_BRAKE, 0);
|
||
|
||
// pwm init
|
||
cm_iomux_set_pin_func(PWM0_IOMUX);
|
||
if(0 != cm_pwm_set_clk(CM_PWM_DEV_0, CM_PWM_CLK_12800K)){//时钟源选择需要放在open之前
|
||
DEBUG("pwm0 set clk fail\r\n");
|
||
return;
|
||
}
|
||
set_out_acc_percent(0); // 设置输出油门大小为0%
|
||
|
||
osThreadAttr_t control_out_task_attr = {0};
|
||
control_out_task_attr.name = "control_out_task";
|
||
control_out_task_attr.stack_size = 4096*4;
|
||
control_out_task_attr.priority = osPriorityNormal;
|
||
|
||
control_out_TaskHandle = osThreadNew((osThreadFunc_t)control_out_task,0,&control_out_task_attr);
|
||
}
|
||
|