226 lines
6.7 KiB
C
226 lines
6.7 KiB
C
#include "b6x.h"
|
||
#include "drvs.h"
|
||
#include "dbg.h"
|
||
#include "sftmr.h"
|
||
#include "sys_config.h"
|
||
#include "stdio.h"
|
||
|
||
uint16_t ACC_Dmin ;
|
||
uint16_t ACC_Dmax ;
|
||
|
||
//管理员模式速度极限
|
||
uint8_t M_mode_sLim =M_DEFAULT_sLim;
|
||
//游客模式速度极限
|
||
uint8_t U_mode_sLim =U_DEFAULT_sLim;
|
||
|
||
// 使能电平
|
||
bool app_control_en[PA_MAX]={
|
||
[IN_GPS] =OE_LOW, // GPS输入0 /边沿触发/L:低使能(反)
|
||
[IN_Back] =OE_LOW, // 倒车 /边沿触发
|
||
[IN_Brake] =OE_LOW, // 制动 /边沿触发
|
||
[IN_Manager_Mode] =OE_LOW, // 管理员模式 /边沿触发
|
||
|
||
[OUT_Buzzer] =OE_HIGH, // 蜂鸣器 /H:高使能
|
||
[OUT_Low_speed] =OE_HIGH, // 使能低速档位 /L:低使能(反)
|
||
[OUT_Back_car] =OE_HIGH, // 使能倒车 /L:低使能倒车(反)
|
||
[OUT_Low_brake] =OE_HIGH, // 使能低制动 /L:低使能(反)
|
||
[OUT_High_brake] =OE_LOW, // 使能高制动 /H:高使能(反)
|
||
[OUT_Door_lock] =OE_HIGH, // 开锁 /H:开锁(同)
|
||
};
|
||
|
||
|
||
// 油门控制输入初始化/PA08/CH3
|
||
// 电池电压检测/充电检测/PA04/CH7
|
||
static void acc_in_bat_in_sadc_init(void)
|
||
{
|
||
// Analog Enable
|
||
GPIO_DIR_CLR(GPIO08 | GPIO04);
|
||
|
||
// ADC
|
||
iom_ctrl(PA04, IOM_ANALOG);//BAT//AIN3//电池电量
|
||
iom_ctrl(PA08, IOM_ANALOG);//ACC//AIN7//油门
|
||
|
||
// sadc init
|
||
#if SADC_VREF==VREF_VDD
|
||
sadc_init(SADC_ANA_VREF_VDD);//设置参考电压(3.3V)
|
||
#elif SADC_VREF==VREF_1V2
|
||
sadc_init(SADC_ANA_VREF_1V2);//设置参考电压(1.2V)
|
||
#elif SADC_VREF==VREF_2V4
|
||
sadc_init(SADC_ANA_VREF_2V4);//设置参考电压(2.4V)
|
||
#endif
|
||
|
||
uint16_t ACC_val=0;
|
||
uint8_t time =0;
|
||
// uint8_t len, dat[20];
|
||
while(1){
|
||
bootDelayMs(300);
|
||
ACC_val =2*((SADC_VREF * sadc_read(SADC_CH_VDD33, 3))/1024);
|
||
|
||
// len =sprintf((char *)dat,"\nVDD:%d\n",ACC_val);
|
||
// uart_send(UART1_PORT,len,dat);
|
||
// uart_send(UART1_PORT,len,dat);
|
||
|
||
if(3000 <=ACC_val && 6 > time){ //&& 3500 >= ACC_val){
|
||
break;
|
||
}
|
||
if(20 < time++){
|
||
ACC_val=3300;
|
||
break;
|
||
}
|
||
}
|
||
ACC_ADC_init(ACC_val); //获取VDD33实际电压，纠正DAC输出电压误差
|
||
}
|
||
|
||
// 获取油门输入信号原始ADC值
|
||
uint16_t get_in_acc_raw(void){
|
||
return (sadc_read(SADC_CH_AIN7, 0));
|
||
}
|
||
|
||
// 获取油门输入电压信号（unit:mV)
|
||
uint16_t get_in_acc_voltage(void){
|
||
return ((((SADC_VREF * sadc_read(SADC_CH_AIN7, 3))/1024)/ACC_DOWN_Res)*(ACC_UP_Res+ACC_DOWN_Res));
|
||
}
|
||
|
||
// 获取输入油门百分比（unit:%)(踏板信号)
|
||
uint16_t get_in_acc_percent(void){
|
||
uint16_t acc_voltage =get_in_acc_voltage();
|
||
if(900 > acc_voltage) {return 0;}
|
||
else if(3800 < acc_voltage){return 100;}
|
||
else{
|
||
return ((acc_voltage-900)/29);
|
||
// return (100*(acc_voltage - 900)/(3800-900));
|
||
}
|
||
}
|
||
|
||
// 获取电池电压值（unit:V)
|
||
float get_bat_voltage(void){
|
||
return (((((SADC_VREF / 1000.0) * sadc_read(SADC_CH_AIN3, 3))/1024.0)/BAT_DOWN_Res)*(BAT_UP_Res+BAT_DOWN_Res));
|
||
}
|
||
|
||
// 油门调节输出初始化/PWM4/PA05
|
||
static void acc_out_pwm_init(void){
|
||
|
||
iom_ctrl(PA05, IOM_SEL_TIMER);
|
||
|
||
// CTMR
|
||
pwm_init(PWM_CTMR, 15, 1023);//fo =16MHz /((15+1)*(1023+1))
|
||
|
||
pwm_chnl_cfg_t chnl_conf;
|
||
chnl_conf.duty = 0;
|
||
chnl_conf.ccer = PWM_CCER_SIPH;
|
||
chnl_conf.ccmr = PWM_CCMR_MODE1;
|
||
pwm_chnl_set(PWM_CTMR_CH4, &chnl_conf);
|
||
|
||
pwm_start(PWM_CTMR);
|
||
}
|
||
|
||
// 设置输出油门大小
|
||
void set_out_acc_percent(uint8_t percent){
|
||
pwm_duty_upd(PWM_CTMR_CH4 , ACC_PERCENT(percent));
|
||
}
|
||
|
||
//中控控制IO初始化
|
||
void app_control_init(void){
|
||
// out
|
||
gpio_dir_output(OUT_Door_lock,OE_LOW);// 默认锁车
|
||
gpio_dir_output(OUT_Low_speed,OE_LOW);// 低速档位，默认失能
|
||
gpio_dir_output(OUT_Back_car,OE_LOW);// 默认前进模式
|
||
gpio_dir_output(OUT_Low_brake,OE_LOW);// 默认关低制动
|
||
gpio_dir_output(OUT_High_brake,OE_HIGH);// 默认关高制动
|
||
gpio_dir_output(OUT_Buzzer,OE_LOW);// 默认关蜂鸣器
|
||
//in
|
||
gpio_dir_input(OUT_Door_lock,IE_DOWN);// 默认锁车
|
||
gpio_dir_input(OUT_Low_speed,IE_DOWN);// 低速档位，默认失能
|
||
gpio_dir_input(OUT_Back_car,IE_DOWN);// 默认前进模式
|
||
gpio_dir_input(OUT_Low_brake,IE_DOWN);// 默认关低制动
|
||
gpio_dir_input(OUT_High_brake,IE_UP);// 默认关高制动
|
||
gpio_dir_input(OUT_Buzzer,IE_DOWN);// 默认关蜂鸣器
|
||
|
||
//默认都失能
|
||
Set_Status(OUT_Door_lock,0);
|
||
Set_Status(OUT_Low_speed,0);
|
||
Set_Status(OUT_Back_car,0); //失能倒车
|
||
Set_Status(OUT_Low_brake,0);
|
||
Set_Status(OUT_High_brake,0);
|
||
Set_Status(OUT_Buzzer,0);
|
||
|
||
// in
|
||
gpio_dir_input(IN_GPS, IE_UP); // Input enable, pull-up
|
||
gpio_dir_input(IN_Back, IE_UP);
|
||
gpio_dir_input(IN_Brake, IE_UP);
|
||
gpio_dir_input(IN_Manager_Mode, IE_UP);
|
||
|
||
// // EXTI config
|
||
// exti_init(EXTI_DBC(15, 4));
|
||
// exti_set(EXTI_FTS, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode)); // falling
|
||
// exti_set(EXTI_DBE, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode));
|
||
// exti_set(EXTI_IER, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode));
|
||
|
||
// IRQ enable
|
||
// NVIC_EnableIRQ(EXTI_IRQn);
|
||
|
||
acc_in_bat_in_sadc_init();
|
||
acc_out_pwm_init();
|
||
|
||
__enable_irq();
|
||
}
|
||
|
||
// GPIO外部中端处理
|
||
void EXTI_IRQHandler(void)
|
||
{
|
||
uint32_t irq_sta = EXTI->RIF.Word;
|
||
|
||
if (irq_sta & EXTI_SRC(IN_GPS))//
|
||
{
|
||
EXTI->ICR.Word = EXTI_SRC(IN_GPS);
|
||
|
||
}
|
||
|
||
if (irq_sta & EXTI_SRC(IN_Back))
|
||
{
|
||
EXTI->ICR.Word = EXTI_SRC(IN_Back);
|
||
|
||
}
|
||
|
||
if (irq_sta & EXTI_SRC(IN_Brake))
|
||
{
|
||
EXTI->ICR.Word = EXTI_SRC(IN_Brake);
|
||
|
||
}
|
||
|
||
if (irq_sta & EXTI_SRC(IN_Manager_Mode))
|
||
{
|
||
EXTI->ICR.Word = EXTI_SRC(IN_Manager_Mode);
|
||
|
||
}
|
||
}
|
||
|
||
// PAD控制进入管理员模式
|
||
bool PAD_Manager_Mode=0;
|
||
// PAD控制进入游客模式
|
||
bool PAD_User_Mode=0;
|
||
// 系统自动刹车
|
||
bool SYS_AUTO_brake=0;
|
||
// 系统自动减速
|
||
bool SYS_AUTO_Speed_Cut=0;
|
||
|
||
// 控制进程，定时调用
|
||
void Control_procedure(void){
|
||
|
||
//电门锁
|
||
Set_Status(OUT_Door_lock,(PAD_User_Mode || PAD_Manager_Mode || Get_Status(IN_GPS) || Get_Status(IN_Manager_Mode)));
|
||
|
||
// 倒车
|
||
Set_Status(OUT_Back_car,Get_Status(IN_Back));
|
||
// 刹车
|
||
Set_Status(OUT_Low_brake,(Get_Status(IN_Brake) || SYS_AUTO_brake));
|
||
Set_Status(OUT_High_brake,(Get_Status(IN_Brake) || SYS_AUTO_brake));
|
||
|
||
//油门控制
|
||
if(Get_Status(OUT_Door_lock)){
|
||
set_out_acc_percent((SYS_AUTO_Speed_Cut?0.5:1)*get_in_acc_percent()*((PAD_Manager_Mode || Get_Status(IN_Manager_Mode)) ? sys_conf_info.M_mode_sLim : sys_conf_info.U_mode_sLim) / 100.0);
|
||
}else{
|
||
set_out_acc_percent(0);
|
||
}
|
||
}
|