/*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_qma7981.h" #include "hal_led.h" #include "hal_battery.h" #include "bll_imu.h" #include "app_flash.h" #include "system.h" #include "hal_mahonyAHRS.h" #include "app_detectIsHost.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD -60 //需要扫描的设备的RSSI最小阈值，-76 #define SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD 80 //充电芯片引脚的最小ADC阈值 #define SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD 3300 //电池分压电阻的最小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_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD 5000 //穿鞋垫的地磁norm值最小阈值 #define SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD 200 //地磁传感器没焊接电容的最小阈值 #define SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD 200 //自检灯显示周期时间，单位ms #define SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD 5000 //中间加速度检测电机最小阈值 #define SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX 416 //数据监测错误累计最大值 /*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_CHARGE_CHIP_PIN_ADC, //自检失败——充电芯片引脚ADC SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC, //自检失败——电池分压电阻ADC SELFCHECK_RESULT_ERR_RSSI, //自检失败——RSSI SELFCHECK_RESULT_ERR_MT, //自检失败——震动电机 SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE, //自检失败——前脚传感器装反 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_FRONT_MAG_DATA_TO_SMALL, //自检失败——前脚传感器地磁数据过小 SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG, //自检失败——后脚传感器配置 SELFCHECK_RESULT_ERR_BACK_NO_DATA, //自检失败——后脚传感器没数据 SELFCHECK_RESULT_ERR_BACK_EXCP_DATA, //自检失败——后脚传感器数据异常(数据持续相等) SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL, //自检失败——后脚传感器地磁数据过小 SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG, //自检失败——中间传感器配置 SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA, //自检失败——中间传感器没数据或数据异常 } 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; //指令 } 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_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(); 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(); Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD * 3); } } static void selfcheck_mt_process(void) { static int16_t max_acc_z = 0; int16_t acc_z = 0; static uint8_t flow = 0; //喂狗 feed_watchdog(); nrf_gpio_pin_write(PIN_MT_EN,1); 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; } //检测震动是否达标 //读取中间传感器数据，计算加速度roll值 if(ob_selfcheck.order == 0x52) { 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[2] < 0)acc_z = ob_selfcheck.m_data.acc[2]*-1; else acc_z = ob_selfcheck.m_data.acc[2]; max_acc_z = (max_acc_z > acc_z)?max_acc_z:acc_z; if(max_acc_z < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT); // SEGGER_RTT_printf(0,"middle acc %d,%d,%d acc_z:%d,max:%d\r\n",ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],acc_z,max_acc_z); } } } #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_process(void) { BLL_IMU_CONFIG_RESULT f_config_result,b_config_result; int16_t adc_value = 0; uint16_t front_mag_norm; uint16_t back_mag_norm; int16_t roll; uint8_t buf[256]; uint8_t L=0; char mac_buf[16]; static uint32_t charge_chip_adc_max = 0; static int16_t battery_adc_max = 0; static uint32_t t_count = 0; static uint32_t battery_adc_t_count = 0; static uint32_t roll_t_count = 0; static uint32_t sensor_t_count = 0; static uint32_t last_tim = 0; static uint32_t continue_trigger = 0; t_count++; //喂狗 feed_watchdog(); //读取中间传感器数据，计算加速度roll值 memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data)); if(drv_qma_get_acc_data(&ob_selfcheck.m_data) != 0) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA); //重新配置 drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ); } Mahony_update(&Self_Mind_Mahony,0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0); //筛选最大的电池分压后的电压 fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&adc_value); adc_value = ADC_RESULT_IN_MILLI_VOLTS(adc_value) * 5 / 3; battery_adc_max = middleFilter(adc_value); //每3秒读取ADC，以5V电压测试电池分压电阻是否焊接，不考虑阻值，设置自检结果。 if(t_count - battery_adc_t_count > (3000/100)) { if(!(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD < battery_adc_max && 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; } //充电芯片检测 fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL,&adc_value); if(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD < adc_value) { charge_chip_adc_max++; } if(charge_chip_adc_max > 10) { 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); } //每3秒检测加速度roll值，设置自检结果。 if(t_count - roll_t_count > (3000/100)) { roll = (int16_t)(Self_Mind_Mahony.roll*100); if(roll < 0)roll *= -1; roll = (roll > 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; } //每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 == 0x52) { 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秒查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。 if(t_count == (3000/100)) { if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); } //第3秒关闭100毫秒震动电机自检线程 if(t_count == (3000/100)) { WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play(); nrf_gpio_pin_write(PIN_MT_EN,0); Process_Stop(selfcheck_mt_process); } //第3+SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD秒初始化自检显示结果线程，设置holdon，全功率运行。 if(t_count == ((3000 + SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD)/100)) { //根据自检结果设置灯 if(ob_selfcheck.selfcheck_result == 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])); if(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD <= front_mag_norm) { ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN; ob_selfcheck.selfcheck_result_flash_num = 1; } else { ob_selfcheck.selfcheck_result_led_color = COLOR_BLUE; ob_selfcheck.selfcheck_result_flash_num = 1; } } else { ob_selfcheck.selfcheck_result_led_color = COLOR_RED; for(int i=0;i>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值 if(TIME_GetTicks()-last_tim >= 1000) { last_tim = TIME_GetTicks(); 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); fml_adc_get_value(PIN_CHARGING_CHANNEL,&adc_value); if(adc_value >= 2000) { if(continue_trigger <= 50)continue_trigger++; } else { if(continue_trigger != 0)continue_trigger--; } // SEGGER_RTT_printf(0,"continue_trigger:%d,%d\r\n",continue_trigger,adc_value); // 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); //第30秒重启 if(t_count >= (30000/100) && !slave_isconnect() && 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)); //恢复出厂信息 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) { //纯粹做覆盖 } /*API ------------------------------------------------------------------------------------------------------------------------------------*/ /** @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; //关闭所有线程，初始化0毫秒自检线程，设置holdon，全功率运行。 Process_All_Stop(); Process_Start(100,"selfcheck_process",selfcheck_process); Process_SetHoldOn(selfcheck_process,1); //关闭扫描，设置任意设备扫描回调，开启扫描 while(TIME_GetTicks() - last_tim <= 1000) { host_disconnect(); if(host_isconnect() == 0)break; } nrf_ble_scan_stop(); host_set_scan_name((char *)"***********",sizeof("***********")); advdata_report_Evt_Regist(scan_report_cb); error = ST_scan_start(); if(error != APP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); //配置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); ADC_SetPinChannel(PIN_CHARGING, PIN_CHARGING_CHANNEL,NRF_GPIO_PIN_NOPULL); fml_adc_sample_update_notify_register(adc_callback); //配置前脚传感器、中间传感器、后脚传感器 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); //初始化震动电机自检线程 Process_Start(SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD,"selfcheck_mt_process",selfcheck_mt_process); Process_SetHoldOn(selfcheck_mt_process,1); //初始化计算roll值算法 Mahony_Init(&Self_Mind_Mahony,104); Mahony_Init(&Self_Front_Mahony,416); //喂狗 feed_watchdog(); }