#include "process_result.h" int time_stamp = 0; float gyr[3]; float acc[3]; float mag[3]; float magBias[3] = {43.6012f, 154.6074f, -263.2141f}; float magScale[3] = {0.0039f, 0.0037f, 0.0038f}; float accBias[3] = {-0.0641278f, 0.01712f, -0.099816f}; float accScale[3] = {1.0010795f, 0.9971222f, 1.004638f}; int16_t h_pos[3]; int16_t h_att[3]; int16_t h_zupt; int16_t jump_send_count; int16_t down_send_count; int16_t is_jump; int16_t is_down; int16_t accel[3]; int16_t gyro[3]; uint16_t h_press; uint16_t s_press; int16_t s_pos[3]; int16_t s_att[3]; int8_t s_rssi; int16_t s_zupt; int left_press_buff[4]; int32_t left_press = 0; int32_t right_press = 0; void sen_data_acc(void) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(accel[0]>>8); buf[L++] = (uint8_t)(accel[0]>>0); buf[L++] = (uint8_t)(accel[1]>>8); buf[L++] = (uint8_t)(accel[1]>>0); buf[L++] = (uint8_t)(accel[2]>>8); buf[L++] = (uint8_t)(accel[2]>>0); send_protocol(DEX_NUM,CMD_HEART,buf,L); } void sen_data_pos(void) { uint8_t buf[256]; uint8_t L=0; static short count = 0; // buf[L++] = (uint8_t)(h_pos[0]>>8); // buf[L++] = (uint8_t)(h_pos[0]>>0); // // buf[L++] = (uint8_t)(h_pos[1]>>8); // buf[L++] = (uint8_t)(h_pos[1]>>0); buf[L++] = (uint8_t)(h_pos[2]>>8); buf[L++] = (uint8_t)(h_pos[2]>>0); buf[L++] = (uint8_t)(s_pos[2]>>8); buf[L++] = (uint8_t)(s_pos[2]>>0); buf[L++] = (uint8_t)(count>>8); buf[L++] = (uint8_t)(count>>0); count ++; send_protocol(DEX_NUM,CMD_HEART,buf,L); } void send_data_press_gyr(void) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(gyro[0]>>8); buf[L++] = (uint8_t)(gyro[0]>>0); buf[L++] = (uint8_t)(gyro[1]>>8); buf[L++] = (uint8_t)(gyro[1]>>0); buf[L++] = (uint8_t)(gyro[2]>>8); buf[L++] = (uint8_t)(gyro[2]>>0); buf[L++] = (uint8_t)(h_press>>8); buf[L++] = (uint8_t)(h_press>>0); send_protocol(DEX_NUM,CMD_HEART,buf,L); } //void send_data_pos_angle(void) //{ // uint8_t buf[256]; // uint8_t L=0; // // buf[L++] = (uint8_t)(h_pos[0]>>8); // buf[L++] = (uint8_t)(h_pos[0]>>0); // // buf[L++] = (uint8_t)(h_pos[1]>>8); // buf[L++] = (uint8_t)(h_pos[1]>>0); // // buf[L++] = (uint8_t)(h_pos[2]>>8); // buf[L++] = (uint8_t)(h_pos[2]>>0); // // buf[L++] = (uint8_t)(angle[0]>>8); // buf[L++] = (uint8_t)(angle[0]>>0); // // buf[L++] = (uint8_t)(angle[1]>>8); // buf[L++] = (uint8_t)(angle[1]>>0); // // buf[L++] = (uint8_t)(angle[2]>>8); // buf[L++] = (uint8_t)(angle[2]>>0); // // send_protocol(DEX_NUM,CMD_HEART,buf,L); //} void send_data_acc(void) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(accel[0]>>8); buf[L++] = (uint8_t)(accel[0]>>0); buf[L++] = (uint8_t)(accel[1]>>8); buf[L++] = (uint8_t)(accel[1]>>0); buf[L++] = (uint8_t)(accel[2]>>8); buf[L++] = (uint8_t)(accel[2]>>0); buf[L++] = (uint8_t)(h_press>>8); buf[L++] = (uint8_t)(h_press>>0); send_protocol(DEX_NUM,CMD_HEART,buf,L); } void sen_data_press(void) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(h_press>>8); buf[L++] = (uint8_t)(h_press>>0); buf[L++] = (uint8_t)(s_press>>8); buf[L++] = (uint8_t)(s_press>>0); send_protocol(DEX_NUM,CMD_HEART,buf,L); } void sen_data_left_press(void) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(left_press>>24); buf[L++] = (uint8_t)(left_press>>16); buf[L++] = (uint8_t)(left_press>>8); buf[L++] = (uint8_t)(left_press>>0); send_protocol(DEX_NUM,CMD_HEART,buf,L); } void send_package_data(uint16_t package_num, int16_t right_pos_data[3], int16_t left_pos_data[3], int16_t right_att_data[3], int16_t left_att_data[3], int16_t s_zupt, int16_t h_zupt,int16_t is_down, int16_t is_jump, int8_t s_rssi, uint8_t time_stamp) { uint8_t buf[256]; uint8_t L=0; buf[L++] = (uint8_t)(right_pos_data[0]>>8); buf[L++] = (uint8_t)(right_pos_data[0]>>0); buf[L++] = (uint8_t)(right_pos_data[1]>>8); buf[L++] = (uint8_t)(right_pos_data[1]>>0); buf[L++] = (uint8_t)(right_pos_data[2]>>8); buf[L++] = (uint8_t)(right_pos_data[2]>>0); buf[L++] = (uint8_t)(left_pos_data[0]>>8); buf[L++] = (uint8_t)(left_pos_data[0]>>0); buf[L++] = (uint8_t)(left_pos_data[1]>>8); buf[L++] = (uint8_t)(left_pos_data[1]>>0); buf[L++] = (uint8_t)(left_pos_data[2]>>8); buf[L++] = (uint8_t)(left_pos_data[2]>>0); buf[L++] = (uint8_t)(right_att_data[0]>>8); buf[L++] = (uint8_t)(right_att_data[0]>>0); buf[L++] = (uint8_t)(right_att_data[1]>>8); buf[L++] = (uint8_t)(right_att_data[1]>>0); buf[L++] = (uint8_t)(right_att_data[2]>>8); buf[L++] = (uint8_t)(right_att_data[2]>>0); buf[L++] = (uint8_t)(left_att_data[0]>>8); buf[L++] = (uint8_t)(left_att_data[0]>>0); buf[L++] = (uint8_t)(left_att_data[1]>>8); buf[L++] = (uint8_t)(left_att_data[1]>>0); buf[L++] = (uint8_t)(left_att_data[2]>>8); buf[L++] = (uint8_t)(left_att_data[2]>>0); buf[L++] = (uint8_t)(s_zupt * 8 + h_zupt * 4 + is_down * 2 + is_jump * 1); buf[L++] = (uint8_t)(s_rssi >> 0); buf[L++] = (uint8_t)(time_stamp); send_protocol(DEX_NUM,package_num,buf,L); } void send_dual_foot_data(void) { static uint8_t time_stamp = 0; send_package_data( 0x04, s_pos, h_pos, s_att, h_att ,s_zupt, h_zupt, is_down, is_jump, s_rssi, time_stamp); time_stamp ++; } void send_data_to_pc(void) { static uint8_t wait_time = 0; if(wait_time == 4) { send_dual_foot_data(); wait_time = 0; } wait_time ++; } void process_motion(IMU_DAT_t* p) { // int16_t accel[3]; // int16_t gyro[3]; // uint16_t h_press; // uint16_t s_press; // int16_t s_pos[3]; // int16_t s_att[3]; // int8_t s_rssi; // int16_t s_zupt; memcpy(accel,p->h.acc,sizeof(accel)); memcpy(accel,p->h.gyr,sizeof(gyro)); memcpy(accel,p->s.pos,sizeof(s_pos)); memcpy(accel,p->s.att,sizeof(s_att)); h_press = p->h.press; s_press = p->s.press; s_rssi = p->s.rssi; s_zupt = p->s.zupt; time_stamp = time_stamp + 1; left_press = (((int32_t)h_press & 0x0000ffff)<<8); right_press = (((int32_t)s_press & 0x0000ffff)<<8); short down_tag = 0; down_tag = press_down_detect_new(time_stamp, left_press, right_press); if (down_tag) { gpio_mt_run(20); down_send_count = 20; } if(down_send_count > 0) { is_down = 1; down_send_count --; } else { is_down = 0; } short jump_tag = 0; jump_tag = press_jump_detect(h_pos , s_pos); if (jump_tag) { gpio_mt_run(20); jump_send_count = 20; } //连续发20次蹲，终端再处理 if(jump_send_count > 0) { is_jump = 1; jump_send_count --; } else { is_jump = 0; } gyr[0] = (float)mMPU9250.gyro_x / GYR_LSB; gyr[1] = (float)mMPU9250.gyro_y / GYR_LSB; gyr[2] = (float)mMPU9250.gyro_z / GYR_LSB; acc[0] = (float)mMPU9250.acc_x / ACC_LSB; acc[1] = (float)mMPU9250.acc_y / ACC_LSB; acc[2] = (float)mMPU9250.acc_z / ACC_LSB; mag[0] = (float)mMPU9250.mag_x; mag[1] = (float)mMPU9250.mag_y; mag[2] = (float)mMPU9250.mag_z; mag[0] = (mag[0] - magBias[0]) * magScale[0]; mag[1] = (mag[1] - magBias[1]) * magScale[1]; mag[2] = (mag[2] - magBias[2]) * magScale[2]; acc[0] = (acc[0] - accBias[0]) * accScale[0]; acc[1] = (acc[1] - accBias[1]) * accScale[1]; acc[2] = (acc[2] - accBias[2]) * accScale[2]; // acc[0] = (acc[0] + 0.004700898f) / 0.99823547f; // acc[1] = (acc[1] - 0.0037821f) / 0.9982976f; // acc[2] = (acc[2] + 0.03741163f) / 1.0116903f; // unsigned char left_tag = footPDR(time_stamp, gyr, acc, mag, left_press, h_pos, h_att, &h_zupt); // // if(h_pos[1] < -20) // { // gpio_mt_run(30); // } // if (h_zupt > 1 || h_zupt < 0) // { //// send_motion_to_phone(MOTION_LEFT, time_stamp); // gpio_mt_run(20); // } // memcpy(pos_n_offset, s_gyro, 3*sizeof(short)); send_data_to_pc(); }