Mobile APP data analysisa and command sending

1. WitMotion protocol data analysis

private void copeWitData(int iLen,byte[] tempBuffer){ byte[] packBuffer = new byte[11]; byte sHead; float fTemp; float[] fData = new float[4]; receivedPackLength = iLen;

for (int i = 0; i < iLen; i++) {queueBuffer.add(tempBuffer[i]);// The data read from the buffer is stored in the queue } while (queueBuffer.size() >= 11) {

if ((queueBuffer.poll()) != 0x55) { iError++; continue; }// peek()返回对首但不删除 poll 移除并返回 sHead = queueBuffer.poll(); if ((sHead & 0xF0) == 0x50) iError = 0; for (int j = 0; j < 9; j++) packBuffer[j] = queueBuffer.poll(); byte value; value = (byte) (0x55 + sHead); for (int i = 0; i < 8; i++) value = (byte) (value + packBuffer[i]); if (value != packBuffer[8]) { Log.e("--", String.format("%2x %2x %2x %2x %2x %2x %2x %2x %2x SUM:%2x %2x", sHead, packBuffer[0], packBuffer[1], packBuffer[2], packBuffer[3], packBuffer[4], packBuffer[5], packBuffer[6], packBuffer[7], packBuffer[8], value)); continue; } for (int i = 0; i < 4; i++) fData[i] = (((short) packBuffer[i * 2 + 1]) << 8) | ((short) packBuffer[i * 2] & 0xff); switch (sHead) { case 0x50: int ms = (int)fData[3]; strDate = String.format("20%02d-%02d-%02d", packBuffer[0], packBuffer[1], packBuffer[2]); strTime = String.format("%02d:%02d:%02d.%03d", packBuffer[3], packBuffer[4], packBuffer[5], ms); break; case 0x51: for (int i = 0; i < 3; i++) ac[i] = fData[i] / 32768.0f * ar; fTempT = fData[3]; if (type == TYPE_AXIS6) T = (float) (fTempT / 340 + 36.53); else T = fTempT/100f; break; case 0x52: for (int i = 0; i < 3; i++) w[i] = fData[i] / 32768.0f * av; if (fData[3] != fTempT) { { if (!eqFilt.init) eqFilt.setInitValue(fData[3]/100f); eq = eqFilt.filt(fData[3]/100f); eqPercent = getEqPercent(eq); mCallback.dataUpdate(HEADER_Eq); } } break; case 0x53: for (int i = 0; i < 3; i++) Angle[i] = fData[i] / 32768.0f * 180; Eular321ToQuat(Angle, q); if (fData[3] != fTempT) { version = fData[3]; } mCallback.dataUpdate(HEADER_q); break; case 0x54://magnetic for (int i = 0; i < 3; i++) h[i] = fData[i]; break; case 0x55://port for (int i = 0; i < 4; i++) d[i] = fData[i]; break; case 0x56://air pressure, altitude

break; case 0x57://latitude and longitude

longitude = (float) (Longitude / 10000000 + ((float) (Longitude % 10000000) / 100000.0 / 60.0));

latitude = (float) (Latitude / 10000000 + ((float) (Latitude % 10000000) / 100000.0 / 60.0)); break; case 0x58://Altitude, heading, ground speed altitude = fData[0] / 10; yaw = fData[1] / 100;

break; case 0x5a://sat no sn = fData[0]; pdop = fData[1]/ 100.0f; hdop = fData[2] / 100.0f; vdop = fData[3] / 100.0f; break; case 0x5f://sat no CopeRegs(regStart,fData); regStart = -1; break; }//switch

if ((sHead >= 0x50) && (sHead <= 0x5a)) { if(protocal==PROTOCAL_NONE) protocal=PROTOCAL_WIT; RecordData(sHead,true); mCallback.dataUpdate(sHead); continue; } } }

static Queue<byte[]> sendBuffer = new LinkedList<byte[]>();//Send command cache, multiple commands need to be sent sequentially at intervals of 100ms public void sendData(byte[] cmd){ if (sendThread==null){ sendThread = new Thread(new Runnable() { @Override public void run() { while(sendBuffer.size()>0) { try { Log.e("--","send start, remain:"+sendBuffer.size()); byte[] byteToSend = sendBuffer.poll(); mCallback.send(byteToSend); Thread.sleep(100); } catch (Exception err) {} } Log.e("--","send complete, delet Thread"); sendThread=null; } }); sendThread.start(); } sendBuffer.add(cmd); } //read register public void readRegs(int address){ regStart = (byte)address; writeReg( 0x27,address); } //write register public void writeReg(int addr,int data){ sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) addr, (byte) (data&0xff), (byte) ((data>>8)&0xff)}); } //unlock register public void unLockReg(){ writeReg(0x69,0xb588);//unlock } //save configuration public void saveReg(){ writeReg(0x00,0x00);//unlock mCallback.saveOK(); } //Unlock the register first and then write data public void writeLockReg(int addr,int data){ unLockReg();//unlock writeReg(addr,data);//write Reg } //Write and save parameters public void writeAndSaveReg(int addr,int data){ unLockReg();//unlock writeReg(addr,data);//write Reg saveReg();//save } //Accelerometer Calibration public void accCali(){ if (type==TYPE_AXIS3) {//3-axis products writeLockReg(0x01, 0x01); new Handler().postDelayed(new Runnable() { @Override public void run() { saveReg(); } }, 3000); } else if(type==TYPE_AXIS6)//6-axis products sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) 0x67}); else if (type==TYPE_AXIS9){//9-axis products writeLockReg(0x01, 0x01 ); new Handler().postDelayed(new Runnable() { @Override public void run() { saveReg(); } }, 3000); } } //Start Magnetic Field Calibration public void startMagCali(){ boolean bTwoFaceCali=false; if (type!=TYPE_AXIS9) return;

if (((int)(version/1000)==17)&&(((int)version)%100>7)) bTwoFaceCali=true; if (((int)(version/1000)==18)&&(((int)version)%100>7)) bTwoFaceCali=true; if (((int)(version/1000)==131)&&(((int)version)%100>5)) bTwoFaceCali=true;//after 13105 version if (((int)(version/1000)==191)&&(((int)version)%100>5)) bTwoFaceCali=true;//after 191 version writeLockReg(0x01, 0x07 ); } //Stop Magnetic Field Calibration public void stopMagCali(){ if (type==TYPE_AXIS9)writeAndSaveReg(0x01, 0x00 ); } //Altitude reset public void clearHeight(){ if (type==TYPE_AXIS9)writeAndSaveReg(0x01, 0x03 ); } //Z-axis angle reset public void clearZAngle(){ if (type==TYPE_AXIS6) sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) 0x52}); else if (type==TYPE_AXIS9)writeAndSaveReg(0x01, 0x04); } //set reference angle public void setRefAngle(){ if (type==TYPE_AXIS9)writeAndSaveReg(0x01, 0x08); } //reset public void restore(){ if (type==TYPE_AXIS9)writeLockReg(0x00, 0x01); }

//BLE device enters firmware upgrade mode public void EnterDFU(){ if (type==TYPE_AXIS9) writeLockReg(0x65, 0x00); } //3-axis product setting angle filter coefficient public void setAngleFilterK(int k){//0~1000 if (checkRange(k,0,1000)==false) return; if (type==TYPE_AXIS3) writeReg(0x6c,k); }

//Set device address public void setAddress(int addr){ if (checkRange(addr,0,255)==false) return; if (type==TYPE_AXIS9) {address = addr;writeAndSaveReg(HEADER_ADDRESS, (byte)addr);} } //set output rate //set output rate //for BWT901: 1:"0.2Hz", 2:"0.5Hz", 3:"1Hz", 4:"2Hz", 5:"5Hz", 6:"10HZ", 7:"20Hz", 8:"50Hz", 9:"100Hz", 10:"125Hz", 11:"200Hz" //for BWT61: 0:20Hz 1:100Hz public void setOutputRate(int index){ if (type==TYPE_AXIS9) { if (checkRange(index, 1, 11) == false) return; writeAndSaveReg(HEADER_OUTPUT_RATE, (byte) index); outputRate = index; } else if (type==TYPE_AXIS6){ if (checkRange(index, 0, 1) == false) return; sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) (0x64 - index)}); } } //set bandwidth //set bandwidth //0:"256HZ", 1:"184HZ", 2:"94HZ", 3:"42HZ", 4:"21HZ", 5:"10HZ", 6:"5HZ" public void setBandwidth(int index){ if (checkRange(index,0,6)==false) return; if (type==TYPE_AXIS6) sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) (0x81+index)}); else if (type==TYPE_AXIS9) writeAndSaveReg(HEADER_BANDWIDTH, (byte)index ); } //set algorithm public void setAlgrithm(int index){ if (checkRange(index,0,1)==false) return; if (type==TYPE_AXIS9) writeAndSaveReg(HEADER_ALGRITHM,index); } //Set the installation orientation public void setDirection(int index){ if (checkRange(index,0,1)==false) return; if (type==TYPE_AXIS6) sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) (0x65+index)}); else if (type==TYPE_AXIS9) writeAndSaveReg(HEADER_DIRECTION,index); } //Set the static detection threshold for 6-axis products public void setStaticDetect(int index){ if (checkRange(index,0,14)==false) return; if (type==TYPE_AXIS6) sendData(new byte[]{(byte) 0xff, (byte) 0xaa, (byte) (0x71+index)}); }