#include "process_result.h" #include "hal_mt.h" #include "hal_flash.h" #include "detect_zero_vel.h" extern uint8_t isGameMode; float gyr[3]; float acc[3]; float mag[3]; uint32_t time_stamp = 0; int16_t accel[3]; int16_t gyro[3]; int16_t magn[3]; int32_t pos[3]; int16_t att[3]; int zupt; int zupt_res; uint16_t laser_distance; uint8_t slave_rssi; uint8_t foot_data_buf[64]; uint8_t foot_data_len; int16_t front_zero; int16_t back_zero; int16_t acc_zero; void init_MOTION(void) { time_stamp = 0; } void foot_data_to_package() { static uint16_t package_time = 0; foot_data_len = 0; foot_data_buf[foot_data_len++] = (uint8_t)(laser_distance>>8); foot_data_buf[foot_data_len++] = (uint8_t)(laser_distance>>0); foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>24); foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>16); foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>24); foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>16); foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>24); foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>16); foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>0); foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>0); foot_data_buf[foot_data_len++] = (uint8_t) slave_rssi; foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>8); foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>0); foot_data_buf[foot_data_len++] = (uint8_t)(magn[0]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(magn[0]>>0); foot_data_buf[foot_data_len++] = (uint8_t)(magn[1]>>8); foot_data_buf[foot_data_len++] = (uint8_t)(magn[1]>>0); package_time ++; } void get_foot_data(uint8_t *buf, uint8_t *buff_len) { memcpy(buf, foot_data_buf, foot_data_len); * buff_len = foot_data_len; } void process_motion(int16_t _acc[3], int16_t _gry[3], int16_t _mag[3], uint16_t _laser_distance, uint8_t _rssi) { /* ½âÎöÊý¾Ý */ memcpy(accel, _acc, 3 * sizeof(int16_t)); accel[2] = -accel[2]; memcpy(gyro , _gry, 3 * sizeof(int16_t)); memcpy(magn , _mag, 3 * sizeof(int16_t)); gyr[0] = (float)gyro[0] / GYR_LSB; gyr[1] = (float)gyro[1] / GYR_LSB; gyr[2] = (float)gyro[2] / GYR_LSB; acc[0] = (float)accel[0] / ACC_LSB; acc[1] = (float)accel[1] / ACC_LSB; acc[2] = (float)accel[2] / ACC_LSB; // acc[0] += 0.03f; // acc[1] += 0.1f; laser_distance = _laser_distance; // slave_rssi = _rssi - 20; // // // if(acc_x_status == 1 || acc_y_status == 1 || acc_z_status == 1) // { // zupt = 1; // } // else // { // zupt = 0; // } // static int acc_x_status; static int acc_y_status; static int acc_z_status; step_detect_by_acc_gyr(acc, gyr, &acc_x_status, &acc_y_status, &acc_z_status, &zupt); if(acc_x_status == 1 || acc_y_status == 1 || acc_z_status == 1) { zupt = 1; } else { zupt = 0; } //zupt_res = cor_zupt_function(zupt); //detect_zero_vel(magn[0], magn[1], accel, &front_zero, &back_zero, &acc_zero); // if(front_zero > 0 || back_zero > 0 ) // { // zupt = 1; // } // else // { // zupt = 0; // } acc[2] = -acc[2]; footPDR(time_stamp, gyr, acc, mag, 0, pos, att, acc_x_status, acc_y_status, zupt_res); // att[1] = -att[1]; // att[2] = mFlash.mStep.stepCur[0]; // att[0] = acc_x_status; // att[1] = acc_y_status; // att[2] = zupt; // detect_step_by_acc(accel, &zupt); // att[2] = zupt; // att[0] = mFlash.mStep.stepCur[0]; foot_data_to_package(); time_stamp ++; }