/*Includes ----------------------------------------------*/ #include "tool.h" #include "hal_MahonyAHRS.h" #include "ble_comm.h" #include "nrf_delay.h" #include "bsp_pwm.h" #include "bsp_time.h" #include "fml_adc.h" #include "bsp_wdt.h" #include "exception.h" #include "selfcheck.h" #include "system.h" #include "drv_lsm6ds3tr_c.h" #include "drv_qmc6310_v2.h" #include "drv_qma7981.h" #include "hal_led.h" #include "hal_battery.h" #include "hal_charge.h" #include "hal_ble_common.h" #include "bll_imu.h" #include "app_flash.h" #include "system.h" #include "hal_mahonyAHRS.h" #include "app_detectIsHost.h" #include "drv_trigger.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_3 -18 //需要扫描的设备的RSSI最小阈值，-76 #define SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2 -60 //需要扫描的设备的RSSI最小阈值，-76 #define SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD 80 //充电芯片引脚的最小ADC阈值 #define SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD 2200 //电池分压电阻的最小ADC阈值 #define SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD 4300 //电池分压电阻的最大ADC阈值，拆掉电阻后，原始值为3721，换算为5.49V #define SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD 0 //中间加速度正向ROLL值最小阈值 #define SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD 35 //中间加速度正向ROLL值最大阈值 #define SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD 160 //中间加速度反向ROLL值最小阈值 #define SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD 180 //中间加速度反向ROLL值最大阈值 #define SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD 200 //地磁传感器没焊接电容的最小阈值 #define SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD 300 //自检灯显示周期时间，单位ms #define SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_2 3000 //中间加速度检测电机最小差值,不震动的差值是129,有个能震动的差值是1800+ #define SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_3 1000 //中间加速度检测电机最小差值,不震动的差值是129,有个能震动的差值是1800+ #define SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX 416 //数据监测错误累计最大值 #define SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT 5 //充电芯片引脚检测次数 /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef enum { SELFCHECK_RESULT_SUCCESS = 0, //自检成功 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS_ID, //自检失败——前脚传感器配置六轴ID SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG_ID, //自检失败——前脚传感器配置地磁ID SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG_ID, //自检失败——后脚传感器配置地磁ID SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE, //自检失败——前脚传感器装反 SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL, //自检失败——前脚传感器地磁数据过小 SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL, //自检失败——后脚传感器地磁数据过小 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS, //自检失败——前脚传感器配置六轴 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG, //自检失败——前脚传感器配置地磁 SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA, //自检失败——前脚传感器六轴没数据 SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA, //自检失败——前脚传感器六轴数据异常(数据持续相等) SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA, //自检失败——前脚传感器地磁没数据 SELFCHECK_RESULT_ERR_FRONT_MAG_EXCP_DATA, //自检失败——前脚传感器地磁数据异常(数据持续相等) SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG, //自检失败——后脚传感器配置 SELFCHECK_RESULT_ERR_BACK_NO_DATA, //自检失败——后脚传感器没数据 SELFCHECK_RESULT_ERR_BACK_EXCP_DATA, //自检失败——后脚传感器数据异常(数据持续相等) SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG, //自检失败——中间传感器配置 SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA, //自检失败——中间传感器没数据或数据异常 SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC, //自检失败——充电芯片引脚ADC SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC, //自检失败——电池分压电阻ADC SELFCHECK_RESULT_ERR_RSSI, //自检失败——RSSI SELFCHECK_RESULT_ERR_MT, //自检失败——震动电机 } SELFCHECK_RESULT_e; typedef struct _selfcheck { uint32_t selfcheck_result; //自检结果 uint32_t selfcheck_result_led_color; //自检结果的led灯颜色 uint32_t selfcheck_result_flash_num; //自检结果闪烁次数 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; //后脚传感器是否读到数据标志位 char order; //指令 uint32_t charge_chip_adc_max; //充电芯片阈值满足数 int16_t battery_adc_max; //电量最大值 } 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, }; static MahonyAHRS_t Self_Mind_Mahony={0}; static MahonyAHRS_t Self_Front_Mahony={0}; /*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/ static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag) { static int16_t last_f_acc[3]; //上一次的前脚加速度值 static int16_t last_f_gry[3]; //上一次的前脚陀螺仪值 static int16_t last_f_mag[3]; //上一次的前脚地磁计值 static int16_t last_b_mag[3]; //上一次的后脚地磁计值 static int16_t last_f_acc_err_sum; //上一次的前脚加速度值错误累计 static int16_t last_f_gry_err_sum; //上一次的前脚陀螺仪值错误累计 static int16_t last_f_mag_err_sum; //上一次的前脚地磁计值错误累计 static int16_t last_b_mag_err_sum; //上一次的后脚地磁计值错误累计 /*前脚加速度*/ if(f_acc != NULL) { if( last_f_acc[0] == f_acc[0] && \ last_f_acc[1] == f_acc[1] && \ last_f_acc[2] == f_acc[2] ) { last_f_acc_err_sum++; if(last_f_acc_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){ ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA); } }else{ last_f_acc_err_sum = 0; } last_f_acc[0] = f_acc[0]; last_f_acc[1] = f_acc[1]; last_f_acc[2] = f_acc[2]; } /*前脚陀螺仪*/ if(f_gry != NULL) { if( last_f_gry[0] == f_gry[0] && \ last_f_gry[1] == f_gry[1] && \ last_f_gry[2] == f_gry[2] ) { last_f_gry_err_sum++; if(last_f_gry_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){ ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA); } }else{ last_f_gry_err_sum = 0; } last_f_gry[0] = f_gry[0]; last_f_gry[1] = f_gry[1]; last_f_gry[2] = f_gry[2]; } /*前脚地磁计*/ if(f_mag != NULL) { if( last_f_mag[0] == f_mag[0] && \ last_f_mag[1] == f_mag[1] && \ last_f_mag[2] == f_mag[2] ) { last_f_mag_err_sum++; if(last_f_mag_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){ ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_EXCP_DATA); } }else{ last_f_mag_err_sum = 0; } last_f_mag[0] = f_mag[0]; last_f_mag[1] = f_mag[1]; last_f_mag[2] = f_mag[2]; } /*后脚地磁计*/ if(b_mag != NULL) { if( last_b_mag[0] == b_mag[0] && \ last_b_mag[1] == b_mag[1] && \ last_b_mag[2] == b_mag[2] ) { last_b_mag_err_sum++; if(last_b_mag_err_sum >= (SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX/2)){ ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_EXCP_DATA); } }else{ last_b_mag_err_sum = 0; } last_b_mag[0] = b_mag[0]; last_b_mag[1] = b_mag[1]; last_b_mag[2] = b_mag[2]; } } 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 scan_report_cb(ble_gap_evt_adv_report_t const * p_adv_report) { ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > p_adv_report->rssi)?ob_selfcheck.max_rssi:p_adv_report->rssi; } static void selfcheck_get_result_led(void); static void selfcheck_result_display_process(void) { static uint32_t led_display_count = 0; //根据自检结果显示结果： //前脚传感器——红色（前脚六轴配置问题闪烁1下，前脚地磁配置问题闪烁2下，前脚六轴数据读取失败或数据异常闪烁3下，前脚地磁数据读取失败或数据异常闪烁4下） //后脚传感器——红色（后脚地磁配置问题闪烁5下，后脚地磁数据读取失败或数据异常闪烁6下） //中间传感器——红色（中间加速度配置问题闪烁7下，加速度roll值不在范围内闪烁8下） //充电芯片和电池分压电阻和蓝牙天线rssi——红色（充电芯片问题闪烁9下，电池分压电阻闪烁10下，蓝牙天线rssi问题闪烁11下） //中间加速度检测震动电机（待定） //上述检测通过，蓝色（1秒周期，500ms亮，500ms灭（断电源线）），若检测到鞋垫，则绿色（1秒周期，500ms亮，500ms灭（断电源线））。 //LED电源引脚亮灯拉高，灭灯拉低。（4秒周期，40ms亮，160ms灭（断电源线）一组） //喂狗 feed_watchdog(); //天线如果没有检测到可以持续检测 if(ob_selfcheck.selfcheck_result_flash_num == SELFCHECK_RESULT_ERR_RSSI && (ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_RSSI)) == 0) { //检测震动电机是否通过 if((ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_MT)) == 0) { //重新根据自检结果设置灯 ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN; ob_selfcheck.selfcheck_result_flash_num = 1; led_display_count = 0; WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); return; } else { //重新根据自检结果设置灯 ob_selfcheck.selfcheck_result_led_color = COLOR_RED; ob_selfcheck.selfcheck_result_flash_num = SELFCHECK_RESULT_ERR_MT; led_display_count = 0; WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); return; } } Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD); led_display_count++; if(led_display_count % 2 == 0) { WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); } else { WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play(); } if(led_display_count >= ob_selfcheck.selfcheck_result_flash_num * 2) { led_display_count = 0; WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); if(ob_selfcheck.selfcheck_result_led_color != COLOR_GREEN)Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD * 3); } } static void selfcheck_led_process(void) { //喂狗 feed_watchdog(); static uint8_t flow = 0; switch(flow) { case 0: WS2812_DisplayDot(COLOR_RED);WS2812_Pwm_Play(); flow = 1; break; case 1: WS2812_DisplayDot(COLOR_GREEN);WS2812_Pwm_Play(); flow = 2; break; case 2: WS2812_DisplayDot(COLOR_BLUE);WS2812_Pwm_Play(); flow = 0; break; } } static void selfcheck_mt_process(void) { static int16_t max_acc_z = -32767; static int16_t min_acc_z = 32767; //喂狗 feed_watchdog(); //检测震动是否达标 //读取中间传感器数据，计算加速度roll值 if(ob_selfcheck.order == 0x02 || ob_selfcheck.order == 0x03) { memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data)); if(drv_qma_get_acc_data(&ob_selfcheck.m_data) == 0) { if(ob_selfcheck.m_data.acc[0]==0 && ob_selfcheck.m_data.acc[1]==0 && ob_selfcheck.m_data.acc[2]==0)return; if(ob_selfcheck.m_data.acc[0]==-1 && ob_selfcheck.m_data.acc[1]==-1 && ob_selfcheck.m_data.acc[2]==-1)return; max_acc_z = (max_acc_z > ob_selfcheck.m_data.acc[2])?max_acc_z:ob_selfcheck.m_data.acc[2]; min_acc_z = (min_acc_z > ob_selfcheck.m_data.acc[2])?ob_selfcheck.m_data.acc[2]:min_acc_z; if(max_acc_z > min_acc_z) { if(ob_selfcheck.order == 0x02) { if((max_acc_z - min_acc_z) < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_2) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT); } else { ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT); } } if(ob_selfcheck.order == 0x03) { if((max_acc_z - min_acc_z) < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_3) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT); } else { ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT); } } } else { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT); } // SEGGER_RTT_printf(0,"max_acc_z-min_acc_z:%d\r\n",max_acc_z-min_acc_z); } } } static void selfcheck_continue_mt_process(void) { static uint8_t flow = 0; static uint32_t shake_ms = 0; static uint32_t shake2_ms = 0; if((ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_MT)) == 0) { shake_ms = 300; shake2_ms = 700; } else { shake_ms = 1000; shake2_ms = 1000; } //喂狗 feed_watchdog(); switch(flow) { case 0: nrf_gpio_pin_write(PIN_MT_EN,1); flow = 1; Process_SetHoldOn(selfcheck_continue_mt_process,1); Process_UpdatePeroid(selfcheck_continue_mt_process,shake_ms); break; case 1: nrf_gpio_pin_write(PIN_MT_EN,0); flow = 0; Process_SetHoldOn(selfcheck_continue_mt_process,0); Process_UpdatePeroid(selfcheck_continue_mt_process,shake2_ms); break; } } #define N 10 //N > 3 int data[N]; int middleFilter(int in_data) { int sum = 0; int temp[N]; int change; int i,j; //记录数据 for(i=0; i<(N-1); i++) { data[i]=data[i+1]; } data[N-1] = in_data; //复制数据 for(i=0; i temp[i+1]) { change = temp[i]; temp[i] = temp[i+1]; temp[i+1] = change; } } //求和 for(i=1; i<(N-1); i++) sum = sum + temp[i]; //返回平均值 return(sum/((N-2))); } static void selfcheck_get_result_led(void) { //根据自检结果设置灯 if(ob_selfcheck.selfcheck_result == 0) { ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN; ob_selfcheck.selfcheck_result_flash_num = 1; } else { ob_selfcheck.selfcheck_result_led_color = COLOR_RED; for(int i=0;i 0 && roll < 100)?1:roll/100; if(!( \ (SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD) || \ (SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD) \ ))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA); //更新计时 roll_t_count = t_count; // SEGGER_RTT_printf(0,"middle roll:%d\r\n",roll); } //每1秒查看前后传感器是否配置且读取成功（至少要1秒才有结果）且值本身是否正常（如地磁没有焊接电容），设置自检结果。 if(t_count - sensor_t_count >= (1000/100)) { //前脚传感器判断 f_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t); if(f_config_result != BLL_IMU_CONFIG_FINISH) { if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS_ID); if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG_ID); if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG); if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS); } else { //配置没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG); ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS); if(ob_selfcheck.f_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA); else { //数据没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA); ob_selfcheck.f_is_read_data = false; 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])); //前脚传感器地磁没焊接电容 if(front_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL); } } //后脚传感器判断 b_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t); if(b_config_result != BLL_IMU_CONFIG_FINISH) { if(b_config_result == BLL_IMU_CONFIG_FAIL_BACK_MAG_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG_ID); else ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG); } else { //配置没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG); if(ob_selfcheck.b_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA); else { //数据没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA); ob_selfcheck.b_is_read_data = false; 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])); //后脚传感器地磁没焊接电容 if(back_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL); } } //任意传感器配置失败，重新配置 if(f_config_result != BLL_IMU_CONFIG_FINISH || b_config_result != BLL_IMU_CONFIG_FINISH || ob_selfcheck.b_is_read_data != true || ob_selfcheck.f_is_read_data != true) { bll_imu_Resume_config_param(&game_bll_imu_param_t); } //更新计时- sensor_t_count = t_count; } //第3秒获取前脚是否装反 if(t_count == (3000/100) && ob_selfcheck.order == 0x03) { roll = (int16_t)(Self_Front_Mahony.roll*100); if(roll < 0)roll *= -1; roll = (roll > 0 && roll < 100)?1:roll/100; if(!(roll >=SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD && roll <= SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD)) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE); } // SEGGER_RTT_printf(0,"front roll:%d\r\n",roll); } //第3秒开启震动 if(t_count == (3000/100)) { nrf_gpio_pin_write(PIN_MT_EN,1); //初始化震动电机自检线程 Process_Start(30,"selfcheck_mt_process",selfcheck_mt_process); Process_SetHoldOn(selfcheck_mt_process,1); } //查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。 if(ob_selfcheck.order == 0x03) { if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_3 >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_RSSI); // SEGGER_RTT_printf(0,"SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD\r\n"); } else { if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2 >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_RSSI); // SEGGER_RTT_printf(0,"SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2\r\n"); } //每4秒读取ADC，以5V电压测试电池分压电阻是否焊接，不考虑阻值，设置自检结果。 if(t_count - battery_adc_t_count >= (4000/100)) { if(!(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD < ob_selfcheck.battery_adc_max && ob_selfcheck.battery_adc_max < SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC); //更新计时 battery_adc_t_count = t_count; // SEGGER_RTT_printf(0,"ob_selfcheck.battery_adc_max:%d\r\n",ob_selfcheck.battery_adc_max); // SEGGER_RTT_printf(0,"ob_selfcheck.charge_chip_adc_max:%d\r\n",ob_selfcheck.charge_chip_adc_max); } //第4秒关闭100毫秒震动电机自检线程 if(t_count == (4000/100)) { Process_Stop(selfcheck_mt_process); Process_Stop(selfcheck_led_process); WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); Process_Start(100,"selfcheck_continue_mt_process",selfcheck_continue_mt_process); Process_SetHoldOn(selfcheck_continue_mt_process,1); } //第4+SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD秒初始化自检显示结果线程，设置holdon，全功率运行。 if(t_count == ((4000 + SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD)/100)) { //根据自检结果设置灯 selfcheck_get_result_led(); Process_Start(SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD,"selfcheck_result_display_process",selfcheck_result_display_process); Process_SetHoldOn(selfcheck_result_display_process,1); } //上报中间传感器的加速度值和roll值 L=0; int16_t rol = (int16_t)(Mahony_M_GetRoll()*100); int16_t pitch = (int16_t)(Mahony_M_GetPitch()*100); int16_t yaw = (int16_t)(Mahony_M_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[2]; int32_t middle_acc_y = ob_selfcheck.m_data.acc[1]; int16_t middle_acc_z = ob_selfcheck.m_data.acc[0]; 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值 if(TIME_GetTicks()-last_tim >= 1000) { last_tim = TIME_GetTicks(); if(ob_selfcheck.max_rssi == -120) { nrf_ble_scan_stop(); host_disconnect(); if(host_isconnect() == 0) { host_set_scan_name((char *)"***********",sizeof("***********")); advdata_report_Evt_Regist(scan_report_cb); Continuous_scan_start(); } } ob_selfcheck.max_rssi = -120; } 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); 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])); 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); //退出开关 uint32_t ch = drv_GetChargeTrig(); if(ch > 0) { if(continue_trigger <= 10)continue_trigger = 10; } else { if(continue_trigger != 0)continue_trigger--; } // SEGGER_RTT_printf(0,"ch:%d\r\n",ch); // SEGGER_RTT_printf(0,"continue_trigger:%d,%d,%d\r\n",continue_trigger,adc_value,ch); // SEGGER_RTT_printf(0,"front mag:%d,%d,%d,%d\r\n",ob_selfcheck.f_data.mag[0],ob_selfcheck.f_data.mag[1],ob_selfcheck.f_data.mag[2],front_mag_norm); // SEGGER_RTT_printf(0,"back mag:%d,%d,%d,%d\r\n",ob_selfcheck.b_data.mag[0],ob_selfcheck.b_data.mag[1],ob_selfcheck.b_data.mag[2],back_mag_norm); //第6秒重启 // if(t_count >= (5000/100) && !slave_isconnect() && continue_trigger==0) if(t_count >= (6000/100) && continue_trigger==0) { memset(mac_buf, 0, sizeof(mac_buf)); sprintf(mac_buf, "%02X%02X%02X%02X%02X%02X", mFlash.macHost[0], mFlash.macHost[1], mFlash.macHost[2], mFlash.mClient.macAddr[3], mFlash.mClient.macAddr[4], mFlash.mClient.macAddr[5]); ST_scan_stop(); host_set_scan_name(mac_buf, strlen(mac_buf)); Flash_DeleteAllInfor(); Flash_DeleteAllStep(); Flash_DeleteAllBackup(); //恢复出厂信息 memset(&mFlash,0xFF,sizeof(mFlash)); memset(&mBackup,0xFF,sizeof(mBackup)); //保存数据到flash if(Flash_SaveStep() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"save step fail"); extern battercb_t battery_record; // extern void printbatter_cb(battercb_t *c,unsigned char *buff); memcpy(&mFlash.mbattercb_t,&battery_record,sizeof(battercb_t)); mBackup.RestartCnt =0; if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save backup fail"); if(Flash_SaveInfomation() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save information fail"); NVIC_SystemReset(); } } static void adc_callback(uint32_t sample_point, Fml_Adc_All_Channel_Adc_Value_t all_adc_value) { int16_t battery_adc_value = 0; int16_t charge_adc_value = 0; //筛选最大的电池分压后的电压 fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&battery_adc_value); battery_adc_value = ADC_RESULT_IN_MILLI_VOLTS(battery_adc_value) * 5 / 3; ob_selfcheck.battery_adc_max = middleFilter(battery_adc_value); //充电芯片检测 fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL,&charge_adc_value); //充电电流超过500ma就异常 if(charge_adc_value >= 1560) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC); } else //充电电流正常的情况 { //电压大于4V，不检测充电芯片，因为充电芯片断闸。 if(battery_adc_value >= 4000) { ob_selfcheck.charge_chip_adc_max = SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT; } if(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD < charge_adc_value)ob_selfcheck.charge_chip_adc_max++; if(ob_selfcheck.charge_chip_adc_max >= SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT)ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC); else ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC); } // SEGGER_RTT_printf(0,"charge_chip_adc_max:%d,%d\r\n",ob_selfcheck.charge_chip_adc_max,charge_adc_value); } static void selfcheck_trigger_Init_Porcess(void) { if(ob_selfcheck.order != 0) { selfcheck_trigger_callback(ob_selfcheck.order); //会关掉所有线程 } } /*API ------------------------------------------------------------------------------------------------------------------------------------*/ /** @brief 自检触发初始化 @param 无 @return 无 */ void selfcheck_trigger_Init(void) { memset(&ob_selfcheck, 0, sizeof(ob_selfcheck.order)); Process_Start(0,"selfcheck_trigger_Init_Porcess",selfcheck_trigger_Init_Porcess); } /** @brief 设置自检触发指令 @param order - [in] 指令类型 @return 无 */ void selfcheck_trigger_set_order(char order) { // SEGGER_RTT_printf(0,"order:0x%x,0x%x\r\n",order,ob_selfcheck.order); if(ob_selfcheck.order == 0)ob_selfcheck.order = order; } /** @brief 自检触发回调 @param order - [in] 回调触发的指令 @return 无 */ void selfcheck_trigger_callback(char order) { int error; uint32_t last_tim = TIME_GetTicks(); //喂狗 feed_watchdog(); //初始化结构体，自检结果为成功 memset(&ob_selfcheck,0,sizeof(ob_selfcheck)); ob_selfcheck.max_rssi = -120; ob_selfcheck.selfcheck_result = SELFCHECK_RESULT_SUCCESS; ob_selfcheck.selfcheck_result_led_color = COLOR_BLACK; ob_selfcheck.f_is_read_data = false; ob_selfcheck.b_is_read_data = false; ob_selfcheck.order = order; // SEGGER_RTT_printf(0,"order:0x%x\r\n",order); //关闭所有线程，初始化0毫秒自检线程，设置holdon，全功率运行。 Process_All_Stop(); Process_Start(100,"selfcheck_process",selfcheck_process); Process_SetHoldOn(selfcheck_process,1); //关闭前后脚传感器电源 drv_qmc6310_power_off(); drv_lsm_power_off(); nrf_delay_ms(200); feed_watchdog(); //关闭扫描，设置任意设备扫描回调，开启扫描 while(TIME_GetTicks() - last_tim <= 1000) { nrf_ble_scan_stop(); host_disconnect(); if(host_isconnect() == 0)break; } host_set_scan_name((char *)"***********",sizeof("***********")); advdata_report_Evt_Regist(scan_report_cb); Continuous_scan_start(); //配置LED电源引脚为输出SOD1 nrf_gpio_cfg( PIN_LED_ENABLE, NRF_GPIO_PIN_DIR_OUTPUT, NRF_GPIO_PIN_INPUT_DISCONNECT, NRF_GPIO_PIN_NOPULL, NRF_GPIO_PIN_S0D1, NRF_GPIO_PIN_NOSENSE); //配置震动电机引脚为输出SOS1 nrf_gpio_cfg_output(PIN_MT_EN); nrf_gpio_pin_write(PIN_MT_EN,0); //配置led灯电源引脚为输出 nrf_gpio_pin_write(PIN_LED_ENABLE,LED_ENABLE); //重新初始化ADC，配置所有通道。 ADC_SetPinChannel(PIN_ADC_CHARGMEASURE, PIN_ADC_CHARGMEASURE_CHANNEL,NRF_GPIO_PIN_NOPULL); ADC_SetPinChannel(PIN_ADC_BAT_IN, PIN_ADC_BAT_CHANNEL,NRF_GPIO_PIN_NOPULL); fml_adc_sample_update_notify_register(adc_callback); //插电IO口初始化 nrf_gpio_cfg_input(PIN_CHARGING, NRF_GPIO_PIN_NOPULL); //配置前脚传感器、中间传感器、后脚传感器 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); error = drv_qma_Init(); if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG); nrf_delay_ms(20); error = drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ); if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG); //前脚地磁自检 if(drv_lsm_selfcheck_mag() != 0) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL); // SEGGER_RTT_printf(0,"drv_lsm_selfcheck_mag() != 0!!!!!!!!!!!!!!\r\n"); } //后脚地磁自检 if(drv_qmc6310_selfcheck_mag() != 0) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL); // SEGGER_RTT_printf(0,"drv_qmc6310_selfcheck_mag() != 0!!!!!!!!!!!!!!\r\n"); } //初始化LED自检线程 Process_Start(300,"selfcheck_led_process",selfcheck_led_process); Process_SetHoldOn(selfcheck_led_process,1); //初始化计算roll值算法 Mahony_Init(&Self_Mind_Mahony,104); Mahony_Init(&Self_Front_Mahony,416); //喂狗 feed_watchdog(); }