/*Includes ----------------------------------------------*/ #include "tool.h" #include "MahonyAHRS.h" #include "ble_comm.h" #include "nrf_delay.h" #include "bsp_pwm.h" #include "bsp_time.h" #include "bsp_adc.h" #include "bsp_wdt.h" #include "exception.h" #include "selfcheck.h" #include "system.h" #include "drv_qma7981.h" #include "hal_led.h" #include "hal_battery.h" #include "bll_imu.h" #include "app_flash.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define SELFCHECK_FLOW_TOOLING_BOARD_PROCESS_CYCLE LowPower_Interval //工装自检线程周期，单位：ms #define SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_1_CYCLE 2000 //完整板自检线程1周期，单位：ms #define SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_2_CYCLE LowPower_Interval //完整板自检线程2周期，单位：ms #define SELFCHECK_SCAN_DEVICE_NAME "123" //需要扫描的设备名 #define SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD -100 //需要扫描的设备的RSSI最小阈值 #define SELFCHECK_LED_ENABLE 0 //LED灯开 #define SELFCHECK_LED_DISABLE 1 //LED灯关 #define SELFCHECK_THRESHOLD_IS_MET(MIN,VALUE,MAX) ((MIN <= VALUE && VALUE <= MAX)?true:false) //阈值匹配 #define SELFCHECK_PAIR_PIN_ADC_MIN_THRESHOLD 100 //配对引脚的最小ADC阈值 #define SELFCHECK_PAIR_PIN_ADC_MAX_THRESHOLD 200 //配对引脚的最大ADC阈值 #define SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD 100 //充电芯片引脚的最小ADC阈值 #define SELFCHECK_CHARGE_CHIP_PIN_ADC_MAX_THRESHOLD 200 //充电芯片引脚的最大ADC阈值 #define SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD 100 //电池分压电阻的最小ADC阈值 #define SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD 200 //电池分压电阻的最大ADC阈值 #define SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD 5 //中间加速度ROLL值最小阈值 #define SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD 10 //中间加速度ROLL值最大阈值 #define SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD 100 //穿鞋垫的地磁norm值最小阈值 #define SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD 200 //穿鞋垫的地磁norm值最大阈值 /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef enum { SELFCHECK_RESULT_SUCCESS, //自检成功 SELFCHECK_RESULT_ERR_ADV_INIT, //自检失败——广播初始化 SELFCHECK_RESULT_ERR_RSSI, //自检失败——RSSI SELFCHECK_RESULT_ERR_READ_ADC, //自检失败——ADC读取失败 SELFCHECK_RESULT_ERR_PAIR_PIN_ADC, //自检失败——配对引脚ADC SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC, //自检失败——充电芯片引脚ADC SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC, //自检失败——电池分压电阻ADC SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG, //自检失败——中间传感器配置 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG, //自检失败——前脚传感器配置 SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG, //自检失败——后脚传感器配置 SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA, //自检失败——中间传感器没数据 SELFCHECK_RESULT_ERR_FRONT_NO_DATA, //自检失败——前脚传感器没数据 SELFCHECK_RESULT_ERR_BACK_NO_DATA, //自检失败——后脚传感器没数据 SELFCHECK_RESULT_ERR_MIDDLE_ACC_ROLL, //自检失败——中间传感器加速度roll值 SELFCHECK_RESULT_ERR_FLASH_WRITE, //自检失败——写flash } SELFCHECK_RESULT_e; typedef struct _selfcheck { uint32_t selfcheck_result; //自检结果 uint32_t selfcheck_result_led_color; //自检结果的led灯颜色 int16_t max_rssi; //最大的RSSI值 fml_imu_data_t f_data; //前脚传感器数据 fml_imu_data_t b_data; //后脚传感器数据 qma_data_t m_data; //中间传感器数据 bool f_is_read_data; //前脚传感器是否读到数据标志位 bool b_is_read_data; //后脚传感器是否读到数据标志位 } SelfCheck_t; /*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/ static SelfCheck_t ob_selfcheck; static const bll_imu_one_way_param_t game_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_ON, //前脚 - 时间戳开启 .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_416HZ, //前脚 - 加速度采样频率 - 104HZ .gry_odr = FML_IMU_GRY_ODR_416HZ, //前脚 - 陀螺仪采样频率 - 104HZ .mag_odr = FML_IMU_MAG_ODR_200HZ, //前脚 - 地磁计采样频率 - 200HZ .fifo_odr = FML_IMU_FIFO_ODR_416HZ, }; static const bll_imu_one_way_param_t game_back_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_200HZ, //后脚 - 地磁计采样频率 - 200HZ .fifo_odr = FML_IMU_FIFO_ODR_OFF, }; static const bll_imu_param_t game_bll_imu_param_t={ .config_param[FML_IMU_DIR_FRONT] = &game_front_param, .config_param[FML_IMU_DIR_BACK] = &game_back_param, }; /*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/ static uint8_t sleep_duration_control_low_power_interval(void) { return 1; } static void UART0_SendChar(unsigned char ch)//发送一位数锯 { NRF_UART0->TXD = (unsigned int)ch; while(NRF_UART0->EVENTS_TXDRDY == 0x0UL); NRF_UART0->EVENTS_TXDRDY = 0x0UL; } static uint32_t get_baud(uint32_t baud) { switch(baud){ case 1200: return UART_BAUDRATE_BAUDRATE_Baud1200; case 2400: return UART_BAUDRATE_BAUDRATE_Baud2400; case 4800: return UART_BAUDRATE_BAUDRATE_Baud4800; case 9600: return UART_BAUDRATE_BAUDRATE_Baud9600; case 14400: return UART_BAUDRATE_BAUDRATE_Baud14400; case 19200: return UART_BAUDRATE_BAUDRATE_Baud19200; case 28800: return UART_BAUDRATE_BAUDRATE_Baud28800; case 31250: return UART_BAUDRATE_BAUDRATE_Baud31250; case 38400: return UART_BAUDRATE_BAUDRATE_Baud38400; case 56000: return UART_BAUDRATE_BAUDRATE_Baud56000; case 57600: return UART_BAUDRATE_BAUDRATE_Baud57600; case 76800: return UART_BAUDRATE_BAUDRATE_Baud76800; case 115200: return UART_BAUDRATE_BAUDRATE_Baud115200; case 230400: return UART_BAUDRATE_BAUDRATE_Baud230400; case 250000: return UART_BAUDRATE_BAUDRATE_Baud250000; case 460800: return UART_BAUDRATE_BAUDRATE_Baud460800; case 921600: return UART_BAUDRATE_BAUDRATE_Baud921600; case 1000000: return UART_BAUDRATE_BAUDRATE_Baud1M; default: return UART_BAUDRATE_BAUDRATE_Baud115200; } } static void UART0_Init(uint32_t txd, uint32_t rxd, uint32_t baud) { NRF_UART0->INTENCLR = UART_INTENCLR_CTS_Msk | UART_INTENCLR_RXTO_Msk | UART_INTENCLR_NCTS_Msk | UART_INTENCLR_ERROR_Msk; NRF_UART0->INTENSET = UART_INTENSET_RXDRDY_Enabled << UART_INTENSET_RXDRDY_Pos; NRF_UART0->PSELTXD = txd; NRF_UART0->PSELRXD = rxd; NRF_UART0->BAUDRATE = get_baud(baud); NRF_UART0->ENABLE |= UART_ENABLE_ENABLE_Enabled; NRF_UART0->TASKS_STARTTX = 0XFFFFFFFF; NRF_UART0->TASKS_STARTRX = 0XFFFFFFFF; NVIC_SetPriority(UARTE0_UART0_IRQn, UART0_IRQ_PRIORITY); NVIC_EnableIRQ(UARTE0_UART0_IRQn); NRF_UART0->EVENTS_TXDRDY = 0x0UL; } static void scan_report_cb(uint8_t *adv_data, uint16_t adv_data_len,int8_t rssi) { if(mFlash.selfcheck_flow == SELFCHECK_FLOW_TOOLING_BOARD) { uint8_t *device_name = ble_advdata_parse(adv_data,adv_data_len,BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME); if(!device_name) { device_name = ble_advdata_parse(adv_data,adv_data_len,BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME); } if(device_name) { char device_name_buff[128] = {0}; uint16_t data_len = (adv_data + adv_data_len - device_name) / sizeof(uint8_t); memcpy(device_name_buff, device_name, data_len); //广播名匹配 if(memcmp(device_name_buff,SELFCHECK_SCAN_DEVICE_NAME,strlen(SELFCHECK_SCAN_DEVICE_NAME)) == 0) { //筛选最大的RSSI ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi : rssi; } } } else if(mFlash.selfcheck_flow == SELFCHECK_FLOW_COMPLETE_BOARD) { static uint32_t last_tim = 0; if(TIME_GetTicks()-last_tim >= 1000) { last_tim = TIME_GetTicks(); ob_selfcheck.max_rssi = -120; } ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi:rssi; } } static void fb_data_notify_cb(uint32_t dir_bit) { int data_len; if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01) { memset(&ob_selfcheck.f_data,0,sizeof(ob_selfcheck.f_data)); data_len = bll_imu_get_data_num(BLL_IMU_DIR_FRONT); for(int i=0; i> BLL_IMU_DIR_BACK) & 0x01) { memset(&ob_selfcheck.b_data,0,sizeof(ob_selfcheck.b_data)); data_len = bll_imu_get_data_num(BLL_IMU_DIR_BACK); for(int i=0; i> BLL_IMU_DIR_FRONT) & 0x01)) { ob_selfcheck.f_is_read_data = true; } if((dir_bit >> BLL_IMU_DIR_BACK)) { ob_selfcheck.b_is_read_data = true; } } static void selfcheck_flow_tooling_board_process(void) { int16_t rol; static uint32_t t_count = 0; //喂狗 feed_watchdog(); //-----读取中间传感器数据，检测加速度roll值 if(drv_qma_get_acc_data(&ob_selfcheck.m_data) != 0)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA); Mahony_process(0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0); t_count++; switch(t_count) { case (2000/LowPower_Interval): //第2秒查看前后传感器是否配置且读取成功 if(bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t) != BLL_IMU_CONFIG_FINISH)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG); if(bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t) != BLL_IMU_CONFIG_FINISH)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG); if(ob_selfcheck.f_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_NO_DATA); if(ob_selfcheck.b_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA); break; case (4000/LowPower_Interval): //第4秒检测加速度roll值 rol = (int16_t)(getRoll()*1); if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD,rol,SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD)))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_ACC_ROLL); break; case (10000/LowPower_Interval): //第10秒能否读取到任意广播名字，且最小的RSSI是否满足条件 if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); //若自检结果为通过，则将自检流程标志位改为“整体自检”，flash保存（先保存，再发送。不然发送完，还没保存就断电，导致没进入下个阶段就被拿去装鞋） if(ob_selfcheck.selfcheck_result == SELFCHECK_RESULT_SUCCESS) { mFlash.selfcheck_flow = SELFCHECK_FLOW_COMPLETE_BOARD; mBackup.selfcheck_flow = SELFCHECK_FLOW_COMPLETE_BOARD; if(Flash_SaveInfomation()!= ZONE_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FLASH_WRITE); if(Flash_SaveBackup() != ZONE_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FLASH_WRITE); } break; } //-----持续将自检结果发送给单片机3分钟，之后重启。（快速进入下一阶段，而不是等断电进入下一阶段，以防flash失败导致自检流程标志位异常） if(t_count > (10000/LowPower_Interval) && t_count < (15000/LowPower_Interval)) { UART0_SendChar(ob_selfcheck.selfcheck_result); } else if(t_count > (15000/LowPower_Interval)) { NVIC_SystemReset(); } } static bool is_wear_insole(void) { uint16_t front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2])); uint16_t back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2])); return (SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD,front_mag_norm,SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD) && \ SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD,back_mag_norm,SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD))?true:false; } static bool is_charge_chip_can_used(void) { #define SELFCHECK_BATTERY_VOL_THRESHOLD_MIN 2500 //2.5V #define SELFCHECK_BATTERY_VOL_THRESHOLD_MAX 4000 //4V #define SELFCHECK_CHARGE_VOL_THRESHOLD 80 //充电前和充电期间的充电电压变化值,单位mv //监测电池和充电数据 int16_t bat_vol; int16_t charge_vol; int16_t charge_threshold; uint32_t ch = nrf_gpio_pin_read(PIN_CHARGING); static uint32_t charge_cycle = 20; static int16_t before_charge_vol; //充电前的电压值 static int16_t charge_vol_max; //充电期间最大的电压值 bool ret = true; if(!ch)//没充电 { charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL)); before_charge_vol = before_charge_vol > charge_vol ? charge_vol : before_charge_vol; charge_vol_max = 0; charge_cycle = 20; } else //充电 { /* 过筛20轮 */ if(charge_cycle != 0){ charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL)); charge_vol_max = charge_vol_max < charge_vol ? charge_vol : charge_vol_max; charge_cycle--; return ret; } charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL)); charge_vol_max = charge_vol_max < charge_vol ? charge_vol : charge_vol_max; bat_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_BAT_CHANNEL))*5/3; //当电池电量没满，充电（经测试，电量没满的充电电压跟电池电压有关，最小充电电压100+mv） charge_threshold = charge_vol_max - before_charge_vol; //充电前和充电期间的充电电压变化值 if(charge_threshold < SELFCHECK_CHARGE_VOL_THRESHOLD && bat_vol < SELFCHECK_BATTERY_VOL_THRESHOLD_MAX) { ret = false; } } return ret; } static void selfcheck_flow_complete_board_process_2(void) { uint8_t buf[256]; uint8_t L=0; static uint32_t t_count = 0; uint32_t t_threshold = (3000/LowPower_Interval); static uint8_t led_control = 0; static uint8_t wdt_count = 2; //喂狗 feed_watchdog(); if(t_count != t_threshold)t_count++; //-----计算中间传感器的roll值 drv_qma_get_acc_data(&ob_selfcheck.m_data); Mahony_process(0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0); //-----当电量未满4.0V时，充电芯片的充电电压是否超过阈值，若未超过，证明充电芯片异常，设置对应的失败灯 if(is_charge_chip_can_used() == false)ob_selfcheck.selfcheck_result_led_color = COLOR_BLUE; if(t_count == t_threshold) { //-----上报中间传感器的加速度值和roll值 L=0; int16_t rol = (int16_t)(getRoll()*100); //-----roll值若有异常，则设置对应的失败灯。 if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD,(rol/100),SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD)))ob_selfcheck.selfcheck_result_led_color = COLOR_LIGHPURPLE; int16_t pitch = (int16_t)(getPitch()*100); int16_t yaw = (int16_t)(getYaw()*100); buf[L++] = (uint8_t)(rol>>8); buf[L++] = (uint8_t)(rol>>0); buf[L++] = (uint8_t)(pitch>>8); buf[L++] = (uint8_t)(pitch>>0); buf[L++] = (uint8_t)(yaw>>8); buf[L++] = (uint8_t)(yaw>>0); buf[L++] = 0; buf[L++] = 0; buf[L++] = 0; buf[L++] = 0; buf[L++] = 0; buf[L++] = 0; buf[L++] = 0; Mahony_send_ANO(0x01,buf,L); L=0; int32_t middle_acc_x = ob_selfcheck.m_data.acc[0]; int32_t middle_acc_y = ob_selfcheck.m_data.acc[1]; int16_t middle_acc_z = ob_selfcheck.m_data.acc[2]; middle_acc_x *= 100; middle_acc_y *= 100; middle_acc_z *= 10; buf[L++] = (uint8_t)(middle_acc_x>>24); buf[L++] = (uint8_t)(middle_acc_x>>16); buf[L++] = (uint8_t)(middle_acc_x>>8); buf[L++] = (uint8_t)(middle_acc_x>>0); buf[L++] = (uint8_t)(middle_acc_y>>24); buf[L++] = (uint8_t)(middle_acc_y>>16); buf[L++] = (uint8_t)(middle_acc_y>>8); buf[L++] = (uint8_t)(middle_acc_y>>0); buf[L++] = (uint8_t)(middle_acc_z>>8); buf[L++] = (uint8_t)(middle_acc_z>>0); Mahony_send_ANO(0x07,buf,L); //-----上报前脚传感器的加速度数据、陀螺仪数据、地磁计开平方和数据 //-----上报后脚传感器的地磁计开平方和数据 //-----上报扫描到的任意广播的rssi值 L=0; buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>0); buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>0); buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>0); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>0); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>0); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>8); buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>0); uint16_t front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2])); uint16_t back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2])); buf[L++] = (uint8_t)(front_mag_norm>>8); buf[L++] = (uint8_t)(front_mag_norm>>0); buf[L++] = (uint8_t)(back_mag_norm>>8); buf[L++] = (uint8_t)(back_mag_norm>>0); buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>> 8); buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>>0); Mahony_send_ANO(0x02,buf,L); //-----第1个100毫秒亮灯，连续9个100毫秒灭灯 led_control++; if(led_control == 1) { nrf_gpio_pin_write(PIN_LED_ENABLE,LED_ENABLE); Pwm_Initialize(); nrf_delay_ms(5); WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color); WS2812_Pwm_Play(); } else { nrf_gpio_pin_write(PIN_LED_ENABLE,LED_DISABLE); Pwm_UnInitialize(); } if(led_control == 10)led_control = 0; //-----检测到充电，震动200ms一次。 if(nrf_gpio_pin_read(PIN_CHARGING)) { if(wdt_count){nrf_gpio_cfg_output(PIN_MT_EN);nrf_gpio_pin_write(PIN_MT_EN,1);wdt_count--;} else nrf_gpio_pin_write(PIN_MT_EN,0); } else { wdt_count = 2; } } } static void selfcheck_flow_complete_board_process_1(void) { int ret = -1; BLL_IMU_CONFIG_RESULT config_result; //喂狗 feed_watchdog(); config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t); //-----判断前后脚IMU配置是否成功，若成功，进行下一步判断；若失败，设置对应的失败灯，且重新配置前后脚。 if(config_result != BLL_IMU_CONFIG_FINISH) { if(config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG)ob_selfcheck.selfcheck_result_led_color = COLOR_LIGHRED;//前脚mag配置失败 if(config_result == BLL_IMU_CONFIG_FAIL_BACK_MAG)ob_selfcheck.selfcheck_result_led_color = COLOR_WHITE;//后脚mag配置失败 if(config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS)ob_selfcheck.selfcheck_result_led_color = COLOR_RED;//前脚lsm配置失败 //重新配置 bll_imu_close(); bll_imu_Init(); bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb); bll_imu_Resume_config_param(&game_bll_imu_param_t); return; } //-----判断是否正常读取前脚IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置前后脚。 if(ob_selfcheck.f_is_read_data == false) { ob_selfcheck.selfcheck_result_led_color = COLOR_CYAN; //重新配置 bll_imu_close(); bll_imu_Init(); bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb); bll_imu_Resume_config_param(&game_bll_imu_param_t); return; } else { ob_selfcheck.f_is_read_data = false; } //-----判断是否正常读取后脚IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置前后脚。 if(ob_selfcheck.b_is_read_data == false) { ob_selfcheck.selfcheck_result_led_color = COLOR_YELLOW; //重新配置 bll_imu_close(); bll_imu_Init(); bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb); bll_imu_Resume_config_param(&game_bll_imu_param_t); return; } else { ob_selfcheck.b_is_read_data = false; } //-----判断是否正常读取中间IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置中间IMU。 memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data)); ret = drv_qma_get_acc_data(&ob_selfcheck.m_data); if(ret != 0) { ob_selfcheck.selfcheck_result_led_color = COLOR_PURPLE; //重新配置 drv_qma_Init(); nrf_delay_ms(20); drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ); return; } //-----上述自检项目都通过，则判断当前是否充电，充电则设置橙灯，否则设置绿灯。 if(nrf_gpio_pin_read(PIN_CHARGING)) { ob_selfcheck.selfcheck_result_led_color = COLOR_PURPLE; } else { ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN; } //-----根据前后脚地磁数据读取成功情况下，若判断为穿鞋垫，则将自检流程标志位改为“自检完成”，flash保存，重启。（避免到用户手上时，亮灯） if(is_wear_insole()) { mFlash.selfcheck_flow = SELFCHECK_FLOW_DONE; mBackup.selfcheck_flow = SELFCHECK_FLOW_DONE; if(Flash_SaveInfomation()!= ZONE_OP_SUCCESS) { while(1) { //喂狗 feed_watchdog(); //打印错误信息 DEBUG_LOG("selfcheck:Flash_SaveInfomation()!= ZONE_OP_SUCCESS\r\n"); //除了配对信息等基础信息外，擦除整个区块，然后重新保存，保存成功，break，否则延时100毫秒 Flash_DeleteAllInfor(); if(mBackup.head == FLASH_HEAD) { uint8_t i =0; for(i=0;i