/*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_SHAKE_TRIGGER_THRESHOLD 18000 //地磁抖动触发检测 #define SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD 200 //地磁传感器没焊接电容的最小阈值 #define SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD 200 //地磁抖动阈值 /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef enum { SELFCHECK_RESULT_SUCCESS = 0, //自检成功 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS, //自检失败——前脚传感器配置六轴 SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG, //自检失败——前脚传感器配置地磁 SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_DATA, //自检失败——前脚传感器六轴没数据或数据异常 SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA, //自检失败——前脚传感器地磁没数据或数据异常 SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG, //自检失败——后脚传感器配置 SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_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_e; typedef struct _selfcheck { uint32_t selfcheck_result; //自检结果 uint32_t selfcheck_result_led_color; //自检结果的led灯颜色 uint32_t selfcheck_result_flash_num; //自检结果闪烁次数 bool selfcheck_is_led_display; //自检结果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 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(uint8_t *adv_data, uint16_t adv_data_len,int8_t rssi) { ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi:rssi; } static void selfcheck_led_display_process(void) { static uint32_t cur_flash_num = 0; static uint8_t level = LED_ENABLE; static uint8_t red_off_led_count = 0; static uint32_t tim = 0; //喂狗 feed_watchdog(); //led display if(ob_selfcheck.selfcheck_is_led_display == true) { if(cur_flash_num == 0)tim = TIME_GetTicks(); //判断是否结束 if(level == LED_ENABLE)cur_flash_num++; if(cur_flash_num > ob_selfcheck.selfcheck_result_flash_num) { nrf_gpio_pin_write(PIN_LED_ENABLE,LED_DISABLE); if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED && (TIME_GetTicks() - tim < 5000))return; cur_flash_num = 0; level = LED_ENABLE; red_off_led_count = 0; ob_selfcheck.selfcheck_is_led_display = false; Process_Stop(selfcheck_led_display_process); return; } if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED) { Process_Start(50,"selfcheck_led_display_process",selfcheck_led_display_process); nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5); if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();} } else { Process_Start(500,"selfcheck_led_display_process",selfcheck_led_display_process); nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5); if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();} } //周期翻转 if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED) { if(level == LED_ENABLE){level = LED_DISABLE;red_off_led_count = 0;} if(level == LED_DISABLE){red_off_led_count++;if(red_off_led_count >= 6)level = LED_ENABLE;} } else { level = (level == LED_ENABLE)?LED_DISABLE:LED_ENABLE; } } } static void selfcheck_result_display_process(void) { uint16_t front_mag_norm; //喂狗 feed_watchdog(); if(ob_selfcheck.selfcheck_is_led_display == false) { //根据自检结果显示结果： //前脚传感器——红色（前脚六轴配置问题闪烁1下，前脚地磁配置问题闪烁2下，前脚六轴数据读取失败或数据异常闪烁3下，前脚地磁数据读取失败或数据异常闪烁4下） //后脚传感器——红色（后脚地磁配置问题闪烁5下，后脚地磁数据读取失败或数据异常闪烁6下） //中间传感器——红色（中间加速度配置问题闪烁7下，加速度roll值不在范围内闪烁8下） //充电芯片和电池分压电阻和蓝牙天线rssi——红色（充电芯片问题闪烁9下，电池分压电阻闪烁10下，蓝牙天线rssi问题闪烁11下） //中间加速度检测震动电机（待定） //上述检测通过，蓝色（1秒周期，500ms亮，500ms灭（断电源线）），若检测到鞋垫，则绿色（1秒周期，500ms亮，500ms灭（断电源线））。 //LED电源引脚亮灯拉高，灭灯拉低。（4秒周期，40ms亮，160ms灭（断电源线）一组） //获取颜色+闪烁次数+周期 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 (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)(Mahony_M_GetRoll()*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_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_OR_EXCP_DATA); else { //数据没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_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_NO_DATA_OR_EXCP_DATA); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA); } } //后脚传感器判断 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)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_OR_EXCP_DATA); else { //数据没问题 ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_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_NO_DATA_OR_EXCP_DATA); else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA); } } //任意传感器配置失败，重新配置 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; } //fornt back mag data check shake if(t_count > (2000/100)) { 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])); if(front_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD) { //get max front_mag_norm_max = (front_mag_norm_max > front_mag_norm)?front_mag_norm_max:front_mag_norm; //get min front_mag_norm_min = (front_mag_norm_min > front_mag_norm)?front_mag_norm:front_mag_norm_min; front_mag_shake_trigger = true; } if(back_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD) { //get max back_mag_norm_max = (back_mag_norm_max > back_mag_norm)?back_mag_norm_max:back_mag_norm; //get min back_mag_norm_min = (back_mag_norm_min > back_mag_norm)?back_mag_norm:back_mag_norm_min; back_mag_shake_trigger = true; } DEBUG_LOG("front_mag_norm_max:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",front_mag_norm,back_mag_norm,front_mag_norm_max,back_mag_norm_max, \ front_mag_norm_min,back_mag_norm_min,front_mag_norm_max - front_mag_norm_min, \ back_mag_norm_max - back_mag_norm_min,front_mag_shake_trigger,back_mag_shake_trigger); if(t_count >= (5000/100)) { if(front_mag_norm_max - front_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && front_mag_shake_trigger) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA); } if(back_mag_norm_max - back_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && back_mag_shake_trigger) { ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA); } } } //第5秒查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。 if(t_count == (5000/100)) { if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI); } //第5秒关闭100毫秒震动电机自检线程，初始化40毫秒自检显示结果线程，设置holdon，全功率运行。 if(t_count == (5000/100)) { nrf_gpio_pin_write(PIN_MT_EN,0); Process_Stop(selfcheck_mt_process); Process_Start(0,"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[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--; } //第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)); //保存数据到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.selfcheck_is_led_display = false; //关闭所有线程，初始化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; } ST_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); //重新初始化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); //初始化500ms震动电机自检线程，拉高震动电机引脚500ms然后拉低500ms（周期） Process_Start(500,"selfcheck_mt_process",selfcheck_mt_process); Process_SetHoldOn(selfcheck_mt_process,1); //初始化计算roll值算法 Mahony_Init(&Self_Mind_Mahony,104); //喂狗 feed_watchdog(); }