123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289 |
- #include "ble_comm.h"
- #include "vl53l1.h"
- #include "tool.h"
- #define GAMEUSEDTALIGE 1
- static unsigned char USEdtalige = 1;
- void IMU_Dtalige_Rowdata_ON(void)
- {
- USEdtalige = 0;
- }
- void IMU_Dtalige_Rowdata_OFF(void)
- {
- USEdtalige = 1;
- }
- unsigned char left_right_buff[60];
- unsigned char left_right_data_len;
- static void d_integrate_left_right(uint8_t* left_buff, uint8_t left_len, uint8_t* right_buff, uint8_t right_len)
- {
- unsigned char L=0;
- for(int i=0;i<left_len;i++)
- {
- left_right_buff[L++]=left_buff[i];
- }
- for(int i=0;i<right_len;i++)
- {
- left_right_buff[L++]=right_buff[i];
- }
- left_right_data_len=L;
- }
- static void d_get_game_package(uint8_t* buff_address, uint8_t * buff_len)
- {
- for(int i=0;i<left_right_data_len;i++)
- {
- buff_address[i]=left_right_buff[i];
- }
- *buff_len=left_right_data_len;
- }
- unsigned char mofoot_buff[30];
- unsigned char mofoot_data_len;
- static void d_process_motion(int32_t time_stamp, int16_t _acc[3], int16_t _gry[3], int16_t front_mag[3], int16_t back_mag[3], uint8_t _rssi)
- {
- int L=0;
-
- mofoot_buff[L++]=_acc[0]>>8;
- mofoot_buff[L++]=_acc[0];
- mofoot_buff[L++]=_acc[1]>>8;
- mofoot_buff[L++]=_acc[1];
- mofoot_buff[L++]=_acc[2]>>8;
- mofoot_buff[L++]=_acc[2];
-
- mofoot_buff[L++]=_gry[0]>>8;
- mofoot_buff[L++]=_gry[0];
- mofoot_buff[L++]=_gry[1]>>8;
- mofoot_buff[L++]=_gry[1];
- mofoot_buff[L++]=_gry[2]>>8;
- mofoot_buff[L++]=_gry[2];
-
- mofoot_buff[L++]=front_mag[0]>>8;
- mofoot_buff[L++]=front_mag[0];
- mofoot_buff[L++]=front_mag[1]>>8;
- mofoot_buff[L++]=front_mag[1];
- mofoot_buff[L++]=front_mag[2]>>8;
- mofoot_buff[L++]=front_mag[2];
-
- mofoot_buff[L++]=back_mag[0]>>8;
- mofoot_buff[L++]=back_mag[0];
- mofoot_buff[L++]=back_mag[1]>>8;
- mofoot_buff[L++]=back_mag[1];
- mofoot_buff[L++]=back_mag[2]>>8;
- mofoot_buff[L++]=back_mag[2];
-
-
- mofoot_data_len=L;
- }
- static void d_get_foot_data(uint8_t *buf, uint8_t *buff_len)
- {
- for(int i=0;i<mofoot_data_len;i++)
- {
- buf[i]=mofoot_buff[i];
- }
- *buff_len=mofoot_data_len;
- }
- typedef struct {
- unsigned char imubuff[80];
- unsigned char imubuff_len;
- uint16_t ts;
- }IMU_Hal_t;
- typedef struct {
- IMU_Hal_t h;
- IMU_Hal_t s;
- }IMU_Hal_RL_t;
- typedef void (*process_motion_t)(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len);
- extern void get_game_package(uint8_t* buff_address, uint8_t * buff_len);
- extern void integrate_left_right(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len);
- extern void get_foot_data(uint8_t *buf, uint8_t *buff_len);
- IMU_Hal_t rev_s={.ts=0};
- IMU_Hal_t rev={.ts=0};
- IMU_Hal_t rev_io;
- IMU_Hal_RL_t temp_IMU_DAT;
- QUEUE_DEF(IMU_Hal_t, IMU_DAT_H_queue, size_4, QUEUE_MODE_OVERFLOW);
- QUEUE_DEF(IMU_Hal_t, IMU_DAT_S_queue, size_4, QUEUE_MODE_OVERFLOW);
- extern uint16_t lose_pack_all;
- void IMU_dtalige(IMU_Hal_RL_t *outp, queue_t *ph, queue_t *ps, process_motion_t process_p)
- {
- static char sta = 0;
- static unsigned short R_timestamp_re = 0;
- switch (sta)
- {
- case 0:
- if (ps->element_count > 0)
- {
- BLE_PRINT( "IMU_dtalige init.\r\n");
- sta = 1;
- queue_reset(ps);
- queue_reset(ph);
- }
- break;
- case 1:
- while (ps->element_count > ph->element_count)
- {
- queue_out(ps, &outp->s);
- R_timestamp_re = outp->s.ts;
- }
- if (ps->element_count > 0)
- {
- queue_out(ps, &outp->s);
- R_timestamp_re++;
- if (R_timestamp_re != outp->s.ts)
- {
- do
- {
- BLE_PRINT( "lose packet %d\r\n", R_timestamp_re);
- #if LOSSPACK_ENANBLE
-
- extern uint16_t lose_pack_all;
- lose_pack_all++;
- #endif
-
- R_timestamp_re++;
- queue_out(ph, &outp->h);
- if (R_timestamp_re == outp->h.ts)
- {
- queue_out(ph, &outp->h);
- process_p(outp->h.imubuff, outp->h.imubuff_len, outp->s.imubuff, outp->s.imubuff_len);
- get_game_package(rev_io.imubuff, &rev_io.imubuff_len);
-
- rev_io.ts++;
- rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
- rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
- if(rev_io.ts%2==0)
- send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
- break;
- }
- if (ph->element_count <= 0)
- {
- sta = 0;
- break;
- }
- } while (1);
- }
- else
- {
- queue_out(ph, &outp->h);
-
- process_p(outp->h.imubuff, outp->h.imubuff_len, outp->s.imubuff, outp->s.imubuff_len);
- get_game_package(rev_io.imubuff, &rev_io.imubuff_len);
-
- rev_io.ts++;
- rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
- rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
- if(rev_io.ts%2==0)
- send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
- }
- }
- break;
- }
- }
- void IMU_Dtalige(void)
- {
- if( USEdtalige )
- {
- #if GAMEUSEDTALIGE
- IMU_dtalige(&temp_IMU_DAT, &IMU_DAT_H_queue, &IMU_DAT_S_queue, integrate_left_right);
- #endif
- }
- }
- void IMU_Rec_data(uint8_t* pdat,uint8_t len)
- {
- if(pdat[3]==4)
- {
- memcpy(rev_s.imubuff,&pdat[4],pdat[1]);
- rev_s.imubuff_len=pdat[1]-7;
- rev_s.ts=((uint16_t)pdat[pdat[1]-3]<<8) |((uint16_t)pdat[pdat[1]-2]<<0);
- if( USEdtalige )
- {
- #if GAMEUSEDTALIGE
- queue_in(&IMU_DAT_S_queue,&rev_s);
- #endif
- }
- }
- }
- void IMU_Process_motion_queue(uint8_t IS_HOST, int32_t time_stamp, int16_t* _acc,int16_t* _gry, int16_t* front_mag, int16_t* back_mag, uint8_t _rssi)
- {
- if( USEdtalige )
- {
- process_motion(time_stamp, _acc, _gry, front_mag, back_mag, _rssi);
- get_foot_data(rev.imubuff,&rev.imubuff_len);
- }
- else
- {
- d_process_motion(time_stamp, _acc, _gry, front_mag, back_mag, _rssi);
- d_get_foot_data(rev.imubuff,&rev.imubuff_len);
- }
- rev.ts++;
-
- if(IS_HOST)
- {
- if( USEdtalige )
- {
- #if GAMEUSEDTALIGE
- queue_in(&IMU_DAT_H_queue,&rev);
- #else
- integrate_left_right(rev.imubuff, rev.imubuff_len, rev_s.imubuff, rev_s.imubuff_len);
- get_game_package(rev_io.imubuff, &rev_io.imubuff_len);
- rev_io.ts++;
- rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
- rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
- if(rev_io.ts%2==0)
- send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
- #endif
- }
- else
- {
- d_integrate_left_right(rev.imubuff, rev.imubuff_len, rev_s.imubuff, rev_s.imubuff_len);
- d_get_game_package(rev_io.imubuff, &rev_io.imubuff_len);
- rev_io.ts++;
- rev_io.imubuff[rev_io.imubuff_len + 1] = rev_io.ts;
- rev_io.imubuff[rev_io.imubuff_len] = rev_io.ts >> 8;
- if(rev_io.ts%2==0)
- send_protocol(0, 4, rev_io.imubuff, rev_io.imubuff_len);
- }
- }
- else
- {
- rev.imubuff[rev.imubuff_len+1]=rev.ts;
- rev.imubuff[rev.imubuff_len]=rev.ts>>8;
- send_protocol(0,4,rev.imubuff,rev.imubuff_len+2);
- }
- }
|