Skip to content

Instantly share code, notes, and snippets.

Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/dbd4b903f55ad8fe32f0b2eacf1498ab to your computer and use it in GitHub Desktop.
Save dj1711572002/dbd4b903f55ad8fe32f0b2eacf1498ab to your computer and use it in GitHub Desktop.
ZED-F9P_MovingBase_IMU_Logger_Arduino_Teensy4.1_USBHOST
//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