Created
May 26, 2022 07:07
-
-
Save dj1711572002/dbd4b903f55ad8fe32f0b2eacf1498ab to your computer and use it in GitHub Desktop.
ZED-F9P_MovingBase_IMU_Logger_Arduino_Teensy4.1_USBHOST
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//LSM9DS1 Library Sparkfun | |
#include <Wire.h> | |
#include <SPI.h> | |
#include <SparkFunLSM9DS1.h> | |
#include <Kalman.h>// | |
//Kalman | |
//x, y, zの順 | |
float acc[3]; | |
float accOffset[3]; | |
float gyro[3]; | |
float gyroOffset[3]; | |
float kalAngleX; | |
float kalAngleY; | |
float kalAngleZ; | |
Kalman kalmanX; | |
Kalman kalmanY; | |
Kalman kalmanZ; | |
long lastMs = 0; | |
long tick = 0; | |
////////////////////////// | |
// LSM9DS1 Library Init // | |
////////////////////////// | |
// Use the LSM9DS1 class to create an object. [imu] can be | |
// named anything, we'll refer to that throught the sketch. | |
LSM9DS1 imu; | |
LSM9DS1 imu2; | |
/////////////////////// | |
// Example I2C Setup // | |
/////////////////////// | |
// SDO_XM and SDO_G are both pulled high, so our addresses are: | |
#define LSM9DS1_M 0x1C //0x1E // Would be 0x1C if SDO_M is LOW | |
#define LSM9DS1_AG 0x6A // 0x6B // Would be 0x6A if SDO_AG is LOW | |
//////////////////////////// | |
// Sketch Output Settings // | |
//////////////////////////// | |
//#define PRINT_CALCULATED | |
#define PRINT_RAW | |
#define PRINT_SPEED 20//250 // 20 ms between prints | |
static unsigned long lastPrint = 0; // Keep track of print time | |
// STA22_USBHOSt_Dual_Buf172_sdFAT_rev098 | |
//RTKMovingBaseモードでのデータログシステムPgm | |
// Base:F9P NAV-PVT出力 | |
//Rover:F9H NAV-RELPOSNED出力 | |
//USB HOST機能:USB HUB1,2とuserialb0-3の4個接続 | |
//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; | |
long length0,length1; | |
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 rNm[10],rEm[10],rDm[10],rLm[10],mmH[10];//MovingBase average initial sample | |
double relN0,relE0,relD0,relL0,mH0;//初回静止時の基準位置 | |
volatile double relN,relE,relD,relL;//基準局からの現在位置 | |
volatile double relNm,relEm,relDm,relLm,mHead;//MovingBase RELPOS値 | |
double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN | |
double rNstd,rEstd,rDstd,rLstd,mHstd;//標準偏差 rNdvNの平方根 | |
double rNmstd,rEmstd,rDmstd,rLmstd,mmHstd;//MovingBase標準偏差 rNdvNの平方根 | |
double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//基準位置平均値 | |
double rNmaveN,rEmaveN,rDmaveN,rLmaveN,mmHaveN;//MovingBase位置平均値 | |
double rNaveN1,rEaveN1,rDaveN1,rLaveN1,mHaveN1;//基準位置平均値1 | |
double rNmaveN1,rEmaveN1,rDmaveN1,rLmaveN1,mmHaveN1;//MovingBase位置平均値1 | |
double rNsum,rEsum,rDsum,rLsum,mHsum;//基準位置総和 | |
double rNmsum,rEmsum,rDmsum,rLmsum,mmHsum;//MovingBase位置総和 | |
double rNsgmN,rEsgmN,rDsgmN,rLsgmN,mHsfmN;//基準位置のstandard deviation N | |
double rNmsgmN,rEmsgmN,rDmsgmN,rLmsgmN,mmHsfmN;//Movingbaseのstandard deviation N | |
double rNsgmN1,rEsgmN1,rDsgmN1,rLsgmN1,mHsgmN1;//基準位置のstandard deviation N+1 | |
double rNmsgmN1,rEmsgmN1,rDmsgmN1,rLmsgmN1,mmHsgmN1;//MovingBaseのstandard deviation N+1 | |
double rNdvsum,rEdvsum,rDdvsum,rLdvsum,mHdvsum;//基準位置のdeviation sum | |
double rNmdvsum,rEmdvsum,rDmdvsum,rLmdvsum,mmHdvsum;//MovingBaseのdeviation sum | |
double rNdvN,rEdvN,rDdvN,rLdvN,mHdvN;//基準位置のdeviation N | |
double rNmdvN,rEmdvN,rDmdvN,rLmdvN,mmHdvN;//MovingBaseのdeviation N | |
double rNdvN1,rEdvN1,rDdvN1,rLdvN1,mHdvN1;//基準位置のdeviation N+1 | |
double rNmdvN1,rEmdvN1,rDmdvN1,rLmdvN1,mmHdvN1;//MovingBaseのdeviation N+1 | |
int avecount; | |
int mbcount; | |
uint8_t califlag; | |
//imu | |
String imus[100]; | |
String imusum=""; | |
//-----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;//UBX_base pvt relposned | |
FsFile myFileR;//UBX_rover pvt relposned | |
FsFile myFileT;//TXT_rtk status statics | |
FsFile myFileI;//TXT_imu & pvt | |
int epNosd,epNosd_1; | |
int n;//counter | |
int i, j, k,l,m; | |
int starttime; | |
int itow, itow_1, itowR; | |
int hacc, accL,numsv; | |
uint8_t flags,mflags; | |
uint8_t flags0,flags0_1,flags1,flags1_1; | |
uint8_t fix0,fix1; | |
double pdop; | |
int JST_day; | |
//timestamp | |
char fnameB[30];//UBX Binary File name | |
char fnameR[30];//Rover Binary | |
char fnameV[30];//UBX Value File name NAV-PVT+NAV-RELPOSNED | |
char fnameI[30];//imu TXT | |
char monC[200];//Monitor Text data Save | |
char imuchr[200]; | |
uint8_t Sflag = 0; | |
char sc; | |
int debugP = 1; | |
//const int chipSelect =BUILTIN_SDCARD;// 10; | |
//String dataString,dataString1; | |
int kaisu; | |
int tubx0,tubx1,ttxt0,ttxt1; | |
//-----USB HOST | |
#include "USBHost_t36.h" | |
#define USBBAUD 460800//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 baseNo;//USb 0 or 1 | |
uint8_t Buf172[172]; | |
uint8_t dBuf0[172], dBuf1[172]; | |
uint8_t PVTdata[172];//userialb0 PVT | |
uint8_t RELPOSdata[172];//userialb0 RELPOS | |
uint8_t PVT1data[172];//userialb1 PVT | |
uint8_t RELPOS1data[172];//userialb1 RELPOS | |
long PVTval[33]; | |
long PVT1val[33]; | |
long RELPOSval[21]; | |
long RELPOS1val[21]; | |
//USB0-USB1=> base-rover | |
long PVTBval[33]; | |
long PVTRval[33]; | |
long RELPOSBval[21]; | |
long RELPOSRval[21]; | |
uint8_t RELPOSBdata[172];//base line RELPOS | |
volatile uint8_t RELPOS1Bdata[172];//base line RELPOS | |
uint8_t epflag0,epflag1; | |
uint8_t fdata[4]; | |
int Num0, Num0_1, Num1, Num1_1; | |
int flag0, flag1; | |
char keyin; | |
//int time0, time0_1, time1, time1_1; | |
int trcv0,trcv1,tsdbin0,tsdbin1,tsdtxt0,tsdtxt1,timu0,timu1; | |
//==============time pulse interrupt================ | |
volatile int tptime,tptime0; | |
volatile int tpitow0; | |
volatile uint8_t tpflag; | |
volatile int tpcount; | |
char c; | |
//IMU | |
int dcount; | |
//================================================================= | |
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; | |
dcount=0; | |
//TIMING Check Pin | |
pinMode(41,OUTPUT);//Userialb0 reading High | |
pinMode(39,OUTPUT);//Userialb1 reading High | |
pinMode(40,OUTPUT);//SD Binary File Writing High | |
//===================IMU LSM9DS1 set===================================== | |
Wire.setSCL(19);//SDA library default Teensy arduino compati | |
Wire.setSDA(18);//SCL library default Teensy arduino compati | |
Wire.begin(); | |
if (imu.begin() == false) // with no arguments, this uses default addresses (AG:0x6B, M:0x1E) and i2c port (Wire). | |
{ | |
Serial.println("Failed to communicate with LSM9DS1."); | |
Serial.println("Double-check wiring."); | |
Serial.println("Default settings in this sketch will " \ | |
"work for an out of the box LSM9DS1 " \ | |
"Breakout, but may need to be modified " \ | |
"if the board jumpers are."); | |
while (1); | |
} | |
//==================time pule interrupt pin========================== | |
//pinMode(38,INPUT_PULLUP);// | |
pinMode(38,INPUT_PULLDOWN);// | |
attachInterrupt(digitalPinToInterrupt(38),timepulse,RISING); | |
//attachInterrupt(digitalPinToInterrupt(38),timepulse,FALLING); | |
//=================Kalman sert up==================================== | |
Serial.println("-----------------Wait a while IMU Kalman Filter Calibrating------------------"); | |
calibration();//IMU Kalman Calibration | |
}//set up | |
//----------------IMU Kalman functions ------------------- | |
void readGyroAcc(){//gyro &acc read | |
//gx gy gz | |
if ( imu.gyroAvailable() ) | |
{ | |
imu.readGyro(); | |
gyro[0]=imu.calcGyro(imu.gx); | |
gyro[1]=imu.calcGyro(imu.gy); | |
gyro[2]=imu.calcGyro(imu.gz); | |
} | |
//accX accY accZ | |
if ( imu.accelAvailable() ) | |
{ | |
imu.readAccel(); | |
acc[0]=imu.calcAccel(imu.ax); | |
acc[1]=imu.calcAccel(imu.ay); | |
acc[2]=imu.calcAccel(imu.az); | |
} | |
//Serial.printf("[readGyro]gyro=%3.2f,%3.2f,%3.2f,acc=%2.2f,%2.2f,%2.2f\n\r",gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); | |
} | |
//---CalResult apply--------- | |
void applyCalibration(){ | |
gyro[0] -= gyroOffset[0]; | |
gyro[1] -= gyroOffset[1]; | |
gyro[2] -= gyroOffset[2]; | |
acc[0] -= accOffset[0]; | |
acc[1] -= accOffset[1]; | |
acc[2] -= accOffset[2]; | |
} | |
//-----------roll pitch yaw----- | |
float getRoll(){ | |
return atan2(acc[1], acc[2]) * RAD_TO_DEG; | |
} | |
float getPitch(){ | |
return atan(-acc[0] / sqrt(acc[1]*acc[1] + acc[2]*acc[2])) * RAD_TO_DEG; | |
} | |
float getYaw(){ | |
return atan(acc[1] / acc[0]) * RAD_TO_DEG; | |
} | |
//--------------Kalman cal-------------- | |
void calibration(){ | |
//補正値を求める | |
float gyroSum[3]; | |
float accSum[3]; | |
int COUNTER = 500; | |
for(int i = 0; i < COUNTER; i++){ | |
readGyroAcc(); | |
gyroSum[0] += gyro[0]; | |
gyroSum[1] += gyro[1]; | |
gyroSum[2] += gyro[2]; | |
accSum[0] += acc[0]; | |
accSum[1] += acc[1]; | |
accSum[2] += acc[2]; | |
delay(10); | |
} | |
gyroOffset[0] = gyroSum[0]/COUNTER; | |
gyroOffset[1] = gyroSum[1]/COUNTER; | |
gyroOffset[2] = gyroSum[2]/COUNTER; | |
accOffset[0] = accSum[0]/COUNTER; | |
accOffset[1] = accSum[1]/COUNTER; | |
accOffset[2] = accSum[2]/COUNTER - 1.0;//重力加速度1G | |
Serial.println(" X Y Z\n"); | |
Serial.printf("%7.2f %7.2f %7.2f\n", gyroOffset[0], gyroOffset[1], gyroOffset[2]); | |
Serial.printf("%7.2f %7.2f %7.2f\n", accOffset[0]*1000, accOffset[1]*1000, accOffset[2]*1000); | |
readGyroAcc(); | |
kalmanX.setAngle(getRoll()); | |
kalmanY.setAngle(getPitch()); | |
kalAngleZ = 0; | |
lastMs = micros(); | |
} | |
//=======TimePulse function========================================================= | |
void timepulse(){ | |
tptime=millis(); | |
tpflag=1; | |
tpcount++; | |
// Serial.println("*"); | |
} | |
//==========LOOP ======================================================================== | |
//======================================================================================= | |
void loop() { | |
if(tpflag==1 ){ | |
if(tpcount==1){ | |
tptime0=tptime; | |
tpitow0=itow0+125; | |
Serial.printf(">>> >>>>>>>>>>>>>>>>>>>>Initial TimePulse Interrupted tptime0=%d, itow0=%d\n\r",tptime0,tpitow0); | |
} | |
else{ | |
Serial.printf(">>>> >>>>>>>>>>>>>>>>>>> TimePulse Interrupted tptime=%d, itow0=%d\n\r",tptime,itow0); | |
} | |
tpflag=0; | |
} | |
myusb.Task(); | |
updateDeviceList(); | |
//Serial.print(b0Open); | |
//} | |
//===useralb0 read========================================================= | |
//@@@@@@@@@@@@@重要 USBバッファが予定数になった順に処理していくと破綻しない@@@@@@@@@@@@@@@@@@@@@@@@@ | |
// if(userialb0.available()>170 || userialb1.available()>170 ){ //Userib0とuserialb1のどちらかが満杯になったら先に処理する同時なら同時処理する。 | |
// if(userialb0.available()>0 || userialb1.available()>0 ){ //Userib0とuserialb1のどちらかが満杯になったら先に処理する同時なら同時処理する。 | |
if(userialb0.available()>171 && userialb1.available()>171 ){ //バッファが満杯になるまで読まないで、一挙によむ | |
trcv0=millis();//debug | |
digitalWrite(41, HIGH); // | |
i=0; | |
while(i<172){ | |
while(userialb0.available()){ | |
dBuf0[i]=userialb0.read(); | |
//Serial.print(dBuf0[i],HEX); | |
i++; | |
} | |
} | |
delay(3); | |
//Serial.println(); | |
j=0; | |
while(j<172){ | |
while(userialb1.available()){ | |
dBuf1[j]=userialb1.read(); | |
//Serial.print(dBuf1[j],HEX); | |
j++; | |
} | |
} | |
digitalWrite(41, LOW); // | |
// delay(3); | |
//Serial.println(); | |
}// | |
//@@@@@@@@@@@@@@@@@@@@@@ READ END @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x07 ){//userialb0 PVT | |
flags0=dBuf0[27]; | |
if(flags0==131 && fix0==0){ | |
miscount0=0; | |
fix0=1; | |
} | |
itow0_1=itow0; | |
itow0 = B2L(dBuf0[9], dBuf0[8], dBuf0[7], dBuf0[6]); //dBuf0 iTow Calc | |
if(itow0-itow0_1>127){ | |
miscount0++; | |
} | |
else{ | |
ep0No++; | |
} | |
if(itow0>itow0_1 && flags0>0){ //<131 | |
Serial.printf("[,%d,]U_0_PVT_i=,%d,[,%d,],mis0=,%d,(,%x,%x,%x,%x),%d,flags0=,%d\n\r",ep0No,i,itow0,miscount0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3],millis(),flags0); | |
} | |
} | |
else if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x3C && itow0>itow0_1){ | |
itow0_1=itow1; | |
itow0 = B2L(dBuf0[13], dBuf0[12],dBuf0[11], dBuf0[10]); //dBuf1 iTow Calc | |
if(itow0-itow0_1>127){ | |
miscount0++; | |
} | |
else{ | |
ep0No++; | |
} | |
if(itow0>itow0_1 && flags0>0){//<131 ){ | |
Serial.printf("[,%d,]U_0_RELPOS_i=,%d,[,%d,],mis0=,%d,(%x,%x,%x,%x),%d\n\r",ep0No,i,itow0,miscount0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3],millis()); | |
} | |
}// if RELPOS head | |
else{ | |
//Serial.printf("U_0_NG_i=,%d,[,%d,],%x,%x,%x,%x\n\r",i,itow0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3]); | |
} | |
//length0 = B2L(RELPOSdata[29], RELPOSdata[28], RELPOSdata[27], RELPOSdata[26]); | |
//Serial.printf("========length0=%d\n\r",length0); | |
// } | |
//===useralb1 read======================================================== | |
//delay(3); | |
if(dBuf1[0]==0xb5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07){//userialb1 PVT | |
flags1=dBuf1[27]; | |
if(flags1==131 && fix1==0){ | |
miscount1=0; | |
fix1=1; | |
} | |
itow1_1=itow1; | |
itow1 = B2L(dBuf1[9], dBuf1[8], dBuf1[7], dBuf1[6]); //PVT1 iTow Calc | |
if(itow1-itow1_1>127){ | |
miscount1++; | |
} | |
else{ | |
ep1No++; | |
} | |
if(itow1>itow1_1 && flags1>0){//<131){ | |
Serial.printf("[,%d,]U_1_PVT_j=,%d,[,%d,],mis1=,%d,(,%x,%x,%x,%x),%d,flags1=,%d\n\r",ep1No,j,itow1,miscount1,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],millis(),flags1); | |
} | |
}//if PVT head | |
else if(dBuf1[0]==0xb5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x3C ){ | |
itow1_1=itow1; | |
itow1 = B2L(dBuf1[13], dBuf1[12],dBuf1[11], dBuf1[10]); //dBuf1 iTow Calc | |
if(itow1-itow1_1>127){ | |
miscount1++; | |
} | |
else{ | |
ep1No++; | |
} | |
if(itow1>itow1_1 && flags1>0){//<131){ | |
Serial.printf("[,%d,]U_1_RELPOS_j=,%d,[,%d,],mis1=,%d,(%x,%x,%x,%x),%d\n\r",ep1No,j,itow1,miscount1,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],millis()); | |
} | |
}// if RELPOS head | |
else{ | |
//Serial.printf("U_1_NG_i=%d,[%d],%x,%x,%x,%x\n\r",i,itow1,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]); | |
epflag1=0;//繰り返し表示防止 | |
} | |
//length1 = B2L(RELPOS1data[29], RELPOS1data[28], RELPOS1data[27], RELPOS1data[26]); | |
//Serial.printf("========length1=%d\n\r",length1); | |
// | |
//digitalWrite(39, LOW); // | |
trcv1=millis();//debug | |
//===================================================================================================== | |
//================================1周期処理===============================================================- | |
//==================================================================================================== | |
//if(itow0-itow0_1==125 || itow1-itow1_1==125){ | |
if(itow0-itow0_1>=123 ){//&& itow0==itow1){ //8Hz=125msecのばらつき123msec以上itow差で周期完了------------------------ | |
digitalWrite(40, HIGH); // Epoch process IN | |
//if(itow0==10){ //stop | |
itow0_1=itow0;//重複プリント防止 | |
itow1_1=itow1; | |
//Serial.printf("<userial.read Finished time= %d>\n\r",millis()); | |
//================================================================= | |
// if(itow0>itow0_1){ | |
//===============dBuf=>PVTdata,RELPOSdata 代入========================== | |
//Serial.print("USB0data:"); | |
for(i=0;i<100;i++){//USB0 PVT | |
PVTdata[i]=dBuf0[i]; | |
//Serial.print(PVTdata[i],HEX); | |
} | |
//Serial.print("-"); | |
for(i=0;i<72;i++){ | |
RELPOSdata[i]=dBuf0[i+100]; | |
//Serial.print(RELPOSdata[i],HEX); | |
} | |
//Serial.println(); | |
//Serial.print("USB1data:"); | |
for(i=0;i<100;i++){//USB1 | |
PVT1data[i]=dBuf1[i]; | |
//Serial.print(PVT1data[i],HEX); | |
} | |
//Serial.print("-"); | |
for(i=0;i<72;i++){ | |
RELPOS1data[i]=dBuf1[i+100]; | |
//Serial.print(RELPOS1data[i],HEX); | |
} | |
//Serial.println(); | |
//} | |
// if(itow0==10){ //stop | |
//================================================================ | |
//================================================================ | |
//*********************************************************************************** | |
//**********************125msec Process********************************************** | |
//*********************************************************************************** | |
sa0=itow0-itow0_1; | |
sa1=itow1-itow1_1; | |
//---------Base Rover 判定 代入------------------------------- | |
int result=PVTcnv(PVTdata,PVTval); | |
//Serial.printf("USB0-PVTcnv=%d,",result); | |
int result1=PVTcnv(PVT1data,PVT1val); | |
//Serial.printf("USB1-PVTcnv=%d,",result1); | |
int resultR=RELPOScnv(RELPOSdata,RELPOSval); | |
//Serial.printf("USB0-RELPOScnv=%d,",resultR); | |
int resultR1=RELPOScnv(RELPOS1data,RELPOS1val); | |
//Serial.printf("USB1-RELPOScnv=%d\n\r",resultR1); | |
// Serial.printf("----USB0 Length=RELPOSval[7]=%4.3f,USB1 Length=RELPOS1val[7]=%4.3f\n\r",(float)RELPOSval[7]/100,(float)RELPOS1val[7]/100); | |
if(RELPOSval[7]>RELPOS1val[7]){//USB0=>Base | |
baseNo=0; | |
for(i=0;i<33;i++){//PVTB | |
PVTBval[i]=PVTval[i]; | |
PVTRval[i]=PVT1val[i]; | |
} | |
for(i=0;i<21;i++){ | |
RELPOSBval[i]=RELPOSval[i]; | |
RELPOSRval[i]=RELPOS1val[i]; | |
} | |
} | |
else{//USB1=>Base | |
baseNo=1; | |
for(i=0;i<33;i++){//PVTB | |
PVTBval[i]=PVT1val[i]; | |
PVTRval[i]=PVTval[i]; | |
} | |
for(i=0;i<21;i++){ | |
RELPOSBval[i]=RELPOS1val[i]; | |
RELPOSRval[i]=RELPOSval[i]; | |
} | |
} | |
//--------------------------------------------------------------- | |
itow=PVTBval[0]; | |
flags=PVTBval[11]; | |
hacc=PVTBval[18]; | |
pdop=PVTBval[27]/100;//((double)PVTdata[82] +PVTdata[83] * 256)/100; | |
numsv=PVTBval[13];//(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 Rover Length RealTime averaging---------------------------------------------------- | |
//---------MovingBase Rover RELPOSNED Analysys--------------------------- | |
mHead=(double)RELPOSRval[8]*0.00001;//Heading deg x10-5 | |
mflags=RELPOSRval[20]; | |
//Serial.printf("mHead=%5.5f,RELPOSdata[8]=%d,mflags=%d\n\r",mHead,RELPOSval[8],mflags); | |
relNm=(double)RELPOSRval[4]+(double)RELPOSRval[10]*0.01; | |
relEm=(double)RELPOSRval[5]+(double)RELPOSRval[11]*0.01; | |
relDm=(double)RELPOSRval[6]+(double)RELPOSRval[12]*0.01; | |
relLm=(double)RELPOSRval[7]+(double)RELPOSRval[13]*0.01;//固定されているので一定値なのでLengthのばらつきを統計処理 | |
//-------------------------------------------------------------------------------------------------------- | |
//****************flags0 flags1 合算************************************************************************ | |
if (flags0<flags1){ | |
flags=flags0; | |
} | |
else{ | |
flags=flags1; | |
} | |
//*****************FIX 判定********************************************************* | |
if(flags0==1 && stflag==0){ | |
RTKstart=millis(); | |
stflag=1; | |
} | |
else if(flags0==131 && flags1==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後のカウント値 | |
} | |
miscount=max(miscount0,miscount1); | |
//fix率計算 allepoch,misepoch;/ | |
if(fixed==1 && epNo>fixepoNo){ | |
allepoch=epNo-fixepoNo; | |
if (flags<131){ | |
misepoch++; | |
} | |
fixpercent=((double)(allepoch-misepoch)/allepoch)*100; | |
} | |
//--------------------------------------------------------------------- | |
//--------------------------------------------------------------------- | |
//CALIBRATION MODE================BaseLine Averaging Standard Deviation ================================================= | |
//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; | |
rLm[avecount]=relLm; //MovingBase length | |
rLmsum=rLmsum+relLm; //MovingBase length sum | |
//mH[avecount]=mHead; | |
//mHsum=mHsum+mHead; | |
} | |
if(avecount==10){//10count ave stdev | |
rNaveN=rNsum/10; | |
rEaveN=rEsum/10; | |
rDaveN=rDsum/10; | |
rLaveN=rLsum/10; | |
rLmaveN=rLmsum/10; //MovingBase Length ave | |
//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); | |
rLmdvsum=rLmdvsum+pow((rLm[cn]-rLmaveN),2);//MovingBse length dev | |
//mHdvsum=mHdvsum+pow((mH[cn]-mHaveN),2); | |
} | |
rNdvN=rNdvsum/10; | |
rEdvN=rEdvsum/10; | |
rDdvN=rDdvsum/10; | |
rLdvN=rLdvsum/10; | |
rLmdvN=rLmdvsum/10;//MovingBase length devN | |
//mHdvN=mHdvsum/10; | |
}//if 10count averaging stdev end | |
//---N1 10-500count cal---- | |
if(avecount >10 && avecount<301){ | |
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); | |
rLmaveN1=((avecount-1)*rLmaveN+relLm)/(avecount);//MovingBase length aveN1 | |
//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); | |
rLmdvN1=((avecount-1)*(rLmdvN+pow(rLmaveN,2))+pow(relLm,2))/avecount-pow(rLmaveN1,2);//MovingBase length dvN1 | |
//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,rLmav=%4.2f,rLmdv1=%4.2f\n\r",flags,hacc,avecount,rNaveN1,rNdvN1,rEaveN1,rEdvN1,rDaveN1,rDdvN1,rLaveN1,rLdvN1,rLmaveN1,rLmaveN1); | |
//Serial.printf("AVE=%d\n\r",avecount); | |
//---change next step--- | |
rNaveN=rNaveN1; | |
rEaveN=rEaveN1; | |
rDaveN=rDaveN1; | |
rLaveN=rLaveN1; | |
rLmaveN=rLmaveN1;//MovingBase length aveN | |
//mHaveN=mHaveN1; | |
rNdvN=rNdvN1; | |
rEdvN=rEdvN1; | |
rDdvN=rDdvN1; | |
rLdvN=rLdvN1; | |
rLmdvN=rLmdvN1;//MovingBase length dvN | |
//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); | |
rLmstd=sqrt(rLmdvN);//MovingBase length std | |
//mHstd=sqrt(mHdvN); | |
//Serial.printf("MON:relLm=%4.3f,rLmaveN1=%4.3f,rLmdvN1=%4.3f\n\r",relLm,rLmaveN1,rLmdvN1); | |
if(avecount>=300){//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,rLmav=%4.2f,rLmdv=%4.2f\n\r",avecount,rNaveN,rNstd,rEaveN,rEstd,rDaveN,rDstd,rLaveN,rLstd,mHead,rLmaveN,rLmdvN); | |
Serial.printf("avcnt=%d,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f,rLmav=%4.2f,rLmdv=%4.2f\n\r",avecount,rLaveN,rLstd,mHead,rLmaveN,rLmdvN); | |
//SD wrinting averaging result text | |
//rNav,rEav,rDqv,rLav,rNdv,rEdv,rDdv,rLdv, | |
String stc="rNav,rEav,rDqv,rLav,rNdv,rEdv,rDdv,rLdv,rLmav,rLmdv"; | |
sprintf(monC,"%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n\r",rNaveN,rEaveN,rDaveN,rLaveN,rNdvN,rEdvN,rDdvN,rLdvN,rLmaveN,rLmdvN); | |
String stc1="epNo,itow,flags,numSV,hacc,fixtime,fixpercent,miscount0,miscount1,pDop,rrN,rrE,rrD,mHead,relN,relE,relD,relL"; | |
myFileT = sd.open(fnameV, FILE_WRITE); | |
myFileT.println(stc); | |
myFileT.write(monC,strlen(monC)); | |
myFileT.println(stc1); | |
myFileT.close(); //毎回クローズして万一のアクシデントに備える | |
delay(8); | |
//reset counter | |
avecount=0;//reset counter | |
califlag=1;//averaging Calculation finished | |
//Averaging sum RESET | |
rNsum=0; | |
rEsum=0; | |
rDsum=0; | |
rLsum=0; | |
rLmsum=0;//MovingBase length sum reset | |
rNdvsum=0; | |
rEdvsum=0; | |
rDdvsum=0; | |
rLdvsum=0; | |
rLmdvsum=0;//MovingBase length dv sum reset | |
}//Averaging End************************************************************************************** | |
//==========SD WRITING FileName set========================================================================== | |
//---FIlename()--- | |
//if(Sflag==0 && millis()-starttime>20000){//startから20秒後にSDログ開始 | |
if(califlag==1 && Sflag==0 ){//Calibration Fnished SD LOG START | |
Filename(); | |
Sflag=1;//File Log OK | |
sc='*'; | |
myFile = sd.open(fnameB, FILE_WRITE); | |
myFileR = sd.open(fnameR, FILE_WRITE); | |
myFileT = sd.open(fnameV, FILE_WRITE); | |
myFileI= sd.open(fnameI, FILE_WRITE); | |
epNosd=epNo; | |
} | |
//---Binary File SD WRITE--- | |
if(Sflag==1){ | |
tsdbin0=millis();//debug | |
//Serial.printf("Bin:Sflag=%d,epNo=%d,mils=%d\n\r",Sflag,epNo,millis()); | |
//-FILE OPEN- | |
//tubx0=millis(); | |
if(baseNo=0){//baseNo=0 USB0先-------------- | |
//Open u0 as Base | |
digitalWrite(39, HIGH); // | |
//myFile = sd.open(fnameB, FILE_WRITE); | |
if (myFile) { //myFileチェックは、書き込み直前で行う。 | |
for(i=0;i<100;i++) { | |
myFile.write(PVTdata[i]); | |
} | |
for(i=0;i<72;i++) { | |
myFile.write(RELPOSdata[i]); | |
} | |
}//fnameB end | |
//delay(8); | |
//if( epNo-epNosd>480 ){ | |
// myFile.close(); | |
// } | |
//Open u1 as Rover | |
//myFileR = sd.open(fnameR, FILE_WRITE); | |
if (myFileR) { //myFileチェックは、書き込み直前で行う。 | |
for(i=0;i<100;i++) { | |
myFileR.write(PVT1data[i]); | |
} | |
for(i=0;i<72;i++) { | |
myFileR.write(RELPOS1data[i]); | |
} | |
}// if my file | |
}//baseNo=0 end | |
//----------------------------------------- | |
else {//baseNo=1 USB1先 | |
//Open u0 as Base | |
digitalWrite(39, HIGH); // | |
//myFile = sd.open(fnameB, FILE_WRITE); | |
if (myFile) { //myFileチェックは、書き込み直前で行う。 | |
for(i=0;i<100;i++) { | |
myFile.write(PVT1data[i]); | |
} | |
for(i=0;i<72;i++) { | |
myFile.write(RELPOS1data[i]); | |
} | |
}//if myFile end | |
//digitalWrite(39, LOW); // | |
//delay(8); | |
//myFile.close(); | |
// myFileR = sd.open(fnameR, FILE_WRITE); | |
if (myFileR) { //myFileチェックは、書き込み直前で行う。 | |
for(i=0;i<100;i++) { | |
myFileR.write(PVTdata[i]); | |
} | |
for(i=0;i<72;i++) { | |
myFileR.write(RELPOSdata[i]); | |
} | |
}//my file end | |
//delay(8); | |
//myFileR.close(); | |
}//else end | |
//delay(8);//**********重要OPEN-CLOSE時間をあける********** | |
//myFile.close(); //毎回クローズして万一のアクシデントに備える | |
//tubx1=millis(); | |
//Serial.printf("======tubx0=%d,tubx1=%d,Sdtime=%d\n\r",tubx0,tubx1,tubx1-tubx0); | |
tsdbin1=millis();//debug | |
if( epNo-epNosd>480 ){ | |
Serial.printf("*******************************SD Close&Open epNo=%d,epNosd=%d\n\r",epNo,epNosd); | |
epNosd=epNo; | |
myFile.close(); | |
//delay(8); | |
myFileR.close(); | |
//delay(8); | |
myFileT.close(); | |
//delay(8); | |
myFileI.close(); | |
delay(8); | |
myFile = sd.open(fnameB, FILE_WRITE); | |
//delay(8); | |
myFileR = sd.open(fnameR, FILE_WRITE); | |
//delay(8); | |
myFileT = sd.open(fnameV, FILE_WRITE); | |
//delay(8); | |
myFileI = sd.open(fnameI, FILE_WRITE); | |
delay(8); | |
} | |
}//Sflag==1 end | |
epNo++; | |
int itow0cut=itow0%10000; | |
int itow1cut=itow1%10000; | |
String HMS0=itowToHMS(itow0); | |
int sa=abs(itow0-itow1); | |
//Serial.printf("epNo=%d,:it0=%4d,it1=%4d,flg=%d,acc=%d,pdop=%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\n\r",epNo,itow0cut,itow1cut,flags,hacc,pdop,numsv,fixpercent,fixtime,miscount0,miscount1,rrN,rrE,rrD,rrL,rLaveN,rLstd); | |
//Serial.printf("mHead=%3.5f,relNm=%3.3f,relEm=%3.3f,relDm=%3.3f,relLm=%3.3f,rLmav=%3.3f,rLmdv=%3.3f,%c,%d,sa=%d\n\r",mHead,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1,sc,avecount,sa); | |
//delay(8);//**********重要OPEN-CLOSE時間をあける********** | |
//******************** | |
//************** | |
//Serial.printf("epNo=%d\n\r",epNo); | |
Serial.printf("itow=%d,epNo=%d,flags=%d,fixtime=%d,%c,%d,sa=%d\n\r",itow,epNo,flags,fixtime,sc,avecount,sa); | |
// 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; | |
//********************************************************************************* | |
//******************MONITOR Data Sending SD print******************** | |
//******************************************************************************** | |
//digitalWrite(39, HIGH); // | |
//*********MovingBase Length ave dev RealTime cal******************************** | |
if(fixflag==1 && allepoch>0){ | |
rLmaveN1=((allepoch-1)*rLmaveN+relLm)/(allepoch);//MovingBase length realTime ave | |
rLmdvN1=((allepoch-1)*(rLmdvN+pow(rLmaveN,2))+pow(relLm,2))/allepoch-pow(rLmaveN1,2);//MovingBase length dviatiion | |
//Serial.printf("*******allepoch=%d,MB length:relLm=%3.4f,rLmaveN1=%3.4f, rLmdvN1=%3.4f,ini10=[%3.4f,%4.3f]\n\r",allepoch, relLm,rLmaveN1, rLmdvN1,rLmaveN,rLmdvN); | |
} | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^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=flags;//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]8byte 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)miscount;//[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 | |
//現在相対座標値 long 4Byte | |
//rrN | |
uint8_t llrelN=(uint8_t)((int)(rrN*100) & 0xFF);//[18]0.00*100で整数 | |
Serial3.write(llrelN);//18 | |
uint8_t lrelN=(uint8_t)(((int)(rrN*100) & 0xFF00)>>8);//[19]0.00*100で整数 | |
Serial3.write(lrelN);//19 | |
uint8_t urelN=(uint8_t)(((int)(rrN*100) & 0xFF0000)>>16);//[20]0.00*100で整数 | |
Serial3.write(urelN);//20 | |
uint8_t uurelN=(uint8_t)(((int)(rrN*100) & 0xFF000000)>>24);//[21]0.00*100で整数 | |
Serial3.write(uurelN);//21 | |
//rrE | |
uint8_t llrelE=(uint8_t)((int)(rrE*100) & 0xFF);//[22]0.00*100で整数 | |
Serial3.write(llrelE);//22 | |
uint8_t lrelE=(uint8_t)(((int)(rrE*100) & 0xFF00)>>8);//[23]0.00*100で整数 | |
Serial3.write(lrelE);//23 | |
uint8_t urelE=(uint8_t)(((int)((rrE*100)) &0xFF0000)>>16);//[24]0.00*100で整数 | |
Serial3.write(urelE);//24 | |
uint8_t uurelE=(uint8_t)(((int)((rrE*100)) &0xFF000000)>>24);//[25]0.00*100で整数 | |
Serial3.write(uurelE);//25 | |
//rrD | |
uint8_t llrelD=(uint8_t)((int)(rrD*100)& 0xFF);//[26]0.00*100で整数 | |
Serial3.write(llrelD);//26 | |
uint8_t lrelD=(uint8_t)(((int)(rrD*100) & 0xFF00)>>8);//[27]0.00*100で整数 | |
Serial3.write(lrelD);//27 | |
uint8_t urelD=(uint8_t)(((int)(rrD*100) & 0xFF0000)>>16);//[28]0.00*100で整数 | |
Serial3.write(urelD);//28 | |
uint8_t uurelD=(uint8_t)(((int)(rrD*100) & 0xFF000000)>>24);//[29]0.00*100で整数 | |
Serial3.write(uurelD);//29 | |
//movingBase Head | |
uint8_t llHead=(uint8_t)(RELPOSRval[8]&0xFF);//[30] head | |
Serial3.write(llHead);//30 | |
uint8_t lHead=(uint8_t)((RELPOSRval[8]&0xFF00)>>8);//[31] head | |
Serial3.write(lHead);//31 | |
uint8_t uHead=(uint8_t)((RELPOSRval[8]&0xFF0000)>>16);//[32] head | |
Serial3.write(uHead);//32 | |
uint8_t uuHead=(uint8_t)((RELPOSRval[8]&0xFF000000)>>24);//[33] head | |
Serial3.write(uuHead);//33 | |
//-------MovingBase Len av dv----------------------------------- | |
//-------MB RELPOSNED monitor----------------------- | |
uint8_t llrNm=(uint8_t)(((int)(relNm*1000) & 0xFF));//[34] | |
Serial3.write(llrNm);//34 | |
uint8_t lrNm=(uint8_t)(((int)(relNm*1000) & 0xFF00)>>8);//[35] | |
Serial3.write(lrNm);//35 | |
uint8_t urNm=(uint8_t)(((int)(relNm*1000) & 0xFF0000)>>16);//[36] | |
Serial3.write(urNm);//36 | |
uint8_t uurNm=(uint8_t)(((int)(relNm*1000) & 0xFF000000)>>24);//[37] | |
Serial3.write(uurNm);//37 | |
//-- | |
uint8_t llrEm=(uint8_t)(((int)(relEm*1000) & 0xFF));//[38] | |
Serial3.write(llrEm);//38 | |
uint8_t lrEm=(uint8_t)(((int)(relEm*1000) & 0xFF00)>>8);//[39] | |
Serial3.write(lrEm);//39 | |
uint8_t urEm=(uint8_t)(((int)(relEm*1000) & 0xFF0000)>>16);//[40] | |
Serial3.write(urEm);//40 | |
uint8_t uurEm=(uint8_t)(((int)(relEm*1000) & 0xFF000000)>>24);//[41] | |
Serial3.write(uurEm);//41 | |
//-- | |
uint8_t llrDm=(uint8_t)(((int)(relDm*1000) & 0xFF));//[42] | |
Serial3.write(llrDm);//42 | |
uint8_t lrDm=(uint8_t)(((int)(relDm*1000) & 0xFF00)>>8);//[43] | |
Serial3.write(lrDm);//43 | |
uint8_t urDm=(uint8_t)(((int)(relDm*1000) & 0xFF0000)>>16);//[44] | |
Serial3.write(urDm);//44 | |
uint8_t uurDm=(uint8_t)(((int)(relDm*1000) & 0xFF000000)>>24);//[45] | |
Serial3.write(uurDm);//45 | |
//-- | |
uint8_t llrLm=(uint8_t)(((int)(relLm*1000) & 0xFF));//[46] | |
Serial3.write(llrLm);//46 | |
uint8_t lrLm=(uint8_t)(((int)(relLm*1000) & 0xFF00)>>8);//[47] | |
Serial3.write(lrLm);//47 | |
uint8_t urLm=(uint8_t)(((int)(relLm*1000) & 0xFF0000)>>16);//[48] | |
Serial3.write(urLm);//48 | |
uint8_t uurLm=(uint8_t)(((int)(relLm*1000) & 0xFF000000)>>24);//[49] | |
Serial3.write(uurLm);//49 | |
//mon debug | |
// Serial.printf("*******rLm_%d_%d_%d_%d=%d,relLm=%3.3f*******\n\r",llrLm,lrLm,urLm,uurLm,(int)(relLm*1000),relLm); | |
//-- | |
uint8_t llrLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF));//[50] | |
Serial3.write(llrLmav);//50 | |
uint8_t lrLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF00)>>8);//[51] | |
Serial3.write(lrLmav);//51 | |
uint8_t urLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF0000)>>16);//[52] | |
Serial3.write(urLmav);//52 | |
uint8_t uurLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF000000)>>24);//[53] | |
Serial3.write(uurLmav);//53 | |
//--- | |
uint8_t llrLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF));//[54] | |
Serial3.write(llrLmdv);//54 | |
uint8_t lrLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF00)>>8);//[55] | |
Serial3.write(lrLmdv);//55 | |
uint8_t urLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF0000)>>16);//[56] | |
Serial3.write(urLmdv);//56 | |
uint8_t uurLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF000000)>>24);//[57] | |
Serial3.write(uurLmdv);//57 | |
//-------BaseLine Calibration AVERAGE STD------------------------------------ | |
// Standard Deviations uint16_t/100 | |
//rNstd | |
uint8_t lrNstd=(uint8_t)(((int)(rNstd*100) & 0xFF));//[58] | |
Serial3.write(lrNstd);//58 | |
uint8_t urNstd=(uint8_t)(((int)(rNstd*100) & 0xFF00)>>8);//[59] | |
Serial3.write(urNstd);//59 | |
//rEstd | |
uint8_t lrEstd=(uint8_t)(((int)(rEstd*100) & 0xFF));//[60] | |
Serial3.write(lrNstd);//60 | |
uint8_t urEstd=(uint8_t)(((int)(rEstd*100) & 0xFF00)>>8);//[61] | |
Serial3.write(urEstd);//61 | |
//rDstd | |
uint8_t lrDstd=(uint8_t)(((int)(rDstd*100) & 0xFF));//[62] | |
Serial3.write(lrDstd);//62 | |
uint8_t urDstd=(uint8_t)(((int)(rDstd*100) & 0xFF00)>>8);//[63] | |
Serial3.write(urDstd);//63 | |
//rLstd | |
uint8_t lrLstd=(uint8_t)(((int)(rLstd*100) & 0xFF));//[64] | |
Serial3.write(lrLstd);//64 | |
uint8_t urLstd=(uint8_t)(((int)(rLstd*100) & 0xFF00)>>8);//[65] | |
Serial3.write(urLstd);//65 | |
//avecount | |
uint8_t lavecount=(uint8_t)(avecount & 0xFF);//[66] | |
Serial3.write(lavecount);//66 | |
uint8_t uavecount=(uint8_t)((avecount & 0xFF00)>>8);//[67] | |
Serial3.write(uavecount);//67 | |
//samax | |
uint8_t lsa=(uint8_t)(sa & 0xFF);//[68] | |
Serial3.write(lsa);//68 | |
uint8_t usa=(uint8_t)((sa & 0xFF00)>>8);//[69] | |
Serial3.write(usa);//69 | |
//digitalWrite(39, LOW); // | |
//---------------------------------------------------------------------------- | |
//epNo=%d,:it0=%4d,it1=%4d,flg=%d,acc=%d,pdop=%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,relN=%5.2f,relE=%5.2f,relD=%5.2f,relNm=%3.3f,relEm=%3.3,relDm=%3.3f,relLm=%3.3f,rLmav=%3.3f,rLmdv=%3.3f%c\n\r",epNo,itow0cut,itow1cut,flags,hacc,pdop,numsv,fixpercent,fixtime,miscount0,miscount1,rrN,rrE,rrD,rrL,rLaveN,rLstd,mHead,relN,relE,relD,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1,sc); | |
sprintf(monC,"%d,%d,%d,%d,%d,%d,%d,%3.1f,%d,%d,%3.2f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f\n\r",itow,millis(),epNo,flags,unumSV,hacc,fixtime,fixpercent,miscount0,miscount1,pdop,rrN,rrE,rrD,mHead,relN,relE,relD,relL,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1); | |
//****************************************************************************** | |
//====SD WRITE TXT====== | |
if(Sflag==1){ | |
//myFileT = sd.open(fnameV, FILE_WRITE); | |
if(myFileT){ | |
myFileT.write(monC,strlen(monC)); | |
//myFileT.write(imuchr,strlen(imuchr)); | |
// myFileT.println(imusum); | |
} | |
//myFileT.close(); | |
//delay(8); | |
} | |
//ttxt1=millis(); | |
//Serial.printf("=====SD Write TXT File ttxt0=%d,ttx1=%d TXTtime=%d\n\r",ttxt0,ttxt1,ttxt1-ttxt0); | |
//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); | |
//tsdtxt1=millis(); | |
//digitalWrite(39, LOW); // | |
//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()); | |
//***************************************************************************************************************************************** | |
//***************************************************************************************************************************************** | |
//***************************************************************************************************************************************** | |
//Serial.printf("<loop() end time=%d>\n\r",millis()); | |
}// itow0-itow0_1==100 end | |
//////////////////////////////////////////////////////////////////////////////////////////////////// | |
///////////////////////////////////IMU PRINT_SPEED=20msec period /////////////////////////////////// | |
if ((lastPrint + PRINT_SPEED) < millis() && Sflag==1) | |
{ | |
timu0=millis();//debug | |
readGyroAcc();// gyro[3] acc[3] Get | |
applyCalibration();// offset adjust data apply | |
float dt = (micros() - lastMs) / 1000000.0; | |
lastMs = micros(); | |
float roll = getRoll(); | |
float pitch = getPitch(); | |
float yaw = getYaw(); | |
// | |
kalAngleX = kalmanX.getAngle(roll, gyro[0], dt); | |
kalAngleY = kalmanY.getAngle(pitch, gyro[1], dt); | |
kalAngleZ += gyro[2] * dt; | |
kalAngleZ = fmod(kalAngleZ, 360); | |
// itow sync | |
int imuitow=millis()-tptime0+tpitow0; | |
//Serial.printf("kx=%3.2f,ky=%3.2f,kz=%3.2f,roll=%3.2f,pitch=%3.2f,yaw=%3.2f,accX=%2.2f,accY=%2.2f,accZ=%2.2f,gx=%3.2f,gy=%3.2f,gz=%3.2f,t=%d,imitow=%d\n\r",kalAngleX,kalAngleY,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],millis(),imuitow); | |
sprintf(imuchr,"%d,%d,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%2.3f,%2.3f,%2.3f,%3.2f,%3.2f,%3.2f\n\r",imuitow,millis(),kalAngleX,kalAngleY,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]); | |
if(myFileI){ | |
myFileI.write(imuchr,strlen(imuchr)); | |
} | |
Serial.printf("imuitow=%d,t=%d,kx=%3.2f,ky=%3.2f\n\r",imuitow,millis(),kalAngleX,kalAngleY);//,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],millis()); | |
lastPrint = millis(); // Update lastPrint time | |
timu1=millis();//debug | |
Serial.printf("timu0=%d,timu1=%d millis=%d\n\r",timu0,timu1,millis()); | |
}//IMU end//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | |
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | |
} //LOOP END | |
//***************************************************************************************************************************** | |
//*******************************loop Finished********************************************************************** | |
//***************************************************************************************************************************** | |
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が0時以前の場合日付を日本日付に修正 | |
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 + "B.ubx"; //UBX Binary File | |
String stimeR ="/" +stime+"R.ubx"; | |
String stimeV = "/" + stime + "T.txt"; //UBX Value Text File | |
String stimeI = "/" + stime + "I.txt"; //imu pvt Text File | |
int slenB = stimeB.length() + 1; | |
int slenR = stimeR.length() +1; | |
int slenV = stimeV.length() + 1; | |
int slenI = stimeI.length() + 1; | |
//ファイル名はchar配列なのでStringからchar配列変換 fname[]を得る | |
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array | |
stimeR.toCharArray(fnameR, slenR); //stimeB to fnameR[] chara Array | |
stimeV.toCharArray(fnameV, slenV); //stimeV to fnameV[] chara Array | |
stimeI.toCharArray(fnameI, slenV); //stimeI to fnameI[] chara Array | |
Serial.println(); | |
Serial.print("fnameB="); | |
Serial.print(stimeB); | |
Serial.print("/fnameR="); | |
Serial.print(stimeR); | |
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.priintln(); | |
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; | |
return (int)pvt[0]; | |
}//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]); | |
//Serial.printf("RELPOScnv:Lenghth=relpos[7]=%4.3f\n\r",relpos[7]); | |
//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]); | |
return (int)relpos[3]; | |
} | |
//+++++++++++++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) { //最上位ビットたっていればマイナス | |
//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); | |
uint32_t i1=b1; | |
uint32_t i2=b2<<8; | |
uint32_t i3=b3<<16; | |
uint32_t i4=b4<<24; | |
uint32_t i5=i1 | i2 | i3| i4; | |
su=(long)i5; | |
} | |
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); | |
} | |
//itow=> HH:MM:SS ================================================== | |
String itowToHMS(int itow){ | |
String HMS; | |
int DAY,HOUR,MIN,SEC,JHOUR; | |
int DAY_MOD,HOUR_MOD,MIN_MOD,SEC_MOD; | |
DAY=int(itow/86400000); | |
DAY_MOD=itow % 86400000; | |
HOUR=int(DAY_MOD/3600000); | |
HOUR_MOD=DAY_MOD % 3600000; | |
MIN=int(HOUR_MOD/60000); | |
MIN_MOD=HOUR_MOD % 60000; | |
SEC=int(MIN_MOD/1000); | |
//--UTC=>JST | |
if(HOUR>15){ | |
JHOUR=HOUR+9-24; | |
} | |
else{ | |
JHOUR=HOUR+9; | |
} | |
//=====18sec 進んでいる?補正======= | |
if(SEC<18){ | |
SEC=60-(18-SEC); | |
} | |
else{ | |
SEC=SEC-18; | |
} | |
//------------- | |
//Serial.printf("itowToHMS:JHOUR=%d,MIN=%d,SEC=%d\n\r",JHOUR,MIN,SEC); | |
HMS=String(JHOUR)+":"+String(MIN)+":"+String(SEC); | |
return HMS; | |
}//itowToHMS end===================================================== |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment