4G_module/custom/control_out/src/control_out.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 uint16_t coulomp_voltage_table[11] = {
    BAT_LEVEL_0PERCENT_VOLT,
    BAT_LEVEL_10PERCENT_VOLT,
    BAT_LEVEL_20PERCENT_VOLT,
    BAT_LEVEL_30PERCENT_VOLT,
    BAT_LEVEL_40PERCENT_VOLT,
    BAT_LEVEL_50PERCENT_VOLT,
    BAT_LEVEL_60PERCENT_VOLT,
    BAT_LEVEL_70PERCENT_VOLT,
    BAT_LEVEL_80PERCENT_VOLT,
    BAT_LEVEL_90PERCENT_VOLT,
    BAT_LEVEL_100PERCENT_VOLT,
};

uint8_t pmu_voltage_to_percent(uint32_t volt){//电压转百分比
    uint8_t i, percent;
    float step, base;
    if((volt < coulomp_voltage_table[0])){
        return 0;
    }
    if((volt >= coulomp_voltage_table[10])){
        return 100;
    }

    for (i = 0; i < 10; i++) {
        if((volt < coulomp_voltage_table[i+1]) && (volt >= coulomp_voltage_table[i])){
            step = (coulomp_voltage_table[i+1] - coulomp_voltage_table[i]) / 10.0;
            base = coulomp_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);
}