Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA22_8Hz_USBHOST_STABLE_M5cali_rev10279.ino Secret

Created May 1, 2022 01:49
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Embed
What would you like to do?
Learn more about clone URLs
Download ZIP
Arduino Teensy4.1 GNSS RTK Data Logger F9P,F9H MovingBase data Saving toSD CARD and monitoring through ESP-NOW WiFi
Raw
STA22_8Hz_USBHOST_STABLE_M5cali_rev10279.ino
// STA22_USBHOSt_Dual_Buf172_sdFAT_rev098
//RTKMovingBaseモードでのデータログシステムPgm
// Base:F9P NAV-PVT出力
//Rover:F9H　NAV-RELPOSNED出力
//USB HOST機能：USB HUB1,2とuserialb0-3の４個接続
//HOST IDGET
uint8_t buf[2048], buf1[200], buf2[200], buf3[200];
uint8_t b0ID[200];//usrialb0 uniqueID
uint8_t b1ID[200];//usrialb0 uniqueID
uint8_t b2ID[200];//usrialb0 uniqueID
uint8_t b3ID[200];//usrialb0 uniqueID
char* b0IDname;
char* b1IDname;
char* b2IDname;
char* b3IDname;
int epNo,epNo_1,ep0No, ep0No_1, ep1No, ep1No_1; //EpochNo
long itow0, itow0_1,itow0_2;
long itow1, itow1_1,itow1_2;
float ellapsedtime, ellapsedtime_1;
int b0byteN, b1byteN, b2byteN, b3byteN;
int b0IDflag, b1IDflag, b2IDflag, b3IDflag;
int Startflag = 0;
int Usbflag0 = 0;
int Usbflag1 = 0;
int IDpoll = 0;
volatile uint8_t b0Open, b1Open, b2Open, b3Open;
//F9P UniqueID
uint8_t Lbase[] = {0x2C, 0x48, 0x82, 0xA1 , 0x9F}; //F9P Left base
uint8_t Rrover[] = {0x7F, 0x4D, 0x02, 0xD7, 0xDB}; //F9H Right rover
uint8_t Rbase[] = {0xEB, 0x4D, 0x22, 0xD2, 0x9C}; //F9P Right base
uint8_t Lrover[] = {0x7F, 0x4D, 0x72, 0x13, 0x19}; //F9H Left rover
//------------------- Monitoring Prameters------------------------
int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間
uint8_t stflag,fixflag,fixed;
uint8_t PVT0flag,RELPOS0flag,PVT1flag,RELPOS1flag;
int fixepoNo,allepoch,misepoch;//epochのカウント
int sa0,sa1,miscount,miscount0,miscount1,miscount00,miscount11;//epoch飛び itow0-itow0_1の差
double fixpercent;//Fix率
double rN[10],rE[10],rD[10],rL[10],mH[10];//average initial sample
double relN0,relE0,relD0,relL0,mH0;//初回静止時の基準位置
volatile double relN,relE,relD,relL,mHead;//基準局からの現在位置
double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN
double rNstd,rEstd,rDstd,rLstd,mHstd;//標準偏差 rNdvNの平方根
double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//基準位置平均値
double rNaveN1,rEaveN1,rDaveN1,rLaveN1,mHaveN1;//平均値
double rNsum,rEsum,rDsum,rLsum,mHsum;//総和
double rNsgmN,rEsgmN,rDsgmN,rLsgmN,mHsfmN;//standard deviation N
double rNsgmN1,rEsgmN1,rDsgmN1,rLsgmN1,mHsgmN1;//standard deviation N+1
double rNdvsum,rEdvsum,rDdvsum,rLdvsum,mHdvsum;//deviation sum
double rNdvN,rEdvN,rDdvN,rLdvN,mHdvN;//deviation N
double rNdvN1,rEdvN1,rDdvN1,rLdvN1,mHdvN1;//deviation N+1
int avecount;
uint8_t califlag;
//-----SD log parameters----------------------
//#include <SD.h>
#include "SdFat.h"
#include <SPI.h>
// Setup for built in SD card.
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)
// SdFat-beta usage
SdFs sd;
FsFile myFile;
int n;//counter
int i, j, k,l,m;
int starttime;
int itow, itow_1, itowR;
int hacc, accL,numsv;
uint8_t flags,mflags;
double pdop;
int JST_day;
//timestamp
char fnameB[30];//UBX Binary File name
char fnameV[30];//UBX Value File name NAV-PVT+NAV-RELPOSNED
uint8_t Sflag = 0;
char sc;
int debugP = 1;
//const int chipSelect =BUILTIN_SDCARD;// 10;
//String dataString,dataString1;
int kaisu;
//-----USB HOST
#include "USBHost_t36.h"
#define USBBAUD 115200//115200 //460800
uint32_t baud = USBBAUD;
uint32_t format = USBHOST_SERIAL_8N1;
USBHost myusb;
USBHub hub1(myusb);
//USBHub hub2(myusb);
//USBHIDParser hid1(myusb);
//USBHIDParser hid2(myusb);
//USBHIDParser hid3(myusb);
//===normal buffer=====
USBSerial userialb0(myusb);
USBSerial userialb1(myusb);
//====Big Buffer=======
//USBSerial_BigBuffer userialb0(myusb);//NG
//USBSerial_BigBuffer userialb1(myusb);//NG
uint8_t dummy;
char data0, datab, datab2;
//USBDriver *drivers[] = {&hub1, &hub2, &hid1, &hid2, &hid3, &userialb0, &userialb1, &userialb2, &userialb3};
USBDriver *drivers[] = {&hub1, &userialb0, &userialb1};
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
//const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "HID1", "HID2", "HID3", "USERIALB0", "USERIALB1", "USERIALB2", "USERIALB3" };
const char * driver_names[CNT_DEVICES] = {"Hub1","USERIALB0", "USERIALB1"};
//bool driver_active[CNT_DEVICES] = {false, false, false, false};
bool driver_active[CNT_DEVICES] = {false, false, false};
//Data
//Data
uint8_t Buf172[172];
uint8_t dBuf0[100], dBuf1[100];
uint8_t PVTdata[172];//base PVT
long PVTval[33];
uint8_t RELPOSBdata[72];//base line RELPOS
long RELPOSBval[21];
uint8_t RELPOSdata[72];//Rover RELPOS
long RELPOSval[21];
uint8_t fdata[4];
int Num0, Num0_1, Num1, Num1_1;
int flag0, flag1;
char keyin;
int time0, time0_1, time1, time1_1;
//===============KEY IN Control ================
char c;
//=================================================================
void setup()
{
//Serial.begin(460800);
Serial.begin(921600);
Serial3.begin(115200);//Rx15-Tx14 M5Atom ESP-NOW
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor
Serial.println("\n\nUSB Host Testing - Serial");
myusb.begin();
Serial1.begin(115200); // We will echo stuff Through Serial1...
//-----------SD CARD Initialize------------------
if (!sd.begin(SD_CONFIG)) {
Serial.println("initialization failed!");
return;
}
Serial.println("initialization done.");
Sflag = 0;
Num0 = 0;
Num1 = 0;
b0IDflag = 0;
b1IDflag = 0;
Serial.println("Waiting SerialMonitor stanby");
delay(10000);//モニター立ち上げ時間待ち
ep0No = 0;
ep1No = 0;
itow0 = 1000;
itow0_1 = 1;
itow1 = 1000;
itow1_1 = 1;
sc=' ';
avecount=0;
starttime = millis();
rNsum=0;
rEsum=0;
rDsum=0;
rLsum=0;
mHsum=0;
califlag=0;//Calibration ON=1
PVT0flag=1;
RELPOS0flag=1;
PVT1flag=1;
RELPOS1flag=1;
}//set up
//=======================================================================================
//==========LOOP ========================================================================
//=======================================================================================
void loop() {
if(Serial.available()){
c=Serial.read();
if(c=='c'){
califlag=0;
avecount=0;
Serial.printf("==========KEY IN=%c califlag=%d========\n\r",c,califlag);
}
} //keyin end
updateDeviceList();
//Serial.print(b0Open);
//}
//===useralb0 read=========================================================
if(b0Open==1){
//Serial.write("A");
if(userialb0.available()){
//Serial.write("B");
dBuf0[0]=userialb0.read();
dBuf0[1]=userialb0.read();
dBuf0[2]=userialb0.read();
dBuf0[3]=userialb0.read();
if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x07 ){
//Serial.printf("u0PVT=%d\n\r",userialb0.available());
PVTdata[0]=dBuf0[0];
PVTdata[1]=dBuf0[1];
PVTdata[2]=dBuf0[2];
PVTdata[3]=dBuf0[3];
i=i+4;
// while(userialb0.available()&& i<172){//old 278
while(userialb0.available()){
PVTdata[i]=userialb0.read();
i++;
}
itow0_1=itow0;
itow0 = B2L(PVTdata[9], PVTdata[8], PVTdata[7], PVTdata[6]); //PVT iTow Calc
}//b5620107 end
else if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x3C ){
//Serial.printf("u0RELP=%d\n\r",userialb0.available());
RELPOSdata[0]=dBuf0[0];
RELPOSdata[1]=dBuf0[1];
RELPOSdata[2]=dBuf0[2];
RELPOSdata[3]=dBuf0[3];
i=i+4;
// while(userialb0.available()&& i<72){
while(userialb0.available()){
RELPOSdata[i]=userialb0.read();
i++;
}
itow0_1=itow0;
itow0 = B2L(RELPOSdata[13], RELPOSdata[12], RELPOSdata[11], RELPOSdata[10]); //RELPOS iTow Calc
//Serial.println("======================b0=RELPOS========================");
}//b562013C end
// Serial.printf("+++++B0=%x,%x,%x,%x,i=%d,T=%d,itow0=%d\n\r",dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3],i,millis(),itow0);
}// -------if userialb0.available end--------------------------------------------------------------------------------
if(itow0-itow0_1>127){miscount0++;}
//Serial.printf("****userialb0:k=%d,u0Ni=%d,itow0=%d,miscount0=%d,tim=%d\n\r",k,i,itow0,miscount0,millis());
i=0;//reset counter
k++;//epoch count
}//b0Open end
delay(3); //NG
//===useralb1 read========================================================
if(b1Open==1){
if(userialb1.available()){
dBuf1[0]=userialb1.read();
dBuf1[1]=userialb1.read();
dBuf1[2]=userialb1.read();
dBuf1[3]=userialb1.read();
if(dBuf1[0]==0xb5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07){
//Serial.printf("u1PVT=%d\n\r",userialb1.available());
PVTdata[0]=dBuf1[0];
PVTdata[1]=dBuf1[1];
PVTdata[2]=dBuf1[2];
PVTdata[3]=dBuf1[3];
i=i+4;
//while(userialb1.available()&& i<172){
while(userialb1.available()){
PVTdata[i]=userialb1.read();
i++;
}
itow1_1=itow1;
itow1 = B2L(PVTdata[9], PVTdata[8], PVTdata[7], PVTdata[6]); //PVT iTow Calc
}//b5620107 end
else if(dBuf1[0]==0xb5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x3C ){
//Serial.printf("u1RELP=%d\n\r",userialb1.available());
RELPOSdata[0]=dBuf1[0];
RELPOSdata[1]=dBuf1[1];
RELPOSdata[2]=dBuf1[2];
RELPOSdata[3]=dBuf1[3];
i=i+4;
// while(userialb1.available()&& i<72){
while(userialb1.available()){
RELPOSdata[i]=userialb1.read();
i++;
}
itow1_1=itow1;
itow1 = B2L(RELPOSdata[13], RELPOSdata[12],RELPOSdata[11], RELPOSdata[10]); //RELP iTow Calc
//Serial.println("======================b1=RELPOS========================");
}//b562013C end
// Serial.printf("+++++B1=%x,%x,%x,%x,i=%d,T=%d,itow1=%d\n\r",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],i,millis(),itow1);
}// userialb1.available end
if(itow1-itow1_1>127){miscount1++;}
//Serial.printf("****userialb1:j=%d,u1Ni=%d,itow1=%d,miscount1=%d,tim=%d\n\r",j,i,itow1,miscount1,millis());
i=0;
j++;
}//b1Open end
//=======================================================================
//if(itow0-itow0_1==125 || itow1-itow1_1==125){
if(itow0-itow0_1>=123 ){
itow0_1=itow0;//重複プリント防止
itow1_1=itow1;
//Serial.printf("<userial.read Finished time= %d>\n\r",millis());
//**********************100msec Process**********************************************
sa0=itow0-itow0_1;
sa1=itow1-itow1_1;
//---PVTcnv()---
int result=PVTcnv(PVTdata,PVTval);
itow=PVTval[0];
flags=PVTval[11];
hacc=PVTval[18];
pdop=((double)PVTdata[82] +PVTdata[83] * 256)/100;
numsv=(int)PVTdata[29];
//---In PVT-RELPOSBdata PRINT COPY --------------
for(m=0;m<72;m++){
RELPOSBdata[m]=PVTdata[m+100];//PVTdata[100-171]=RELPOSBdata[0-71
}
//------------------------------------------------------------------
//---------BaseLine Analysys---------------------------------------
result=RELPOScnv(RELPOSBdata,RELPOSBval);
relN=(double)RELPOSBval[4]+(double)RELPOSBval[10]*0.01;
relE=(double)RELPOSBval[5]+(double)RELPOSBval[11]*0.01;
relD=(double)RELPOSBval[6]+(double)RELPOSBval[12]*0.01;
relL=(double)RELPOSBval[7]+(double)RELPOSBval[13]*0.01;
//---------MovingBase RELPOSNED Analysys---------------------------
result=RELPOScnv(RELPOSdata,RELPOSval);
mHead=(double)RELPOSval[8]*0.00001;//Heading deg x10-5
mflags=RELPOSval[20];
//Serial.printf("mHead=%5.5f,RELPOSdata[8]=%d,mflags=%d\n\r",mHead,RELPOSval[8],mflags);
//---------------------------------------------------------------------
//---------------------------------------------------------------------
//================BaseLine Averaging Stddeviation =================================================
Serial.printf("if--califlag=%d\n\r",califlag);
if(fixflag==1 && califlag==0 ){
avecount++;
if(avecount<11){//[1-10] 10count
rN[avecount]=relN;
rNsum=rNsum+relN;
rE[avecount]=relE;
rEsum=rEsum+relE;
rD[avecount]=relD;
rDsum=rDsum+relD;
rL[avecount]=relL;
rLsum=rLsum+relL;
//mH[avecount]=mHead;
//mHsum=mHsum+mHead;
}
if(avecount==10){//10count ave stdev
rNaveN=rNsum/10;
rEaveN=rEsum/10;
rDaveN=rDsum/10;
rLaveN=rLsum/10;
//mHaveN=mHsum/10;
int cn;
for(cn=1;cn<11;cn++){
rNdvsum=rNdvsum+pow((rN[cn]-rNaveN),2);
rEdvsum=rEdvsum+pow((rE[cn]-rEaveN),2);
rDdvsum=rDdvsum+pow((rD[cn]-rDaveN),2);
rLdvsum=rLdvsum+pow((rL[cn]-rLaveN),2);
//mHdvsum=mHdvsum+pow((mH[cn]-mHaveN),2);
}
rNdvN=rNdvsum/10;
rEdvN=rEdvsum/10;
rDdvN=rDdvsum/10;
rLdvN=rLdvsum/10;
//mHdvN=mHdvsum/10;
}//if 10count averaging stdev end
//---N1 10-500count cal----
if(avecount >10 && avecount<501){
rNaveN1=((avecount-1)*rNaveN+relN)/(avecount);//avecount Nave
rEaveN1=((avecount-1)*rEaveN+relE)/(avecount);
rDaveN1=((avecount-1)*rDaveN+relD)/(avecount);
rLaveN1=((avecount-1)*rLaveN+relL)/(avecount);
//mHaveN1=((avecount-1)*mHaveN+mHead)/(avecount);
rNdvN1=((avecount-1)*(rNdvN+pow(rNaveN,2))+pow(relN,2))/avecount-pow(rNaveN1,2);
rEdvN1=((avecount-1)*(rEdvN+pow(rEaveN,2))+pow(relE,2))/avecount-pow(rEaveN1,2);
rDdvN1=((avecount-1)*(rDdvN+pow(rDaveN,2))+pow(relD,2))/avecount-pow(rDaveN1,2);
rLdvN1=((avecount-1)*(rLdvN+pow(rLaveN,2))+pow(relL,2))/avecount-pow(rLaveN1,2);
//mHdvN1=((avecount-1)*(mHdvN+pow(mHaveN,2))+pow(mHead,2))/avecount-pow(mHaveN1,2);
Serial.printf("AVE:flags=%d,hacc=%d,avcnt=%d,rNav=%4.2f,rNdv1=%4.2f,rEav=%4.2f,rEdv1=%4.2f,rDav=%4.2f,rDd1v=%4.2f,rLav=%4.2f,rLdv1=%4.2f\n\r",flags,hacc,avecount,rNaveN1,rNdvN1,rEaveN1,rEdvN1,rDaveN1,rDdvN1,rLaveN1,rLdvN1);
Serial.printf("------mHaveN1=%5.5f,mHdvN1=%5.5f\n\r",mHaveN1,mHdvN1);
//---change next step---
rNaveN=rNaveN1;
rEaveN=rEaveN1;
rDaveN=rDaveN1;
rLaveN=rLaveN1;
//mHaveN=mHaveN1;
rNdvN=rNdvN1;
rEdvN=rEdvN1;
rDdvN=rDdvN1;
rLdvN=rLdvN1;
//mHdvN=mHdvN1;
Serial.printf("avecount=%d:",avecount);
}//avecount 10-500
}// ave stdev end
//====SET Statistic Result rrN rrE rrD rrL =================
rrN=relN-rNaveN;
rrE=relE-rEaveN;
rrD=relD-rDaveN;
rrL=relL-rLaveN;
//standad deviation σ
rNstd=sqrt(rNdvN);
rEstd=sqrt(rEdvN);
rDstd=sqrt(rDdvN);
rLstd=sqrt(rLdvN);
//mHstd=sqrt(mHdvN);
if(avecount>=500){//averaging result print
Serial.printf("========avcnt=%d,rNav=%4.2f,rNstd=%4.2f,rEav=%4.2f,rEstd=%4.2f,rDav=%4.2f,rDstd=%4.2f,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f\n\r",avecount,rNaveN,rNstd,rEaveN,rEstd,rDaveN,rDstd,rLaveN,rLstd,mHead);
avecount=0;//reset counter
califlag=1;//averaging Calculation finished
//Averaging sum RESET
rNsum=0;
rEsum=0;
rDsum=0;
rLsum=0;
rNdvsum=0;
rEdvsum=0;
rDdvsum=0;
rLdvsum=0;
}
//==================================================================================================
//---FIlename()---
//Serial.printf("Sflag=%d,st=%d,mils=%d\n\r",Sflag,starttime,millis());
if(Sflag==0 && millis()-starttime>20000){//startから20秒後にSDログ開始
Filename();
Sflag=1;//File Log OK
sc='*';
}
//---SD WRITE---
if(Sflag==1){
//-FILE OPEN-
myFile = sd.open(fnameB, FILE_WRITE);
n = 0;//172 byte counter
while (n < 244) { //172 byte PVT,RELP write to SD card
if (myFile) { //myFileチェックは、書き込み直前で行う。
if (n < 100) {
myFile.write(PVTdata[n]);
}
if (n >= 100 && n < 172) {
myFile.write(RELPOSdata[n - 100]);
}
if (n >= 172 && n < 244) {
myFile.write(RELPOSBdata[n - 172]);
}
} else {
Serial.print("myFile Error");
exit(0);
}
n++;
}//while write finished
delay(8);//**********重要OPEN-CLOSE時間をあける**********
myFile.close(); //毎回クローズして万一のアクシデントに備える
}//Sflag==1 end
epNo++;
int itow0cut=itow0%10000;
int itow1cut=itow1%10000;
Serial.printf("epNo=%d,:it0=%4d,it1=%4d,flg=%d,acc=%d,Dop=%2.1f,nSV=%d,F%%=%3.1f,sec=%d,mis=%d,%d,rN=%4.2f,rE=%5.2f,rD=%5.2f,rL=%5.2f,Lav=%4.2f,Lstd=%2.2f,mHead=%3.5f,%c\n\r",epNo,itow0cut,itow1cut,flags,hacc,pdop,numsv,fixpercent,fixtime,miscount0,miscount1,rrN,rrE,rrD,rrL,rLaveN,rLstd,mHead,sc);
//Serial.printf("epNo=%d,flags=%d,hacc=%d\n\r",epNo,flags,hacc);
//Serial.printf("epNo=%d,:itow0=%d,itow1=%d,flags=%d,hacc=%d,Fix%%=%3.1f,fixsec=%d,mis0=%d,mi1=%d,rN=%7.2f,rE=%7.2f,rD=%7.2f,rL=%7.2f,%c\n\r",epNo,itow0cut,itow1cut,flags,hacc,fixpercent,fixtime,miscount0,miscount1,relN,relE,relD,relL,sc);
itow1_1=itow1;
k=0;//ucenter k=1
while(k){
//========Ucenter Binary OUTPUT=======================
for (i = 0; i < 172; i++) {
if (i < 100) {
Serial.write(PVTdata[i]);
}
delay(1);
if (i >= 100 && i < 172) {
Serial.write(RELPOSdata[i-100]);
}
}//Ucenter send finished
}//ucenter end
//*********************************************************************************
//******************MONITOR Data Sending SD print********************
//********************************************************************************
if(flags==1 && stflag==0){
RTKstart=millis();
stflag=1;
}
else if(flags==131 && fixflag==0){//fixed
RTKfix=millis();
fixflag=1;
}
if(stflag==1 && fixflag==1 && fixed==0){
fixtime=int(((double)RTKfix-RTKstart)/1000);//sec
fixed=1;
fixepoNo=epNo;//FixしたときのEpochNo
fixitow=itow0;
miscount0=0;//Fix後のカウント値
miscount1=0;//Fix後のカウント値
}
//fix率計算 allepoch,misepoch;/
if(fixed==1 && epNo>fixepoNo){
allepoch=epNo-fixepoNo;
if (flags<131){
misepoch++;
}
fixpercent=((double)(allepoch-misepoch)/allepoch)*100;
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^Send Parameters^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
uint8_t llitow=PVTdata[6];//[1]itow 1byte (PVTdata[9], PVTdata[8], PVTdata[7], PVTdata[6]); //PVT iTow Calc
Serial3.write(llitow);//1
//Serial.print(llitow,HEX);
uint8_t litow=PVTdata[7];//[2]itow 2byte
Serial3.write(litow);//2
//Serial.print(litow,HEX);
uint8_t uitow=PVTdata[8];//[3]itow 3byte
Serial3.write(uitow);//3
// Serial.print(uitow,HEX);
uint8_t uuitow=PVTdata[9];//[4]itow 4byte
Serial3.write(uuitow); //4
//Serial.print(uuitow,HEX);
uint8_t uflags=PVTdata[27];//[5]1byte
Serial3.write(uflags);//5
//Serial.print(uflags,HEX);
uint8_t unumSV=PVTdata[29];//[6]2byte
Serial3.write(unumSV);//6 numSV
//Serial.print(unumSV,HEX);//numSV
//hAcc[46-49] u4 mm
uint8_t llhacc=PVTdata[46];//[7]3byte hacc　下下位バイト
Serial3.write(llhacc);//7
uint8_t lhacc=PVTdata[47];//[8]4byte hacc 下位バイト
Serial3.write(lhacc);//8
uint8_t lfixtime=fixtime%256;//[9]5byte fixtime下位バイト
Serial3.write(lfixtime);//9
uint8_t ufixtime=(uint8_t)(int(fixtime/256));//[10]6byte fixtime上位バイト
Serial3.write(ufixtime);//10
uint8_t lfixpercent=(uint8_t)(int(fixpercent*10)%256);//[11]7byte fixpercent*10 下位バイト
Serial3.write(lfixpercent);//11
uint8_t ufixpercent=(uint8_t)(int(fixpercent*10/256));//[12]８byte fixpercent*10 上位バイト
Serial3.write(ufixpercent);//12
uint8_t lepNo=epNo%256;//[13]
Serial3.write(lepNo);//13
uint8_t uepNo=(uint8_t)(int(epNo/256));//[14]
Serial3.write(uepNo);//14
uint8_t umiscount1=(uint8_t)miscount1;//[15]
Serial3.write(umiscount1);//15
uint8_t lpDOP=PVTdata[82]; //[16] pDOPd[82] + d[83] * 256;
Serial3.write(lpDOP);//16
uint8_t upDOP=PVTdata[83];//[17]
Serial3.write(upDOP);//17
//現在相対座標値
uint8_t lrelN=(uint8_t)((int)(rrN*100) & 0XFF);//[18]0.00*100で整数
Serial3.write(lrelN);//18
uint8_t urelN=(uint8_t)(((int)(rrN*100) & 0xFF00)>>8);//[19]0.00*100で整数
delay(1);
Serial3.write(urelN);//19
Serial.printf("**epNo=%d,rrN=%4.2f,lrelN=%x,urelN=%x\n\r",epNo,rrN,lrelN,urelN);
uint8_t lrelE=(uint8_t)((int)(rrE*100) & 0xFF);//[20]0.00*100で整数
Serial3.write(lrelE);//20
uint8_t urelE=(uint8_t)(((int)((rrE*100)) &0xFF00)>>8);//[21]0.00*100で整数
Serial3.write(urelE);//21
uint8_t lrelD=(uint8_t)((int)(rrD*100)& 0xFF);//[22]0.00*100で整数
Serial3.write(lrelD);//22
uint8_t urelD=(uint8_t)(((int)(rrD*100) & 0xFF00)>>8);//[23]0.00*100で整数
Serial3.write(urelD);//23
//movingBase Head
// relpos[8] = B2L(d[s + 33], d[s + 32], d[s + 31], d[s + 30]);
uint8_t llHead=RELPOSdata[30];//[24] head
Serial3.write(llHead);//24
uint8_t lHead=RELPOSdata[31];//[25] head
Serial3.write(lHead);//25
uint8_t uHead=RELPOSdata[32];//[26] head
Serial3.write(uHead);//26
uint8_t uuHead=RELPOSdata[33];//[27] head
Serial3.write(uuHead);//27
//-------Calibration AVERAGE STD------------------------------------
uint8_t llrNaveN=(uint8_t)((int)(rNaveN*100) & 0xFF);//[28](double)xx.ss*100=>(int) xxss=>%256 (uint8_t)
Serial3.write(llrNaveN);//28
uint8_t lrNaveN=(uint8_t)(((int)(rNaveN*100) & 0xFF00)>>8);//[29]
Serial3.write(lrNaveN);//29
uint8_t urNaveN=(uint8_t)(((int)(rNaveN*100) & 0xFF0000)>>16);//[30]
Serial3.write(urNaveN);//30
uint8_t uurNaveN=(uint8_t)(((int)(rNaveN*100) & 0xFF000000)>>24);//[31]
Serial3.write(uurNaveN);//31
//Serial.printf("ANDCal:rNaveN*100=%d,ll=%x.l=%x,u=%x,uu=%x\n\r",(int)rNaveN*100,llrNaveN,lrNaveN,urNaveN,uurNaveN);
//
uint8_t llrEaveN=(uint8_t)(((int)(rEaveN*100) & 0xFF));//[32]
Serial3.write(llrEaveN);//32
uint8_t lrEaveN=(uint8_t)(((int)(rEaveN*100) & 0xFF00)>>8);//[33]
Serial3.write(lrEaveN);//33
uint8_t urEaveN=(uint8_t)(((int)(rEaveN*100) & 0xFF0000)>>16);//[34]
Serial3.write(urEaveN);//34
uint8_t uurEaveN=(uint8_t)(((int)(rEaveN*100) & 0xFF000000)>>24);//35]
Serial3.write(uurEaveN);//35
//
uint8_t llrDaveN=(uint8_t)(((int)(rDaveN*100) & 0xFF));//[36]
Serial3.write(llrDaveN);//36
uint8_t lrDaveN=(uint8_t)(((int)(rDaveN*100) & 0xFF00)>>8);//[37]
Serial3.write(lrDaveN);//37
uint8_t urDaveN=(uint8_t)(((int)(rDaveN*100) & 0xFF0000)>>16);//[38]
Serial3.write(urDaveN);//38
uint8_t uurDaveN=(uint8_t)(((int)(rDaveN*100) & 0xFF000000)>>24);//[39]
Serial3.write(uurDaveN);//39
//
uint8_t llrLaveN=(uint8_t)(((int)(rLaveN*100) & 0xFF));//[40]
Serial3.write(llrLaveN);//40
uint8_t lrLaveN=(uint8_t)(((int)(rLaveN*100) & 0xFF00)>>8);//[41]
Serial3.write(lrLaveN);//41
uint8_t urLaveN=(uint8_t)(((int)(rLaveN*100) & 0xFF0000)>>16);//[42]
Serial3.write(urLaveN);//42
uint8_t uurLaveN=(uint8_t)(((int)(rLaveN*100) & 0xFF000000)>>24);//[43]
Serial3.write(uurLaveN);//43
//
// Standard Deviations
uint8_t lrNstd=(uint8_t)(((int)(rNstd*100) & 0xFF));//[44]
//Serial.printf("(int)rNstd*100 & 0xFF=%d\n\r",(int)rNstd*100 & 0xFF);
Serial3.write(lrNstd);//44
uint8_t urNstd=(uint8_t)(((int)(rNstd*100) & 0xFF00)>>8);//[45]
//Serial.printf("(((int)rNstd*100 & 0xFF00)>>8)=%d\n\r",((int)rNstd*100 & 0xFF00)>>8);
Serial3.write(urNstd);//45
//
uint8_t lrEstd=(uint8_t)(((int)(rEstd*100) & 0xFF));//[46]
Serial3.write(lrNstd);//46
uint8_t urEstd=(uint8_t)(((int)(rEstd*100) & 0xFF00)>>8);//[47]
Serial3.write(urEstd);//47
//
uint8_t lrDstd=(uint8_t)(((int)(rDstd*100) & 0xFF));//[48]
Serial3.write(lrDstd);//48
uint8_t urDstd=(uint8_t)(((int)(rDstd*100) & 0xFF00)>>8);//[49]
Serial3.write(urDstd);//49
//
uint8_t lrLstd=(uint8_t)(((int)(rLstd*100) & 0xFF));//[50]
Serial3.write(lrLstd);//50
uint8_t urLstd=(uint8_t)(((int)(rLstd*100) & 0xFF00)>>8);//[51]
Serial3.write(urLstd);//51
//
uint8_t lavecount=(uint8_t)(avecount & 0xFF);//[52]
Serial3.write(lavecount);//52
uint8_t uavecount=(uint8_t)((avecount & 0xFF00)>>8);//[53]
Serial3.write(uavecount);//53
//
//Serial.printf("=======lrNstd=%x,urNstd=%x,lrEstd=%x,urEstd=%x,lrDstd=%x,urDstd=%x,lrLstd=%x,urLstd=%x\n\r",lrNstd,urNstd,lrEstd,urEstd,lrDstd,urDstd,lrLstd,urLstd);
//Serial.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n\r",llitow,litow,uitow,uuitow,uflags,unumSV,llhacc,lhacc,lfixtime,ufixtime,lfixpercent,ufixpercent);
//Serial.printf("uflags=%d,unumSV=%d,llhacc=%d,lhacc=%d,uhacc=%d,uuhacc=%d,time=%d\n\r",uflags,unumSV,llhacc,lhacc,uhacc,uuhacc,millis());
//*****************************************************************************************************************************************
//*****************************************************************************************************************************************
//*****************************************************************************************************************************************
//delay(80);//わざと遅らせて受信状態をみるt0msecだと丁度100msecの半分になった。80msecでは安定しなかった
//Serial.printf("<loop() end time=%d>\n\r",millis());
}// itow0-itow0_1==100 end
//===================================================
//********************************Data Acqusition Finished**********************************************************************
} //LOOP END
void Filename (){
//*********ファイル名をタイムスタンプで作ってファイルOPEN FIXしたら flags==131 *****************************************************************************
//if (PVTval[11] == 131 && Sflag == 0) { //start
Sflag = 1;
//---PVTval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec---
int JST = (PVTval[4] + 9) % 24; //UTC hourをJSTに変換
if (JST<9){//UTCが０時以前の場合日付を日本日付に修正
JST_day=1;
}
else{
JST_day=0;
}
String stime = String(PVTval[2], DEC) + "-" + String(PVTval[3]+JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVTval[5], DEC); //MMDDHHMM
String stimeB = "/" + stime + ".ubx"; //UBX Binary File
String stimeV = "/" + stime + ".txt"; //UBX Value Text File
int slenB = stimeB.length() + 1;
int slenV = stimeV.length() + 1;
//ファイル名はchar配列なのでStringからchar配列変換　fname[]を得る
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array
stimeV.toCharArray(fnameV, slenV); //stimeV to fnameV[] chara Array
Serial.println();
Serial.print("fnameB=");
Serial.print(stimeB);
Serial.print("/fnameV=");
Serial.println(stimeV);
Serial.print("TimeStamp:Sflag=");
Serial.println(Sflag);
//myFile = sd.open(fnameB, FILE_WRITE);
//myFile.truncate(0);//Appned　
// }//timeStamp making end
}//Filename() end ******************************************************************************************************************************************
//**************************************SUB***************************************************
//NAV-SECを送って、固有IDを受信する**************************************************************
//余計なセンテンスが混じって受信された場合に備えbuf[2048]まで用意して、その中からNAV-SECの返信IDを探す
//*******************navSEC Polling read buffer and search SEC frame*************************
//********************************************************************************************
void navSEC() {
// B5 62 27 03 00 00 2A A5
uint8_t uniqID[8] = {0xB5, 0x62, 0x27, 0x03, 0x00, 0x00, 0x2A, 0xA5};
Serial.println("--navSEC---");
delay(1000);
//--------------------------b0ID get-----------------------------
if (b0IDflag == 0) {
for (i = 0; i < 8; i++) {
userialb0.write(uniqID[i]);//SEC Request
}
i = 0;
//Serial.println();
if (userialb0.available() > 15) { //SEC header search
while (userialb0.available() > 0) { //buffer read
buf[i % 2048] = userialb0.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize = i - 1; //default SEC 17bytes
//Serial.printf("-------b0_bufsize=%d--------\n\r",bufsize);
for (i = 0; i < bufsize - 3; i++) {
if (buf[i] == 0xb5 && buf[i + 1] == 0x62 && buf[i + 2] == 0x27 && buf[i + 3] == 0x03) {
// b0ID[0]=buf[i+10];b0ID[1]=buf[i+11]; b0ID[2]=buf[i+12]; b0ID[3]=buf[i+13]; b0ID[4]=buf[i+14];
for (j = 0; j < 5; j++) {
b0ID[j] = buf[i + 10 + j]; //b0ID get
//Serial.print(b0ID[j],HEX);
//Serial.print(",");
}
if (b0ID[0] == 0x7F) {
b0IDname = "Rrover";
//Serial.printf("%s\n\r",b0IDname);
b0byteN = 72;
}
if (b0ID[0] == 0xEB) {
b0IDname = "Rbase";
//Serial.printf("%s\n\r",b0IDname);
b0byteN = 100;
}
b0IDflag = 1; //
Serial.printf("******Userialb0:%s:b0byteN=%d********\n\r", b0IDname, b0byteN);
break;
}
}
}
for (i = 0; i < 2048; i++) {
buf[i] = "";
}
}//b0ID flag end
//-----------------------------------------------------------------------------------------------------------
//--------------------------b1ID-----------------------------
if (b1IDflag == 0) {
for (i = 0; i < 8; i++) {
userialb1.write(uniqID[i]);
}
i = 0;
//Serial.println();
if (userialb1.available() > 15) { //SEC header search
while (userialb1.available() > 0) { //buffer read
buf[i % 2048] = userialb1.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize = i - 1; //default SEC 17bytes
//Serial.printf("-------b1_bufsize=%d--------\n\r",bufsize);
for (i = 0; i < bufsize - 3; i++) {
if (buf[i] == 0xb5 && buf[i + 1] == 0x62 && buf[i + 2] == 0x27 && buf[i + 3] == 0x03) {
//Serial.printf("*******start i=%d:b1_Id=",i);
// b1ID[0]=buf[i+10];b1ID[1]=buf[i+11]; b1ID[2]=buf[i+12]; b1ID[3]=buf[i+13]; b1ID[4]=buf[i+14];
for (j = 0; j < 5; j++) {
b1ID[j] = buf[i + 10 + j];
//Serial.print(b1ID[j],HEX);
//Serial.print(",");
}
if (b0ID[0] == 0x7F) {
b1IDname = "Rrover";
//Serial.printf("%s\n\r",b1IDname);
b1byteN = 72;
}
if (b1ID[0] == 0xEB) {
b1IDname = "Rbase";
//Serial.printf("%s\n\r",b1IDname);
b1byteN = 100;
}
Serial.printf("******Userialb1:%s:b1byteN=%d********\n\r", b1IDname, b1byteN);
b1IDflag = 1;
break;
//Serial.println();
}
}
}
}//b1IDflag end
for (i = 0; i < 2048; i++) {
buf[i] = "";
}
/*
//-----------------------------------------------------------------------------------------------------------
//--------------------------b2ID-----------------------------
if(b2IDflag==0){
for (i=0;i<8;i++){
userialb2.write(uniqID[i]);
}
i=0;
//Serial.println();
if(userialb2.available()>15){//SEC header search
while(userialb2.available()>0){//buffer read
buf[i%2048]=userialb2.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize=i-1;//default SEC 17bytes
//Serial.printf("-------b2_bufsize=%d--------\n\r",bufsize);
for(i=0;i<bufsize-3;i++){
if (buf[i]==0xb5 && buf[i+1]==0x62 && buf[i+2]==0x27 && buf[i+3]==0x03){
//Serial.printf("******Start i=%d:b2_Id=",i);
// b2ID[0]=buf[i+10];b2ID[1]=buf[i+11]; b2ID[2]=buf[i+12]; b2ID[3]=buf[i+13]; b2ID[4]=buf[i+14];
for (j=0;j<5;j++){
b2ID[j]=buf[i+10+j];
//Serial.print(b2ID[j],HEX);
//Serial.print(",");
}
b2IDflag=1;
//Serial.printf("b2IDflag=%d\n\r",b2IDflag);
break;
//Serial.println();
}
}
}
}//b2IDflag end
for(i=0;i<2048;i++){
buf[i]="";
}
//-----------------------------------------------------------------------------------------------------------
//--------------------------b3ID-----------------------------
if(b3IDflag==0){
for (i=0;i<8;i++){
userialb3.write(uniqID[i]);
}
i=0;
//Serial.println();
if(userialb3.available()>15){//SEC header search
while(userialb3.available()>0){//buffer read
buf[i%2048]=userialb3.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize=i-1;//default SEC 17bytes
Serial.printf("-------b3_bufsize=%d--------\n\r",bufsize);
for(i=0;i<bufsize-3;i++){
if (buf[i]==0xb5 && buf[i+1]==0x62 && buf[i+2]==0x27 && buf[i+3]==0x03){
Serial.printf("start i=%d:b3_Id=",i);
// b3ID[0]=buf[i+10];b3ID[1]=buf[i+11]; b3ID[2]=buf[i+12]; b3ID[3]=buf[i+13]; b3ID[4]=buf[i+14];
for (j=0;j<5;j++){
b3ID[j]=buf[i+10+j];
Serial.print(b3ID[j],HEX);
Serial.print(",");
}
b3IDflag=1;
Serial.printf("b3IDflag=%d\n\r",b3IDflag);
break;
//Serial.println();
}
}
}
}//b3IDflag end
for(i=0;i<2048;i++){
buf[i]="";
}
*/
//-----------------------------------------------------------------------------------------------------------
}//navSEC end
//**********************************************************************************************
//**********************************************************************************************
void updateDeviceList() {
// Print out information about different devices.
for (uint8_t i = 0; i < CNT_DEVICES; i++) {
if (*drivers[i] != driver_active[i]) {
if (driver_active[i]) {
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
driver_active[i] = false;
} else {
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
driver_active[i] = true;
const uint8_t *psz = drivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = drivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = drivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
// If this is a new Serial device.
if (drivers[i] == &userialb0) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb0.begin(baud);
b0Open = 1;
}
}
if (drivers[i] == &userialb1) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb1.begin(baud);
b1Open = 1;
}
/*
if (drivers[i] == &userialb2) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb2.begin(baud);
b2Open = 1;
}
if (drivers[i] == &userialb3) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb3.begin(baud);
b3Open = 1;
}
*/
}
}
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
int PVTcnv(uint8_t d[100], long pvt[33]) {
//PVT header[0-6]
//0:itow[6-9]
pvt[0] = B2L(d[9], d[8], d[7], d[6]);
//Serial.printf("PVTcnv:itow=%d\n\r",pvt[0]);
//1:year[10-12]
pvt[1] = (long)d[10] + d[11] * 256;
//2:month[12]
pvt[2] = (long)d[12];
//3:day[13]
pvt[3] = (long)d[13];
//4:hour[14]
pvt[4] = (long)d[14];
//5:min[15]
pvt[5] = (long)d[15];
//6:sec[16]
pvt[6] = (long)d[16];
//7:valid[17]
pvt[7] = (long)d[17];
//8:tAcc[18-21]
pvt[8] = B2L(d[21], d[20], d[19], d[18]);
//9:nano[22-25]
pvt[9] = B2L(d[25], d[24], d[23], d[22]);
//10:fixType[26]
pvt[10] =(long) d[26];
//11:flags[27] This is Fix status 131
pvt[11] = (long)d[27];
//12:flags2[28]
pvt[12] = (long)d[28];
//13:numSV[29]
pvt[13] = (long)d[29];
//14:lon[30-33]
pvt[14] = B2L(d[33], d[32], d[31], d[30]);
//15:lat[34-37]
pvt[15] = B2L(d[37], d[36], d[35], d[34]);
//16:height[38-41]
pvt[16] = B2L(d[41], d[40], d[39], d[38]);
//17:hMSL[42-45]
pvt[17] = B2L(d[45], d[44], d[43], d[42]);
//18:hAcc[46-49]
pvt[18] = B2L(d[49], d[48], d[47], d[46]);
//19:vAcc[50-53]
pvt[19] = B2L(d[53], d[52], d[51], d[50]);
//20:velN[54-57]
pvt[20] = B2L(d[57], d[56], d[55], d[54]);
//21:velE[58-61]
pvt[21] = B2L(d[61], d[60], d[59], d[58]);
//22:velD[62-65]
pvt[22] = B2L(d[65], d[64], d[63], d[62]);
//23:gSpeed[66-69]
pvt[23] = B2L(d[69], d[68], d[67], d[66]);
//24:headMot[70-73]
pvt[24] = B2L(d[73], d[72], d[71], d[70]);
//25:sAcc[74-77]
pvt[25] = B2L(d[77], d[76], d[75], d[74]);
//26:headAcc[78-81]
pvt[26] = B2L(d[81], d[80], d[79], d[78]);
//27:pDOP[82-83]
pvt[27] = (long)d[82] + d[83] * 256;
//28:flags3[84]
pvt[28] = (long)d[84];
//29:reserved1[85]
pvt[29] = (long)d[85];
//30:headVeh[86-89]
pvt[30] = B2L(d[89], d[88], d[87], d[86]);
//31:magDec[90-91]
pvt[31] = (long)d[90] + d[91] * 256;
//32:magAcc[92-93]
pvt[32] = (long)d[92] + d[93] * 256;
}//PVTcnv() end
//--RELPOScnv---------------------------
int RELPOScnv(uint8_t d[72], long relpos[20]) {
//RELPOS header[0-5]
int s = 0;
//0:ver[6]
relpos[0] = (long)d[s + 6];
//1:reserved1[7]
relpos[1] =(long) d[s + 7];
//2:refStationId[8-9]
relpos[2] = (long)d[s + 8];
//3:itow[10-13]
relpos[3] = (long)B2L(d[s + 13], d[s + 12], d[s + 11], d[s + 10]);
//Serial.printf("RELPOScnv:itow=%d\n\r",relpos[3]);
//4:relposN[14-17]
relpos[4] = B2L(d[s + 17], d[s + 16], d[s + 15], d[s + 14]);
//5:relposE[18-21]
relpos[5] = B2L(d[s + 21], d[s + 20], d[s + 19], d[s + 18]);
//6:relposD[22-25]
relpos[6] = B2L(d[s + 25], d[s + 24], d[s + 23], d[s + 22]);
//7:relposLength[26-29]
relpos[7] = B2L(d[s + 29], d[s + 28], d[s + 27], d[s + 26]);
//8:relposHeading[30-33]
relpos[8] = B2L(d[s + 33], d[s + 32], d[s + 31], d[s + 30]);
//Serial.printf("relposHeading[8]=%d,[33]%x,[32]%x,[31]%x,[30]%x,\n\r",relpos[8],d[33],d[32],d[31],d[30]);
//9:reserved2[34]
relpos[9] = B2L(d[s + 37], d[s + 36], d[s + 35], d[s + 34]);
//10:relposHPN[35]
relpos[10] =(long)(d[s+ 38] & 127 - (d[s+38]) & 128);
//Serial.printf("HPN=%d,d[38]=%x,d[39]=%x,d[40]=%x,d[41]=%x,&127=%dx,&128=%d\n\r",relpos[10],d[38],d[39],d[40],d[41],d[38] && 127,d[38] && 128);
//11:relposHPE[36]
relpos[11] = (long)((d[s + 39] & 127)-(d[s+39] & 128));
//12:relposHPD[37]
relpos[12] = (long)((d[s + 40] & 127)-(d[s+40] & 128));
//13:relposHPLength[38]
relpos[13] = (long)((d[s + 41] & 127)-(d[s+41] & 128));
//14:accN[38-41]
relpos[14] = B2L(d[s + 41], d[s + 40], d[s + 39], d[s + 38]);
//15:accE[42-45]
relpos[15] = B2L(d[s + 45], d[s + 44], d[s + 43], d[s + 42]);
//16:accD[46-49]
relpos[16] = B2L(d[s + 49], d[s + 48], d[s + 47], d[s + 46]);
//17:accLength[50-53]
relpos[17] = B2L(d[s + 53], d[s + 52], d[s + 51], d[s + 50]);
//18:accHeading[54-57]
relpos[18] = B2L(d[s + 57], d[s + 56], d[s + 55], d[s + 54]);
//19:reserved[57-60]
relpos[19] = B2L(d[s + 60], d[s + 59], d[s + 58], d[s + 57]);
//20:flags[60-63]
relpos[20] = B2L(d[s + 63], d[s + 62], d[s + 61], d[s + 60]);
}
//+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++
long B2L(uint8_t b4 , uint8_t b3 , uint8_t b2 , uint8_t b1 ) {
//pc.printf("B2L IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80);
//pc.printf("B2L IN=b4&0x80=%d\n\r",b4 & 0x80);
long su;
if ((b4 & 0x80) && 0x80) { //最上位ビットたっていればマイナス
su = -(256 - (long)b1) + (255 - (long)b2) * 256 + (255 - (long)b3) * 65536 + (255 - (long)b4) * 256 * 256 * 256;
//pc.printf("B2L-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
}
else {
su = (long)b1 + (long)b2 * 256 + (long)b3 * 65536 + (long)b4 * 256 * 256 * 256;
//pc.printf("B2L+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
}
return su;
}
//=================================================================================
//+++++++++++++++i_to_char++++++++++++++++++++++++++++++++++++
// i=IntegerValueData,*d=Array pointer, n=Array start No
void i_to_char(int i, uint8_t *d, int n)
{
d[n] = i & 0x000000ff;
d[n + 1] = (i & 0x0000ff00) >> 8;
d[n + 2] = (i & 0x00ff0000) >> 16;
d[n + 3] = (i & 0xff000000) >> 24;
}
//++++++++++++++++String to CharArray Trans++++++++++++++++++++
void str2char(char c[], String dataS)
{
//String dataS;
//dataS="HELLO dataS";
int dataS_len = dataS.length() + 1;
char char_array[dataS_len];
dataS.toCharArray(c, dataS_len);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment