Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/F9P-BNO_MagEnc_itow_230517_rev107KaitenMag.ino Secret

Created June 4, 2023 09:07
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/d527f34af515c85436968a288f0067dc to your computer and use it in GitHub Desktop.
Save dj1711572002/d527f34af515c85436968a288f0067dc to your computer and use it in GitHub Desktop.
Download ZIP
Teens4.1 F9P-BNO055-MagnetEncoder 　Circular JIG Measurement
Raw
F9P-BNO_MagEnc_itow_230517_rev107KaitenMag.ino
//STA22 のIP用システムの初期動作チェック
//2022-11-06 Serial1,4,5 同時受信OK
//2022-11-07 SDカードログ　３個のGNSSデータを３個のファイル
//22-11-28 sdFat1BasicExampleをコピペrev01
//22-12-01 OPEN-CLOSEを1対にして、キーインでO Cで操作することで安定したrev02
//22-12-02 PreProcessもp でキーイン選択としたrev03
//23-0103 F9P-F9P-M9N 歩行用システム　FILE List追加 d KeyでREBOOT
//@@@@@@@@@@@@新規更新メモ2023/4/27から@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
// rev102でSDログのハングなかったので、102ベースで改造を行う。
//rev120_00Monは、[bnread] [BNO]に変更、　[p]モードが終わらないのはfixflagを見ているので、bmode==1をいれてfix無しでもp動作させた。
//rev120_0Kaitenは、BNOのmillis()ダミーを実データに変更して回転実験用に変更
//STA23_F9P-BNO_TMP_itow_230424_rev102_01Kaitenは、BN出力をEulerとLAccにして、カウンタを減らした。
#include "SdFat.h"
//Adafruit BNO055 library
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
//#define BNO055_SAMPLERATE_DELAY_MS (100)
// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
// id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
// Uncomment 'USE_EXTERNAL_SD' define to use an external SD card adapter or leave
// it commented to use the built in sd card.
//#define USE_EXTERNAL_SD
#define SD_FAT_TYPE 3
#ifdef USE_EXTERNAL_SD
const uint8_t SD_CS_PIN = SS;
#define SPI_CLOCK SD_SCK_MHZ(10)
#define SD_CONFIG SdSpiConfig(SD_CS_PIN, SHARED_SPI, SPI_CLOCK)
#else // Use built in SD card.
#ifdef SDCARD_SS_PIN
const uint8_t SD_CS_PIN = SDCARD_SS_PIN;
#endif // SDCARD_SS_PIN
// Setup for built in SD card.
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)
#endif // USE_EXTERNAL_SD
// SdFat-beta usage
//#define FILE_READ 0 //読み込み
//#define FILE_WRITE 1 //追記書き込み
#define FILE_WRITE_BEGIN 2 //重ね書き込み
//SdFs sd;
//FsFile myFile;
//FsFile myFileB;
//FsFile myFileR;
//FsFile myFileM;
FsFile myFileA;
//FsFile myFileAv;
//
//SD List 230103=========================================================
#if SD_FAT_TYPE == 0
File dir;
File file;
#elif SD_FAT_TYPE == 1
SdFat32 sd;
File32 dir;
File32 file;
#elif SD_FAT_TYPE == 2
SdExFat sd;
ExFile dir;
ExFile file;
#elif SD_FAT_TYPE == 3
SdFs sd;
FsFile dir;
FsFile file;
#else // SD_FAT_TYPE
#error invalid SD_FAT_TYPE
#endif // SD_FAT_TYPE
//------------------------------------------------------------------------------
// Store error strings in flash to save RAM.
#define error(s) sd.errorHalt(&Serial, F(s))
//------------------------------------------------------------------------------
//============Timer def=========================================
IntervalTimer myTimer;
//IntervalTimer myTimer1;
static int timersetInit=0;
//data def
static uint8_t dBuf1[172], dBuf4[172],dBuf5[300];//バッファなので32の倍数でないといけない
static uint8_t dBuf1c[172],dBuf4c[172],dBuf5c[300];//Currentバッファで、受信して次の周期で渡す１個遅延させる用
static uint8_t dBuf11[172], dBuf44[172],dBuf55[300];//SORT用ダミーバッファ
static int mcounter=0;//M9Nの配列番号カウンタ
//32の倍数でバッファ定義
int dcount;
int ep0No=0;
static uint8_t serialbuffer1[192];//Base PVT+RELPOS172bytes
static uint8_t serialbuffer2[512];//BlueTooth
static uint8_t serialbuffer4[192];//Rover PVT+RELPOS172bytes
static uint8_t serialbuffer5[128];//重要!!! M9Nまとめ読みダメだった PVT100bytes＝＞バッファ 128
const int ledPin = 13;
int i,ii,j,jj,k,kk;
static int startT;//,endT;
static int bmode;//bno log
int initflag=0;
int flag10sec=0;
// preprocess 追加GLOBAL変数
static uint8_t preflag=0;
static uint8_t stflag,fixflag,fixed;
static uint8_t namedflag;
int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間
static int avecount;
int mbcount;
float ellapsedtime, ellapsedtime_1;
int calcount=500;//averaging count
uint8_t califlag=0;
//------------------- Monitoring Prameters------------------------
int uploadflag=0;
char mode;
static int stopserial=0;//Serial受信を止める
int tperiod=10;//10dataに一回送信
volatile static long itow0, itow0_1,itow0_2;
volatile static long itow03k;//itow0%3000==0時点でのitow0値
volatile static int tp03k;//itow0%3000==0時点でのクロック時刻
long itow1, itow1_1,itow1_2;
long length0,length1;
int itow, itow_1,itowB, itowR;//Base Rover
int itowm;//ROver
int hacc, accL,numsv;
int haccB,haccR;
uint8_t flags,mflags;
uint8_t flagsB,flagsR;
uint8_t flags0,flags0_1,flags1,flags1_1,flagsm;
uint8_t fix0,fix1;
double pdop,pdopB,pdopR;
int gSpeedB,headMotB,gSpeedR,headMotR;
int headmoti,gspeedi;
double headmotd,gspeedd;
//int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間
//uint8_t stflag,fixflag,fixed;
uint8_t PVT0flag,RELPOS0flag,PVT1flag,RELPOS1flag;
int epNo,fixepoNo,allepoch,misepoch;//epochのカウント
int sa0,sa1,miscount,miscount0,miscount1,miscount00,miscount11;//epoch飛び itow0-itow0_1の差
int misflags=0;//[s]中のfalgas<=67をカウント
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;//初回静止時の基準位置
double relN,relE,relD,relL;//基準局からの現在位置
double relNm,relEm,relDm,relLm,mHead;//MovingBaseRover RELPOS値
double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN
double rNstd,rEstd,rDstd,rLstd,mHstd;//標準偏差 rNdvNの平方根
double rNmstd,rEmstd,rDmstd,rLmstd,mmHstd;//MovingBaseRover 標準偏差 rNdvNの平方根
double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//基準位置平均値
double rNmaveN,rEmaveN,rDmaveN,rLmaveN,mmHaveN;//MovingBaseRover 位置平均値
double rNaveN1,rEaveN1,rDaveN1,rLaveN1,mHaveN1;//基準位置平均値1
double rNmaveN1,rEmaveN1,rDmaveN1,rLmaveN1,mmHaveN1;//MovingBaseRover位置平均値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;//MovingbaseRoverのstandard deviation N
double rNsgmN1,rEsgmN1,rDsgmN1,rLsgmN1,mHsgmN1;//基準位置のstandard deviation N+1
double rNmsgmN1,rEmsgmN1,rDmsgmN1,rLmsgmN1,mmHsgmN1;//MovingBaseRoverのstandard deviation N+1
double rNdvsum,rEdvsum,rDdvsum,rLdvsum,mHdvsum;//基準位置のdeviation sum
double rNmdvsum,rEmdvsum,rDmdvsum,rLmdvsum,mmHdvsum;//MovingBaseRoverのdeviation sum
double rNdvN,rEdvN,rDdvN,rLdvN,mHdvN;//基準位置のdeviation N
double rNmdvN,rEmdvN,rDmdvN,rLmdvN,mmHdvN;//MovingBaseRoverのdeviation N
double rNdvN1,rEdvN1,rDdvN1,rLdvN1,mHdvN1;//基準位置のdeviation N+1
double rNmdvN1,rEmdvN1,rDmdvN1,rLmdvN1,mmHdvN1;//MovingBaseRoverのdeviation N+1
//モニター　補間用
//F9P補間用データ追加
long gSpeed,headMot;//Base motion
long gSpeedm,headMotm;//Rover motion
long velN,velE,velD;//Base velocity
long velNm,velEm,velDm;//Rover velocity
//M9N 補間用データ
uint8_t PVTm9n_1[100],PVTm9n_2[100],PVTm9n_3[100];
long PVTvalm9n_1[33],PVTvalm9n_2[33],PVTvalm9n_3[33];
long itowm9n_1,itowm9n_2,itowm9n_3;
long velNm9n_1,velNm9n_2,velNm9n_3;
long velEm9n_1,velEm9n_2,velEm9n_3;
long velDm9n_1,velDm9n_2,velDm9n_3;
long gSpeedm9n_1, gSpeedm9n_2, gSpeedm9n_3;
long headMotm9n_1,headMotm9n_2,headMotm9n_3;
int shokai0=0;
//=====Magnetic Encoder OH182E=============
static long mt[100];//itow換算時刻float
static long mti[100];//millis
static int mtn=0;//エポックでたまったデータ数
//=====-BNO055 imu================
int BNOinn,BNOout;
//eX[no]=yaw;eY[no]=pitch;eZ[no]=roll;
static float bnHosei;//mHeadに合わせる補正値　mHead=bnHosei+yaw
static double yaw,pitch,roll;
//配列は、３秒分保持　3x25=75個 itow%3000==0で[0]で合わせてログ出力
static byte fb[4];//header
volatile static long btime[75]={};//BNO Sampling itow
static float eX[75]={};//yaw
static float eY[75]={};//pitch
static float eZ[75]={};//roll
static float qW[75]={};
static float qX[75]={};
static float qY[75]={};
static float qZ[75]={};
static float aX[75]={};
static float aY[75]={};
static float aZ[753]={};
//Linear Acc
static double laX[75]={};
static double laY[75]={};
static double laZ[75]={};
//Gravity
static double grX[75]={};
static double grY[75]={};
static double grZ[75]={};
static int epochin;
static int epochinT,epochinT_1;
static int tst[75];
//bnbuf[180]=header4byte(BNO)+　Euler12(4bytex3)+Quaternion16(4bytex4)+LinearAcc(4bytex3)+Gravity(4bytex3)+time4(4bytex1)=36 byte　60x3=180byte-
uint8_t bnbuf[180];//'BNO'floatを100000倍して整数バイト変換して、バイト配列にしてSD保存:
volatile static int tp,tp_1,tp0,tpflag,tp3flag;//timepulse inteerupt millis
volatile static int bn=0;//counter
volatile static int bns_1=0;//１個前のcounter
volatile static int bns=0;//TImer割り込みの累積カウンタ　bn=bns%75
static int itowbn0;//起動後最初にTimePulseが入ってBN測定開始時点のitow
int bnotime;
static int bninitflag=0;
static int bninitreset=0;
String imus[100];
String imusum="";
//usb シリアル接続かBlueTooth接続か判定
int usborbt=1;//通常はUSBでゼロでBT
//デバイス有無機能
int goflag=0;//デバイスが１個でも受信していればログをgoしていいフラグ
int rcvflagB=0;//１回でも受信にはいれば、１に立つ
int rcvflagR=0;//１回でも受信にはいれば、１に立つ
int rcvflagM=0;//１回でも受信にはいれば、１に立つ
int umuB=1;//３０秒後umuB=rcvflagBで無しならゼロで、umuB==0ならflagB=1に常時しておく
int umuR=1;//
int umuM=1;//
int sd1stflag=1;//1回目だけOPENするためのフラグ
//time
int stt,ett;
//Sorintg -------------------------------------------
static int PVT1startNo,PVT4startNo,PVT5startNo;
static int sortflagB=0;//Base data order flag=0 OK flag=1 NG
static int sortflagR=0;//Rover data order flag=0 OK flag=1 NG
static int sortflagM=0;//M9N data order flag=0 OK flag=1 NG
//-----------------キーイン-------------------------
//static int BRflg,RRflg,MRflg;//Serial Read finish flag
char ckey;//File Name Make='s' Open='o' Close='c'
char keyin;// control
char keyinC[3];//0,1,2　チャタリング防止
int ki=0; //key count
int sdstart,sdend;//time measure
static int flagB,flagR,flagM;
static int openflag;
static long count_upB,count_upR,count_upM;
static long count_upB0;
static long count_upB_1;//,count_upR_1,count_upM_1;
static long logcount=0;
//static long logcount_1=0;
//FileNames
char BaseFname[]="B110711.ubx";
char RoverFname[]="R110711.ubx";
char M9NFname[]="M110711.ubx";
char AllFname[]="InitAllin.ubx";
int JST_day;
//timestamp file name
uint8_t Sflag = 0;
uint8_t logflag=0;
char fnameB[30];//UBX Binary File name
char fnameR[30];//Rover Binary
char fnameM[30];//UBX Value File name NAV-PVT+NAV-RELPOSNED
char fnameI[30];//imu TXT
char fnameAv[30];//averaging File
char monC[200];//Monitor Text data Save
//---ubx 変換データ---
uint8_t PVTdata[100];
uint8_t RELPdata[72];
long PVTval[33];
long PVT1val[33];
long RELPOSval[21];
long RELPOS1val[21];
long PVTBval[33];//Base PVT
long PVTRval[33];
long RELPOSBval[21];
long RELPOSRval[21];
//----------------------
uint8_t RELPOSBdata[172];//base line RELPOS
volatile uint8_t RELPOS1Bdata[172];//base line RELPOS
//--USB Serial connected
int usbs=0;
//============初期　関数================================-
void sdlist(){
int ci=0;//file couter
// Open root directory
if (!dir.open("/")){
error("dir.open failed");
}
// Open next file in root.
// Warning, openNext starts at the current position of dir so a
// rewind may be necessary in your application.
while (file.openNext(&dir, O_RDONLY)) {
ci++;
file.printFileSize(&Serial);
delay(1);
file.printFileSize(&Serial2);
Serial.write(' ');
Serial2.write(' ');
file.printModifyDateTime(&Serial);
delay(1);
file.printModifyDateTime(&Serial2);
Serial.write(' ');
delay(1);
Serial2.write(' ');
file.printName(&Serial);
delay(1);
file.printName(&Serial2);
if (file.isDir()) {
// Indicate a directory.
Serial.write('/');
Serial2.write('/');
}
Serial.println();
Serial2.println();
file.close();
}
if (dir.getError()) {
Serial.println("openNext failed");
} else {
Serial.println("Done!");
}
}
//==================================
void doReboot() {
Serial.println("#####################DO REBOOT##############");
SCB_AIRCR = 0x05FA0004;
}
//===============MagEncodr Interrupt==============================
void magInterrupt(){
mtn++;
//mti[mtn%100]=millis();
mt[mtn%100]=millis()-tp03k+itow03k;
//Serial2.print('@');
}
//===============TimePulse Interrupt===============================
// -----BNO interrupt timepulse pin22
void myInterrupt(){
tp0=micros();//以後40msec周期でサンプリング
tpflag=1;
int mod3000=itow0%3000;
if(mod3000==2880)
{
tp03k=millis();
itow03k=itow0+120;
}
if(timersetInit==0)//初回だけタイマーセット
{
myTimer.begin(bnoreadT, 40000);
timersetInit=1;
Serial2.printf("------------------myTimer Start-----------------bns=%d,tp0=%d,timersetInitflag=%d\n\r",bns,tp0,timersetInit);
}
//Serial2.printf("------------------myInterrupt-----------------bns=%d,bns_1=%d,tp0=%d,timersetInitflag=%d\n\r",bns,bns_1,tp0,timersetInit);
}
void bnoreadTC()//40msecでbnsインクリメント
{
bns++;
//tp=micros();
}
//=================BNO Timepulse Itow 同期====================
//Timepulseが入った瞬間にこの関数を実行して、itow0が3000msecに近ければ、bn番号ゼロを返す。
int bn_itow3()
{
int mod3000=itow0%3000;
int rn;
if(mod3000==2880)
{
rn=0;
}
else
{
rn=1;
}
return rn;
}
//==================itow番号とBNO配列番号照合関数 BNO先頭番号を計算========================
int bNo()
{
int mod3000=itow0%3000;
int bNo=(int)(mod3000/40);
return bNo;
}
//SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--SETUP--
//#######################################################################################
//***************************************************************************************
//**********************SETUP**********************************
void setup() {
Serial.begin(460800);
Serial2.begin(115200);
delay(5000);
//USB Serial Connection TEST
Serial.println("This may be sent before your PC is able to receive");
Serial2.println("This may be sent before your PC is able to receive");
//while (!Serial) {
if(Serial)
{
usbs=1;
// wait for Arduino Serial Monitor to be ready
}
else{
usbs=0;
}
Serial.println("This line will definitely appear in the serial monitor");
Serial2.println("This line will definitely appear in the serial monitor");
//----time pulse interrupt set---------
attachInterrupt(digitalPinToInterrupt(22), myInterrupt, RISING);
//----MagnetEncoder interrupt set------
attachInterrupt(digitalPinToInterrupt(41), magInterrupt, FALLING);//magnet Interrupt in Pin 41 magInterrupt()
//---------------------------
//Serial1 def Base
Serial1.begin(115200);//RX1(p0)-TX1(p1) PVT+RELPOS from Base UART2 rcv=ubx ,tx=RTCM3
Serial1.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1));
//Serial2 def BlueTooth
Serial2.begin(115200);//Rx5(p21)-Tx5(p20) PVT from M9N uart1
Serial2.addMemoryForRead(serialbuffer2, sizeof(serialbuffer2));
//Serial4 def F9H
Serial4.begin(115200);//Rx2(p16)-Tx2(p17) from BlueTooth
Serial4.addMemoryForRead(serialbuffer4, sizeof(serialbuffer4));
//Serial5 def m9N
Serial3.begin(115200);//Rx5(p21)-Tx5(p20) PVT from M9N uart1
Serial3.addMemoryForRead(serialbuffer5, sizeof(serialbuffer5));
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor
//付加機能用
count_upB=0;
count_upR=0;
count_upM=0;
//------BNO055 setup---------------------------------------------
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
//while(1);
}
delay(1000);
/* Use external crystal for better accuracy */
bno.setExtCrystalUse(true);
/* Display some basic information on this sensor */
displaySensorDetails();
//-----------------------------------------------------------------------------
/* Use external crystal for better accuracy */
bno.setExtCrystalUse(true);
/* Display some basic information on this sensor */
displaySensorDetails();
//================SD CARD LIST===============================================
//---------------SD Card Write SetUp---------------------------------
if (!sd.begin(SD_CONFIG)) {
Serial.println("SD CARD initialization failed!");
Serial2.println("SD CARD initialization failed!");
return;
}
Serial.println("SD initialization done.");
Serial2.println("SD initialization done.");
sdlist();
delay(20);
readkijun();
delay(20);
//===========================================================================
//F9Pの起動時間待ち
Serial.println("****************Waiting KEYIN from USB Serial OR BlueToothSPP *************");
delay(1);
Serial2.println("****************Waiting KEYIN from USB Serial OR BlueToothSPP *************");
//USB connection
Serial.printf("usbs=%d\n\r",usbs);
Serial2.printf("usbs=%d\n\r",usbs);
// delay(10000);
pinMode(ledPin, OUTPUT);
int keyato=0;
while (keyato==0)
{
//noInterrupts();
if(Serial2.available()){
usborbt=0;
keyato=1;
}
if(Serial.available()){
usborbt=1;
keyato=1;
}
}
startT=millis();
//Serial.println("----------PRE PROCESSING START BaseLine Check-------------------");
//-----------------------
//Serial2.printf("*********************Serial2 TEST*************************\n\r");
tp_1=0;
//interrupts();
myTimer.priority(192);//BNOreadの優先をさげておく
}
//-----------------------------------------------------------------
//----------------------------SET UP END----------------------------------------------------
//------------------------------------------------------------------------------------------
//===========================================================================
//============BNO055 SUB=================================================
//==========================================================================--
void printEvent(sensors_event_t* event,int no) {
double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
if (event->type == SENSOR_TYPE_ACCELEROMETER) {
//Serial.print("Accl:");
x = event->acceleration.x;
y = event->acceleration.y;
z = event->acceleration.z;
}
else if (event->type == SENSOR_TYPE_ORIENTATION) {
//Serial.print("Orient:");
x = event->orientation.x;
y = event->orientation.y;
z = event->orientation.z;
}
else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
//Serial.print("Mag:");
x = event->magnetic.x;
y = event->magnetic.y;
z = event->magnetic.z;
}
else if (event->type == SENSOR_TYPE_GYROSCOPE) {
//Serial.print("Gyro:");
x = event->gyro.x;
y = event->gyro.y;
z = event->gyro.z;
}
else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
//Serial.print("Rot:");
x = event->gyro.x;
y = event->gyro.y;
z = event->gyro.z;
}
else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
//Serial.print("Linear:");
laX[no] = event->acceleration.x;
laY[no] = event->acceleration.y;
laZ[no] = event->acceleration.z;
}
else if (event->type == SENSOR_TYPE_GRAVITY) {
//Serial.print("Gravity:");
grX[no] = event->acceleration.x;
grY[no]= event->acceleration.y;
grZ[no] = event->acceleration.z;
}
else {
//Serial.print("Unk:");
}
//Serial.print("\tx= ");
//Serial.print(x);
//Serial.print(" |\ty= ");
//Serial.print(y);
//Serial.print(" |\tz= ");
//Serial.println(z);
}
void BNO055_Orient(float ox,float oy,float oz){
sensors_event_t event;
bno.getEvent(&event);
ox=360-(float)event.orientation.x;
oy=(float)event.orientation.y;
oz=(float)event.orientation.z;
}
/**************************************************************************/
void displaySensorDetails(void)
{
sensor_t sensor;
bno.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: "); Serial.println(sensor.name);
Serial.print ("Driver Ver: "); Serial.println(sensor.version);
Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx");
Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx");
Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
//---------------------------------------------------------------------------------------
void bnoread(int no){
//delay(20);//15+5=20msec半周期ずらす
sensors_event_t event;
sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
bno.getEvent(&event);
/*
//Euler angle
eX[no]=360-(float)event.orientation.x;
eY[no]=(float)event.orientation.y;
eZ[no]=(float)event.orientation.z;
*/
//Quartenion Copy from https://nobita-rx7.hatenablog.com/entry/27642606
imu::Quaternion quat = bno.getQuat();
qW[no]=(float)quat.w();
qX[no]=(float)quat.x();
qY[no]=(float)quat.y();
qZ[no]=(float)quat.z();
//time pulse millis
tst[no]=tp;
//Calc Euler from Quaternion
double w=qW[no];
double x=qX[no];
double y=qY[no];
double z=qZ[no];
double ysqr = y * y;
// roll (x-axis rotation)
double t0 = +2.0 * (w * x + y * z);
double t1 = +1.0 - 2.0 * (x * x + ysqr);
roll = atan2(t0, t1);
// pitch (y-axis rotation)
double t2 = +2.0 * (w * y - z * x);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
pitch = asin(t2);
// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 - 2.0 * (ysqr + z * z);
//rad->deg
yaw = atan2(t3, t4);
roll *= 57.2957795131;
pitch *= 57.2957795131;
yaw *= 57.2957795131;
if(yaw<0){//yaw 0-> -180deg=
yaw=360+yaw;;
}
yaw=360-yaw;
//------------------------------
btime[no]=millis();
eX[no]=yaw;
eY[no]=pitch;
eZ[no]=roll;
//--------------get Linear Acc----------------------
bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
printEvent(&linearAccelData,no);
//------------get Gravity------------------------
bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
printEvent(&gravityData,no);
//Serial2.printf("bnoread:%d:%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d,%d,%4.3f,%4.3f,%4.3f\n\r",no,eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no],millis()-tp0,tst[no],yaw,pitch,roll);
}
//Timer割り込み用bnoreadT
void bnoreadT(){
tp=micros();//割り込みread 開始時刻
sensors_event_t event;
sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
bno.getEvent(&event);
int no=bn%75;
bn++;
/*
//Euler angle
eX[no]=360-(float)event.orientation.x;
eY[no]=(float)event.orientation.y;
eZ[no]=(float)event.orientation.z;
*/
//Quartenion Copy from https://nobita-rx7.hatenablog.com/entry/27642606
imu::Quaternion quat = bno.getQuat();
qW[no]=(float)quat.w();
qX[no]=(float)quat.x();
qY[no]=(float)quat.y();
qZ[no]=(float)quat.z();
//time pulse millis
tst[no]=tp;
//Calc Euler from Quaternion
double w=qW[no];
double x=qX[no];
double y=qY[no];
double z=qZ[no];
double ysqr = y * y;
// roll (x-axis rotation)
double t0 = +2.0 * (w * x + y * z);
double t1 = +1.0 - 2.0 * (x * x + ysqr);
roll = atan2(t0, t1);
// pitch (y-axis rotation)
double t2 = +2.0 * (w * y - z * x);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
pitch = asin(t2);
// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 - 2.0 * (ysqr + z * z);
//rad->deg
yaw = atan2(t3, t4);
roll *= 57.2957795131;
pitch *= 57.2957795131;
yaw *= 57.2957795131;
if(yaw<0){//yaw 0-> -180deg=
yaw=360+yaw;;
}
yaw=360-yaw;
//------------------------------
btime[no]=millis();
eX[no]=yaw;
eY[no]=pitch;
eZ[no]=roll;
//--------------get Linear Acc----------------------
bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
printEvent(&linearAccelData,no);
//------------get Gravity------------------------
bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
printEvent(&gravityData,no);
//BNO時間をitowに変換
btime[no]=(millis()-tp03k)+itow03k;
// Serial2.printf("[bnoread],%d,%4.3f,%4.3f,%4.3f,%d,%d,%d,%d,%d\n\r",no,eX[no],eY[no],eZ[no],tp,tp0,tp-tp0,micros(),tpflag);
if(tpflag==1){tpflag=0;}//tpflagを40msecだけ１にする
}
//---------------bno dataを100000倍して整数変換してまとめてSD保存 bdata[]------------
//--------変換関数　i_to_char(int i, uint8_t *d, int n)　 下位からd[n]d[n+1]d[n+2]d[n+3]上位に格納-----
//--------bnbuf[180]=header4byte(BNO)+　Euler12(4byte*3)+Quaternion16(4byte*4)+LinearAcc(4byte*3)+Gravity(4byte*3)+time4(4byte*1)=36 byte　60x3=180byte-------
void bnoTobuf(int startN){//bnobuf[180]
int bi=0;
int iib,nnb;
int bl=60;//bn data block length 60byte
int hd=4;
for (bi=0;bi<3;bi++){ //3周期分　0msec 40msec 80msecの３個
bnbuf[0+bi*bl]='B';
bnbuf[1+bi*bl]='N';
bnbuf[2+bi*bl]='O';
bnbuf[3+bi*bl]=char(bi);
// itochar
int iex=(int)(eX[bi+startN]*1000000);
int iey=(int)(eY[bi+startN]*1000000);
int iez=(int)(eZ[bi+startN]*1000000);
int iqw=(int)(qW[bi+startN]*1000000);
int iqx=(int)(qX[bi+startN]*1000000);
int iqy=(int)(qY[bi+startN]*1000000);
int iqz=(int)(qZ[bi+startN]*1000000);
int ilax=(int)(laX[bi+startN]*1000000);
int ilay=(int)(laY[bi+startN]*1000000);
int ilaz=(int)(laZ[bi+startN]*1000000);
int igrx=(int)(grX[bi+startN]*1000000);
int igry=(int)(grY[bi+startN]*1000000);
int igrz=(int)(grZ[bi+startN]*1000000);
//Serial.printf("igrz[%d]=%d\n\r",bi,igrz);
nnb=0+hd+bi*bl;
i_to_char(iex,bnbuf,nnb);
//Serial.printf("eX[%d]=%d,<bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x>\n\r",no,iex,bnbuf[nnb],bnbuf[nnb+1],bnbuf[nnb+2],bnbuf[nnb+3]);
nnb=4+hd+bi*bl;
i_to_char(iey,bnbuf,nnb);
//Serial.printf("eY[%d]=%d,<bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x>\n\r",no,iey,bnbuf[nnb],bnbuf[nnb+1],bnbuf[nnb+2],bnbuf[nnb+3]);
nnb=8+hd+bi*bl;
i_to_char(iez,bnbuf,nnb);
//Serial.printf("eZ[%d]=%d,<bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x,bnbuf[%d]=0x%x>\n\r",no,iez,bnbuf[nnb],bnbuf[nnb+1],bnbuf[nnb+2],bnbuf[nnb+3]);
nnb=12+hd+bi*bl;
i_to_char(iqw,bnbuf,nnb);//qw
nnb=16+hd+bi*bl;
i_to_char(iqx,bnbuf,nnb);//qx
nnb=20+hd+bi*bl;
i_to_char(iqy,bnbuf,nnb);//qy
nnb=24+hd+bi*bl;
i_to_char(iqz,bnbuf,nnb);//qz
//lacc
nnb=28+hd+bi*bl;
i_to_char(ilax,bnbuf,nnb); // lacX
nnb=32+hd+bi*bl;
i_to_char(ilay,bnbuf,nnb); // lacY
nnb=36+hd+bi*bl;
i_to_char(ilaz,bnbuf,nnb); // lacZ
//gravity
nnb=40+hd+bi*bl;
i_to_char(igrx,bnbuf,nnb); // grX
nnb=44+hd+bi*bl;
i_to_char(igry,bnbuf,nnb); // grY
nnb=48+hd+bi*bl;
i_to_char(igrz,bnbuf,nnb); // grZ
//
nnb=52+hd+bi*bl;
i_to_char(tst[bi+startN],bnbuf,nnb);
}//3times end
for(iib=0;iib<180;iib++){
//Serial.printf(",bnbuf[%d]=%x,",iib,bnbuf[iib]);
Serial.print(bnbuf[iib],HEX);
Serial.print(",");
}
Serial.printf("bnTobuf:%d\n\r",millis());
}
// SD data BNO ASCII UPLOAD Transform
//--------bnbuf[180]=header4byte(BNO)+　Euler12(4bytex3)+Quaternion16(4bytex4)+LinearAcc(4bytex3)+Gravity(4bytex3)+time4(4bytex1)=36 byte　60x3=180byte-------
//
int ToInt32(uint8_t *buf,int n)
{
int result;
result=buf[n]+buf[n+1]*256*buf[n+2]*256*256+buf[n+3]*256*256*256+buf[n+3]*256*256*256;
return result;
}
void bnoBintoDbl(uint8_t *bnbuf)
{
int bi=0;
int iib,nnb;
int bl=60;//bn data block length 60byte
int hd=4;
for (bi=0;bi<3;bi++){
bnbuf[0+bi*bl]='B';
bnbuf[1+bi*bl]='N';
bnbuf[2+bi*bl]='O';
bnbuf[3+bi*bl]=char(bi);
//public static int ToInt32 (byte[] value, int startIndex);
//---EULER
nnb=4;//eX
int iex=ToInt32(bnbuf,nnb);
eX[bi]=(double)iex/1000000;
nnb=8;//eY
int iey=ToInt32(bnbuf,nnb);
eY[bi]=(double)iey/1000000;
nnb=12;//eZ
int iez=ToInt32(bnbuf,nnb);
eZ[bi]=(double)iez/1000000;
//---Quarternion
nnb=16;//qW
int iqw=ToInt32(bnbuf,nnb);
qW[bi]=(double)iex/1000000;
nnb=20;//eX
int iqx=ToInt32(bnbuf,nnb);
qX[bi]=(double)iqx/1000000;
nnb=24;//qY
int iqy=ToInt32(bnbuf,nnb);
qY[bi]=(double)iey/1000000;
nnb=28;//qZ
int iqz=ToInt32(bnbuf,nnb);
qZ[bi]=(double)iqz/1000000;
//---LinearAcc
nnb=32;//laX
int ilax=ToInt32(bnbuf,nnb);
laX[bi]=(double)ilax/1000000;
nnb=36;//laY
int ilay=ToInt32(bnbuf,nnb);
laY[bi]=(double)ilay/1000000;
nnb=40;//laZ
int ilaz=ToInt32(bnbuf,nnb);
laZ[bi]=(double)ilaz/1000000;
//---Gravity
nnb=44;//grX
int igrx=ToInt32(bnbuf,nnb);
grX[bi]=(double)igrx/1000000;
nnb=48;//grY
int igry=ToInt32(bnbuf,nnb);
grY[bi]=(double)igry/1000000;
nnb=52;//grZ
int igrz=ToInt32(bnbuf,nnb);
laZ[bi]=(double)igrz/1000000;
//---tst time
nnb=56;
tst[bi]=ToInt32(bnbuf,nnb);
}
}// bnoBintoDbl
//=========================================================
//********************************************************************************************************************
//*********************************************************************************************************************
//*********************************************************************************************************************
//*********************************************************************************************************************
void loop() {
// digitalWrite(ledPin, HIGH); // set the LED on
//===Serial1Base(F9P) read=========================================================
if(Serial1.available()>17){ //バッファ1が172バイトになるまで処理しない
//Serial.println();
i=0;//Base Serial1 counter
while(i<172){
digitalWrite(ledPin, HIGH); // set the LED on
stt=millis();
while(Serial1.available()){
dBuf1[i]=dBuf1c[i];
dBuf1c[i]=Serial1.read();
//Serial.print(dBuf1[i],HEX);
i++;
}
//Serial.printf("Base(F9P):%x,%x,%x,%x,",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]);
}
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------
if(dBuf1[0]==0xB5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07)
{
//-----------------最新のitow0計算------------------------
itow0=B2L(dBuf1c[9], dBuf1c[8], dBuf1c[7], dBuf1c[6]);
}
else{
sortflagB=1;
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>Base(F9P):Recieve ORDER ERROR dBuf1[0]:%x,%x,%x,%x,%x\n\r",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]);
}
//-----------------------------------------------------------------------------------------------------------------------------------
ett=millis();
if(initflag==0 && count_upB%50==0){
flags=dBuf1[27];
//Serial.printf("[%d][%d][%d]F9P:%d,%d\n\r",count_upB,flags,avecount,stt,ett);
//Serial.print("B");
}
digitalWrite(ledPin, LOW); // set the LED on
flagB=1;
rcvflagB=1;
count_upB++;
}//
//===Serial4 Rover(F9P) read=========================================================
if(Serial4.available()>171){ //バッファ4が172バイトになるまで処理しない
j=0;//Rover Serial1 counter
while(j<172){
digitalWrite(ledPin, HIGH); // set the LED on
//Serial.printf("Rover(F9P):%d,",millis());
stt=millis();
while(Serial4.available()){
dBuf4[j]=dBuf4c[j];
dBuf4c[j]=Serial4.read();
j++;
}
//Serial.printf("Rover(F9P):%x,%x,%x,%x,",dBuf4[0],dBuf4[1],dBuf4[2],dBuf4[3]);
}
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------
if(dBuf4[0]==0xB5 && dBuf4[1]==0x62 && dBuf4[2]==0x01 && dBuf4[3]==0x07){
}
else{
sortflagR=1;
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>Rover(F9H):Recieve ORDER ERROR dBuf4[0]:%x,%x,%x,%x,%x\n\r",dBuf4[0],dBuf4[1],dBuf4[2],dBuf4[3]);
}
//-----------------------------------------------------------------------------------------------------------------------------------
ett=millis();
if(initflag==0 && count_upR%50==0){
//Serial.printf("F9H:%d,%d\n\r",stt,ett);
//Serial.print("R");
}
digitalWrite(ledPin, LOW); // set the LED on
flagR=1;
rcvflagR=1;
count_upR++;
}//
//===Serial5 M9N read=========================================================
//int flagM;
if(Serial3.available()>99 ){ //バッファ5が100バイトになるまで処理しない
//Serial.println();
k=0;//Rover Serial5 counter
if(mcounter>299){
mcounter=0;
}
//Serial.printf("Start:mcounter=%d,t=%d\n\r",mcounter,millis());
while(k<100){
// digitalWrite(ledPin, HIGH); // set the LED on
//Serial.printf("M9N:%d,",millis());
stt=millis();
while(Serial3.available()){
dBuf5[mcounter%300]=dBuf5c[mcounter%300];
dBuf5c[mcounter%300]=Serial3.read();
k++;
mcounter++;
//Serial.print(dBuf5[mcounter%300],HEX);
}
//Serial.println();
//Serial.printf("END:mcounter=%d,t=%d\n\r",mcounter,millis());
}
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------
if(dBuf5[0]==0xB5 && dBuf5[1]==0x62 && dBuf5[2]==0x01 && dBuf5[3]==0x07){
sortflagM=0;//OK
}
else{
sortflagM=1;//Sort must
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>M9N:Recieve ORDER ERROR dBuf4[0]:%x,%x,%x,%x,%x\n\r",dBuf5[0],dBuf5[1],dBuf5[2],dBuf5[3]);
}
//-----------------------------------------------------------------------------------------------------------------------------------
//delay(3);
ett=millis();
if(initflag==0 && count_upM%50==0){
//Serial.printf("M9N:%d,%d\n\r",stt,ett);
//Serial.print("M");
}
digitalWrite(ledPin, LOW); // set the LED on
flagM=1;
rcvflagM=1;
count_upM++;
}//
//==============================================================================================
//===============================BNO055 read=====================================================
//============bnoread()配列に25エポック周期で収納 25エポックx3個=75個配列====================================================
//タイムパルスに同期させる tp0が3秒に一回itowと周期が合う
//===============================================================
int sabn=(millis()-tp0)%40;
//Serial2.printf("BN0start:millis=%d,tp=%d,tp_1=%d,tp0=%d,sabn=%d\n\r",millis(),tp,tp_1,tp0,sabn);
/*
if(tpflag==1 )//&& bn_itow3()==0 )// timepulseが入って、itowが2880だったらbn=0
{
bn=0;
//bnoread(bn%75); //初回
// Serial2.printf("First-BNread:itow0=%d,bn_itow3=%d,tp0=%d:bn=%d,tpflag=%d,tp=%d,tp_1=%d\n\r",itow0,bn_itow3(),tp0,bn,tpflag,tp,tp_1);
tpflag=0;
//bn=1;
bninitflag=1;//スタートフラグ
bninitreset=1;//初回だけ
}
//75カウントがでるelse if(bninitflag==1)
//if(bninitflag==1 || bninitreset==1)
*/
//Serial.printf("LOOP:bns=%d,bns_1=%d\n\r",bns,bns_1);
/*
noInterrupts();//bnsを使うので割り込み禁止
if(bns-bns_1>0)
{
bns_1=bns;
bn=bn%75;
int tps=micros();
bnoread(bn);
int tpe=micros();
/*
//Serial2.printf("else:tpflag=%d,tp,tp_1=%d,%d,%d\n\r",tpflag,tp,tp_1,tp-tp_1);
bninitreset=0;//リセットは一回で戻す
int BNOinn=millis();
tp_1=tp;
tpflag=0;
bns++;
}
interrupts();//割り込み戻す
*/
//============================================================================================
//============================================================================================
//============================================================================================
//------------------キーイン文字-----------------------------------------
// flags=dBuf1[27];
//hbSerial.printf("-----flags=%d,initflag=%d,preflag=%d\n\r",flags,initflag,preflag);
if(Serial.available()){
keyinC[ki%3]=Serial.read();
ki++;
if(keyinC[0]==keyinC[1] && keyinC[1]==keyinC[2]){
Serial.printf("keyinC0,1,2=%c,%c,%c\n\r",keyinC[0],keyinC[1],keyinC[2]);
keyin=keyinC[0];
}
else{
keyin=0x00;
}
Serial.printf("<<<<<<<<<<<<<<<<<<<<<<<<Serial KEY IN=%c>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n\r",keyin);
}
delay(1);
if(Serial2.available()){
keyin=Serial2.read();
Serial2.printf("Serial2 KEY IN=%c\n\r",keyin);
Serial2.printf("<<<<<<<<<<<<<<<<<<<<<<<<Serial2 BT KEY IN=%c>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n\r",keyin);
}
mode='m';//monitor mode
//#######################################################################################
//#######################################################################################
//#######################################################################################
//---デバイス有無判定--
if((millis()-startT > 10000) && flag10sec==0 && preflag==0){//10秒後にデバイス有無判定
flag10sec=1;
//デバイス判定
umuB=rcvflagB;
umuR=rcvflagR;
umuM=rcvflagM;
if(umuB==0 && umuR==0 && umuM==0){//デバイス接続ゼロ警告だして停止
Serial.println("<<<<<<<<<<<<<<<<<No Device Error!!>>>>>>>>>>>>>>>>>>>>>");
while(umuB==0 && umuR==0 && umuM==0){}
}
Serial.printf("rcvflagB=%d,umuB=%d,rcvflagR=%d,umuR=%d,rcvflagM=%d,umuM-%d\n\r",rcvflagB,umuB,rcvflagR,umuR,rcvflagM,umuM);
//順序判定
Serial.printf("dBuf1[0]=%x,dBuf1[1]=%x,dBuf1[2]=%x,dBuf1[3]=%x\n\r",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]);
if(dBuf1[0]==0xB5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07){
Serial.println("*********************Recieving OK**********************************");
}
else{
Serial.println("**********************Recieving NG**********************************");
_reboot_Teensyduino_();
}
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@エポック処理へ入る@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
// if(count_upB % 50==0 && preflag==1){
// int cp=umuB*flagB+umuR*flagR+umuM*flagM;
// Serial.printf("cp=%d,countup_B=%d,countup_B_1=%d,preflag=%d\n\r",cp,count_upB,count_upB_1,preflag);
// }
//Serial2.printf("EpochFront:bn=%d time=%d\n\r",bn,millis());
epochinT=millis();//EPOCHに入るための時間カウンタ
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(umuB*flagB+umuR*flagR+umuM*flagM>0 && count_upB>count_upB_1 && (epochinT-epochinT_1) >119 ){//１デバイスでもイン可能 2023/4/21 epochinTで時間規制 119msec以上
epochinT_1=epochinT;
//Serial.print("*");
count_upB_1=count_upB;
//Serial.printf("%d,",count_upB);
epochin=millis();
//SORTING===================================================================================================================================
//CHECK BNO array
//int bnum=bNo();
//Serial2.printf("CHECK[BNO],eX[%d]=%4.1f,eX[%d]=%4.1f,eX[%d]=%4.1f\n\r",bnum,eX[bnum],bnum+1,eX[bnum+1],bnum+2,eX[bnum+2]);
//===Base SORT========================================================
if(sortflagB==1){
Serial.println("----------------------Base Recieve ORDER ERROR＝＞SORT dBuf1=>dBuf11-------------------------------------");
Serial.print(" dBuf1:");
for (j = 0; j < 172; j++) //Search PVT Header------------------------------------------------------
{
if (dBuf1[j] == 0xb5 && dBuf1[j + 1] == 0x62 && dBuf1[j + 2] == 0x01 && dBuf1[j + 3] == 0x07) //Serial1 PVT header
{
PVT1startNo = j;
Serial.printf("PVT1startNo=%d\n\r",PVT1startNo);
}
}
Serial.print(dBuf1[j],HEX);
// Serial.println();
//Sorting--------------------------------------------------------------------------------
for (jj = 0; jj < 172 - PVT1startNo; jj++) //Sort dBuf1[PVT1startNo]～dBuf1[171]　To dBuf11[0]～dBuf11[171-PVT1startNo]
{
dBuf11[jj] = dBuf1[PVT1startNo + jj];
}
for (jj = 0; jj < PVT1startNo; jj++) //Sort
{
dBuf11[jj + 172 - PVT1startNo] = dBuf1[jj];
}
//Serial.print("SORTED:dBuf11:");
for(j=0;j<172;j++)
{
//Serial.print(dBuf11[j],HEX);
dBuf1[j]=dBuf11[j];
}
Serial.print("B");
sortflagB=0;//SORT終わったので戻す
}//sortflagB==1 END
//===Rover SORT========================================================
if(sortflagR==1){//Rover dBuf4 順序ソート
Serial.println("----------------------Rover Recieve ORDER ERROR＝＞SORT dBuf4=>dBuf44-------------------------------------");
Serial.print(" dBuf4:");
for (j = 0; j < 172; j++) //Search PVT Header------------------------------------------------------
{
if (dBuf4[j] == 0xb5 && dBuf4[j + 1] == 0x62 && dBuf4[j + 2] == 0x01 && dBuf4[j + 3] == 0x07) //Serial4 PVT header
{
PVT4startNo = j;
//Serial.println();
Serial.printf("PVT4startNo=%d\n\r",PVT4startNo);
}
Serial.print(dBuf4[j],HEX);
}
//Serial.println();
//Sorting--------------------------------------------------------------------------------
for (jj = 0; jj < 172 - PVT4startNo; jj++) //Sort dBuf1[PVT1startNo]～dBuf1[171]　To dBuf11[0]～dBuf11[171-PVT1startNo]
{
dBuf44[jj] = dBuf4[PVT4startNo + jj];
//Serial.printf("dBuf44[%d]=%x,",jj,dBuf44[jj]);
}
//Serial.println();
for (jj = 0; jj < PVT4startNo; jj++) //Sort
{
dBuf44[jj + 172 - PVT4startNo] = dBuf4[jj];
//Serial.printf("dBuf44[%d]=%x,",jj + 172 - PVT4startNo,dBuf44[jj + 172 - PVT4startNo]);
}
Serial.print("SORTED:dBuf44:");
for(j=0;j<172;j++)
{
Serial.print(dBuf44[j],HEX);
dBuf4[j]=dBuf44[j];
}
Serial.print("R");
sortflagR=0;//SORT終わったので戻す
}//sortflagR==1 END
//===M9N SORT 300byteを１００ずつばらしてソート===================================================
if(sortflagM==1){
if(dBuf5[0]==0x62 && dBuf5[99]==0xB5){
for(j=98;j>-1;j--){//j=98-0
dBuf5[j+1]=dBuf5[j];//M9N1 [98]=>[99],[0]=>[1],,,[0]=0xB5
dBuf5[(j+100)+1]=dBuf5[j+100];//M9N2 [198]=>[199],[100]=>[101],,,[100]=0xB5
dBuf5[(j+200)+1]=dBuf5[j+200];//M9N3 [298]=>[299],[299]=>[1],,,[200]=0xB5
}
Serial.print("M9N1:");
for(j=0;j<100;j++){
Serial.print(dBuf5[j],HEX);
}
Serial.print("M9N2:");
for(j=100;j<200;j++){
Serial.print(dBuf5[j],HEX);
}
Serial.print("M9N3:");
for(j=200;j<300;j++){
Serial.print(dBuf5[j],HEX);
}
}
}//sortflagR==1 END
//SORTING FINISHED==============================================================================================================================================================
//---------------ERROR COUNTER--------------------------------------
//----itow itow0　飛び番エラーチェック 2023/3/15------------
//pvt[0] = B2L(d[9], d[8], d[7], d[6]);
itow0_1=itow0;
//itow0=B2L(dBuf1[9], dBuf1[8], dBuf1[7], dBuf1[6]);
// BNO用　itow check ３秒毎にtp3flag=1にする　2880msec
if(itow0%3000==0)
{
tp3flag=1;
}
else
{
tp3flag=0;
}
if(itow0-itow0_1>122 && bmode==0)
{
Serial.printf("*MISSCOUNT0=%d:[%d],itowCheck:itow=%d,itow0=%d\n\r",miscount0,count_upB,itow0,itow0_1);
Serial2.println();
Serial2.printf("*MISSCOUNT0=%d:[%d],itowCheck:itow=%d,itow0=%d\n\r",miscount0,count_upB,itow0,itow0_1);
}
else
{
ep0No++;
}
//----flags Check 2023/3/15------------------------------------
flags=(long)dBuf1[27];
//Serial.printf("*MISFlags=%d:[%c][%d],flags=%d\n\r",misflags,mode,count_upB,flags);
if(logflag==1 && flags<=67 && bmode==0)
{
misflags++;
Serial.printf("*MISFlags=%d:[%d],itowCheck:flags=%d\n\r",misflags,count_upB,flags);
Serial2.printf("*MISFlags=%d:[%d],itowCheck:flags=%d\n\r",misflags,count_upB,flags);
}
//-----bnoとitow周期Epochあわせ--------------------------------------
int bnmod=itow0%3000;//bnmod=0から2880まで２５段階
int bnstartN=(int)bnmod/120;//0-24EPOCH分
//bno data to bnobuf[180]
int BNObufin=millis();
//Serial2.printf("bnoTobuf:bn=%d,itow0=%d,tp0=%d,tp=%d\n\r",bn,itow0,tp0,tp);
bnoTobuf(bNo());//ログ周期に合わせたbNo０－７４まで３個おきにログ
//Serial.printf("bnoTobuf time=%d-%d\n\r",BNObufin,millis());
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<KEY CONTROL>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>.
//*****************************************************************************************************************************************************************************
//プリプロセス preprocess(uint8_t *db1){//dBuf1[]を配列ポインタで渡される
//*******************************************************************************************************************************************************************************
//Serial.printf("keyin=%c\n\r",keyin);
if(keyin=='p' && logflag==0){//logｓてないときにPREPROCESSできる
mode='p';//Preprocess mode
//Serial.println("<<<<<<<<<PreProcess START BaseLine Measjurement Averaging>>>>>>>>>>>>>>>>>>>>>>>");
Serial.print("p");
califlag=0;//averaging Calculation reset
preprocess(dBuf1);
}
//BNO055 補正値
if(keyin=='b' && logflag==0){//bno yaw角をmHeadに合わせる
bnHosei=mHead-yaw;//mHead=bnHosei+yaw
Serial.printf("<<bnHosei=%4.2f,yaw=%4.3f,mHead=%4.3f>>\n\r",bnHosei,yaw,mHead);
keyin=0x00;
}
// time interval change 1 or 10
if(keyin=='t')
{
if(tperiod==10)
{
tperiod=1;
}
else
{
tperiod=10;
}
keyin="";
}
//bmode RTKがアンテナ無しのときでもログ　モニターできるようにする n push
if(keyin=='n')
{
if(bmode==1)
{
bmode=0;
}
else
{
bmode=1;
}
keyin="";
ckey=0x00;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++LOG Control+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
delay(1);
Serial.printf("---------s key action: keyin=%c,flags=%d,bmode=%d,ckey=%d\n\r",keyin,flags,bmode,ckey);
if((flags>=67 || bmode==1) && keyin=='s'&& ckey==0x00){//FILE NAME 作成 's' bmode=1 ならflags!=67でもログする
Serial.printf("LC:keyin=%c,flags=%d,initflag=%d,preflag=%d,keyin=%c,ckey=%c\n\r",keyin,flags,initflag,preflag,keyin,ckey);
dBuf_To_PVT_RELP(dBuf1,PVTval,RELPOSval);//dBufからPVT値　RELPOS値を得る
Filename(PVTval);//グローバルchar[]のfnameBにファイル名を格納する
keyin=0x00;
initflag=1;
count_upB0=count_upB;
miscount0=0;//s mode reset
int misflags_1=misflags;
misflags=0;//s mode reset
Serial.printf("FileName:misflags_1=%d,misflags=%d Reset\n\r",misflags_1,misflags);
mode='s';//logging Start mode
Serial.printf("LC:Fnamed:t=%d\n\r",millis());
}
if(initflag==1 && logflag==0 && ckey==0x00){//sボタン押し済み＆＆ログしてない＆＆　ckey==0
ckey='o';
logflag=1;
openflag=1;
mode='s';
Serial.printf("LC:keyins:initflag=%d,logflag=%d,ckey=%c\n\r",initflag,logflag,ckey);
Serial.printf("LC:oKey:t=%d\n\r",millis());
}
if(logflag==1 && (count_upB-count_upB0)%250==0 && openflag==1 && ckey==0x00){//1000epoch 毎にCLOSE　１EPOCH抜けるが不良率0.1%３．２９シグマ
ckey='c';
//logflag=0;
mode='m';//monitor mode
Serial.printf("LC:(count_upB-count_upB0)=%d,:t=%d\n\r",(count_upB-count_upB0),millis());
}
//----OPEN FILE--------------------------------------------------------
if(ckey=='o'){
Serial.printf("Open:t=%d\n\r",millis());
keyin=0x00;
myFileA = sd.open(fnameB, FILE_WRITE);//timestampファイル名fnameB
// myFileA.open(fnameB, FILE_WRITE);
if(!myFileA){
Serial.println("=============File Open Error============");
Serial2.println("=============File Open Error============");
// openflag=0;
}
else{
openflag=1;
Serial.println("==============File Open Successed=========");
Serial2.println("==============File Open Successed=========");
}
//namedflag=0;
//logcount=0;
Sflag=1;
//count_upB0=count_upB;//start counter cout_upB0
Serial.printf("===========OpenOK:count_upB=%d,count_upB0=%d,Sflag=%d=========\n\r",count_upB,count_upB0,Sflag);
ckey=0x00;
}//ckey="o" end
//----close File----------------
if(ckey=='c' && logflag==1 && openflag==1){
myFileA.close();
openflag=0;
//Serial.println();
Serial.println("==============File Closed==================");
Serial2.println("==============File Closed==================");
ckey=0x00;
logflag=0;
}
//QUIT SD Logging
if(keyin=='q' && openflag==1 ){
Serial.println("=======QUIT============");
Serial2.println("=======QUIT============");
myFileA.close();
openflag=0;
delay(1);
//keyin=0x00;
preflag=0;
initflag=0;
logflag=0;
mode='m';
ckey=0x00;
}
//^^^^^^^^^^^^^^^SD DATA Ascii UPLOAD ^^^^^^^^^^^^^^^^^^^^^^^^^^
if(keyin=='u')
{
uploadflag=1;//uplload中
//再読み込みでカウントが残っているパラメータをリセット
miscount0=0;
misflags=0;
count_upB=0;
count_upB0=0;
Serial.println("==================UPLOAD START=======================");
Serial2.println("==================UPLOAD START=======================");
int UPLtim0=millis();
int epochn=fup(fnameB);//
int UPLtim1=millis();
openflag=0;
//Serial.println();
delay(5);
Serial.printf("==============File Uploaded==================epochN=%d,Time=%d,time=%d\n\r",epochn,epochn*120,UPLtim1-UPLtim0);
Serial2.printf("==============File Uploaded================epochnN=%d,Time=%d,time=%d\n\r",epochn,epochn*120,UPLtim1-UPLtim0);
ckey=0x00;
logflag=0;
keyin='d';
uploadflag=0;
}
//==================Quit & List=====================================
if(keyin=='d' ){
Serial.println("=======QUIT & LIST============");
Serial2.println("=======QUIT & LIST============");
if(openflag==1){
myFileA.close();
openflag=0;
//Serial.println();
Serial.println("==============File Closed==================");
Serial2.println("==============File Closed==================");
}
doReboot();
/*
ckey=0x00;
delay(10);
sdlist();
while(keyin=='d'){
Serial.print("w");
if(Serial.available()){
keyin=Serial.read();
Serial.printf("<<<<<<<<<<<<<<<<<<<<<<<<Serial KEY IN=%c>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n\r",keyin);
doReboot();
}
if(Serial2.available()){
keyin=Serial2.read();
Serial2.printf("<<<<<<<<<<<<<<<<<<<<<<<<Serial KEY IN=%c>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n\r",keyin);
doReboot();
}
delay(100);
}
//ckey='c';
keyin=0x00;
preflag=0;
initflag=0;
logflag=0;
*/
}
delay(1);
//}
Serial.printf("FileWrite :openflag=%d,Sflag=%d,myFileA=%d\n\r",openflag,Sflag,myFileA);
if(openflag==1 && Sflag==1 && myFileA ){
Serial.print("+");
Serial2.print("+");
mode='s';
for(i=0;i<172;i++){
if(myFileA){
myFileA.write(dBuf1[i]);
}
else {
Serial.println("dBuf1 Write Error>>>>>>>>>>>>>>>>error opening myFile");
Serial2.println("dBuf1 Write Error>>>>>>>>>>>>>>>>error opening myFile");
mode='E';
}
}
for(i=0;i<172;i++){
if(myFileA){
myFileA.write(dBuf4[i]);
}
else {
Serial.println("dBuf4 Write Error>>>>>>>>>>>>>>>>error opening myFile");
Serial2.println("dBuf4 Write Error>>>>>>>>>>>>>>>>error opening myFile");
mode='E';
}
}
for(i=0;i<300;i++){
if(myFileA){
myFileA.write(dBuf5[i]);
}
else {
Serial.println("dBuf5 Write Error>>>>>>>>>>>>>>>>error opening myFile");
Serial2.println("dBuf5 Write Error>>>>>>>>>>>>>>>>error opening myFile");
mode='E';
}
}
//BNO055 data write bnbuf[180] write
for(i=0;i<180;i++){
if(myFileA){
myFileA.write(bnbuf[i]);
}
else {
Serial.println("bnbuf Write Error>>>>>>>>>>>>>>>>error opening myFile");
Serial2.println("bnbuf Write Error>>>>>>>>>>>>>>>>error opening myFile");
mode='E';
}
}
}//myFileA WRITE FINISHED
flagB=0;
flagR=0;
flagM=0;
//Serial.println();
int btmonstart;
// btmonstart=millis();
if(count_upB%tperiod==0)
{
BTMon(mode);
Serial.printf("BTMon:mode=%c,%d\n\r",mode,millis());
}
//Serial.printf("count_upB=%d,tperiod=%d,mode=%c\n\r",count_upB,tperiod,mode);
int epochoutT=millis();
//Serial2.printf("***********************epochinT=%d,epocoutT=%d,count_upB=%d,tperiod=%d,mode=%c\n\r",epochinT,epochoutT,count_upB,tperiod,mode);
}//Epoch END if flagB,flagR,flagM
}//loop end
//*********************************SUBs************************************************************
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^SD DATA ASCII UPLOAD^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
int fup(char *fname)//u keyin
{
Serial2.printf("-----------fup in------------");
int fb=0;
int nc=0;;
int epochn=0;
char cd[4];
char c;
uint8_t bnbuf[180];
if(openflag==1)
{
myFileA.close();
}
if(myFileA.open(fname,FILE_READ)){//fnameB
// if(myFileA.open("3-12-10-10.ubx",FILE_READ)){//3-12-10-10.ubx
while (myFileA.available())
{
//-------[Base]172byte--------
//Serial.printf("Epoch[%d][Base]dBuf1:",epochn);
for(nc=0;nc<172;nc++)
{
dBuf1[nc]=myFileA.read();
//Serial.print(dBuf1[nc],HEX);
fb++;
}
//Serial.printf("[Base]fb=%d\n\r",fb-1);
//-------[Rover]172byte-------
//Serial.printf("Epoch[%d][Rover]dBuf4:",epochn);
for(nc=0;nc<172;nc++)
{
dBuf4[nc]=myFileA.read();
//Serial.print(dBuf4[nc],HEX);
fb++;
}
//Serial.printf("[Rover]fb=%d\n\r",fb-1);
//------[M9N]300byte------------
//Serial.printf("Epoch[%d][M9N]dBuf5:",epochn);
for(nc=0;nc<300;nc++)
{
dBuf5[nc]=myFileA.read();
//Serial.print(dBuf5[nc],HEX);
fb++;
}
//Serial.printf("[M9N]fb=%d\n\r",fb-1);
//------[BNO]180byte------------
//Serial.printf("Epoch[%d][BNO]bnbuf:",epochn);
for(nc=0;nc<180;nc++)
{
bnbuf[nc]=myFileA.read();
//Serial.print(bnbuf[nc],HEX);
fb++;
}
bnoBintoDbl(bnbuf);//BNO[0-2] Binary To Double
//Serial.printf("[BNO]fb=%d\n\r",fb-1);
epochn++;
BTMon('s');
} //while myFileA.available() end
} //if myFileA Open
else //OPEN ERROR
{
Serial.println("UPLOAD: Read File Open Eror");
}
//read finished
myFileA.close();
return epochn;
} //fup end
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++MONITOR BLUETOOTH SPP++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//測定中変化するパラメータ1：itow,flags,pdop,hacc,numSV,miscount
//測定中変化するパラメータ2：double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN
//測定中変化するパラメータ3：gSpeed,headMot,velN,velE,velD,relN,relE,relD
//初期確認パラメータ3：int fixtime,double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//FIX時間と基準位置平均値5個
//初期確認パラメータ4：double rNsgmN,rEsgmN,rDsgmN,rLsgmN,mHsfmN;//基準位置のσ値　5個
//
void BTMon(char md){
dBuf_To_PVT_RELP(dBuf1,PVTBval,RELPOSBval);//dBuf1から[BASE]PVT値　RELPOS値を得る
long monflags=PVTBval[11];
long monpdop=PVTBval[27];
double pdop=(double)monpdop*0.01;//
long monhacc=PVTBval[18];
long monnumsv=PVTBval[13];
//Base: relN,relE,relD,relL,velN,velE,velD,gSpeed,headMot M9N: velNm9n,velEm9n,velDm9n,headMotm9n
itow=PVTBval[0];
velN=PVTBval[20];
velE=PVTBval[21];
velD=PVTBval[22];
gSpeedB=PVTBval[23];
headMotB=PVTBval[24];
//Serial.printf(">>>>PVTBval[24]=%d\n\r",PVTBval[24]);
// headMot=(int)((float)(PVTBval[24]*0.00001));
//Serial.printf("PVTBval[24]=%d,PVTBva*0.00001=%3.6f,headMot=%d\n\r",PVTBval[24],(float)(PVTBval[24]*0.00001),headMot);
//基準位置ゼロにした現在位置
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;
// Serial.printf("BTMon:flags=%d,pdop=%3.2f,hacc=%d,numSV=%d,miscount=%d,relN=%4.2f,relE=%4.2f,relD=%4.2f,relL=%4.2f\n\r",monflags,pdop,monhacc,monnumsv,miscount,relN,relE,relD,relL);
//Rover モニター----------------------------------------
//double relNm,relEm,relDm,relLm,mHead;//MovingBaseRover RELPOS値
dBuf_To_PVT_RELP(dBuf4,PVTRval,RELPOSRval);//dBuf4 [Rover] PVT RELPOS 変換
itowm=PVTRval[0];
flagsm=PVTRval[11];
velNm=PVTRval[20];
velEm=PVTRval[21];
velDm=PVTRval[22];
gSpeedR=PVTRval[23];
headMotR=PVTRval[24];
//headMotm=(int)((float)(PVTRval[24]*0.00001));
//Serial.printf("PVTRval[24]=%d,PVTRva*0.00001=%3.6f,headMotm=%d\n\r",PVTRval[24],(float)(PVTRval[24]*0.00001),headMotm);
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;
mHead=(double)RELPOSRval[8]*0.00001;
//状態行
flagsB=monflags;
pdopB=monpdop;
haccB=PVTBval[18];
flagsR=PVTRval[11];
pdopR=PVTRval[27];
haccR=PVTRval[18];
//受信状態確認 uploadflag==1 の場合　flags,itowでミスカウント
if(uploadflag==1 && bmode==0)
{
flags0=PVTBval[11];//flags //dBuf0[27];//Base PVT flags0
if(flags0==131 && fix0==0){
miscount0=0;
fix0=1;
}
itow0_1=itow0;
itow0 =PVTBval[0];// B2L(dBuf0[9], dBuf0[8], dBuf0[7], dBuf0[6]); //dBuf0 iTow Calc
if(itow0-itow0_1>122 && bmode==0){//120msec
miscount0++;
Serial.printf("*UPLOAD:MISSCOUNT0=%d:[%d],itowCheck:itow0=%d,itow0_1=%d\n\r",miscount0,count_upB,itow0,itow0_1);
Serial2.printf("*UPLOAD:MISSCOUNT0=%d:[%d],itowCheck:itow0=%d,itow0_1=%d\n\r",miscount0,count_upB,itow0,itow0_1);
}
else{
ep0No++;
}
//
//uploadでのflagsエラーカウント misflags
if(flagsB<=67 && bmode==0 )
{
misflags++;
Serial.printf("*UPLOAD:MISSFlags=%d:[%d],misflags=%d\n\r",misflags,count_upB,flags);
Serial2.printf("*UPLOAD:MISSFlags=%d:[%d],misflags=%d\n\r",misflags,count_upB,flags);
}
count_upB++;
}
//M9N 補間評価用モニター
// 校正基準位置　rNaveN,rNstd,rEaveN,rEstd,rDaveN,rDstd,rLaveN,rLstd,mHead,rLmaveN,rLmdvN
//m9n
m9nToPVT(dBuf5);//dBuf5 [M9N]M9N３個分を変換
//Base行
itowB=itow;
float NYB=relN-rNaveN;
float EXB=relE-rEaveN;
float DZB=relD-rDaveN;
//Rover行
itowR=PVTRval[0];
float NYR=NYB+relNm;
float EXR=EXB+relEm;
float DZR=DZB+relDm;
//結果
//usbs=1
if(usbs==1)
{
if(shokai0==0 ){
shokai0=1;
Serial.printf("itow,velN,velE,velD,gSpeed,headMot,");
Serial.printf("itowm9n_1,velNm9n_1,velEm9n_1,velDm9n_1,gSpeedm9n_1,headMotm9n_1,");
Serial.printf("itowm9n_2,velNm9n_2,velEm9n_2,velDm9n_2,gSpeedm9n_2,headMotm9n_2,");
Serial.printf("itowm9n_3,velNm9n_3,velEm9n_3,velDm9n_3,gSpeedm9n_3,headMotm9n_3\n\r");
//Serial2.printf("[Rover],%d,%4.2f,%4.2f,%4.2f,%d,%4.2f,%4.2f,%4.1f\n\r",itowR,NYR,EXR,DZR,gSpeedR,mHead,bnHosei+eX[0],relLm);
}
//USB
Serial.printf("[%c],%d,%d,%4.2f,%4.2f,%d,%d,%d,%d,%d,%d,%d\n\r",md,flagsB,flagsR,pdopB,pdopR,haccB,haccR,miscount0,misflags,count_upB-count_upB0,tp,tp0);
Serial.printf("[Base],%d,%4.2f,%4.2f,%4.2f,%d,%d,\n\r",itowB,NYB,EXB,DZB,gSpeedB,headMotB);
Serial.printf("[Rover],%d,%4.2f,%4.2f,%4.2f,%d,%4.2f,%4.2f,%4.1f\n\r",itowR,NYR,EXR,DZR,gSpeedR,mHead,bnHosei+eX[0],relLm);
}
else{
//BlueTooth
Serial2.printf("[%c],%d,%d,%4.2f,%4.2f,%d,%d,%d,%d,tp=,%d,tp0=,%d,itow0=,%d,bn=,%d,bns=,%d\n\r",md,flagsB,flagsR,pdopB,pdopR,haccB,haccR,miscount0,misflags,tp,tp0,itow0,bn,bns);
Serial2.printf("[Base],%d,%4.2f,%4.2f,%4.2f,%d,%d,\n\r",itowB,NYB,EXB,DZB,gSpeedB,headMotB);
Serial2.printf("[Rover],%d,%4.2f,%4.2f,%4.2f,%d,%4.2f,%4.1f\n\r",itowR,NYR,EXR,DZR,gSpeedR,mHead,relLm);
int bnc=bn;//ここで変数を固定cして、割り込みで変更されても不変にする
int bnsc=bns;//ここで変数を固定して、割り込みで変更されても不変にする
int tpc=tp;
int tp0c=tp0;
int tpflagc=tpflag;
for(i=bnsc;i<bnc;i++)
{
//Serial2.printf("[BNO],%d,%d,%d,%4.3f,%4.3f,%4.3f,%d,%d,%d,%d\n\r",i%75,bnsc,bnc,eX[i%75],eY[i%75],eZ[i%75],tpc,tp0c,millis(),btime[i%75]);
Serial2.printf("[BNO],%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",i%75,eX[i%75],eY[i%75],eZ[i%75],laX[i%75],laY[i%75],laZ[i%75],grX[i%75],grY[i%75],grZ[i%75],btime[i%75]);
}
bns=bnc;//bnsで次回に出力済みのbn番号を知らせる
//Magenoder output
if(mtn>0)
{
Serial2.printf("[MAG],");
for(i=1;i<mtn+1;i++)
{
//debug Serial2.printf(",<%d,%d,%d,%d>,",mt[i],tp03k,itow03k,mti[i]);
Serial2.printf("%d,",mt[i]);
}
mtn=0;
Serial2.println();
}
else{
Serial2.printf("[MAG],\n\r");
}
/*
int bnum=bNo();
Serial2.printf("[BNO],%d:,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f%,%d,%d,\n\r",bnum,eX[bnum],eY[bnum],eZ[bnum],laX[bnum],laY[bnum],laZ[bnum],grX[bnum],grY[bnum],grZ[bnum],btime[bnum],tp0);
int bnum1=(bnum+1)%75;
Serial2.printf("[BN1],%d:,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f%,%d,%d,\n\r",bnum1,eX[bnum1],eY[bnum1],eZ[bnum1],laX[bnum1],laY[bnum1],laZ[bnum1],grX[bnum1],grY[bnum1],grZ[bnum1],btime[bnum1],tp0);
int bnum2=(bnum+2)%75;
Serial2.printf("[BN2],%d:,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f%,%d,%d,\n\r",bnum2,eX[bnum2],eY[bnum2],eZ[bnum2],laX[bnum2],laY[bnum2],laZ[bnum2],grX[bnum2],grY[bnum2],grZ[bnum2],btime[bnum2],tp0);
*/
//GRAPH用
// Serial2.printf("[Graph],%d,%d,%4.2f,%d,%d,%d,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f%,%d,\n\r",count_upB,itow0,mHead,tp,tp0,bnum,eX[bnum],eY[bnum],eZ[bnum],laX[bnum],laY[bnum],laZ[bnum],grX[bnum],grY[bnum],grZ[bnum],btime[bnum]);
}// not USB = BlueTooth
}//btmon end
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//M9N dBuf5[] to PVT 3Blocks
void m9nToPVT(uint8_t *dB){//300byte データ配列のポインタを渡す
uint8_t db1[100],db2[100],db3[100];
for(i=0;i<300;i++){//300byteを3分割
if(i<100){
db1[i]=dBuf5[i];
}
if(i>=100 && i<200){
db2[i-100]=dBuf5[i];
}
if(i>=200 && i<300){
db3[i-200]=dBuf5[i];
}
}
//PVT変換
PVTcnv(db1,PVTvalm9n_1);
PVTcnv(db2,PVTvalm9n_2);
PVTcnv(db3,PVTvalm9n_3);
//itowm9n[3],velNm9n[3],velEm9n[3],velDm9n[3],gSpeedm9n[3],headMotm9n[3];
itowm9n_1=PVTvalm9n_1[0];
itowm9n_2=PVTvalm9n_2[0];
itowm9n_3=PVTvalm9n_3[0];
//
velNm9n_1=PVTvalm9n_1[20];
velNm9n_2=PVTvalm9n_2[20];
velNm9n_3=PVTvalm9n_3[20];
//
velEm9n_1=PVTvalm9n_1[21];
velEm9n_2=PVTvalm9n_2[21];
velEm9n_3=PVTvalm9n_3[21];
//
velDm9n_1=PVTvalm9n_1[22];
velDm9n_2=PVTvalm9n_2[22];
velDm9n_3=PVTvalm9n_3[22];
//
gSpeedm9n_1=PVTvalm9n_1[23];
gSpeedm9n_2=PVTvalm9n_2[23];
gSpeedm9n_3=PVTvalm9n_3[23];
//
headMotm9n_1=PVTvalm9n_1[24];
headMotm9n_2=PVTvalm9n_2[24];
headMotm9n_3=PVTvalm9n_3[24];
//
}
//---------------Base（Serial1）から1エポックデータ取得----------
void oneBaseRcv(uint8_t *d){
int i;
int counterB=0;
while(counterB<172){
//--Serial1 Base F9P read--
if(Serial1.available()>171){ //バッファ1が172バイトになるまで処理しない
//Serial.println();
i=0;//Base Serial1 counter
while(i<172){
digitalWrite(ledPin, HIGH); // set the LED on
while(Serial1.available()){
d[i]=Serial1.read();
i++;
}
}
digitalWrite(ledPin, LOW); // set the LED on
counterB++;
}//
}//while couterB>172
}//oneBaseRcv end
//---------------dBufからPVTとRELPOSへ変換-------------------
//--------------pointer渡し---------------------------------
//---uint_8t *dは、d[172]F9Pの生バイナリーPVT+RELPOS 172byte---
//---uint8_t *pvtは、pvtdata[100]
//---uint8_t *relpは、relpdata[72]
//-----------------------------------------------------------
void dBuf_To_PVT_RELP(uint8_t *d,long *pvtval,long *relpval){
//--dataから分離--
uint8_t pvt[100];
uint8_t relp[72];
int i=0;
int k=0;
int j=0;
for(i=0;i<172;i++){
if(i<100){//PVT抽出pvt[0]-pvt[99]
pvt[k]=d[i];
k++;
}
else if(i>99){//relp抽出relp[0]-relp[71]
relp[j]=d[i];
j++;
}
}
//--PVT変換--
PVTcnv(pvt,pvtval);
//--relp変換--
RELPOScnv(relp,relpval);
}//dBuf_To_pvt_relp()　end
//-------------------Filename()-------------------------------
//--------Baseデータからタイムスタンプのファイル名をつける--------
//--------------pointer渡し----------------------------------　
//---uint_8t *dは、d[172]F9Pの生バイナリーPVT+RELPOS 172byte---
//---long *PVT,*RELPは、変換後のlong配列 PVT[33],RELP[20]------
//---char *fnmは、ファイル名char配列　fnm[30]-------------------
//-----------------------------------------------------------
void Filename (long *PVT) {//ファイル名をタイムスタンプ
//if (PVTBval[11] == 131 && Sflag == 0) { //start
//---PVTBval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec---
int JST = (PVT[4] + 9) % 24; //UTC hourをJSTに変換
if (JST < 9) { //UTCが０時以前の場合日付を日本日付に修正
JST_day = 1;
}
else {
JST_day = 0;
}
String stime = String(PVT[2], DEC) + "-" + String(PVT[3] + JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVT[5], DEC); //MMDDHHMM
String stimeB = stime + ".ubx"; //UBX Binary File
String stimeAv="Ave"+ stime + ".txt"; //Averaging txt File
int slenB = stimeB.length() + 1;
int slenAv=stimeAv.length()+1;
//ファイル名はchar配列なのでStringからchar配列変換　fname[]を得る
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array
stimeAv.toCharArray(fnameAv, slenAv); //stimeAv to fnameAv[] chara Array
Serial.print("fnameB=");
Serial.println(stimeB);
Serial.print("fnameAv=");
Serial.println(stimeAv);
delay(10);
}//Filename() end ******************************************************************************************************************************************
void readkijun(){
//--------再起動時の基準位置読み込み　kijun.txtを読み込む preprocessが新たに更新されない限りこのkijun値がデフォルト-------
String str;
if(myFileA.open("kijun.txt",FILE_READ)){
while (myFileA.available())
{
str+=char(myFileA.read());
}
Serial.println(str);
} else
{
Serial.println("kijun File Open Eror");
}
String kdatastr[200];
double kdata[200];
int index =split(str,',',kdatastr);
// Serial.printf("str.len=%d\n\r",sizeof(str));
for(i=0;i<index;i++)
{
//Serial.println(kdatastr[i]);
kdata[i]=kdatastr[i].toFloat();
// Serial.printf("kdata[%d]=%4.2f\n\r",i,kdata[i]);
}
delay(8);
// myFileT.println(stc1);
myFileA.close(); //毎回クローズして万一のアクシデントに備える
//-----------------------------------------------
//rNaveN,rEaveN,rDaveN,rLaveN,rNstd,rEstd,rDstd,rLstd,rLmaveN,rLmstd,mHead);
rNaveN=kdata[0];
rEaveN=kdata[1];
rDaveN=kdata[2];
rLaveN=kdata[3];
rNstd=kdata[4];
rEstd=kdata[5];
rDstd=kdata[6];
rLstd=kdata[7];
rLmaveN=kdata[8];
rLmstd=kdata[9];
mHead=kdata[10];
int pday=kdata[11];
int phour=kdata[12]+9;
int pmin=kdata[13];
int psec=kdata[14];
Serial.printf("KijunData:rNaveN=%4.2f,rEaveN=%4.2f,rDaveN=%4.2f,rLaveN=%4.2f,rNstd=%4.2f,rEstd=%4.2f,rDstd=%4.2f,rLstd=%4.2f,rLmaveN=%4.2f,rLmstd=%4.2f,mHead=%4.2f,date=%d:%d:%d:%d\n\r",rNaveN,rEaveN,rDaveN,rLaveN,rNstd,rEstd,rDstd,rLstd,rLmaveN,rLmstd,mHead,pday,phour,pmin,psec);
Serial2.printf("KijunData:%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,<%d:%d:%d:%d>,\n\r",rNaveN,rEaveN,rDaveN,rLaveN,rNstd,rEstd,rDstd,rLstd,rLmaveN,rLmstd,mHead,pday,phour,pmin,psec);
}
//------------------split 関数---------------------------------
int split(String data, char delimiter, String *dst){
int index = 0;
int datalength = data.length();
for (int i = 0; i < datalength; i++) {
char tmp = data.charAt(i);
if ( tmp == delimiter ) {
index++;
}
else dst[index] += tmp;
}
Serial.printf("split:datalength=%d,index=%d\n\r",datalength,index);
return (index + 1);
}
//+++PVTcnv++++++++++++++++++++++++++++++++++++++++++++++++++++++
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];
//Serial.printf("PVTcnv:hour=%d\n\r",pvt[4]);
//5:min[15]
pvt[5] = (long)d[15];
//Serial.printf("PVTcnv:min=%d\n\r",pvt[5]);
//6:sec[16]
pvt[6] = (long)d[16];
//Serial.printf("PVTcnv:sec=%d\n\r",pvt[6]);
//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]=%d\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 + 61], d[s + 60], d[s + 59], d[s + 58]);
//20:flags[60-63]
relpos[20] = B2L(d[s + 65], d[s + 64], d[s + 63], d[s + 62]);
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;
// Serial.printf("itochar:i=%d,d[0]=%x,d[1]=%x,d[2]=%x,d[3]=%x\n\r",i,d[0],d[1],d[2],d[3]);
}
//++++++++++++++++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=====================================================
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXプリプロセスで全部関数内で処理して、測定メインを使わないXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
void preprocess(uint8_t *db1){//dBuf1[]を配列ポインタで渡される
//Serial.println("<<<<<<<<<<<<<<<<<preprocess IN>>>>>>>>>>>>>>>>>>");
// ============GNSS RTK top terms========================
/*GLOBAL 変数の参照用コピー
static uint8_t dBuf1[172], dBuf4[172],dBuf5[300];//バッファなので32の倍数でないといけない
//---ubx 変換データ---
uint8_t PVTdata[100];
uint8_t RELPdata[72];
long PVTval[33];
long PVT1val[33];
long RELPOSval[21];
long RELPOS1val[21];
long PVTBval[33];//Base PVT
long PVTRval[33];
long RELPOSBval[21];
long RELPOSRval[21];
*/
//データ配列
//=============================================================================================
dBuf_To_PVT_RELP(dBuf1,PVTBval,RELPOSBval);//Base PVT RELPOS 変換
dBuf_To_PVT_RELP(dBuf4,PVTRval,RELPOSRval);//Rover PVT RELPOS 変換
//=============================================================================================
//変換結果確認
// int nn;
// for(nn=0;nn<21;nn++){
// Serial.printf("RELPOSBval[%d]=%d\n\r",nn,RELPOSBval[nn]);
// }
//=============================================================================================
//Fix判定
//=============================================================================================
//受信状態確認　flags,itowでミスカウント
flags0=PVTBval[11];//flags //dBuf0[27];//Base PVT flags0
if(flags0==131 && fix0==0){
miscount0=0;
fix0=1;
}
itow0_1=itow0;
itow0 =PVTBval[0];// B2L(dBuf0[9], dBuf0[8], dBuf0[7], dBuf0[6]); //dBuf0 iTow Calc
if(itow0-itow0_1>122){//120msec
miscount0++;
}
else{
ep0No++;
}
if(itow0>itow0_1 && flags0>0 && Sflag==0){ //<131
// Serial.printf("Base_PVT_i=,%d,[,%d,],mis0=,%d,(,%x,%x,%x,%x),%d,flags0=,%d\n\r",i,itow0,miscount0,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],millis(),flags0);
}
//============================================================================================================
//Base 受信して、基線長解析をおこなう
//------------------------------------------------------------------
//---------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;
// Serial.printf("relN=%4.2f,relE=%4.2f,relD=%4.2f,relL=%4.2f\n\r",relN,relE,relD,relL);
//--------------------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のばらつきを統計処理
//Serial.printf("relLm=%4.2f\n\r",relLm);
//--------------------------------------------------------------------------------------------------------
//*****************FIX 判定*********************************************************
if(flags0==1 && stflag==0){
RTKstart=millis();
stflag=1;
//Serial.printf("@@@@@@@@RTKstart=%d\n\r",RTKstart);
}
else if(flags0>=67 && fixflag==0){//fixed
RTKfix=millis();
fixflag=1;
//Serial.printf("@@@@@@@@RTKfix=%d\n\r",RTKfix);
//Fixしたらファイル名を作る
Filename (PVTBval);//グローバルchar[]のfnameAvにファイル名を格納する
}
if(stflag==1 && fixflag==1 && fixed==0){
fixtime=int(((double)RTKfix-RTKstart)/1000);//sec
//Serial.printf(" <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<fixtime=%d,RTKfix=%d,RTKstart=%d>>>>\n\r",fixtime,RTKfix,RTKstart);
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("avecount=%d,BL_Averaging_fixflag=%d,califlag=%d\n\r",avecount,fixflag,califlag);
if((fixflag==1 || bmode==1) && califlag==0 ){//bmode==1 室内アンテナ状態で、fixしなくてもpを回す。
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
// Serial.printf("10==:rLaveN=%4.2f,rLsum=%4.2f\n\r",rLaveN,rLsum);
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
//Serial.printf("10==:rLdvN=%4.2f,rLdvsum=%4.2f,\n\r",rLdvN,rLdvsum);
mHdvN=mHdvsum/10;
//Serial.printf("mHead=%4.3f,mHdvN=%4.3f\n\r",mHead,mHdvN);
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>10>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,rNaveN,rNdvN,rEaveN,rEdvN,rDaveN,rDdvN,rLaveN,rLdvN,rLmaveN,rLmaveN);
}//if 10count averaging stdev end
//---N1 10-500count cal----
if(avecount >10 && avecount<calcount){
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
//Serial.printf("rLmaveN1=%4.2f\n\r",rLmaveN1);
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
//Serial.printf("rLmdvN1=%4.2f\n\r",rLmdvN1);
mHdvN1=((avecount-1)*(mHdvN+pow(mHaveN,2))+pow(mHead,2))/avecount-pow(mHaveN1,2);
//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);
//Serial.printf("mHead=%4.3f,mHdvN1=%4.3f,mHdvN=%4.3f\n\r",mHead,mHdvN1,mHdvN);
//Serial.printf("10<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("mHaveN=%4.3f,mHstd=%4.3f\n\r",mHaveN,mHstd);
//Serial.printf("MON:relLm=%4.3f,rLmaveN1=%4.3f,rLmdvN1=%4.3f\n\r",relLm,rLmaveN1,rLmdvN1);
Serial2.printf("CaliLAST:avecount=%d,calcount=%d\n\r",avecount,calcount);
//Serial.printf("avecount=%d\n\r",avecount);
//Serial.print(",");
if(avecount>=calcount){//averaging result print
Serial.println();
Serial.printf("<Base<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("<Rover<avcnt=%d,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f,rLmav=%4.2f,rLmdv=%4.2f,mHaveN=%4.2f,mHstd=%4.3f>>\n\r",avecount,rLaveN,rLstd,mHead,rLmaveN,rLmdvN,mHaveN,mHstd);
Serial2.printf("<Base<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);
Serial2.printf("<Rover<avcnt=%d,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f,rLmav=%4.2f,rLmdv=%4.2f,mHaveN=%4.2f,mHstd=%4.3f>>\n\r",avecount,rLaveN,rLstd,mHead,rLmaveN,rLmdvN,mHaveN,mHstd);
//---PVTBval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec---
//Serial.printf("<<epNo=%d\n\r",epNo);//epNoのみ=%3.3f,%c,%d,sa=%d>>\n\r",mHead,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1,sc,avecount,sa);
//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,%4.2f,%d,%d,%d,%d\n\r",rNaveN,rEaveN,rDaveN,rLaveN,rNstd,rEstd,rDstd,rLstd,rLmaveN,rLmstd,mHaveN,PVTBval[3],PVTBval[4],PVTBval[5],PVTBval[6]);
//String stc1="epNo,itow,flags,numSV,hacc,fixtime,fixpercent,miscount0,miscount1,pDop,rrN,rrE,rrD,mHead,relN,relE,relD,relL";
//---Averaging Result File Write
//myFileAv.open(fnameAv,FILE_WRITE);
if(myFileA.open(fnameAv,FILE_WRITE)){
myFileA.write(monC,strlen(monC));
Serial.println("Preprocess ResultFile Write Successed!");
}
else{
Serial.println("Preprocess ResultFile Write FAILED!");
}
delay(8);
// myFileT.println(stc1);
myFileA.close();//毎回クローズして万一のアクシデントに備える
//--------再起動時の基準位置読み込み　kijun.txtを読み込む--------
for(i=0;i<strlen(monC);i++){
Serial.printf("monC[%d]=%c\n\r",i,monC[i]);
}
if(myFileA.open("kijun.txt",FILE_WRITE_BEGIN)){//重ね書き指定
myFileA.write(monC,strlen(monC));
Serial.println("Preprocess ResultFile Write Successed!");
}
else{
Serial.println("Preprocess Result kijun File Write FAILED!");
}
delay(8);
// myFileT.println(stc1);
myFileA.close(); //毎回クローズして万一のアクシデントに備える
//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
preflag=1; //preprocess STOP
keyin=0x00;//keyin=p finished
mode='m';//mode m に戻す
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment