Instantly share code, notes, and snippets.
Created
October 10, 2021 23:09
-
Save dj1711572002/d63d78ee697406e8d5bc8270a6da1d13 to your computer and use it in GitHub Desktop.
M5Stack Core2 RTK-MovingBase Monitor Program MovingBase Data Dsilaying with Sprite
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| //#include <M5Stack.h> | |
| #include <M5Core2.h> | |
| #include <esp_now.h> | |
| #include <WiFi.h> | |
| #define ESP32 | |
| AXP192 power; | |
| #include <driver/i2s.h> | |
| #define CONFIG_I2S_BCK_PIN 12 | |
| #define CONFIG_I2S_LRCK_PIN 0 | |
| #define CONFIG_I2S_DATA_PIN 2 | |
| #define CONFIG_I2S_DATA_IN_PIN 34 | |
| #define Speak_I2S_NUMBER I2S_NUM_0 | |
| esp_now_peer_info_t slave; | |
| #include "SD.h" | |
| #include <AXP192.h> | |
| //---------------------swipe-------------- | |
| // Defines gestures | |
| Gesture swipeRight("swipe right", 160, DIR_RIGHT, 30, true); | |
| Gesture swipeDown("swipe down", 120, DIR_DOWN, 30, true); | |
| Gesture swipeLeft("swipe left", 160, DIR_LEFT, 30, true); | |
| Gesture swipeUp("swipe up", 120, DIR_UP, 30, true); | |
| String objNm; | |
| //---------------------------------------------------------------------------------------- | |
| //--Sprite-- | |
| TFT_eSprite tftSprite = TFT_eSprite(&M5.Lcd); | |
| //TFT_eSprite tftSprite = TFT_eSprite(&M5.Lcd); | |
| //TFT_eSprite DisIMUbuff = TFT_eSprite(&M5.Lcd); | |
| #define MAGENTA 0xF81F | |
| #define GREENYELLOW 0xAFE5 /* 173, 255, 47 */ | |
| #define YELLOW 0xFFE0 /* 255, 255, 0 */ | |
| #define WHITE 0xFFFF /* 255, 255, 255 */ | |
| #define FORMAT_SPIFFS_IF_FAILED true | |
| uint16_t Col1,Col2; | |
| //IMU | |
| float accX = 0.0F; | |
| float accY = 0.0F; | |
| float accZ = 0.0F; | |
| //---esp-now-- | |
| int nc;//ButtonA ColorCount | |
| int navflag=0; | |
| int N172=0; | |
| int N172_1; | |
| int chN=0;//Print Data Counter | |
| int chN_1;//valid data No | |
| int totalN=0; | |
| uint8_t cdata[1]; | |
| uint8_t CH0data[57];//CH0 right data | |
| uint8_t CH1data[57];//CH1 leftdata | |
| //File declaraation | |
| //#include "FS.h" | |
| //#include "SPIFFS.h" | |
| int sNo=0; | |
| #define LED_PIN 10 | |
| //sd Para----------------------- | |
| String bufstr[50]; | |
| String datastr; | |
| String datastrArry[50]; | |
| String relArry[50]; | |
| String relstr; | |
| String timestr; | |
| int sdataN=0; | |
| char GGAc[83]; | |
| File file; | |
| File f; | |
| File root; | |
| int loopN=0; | |
| String stime;//filename | |
| int kaisu=0; | |
| int ch0N,ch1N; | |
| String sdfname[100]; | |
| char fname[30];//File name char Array | |
| String str; | |
| String strTotal; | |
| int i,j,k; | |
| String Listname[100] ; | |
| char file_name[20] = "/GGA.txt"; | |
| int var_size = 2000; | |
| //Stastic==================================== | |
| int coff; | |
| static int dcount; | |
| static double haccAve[10],haccSum[10]; | |
| static int haccNG[10]; | |
| static double LenAve[10],LenSum[10]; | |
| static int LenNG[10]; | |
| //Mode | |
| int Monitormode=1; | |
| int Savemode=0; | |
| int Listmode=0; | |
| int Dirmode=0; | |
| //Menu sd ------------- | |
| static uint8_t sdflag=0; | |
| static uint8_t sdFlag,sdFlag_1; | |
| static uint8_t sdFlag1; | |
| static uint8_t sdFlag2; | |
| static int firstFlag=1; | |
| static int openflag; | |
| static int haltflag=0; | |
| static int initFlag=1; | |
| uint8_t dFlag;//Dump Mode | |
| uint8_t monFlag;//Monitor Flag | |
| int t,t_1,tim; | |
| char Logmark; | |
| int ch0Flag,ch1Flag; | |
| int ch_Flag; | |
| int Nextchflag=0; | |
| int autoFlag=0; | |
| int logFlag=0; | |
| int p0x=160; | |
| int p0y=60; | |
| int xoff=40; | |
| int yoff=20; | |
| int ch; | |
| static float dvx,dvy; | |
| static int Rxw,Ryw; | |
| //----------plot function | |
| int px,py; | |
| int Lx,Ly; | |
| int px1,py1; | |
| int Lx1,Ly1; | |
| //NAV------------------- | |
| int n; | |
| //=============monitor data================ | |
| int data_len; | |
| static int iyear; | |
| static int imonth; | |
| static int iday; | |
| static int ihour; | |
| static int imin; | |
| static int isec; | |
| static int csec; | |
| // ch change----------------- | |
| //int ch=0; | |
| static int iTow[50]; | |
| static long relN[50]; | |
| static long relE[50]; | |
| static long relD[50]; | |
| static long accN[50]; | |
| static long accE[50]; | |
| static long accD[50]; | |
| static long accLen[50]; | |
| static long Length[50]; | |
| static int relHPN[50];//小数点精度用符号付バイト | |
| static int relHPE[50];//小数点精度用符号付バイト | |
| static int relHPD[50];//小数点精度用符号付バイト | |
| static int relHPLen[50];//小数点精度用符号付バイト | |
| static float relNf[50];//浮動小数点relN | |
| static float relEf[50];//浮動小数点relE | |
| static float relDf[50];//浮動小数点relD | |
| static float Lengthf[50];//浮動小数点relLength | |
| static long Long[50]; | |
| static long Lati[50]; | |
| static long Acc2d[50]; | |
| static long Acc3d[50]; | |
| static int Acc2d0[50]; | |
| static int Acc2d1[50]; | |
| static int numSV[50]; | |
| //static int numSV0[50]; | |
| //static int numSV1[50];//satellite number | |
| static long Head[50]; | |
| static long gSpeed[50];//2021/3/16 add | |
| static long headMot[50];//2021/3/16 add | |
| static long Seah[50];//2021/3/17 add | |
| String sdata; | |
| // AVERAING | |
| float relNfsum,relEfsum,relDfsum,Lengthfsum; | |
| float relNave,relEave,relDave,Lengthave; | |
| float relNstd,relEstd,relDstd,Lengthstd; | |
| //---------------------------- | |
| //long Acc2d; | |
| //char numSV;//satellite number | |
| //long Length; | |
| long su; | |
| int i2,i1; | |
| //+++++++++++++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); | |
| if ((b4 & 0x80) && 0x80){//最上位ビットたっていればマイナス | |
| su = -(256-(int)b1)+(255-(int)b2)*256+(255-(int)b3)*65536+(255-(int)b4)*256*256*256; | |
| //pc.printf("B2L-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
| } | |
| else { | |
| su=(int)b1+(int)b2*256+(int)b3* 65536+(int)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; | |
| } | |
| //++++++2バイトを整数i2++++++++++++ | |
| long B2I(uint8_t b2,uint8_t b1){ | |
| i2=b1+b2*256; | |
| return i2; | |
| } | |
| //+++++++++++符号付1バイトsbを符号付整数i１へ変換++++++++++++ | |
| int B2sb(uint8_t sb ) { | |
| if( sb > 128 )//最上位8ビット目が１だったら負数 | |
| { | |
| i1 = 128 - int(sb); | |
| } | |
| else//整数なら | |
| { | |
| i1 = int(sb); | |
| } | |
| return i1; | |
| } | |
| // 送信コールバック | |
| void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { | |
| char macStr[18]; | |
| snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X", | |
| mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); | |
| Serial.print("Last Packet Sent to: "); | |
| Serial.println(macStr); | |
| Serial.print("Last Packet Send Status: "); | |
| Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail"); | |
| // 画面にも描画 | |
| //tftSprite.fillScreen(BLACK); | |
| // tftSprite.setCursor(10,160); | |
| //tftSprite.print("Last Packet Sent to: \n "); | |
| // tftSprite.println(macStr); | |
| // tftSprite.print("Last Packet Send Status: \n "); | |
| //tftSprite.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail"); | |
| } | |
| //---------------------SPIFFS Utility------------------------------------- | |
| 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); | |
| } | |
| //************************************************************************************************* | |
| //+++++++++++++++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; | |
| } | |
| //---------------------------------------------------------------------------- | |
| //---------------------------------------------------------------------------- | |
| // ESP-NOW 受信コールバック | |
| //---------------------------------------------------------------------------- | |
| //----------------------------------------------------------------------------- | |
| void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) | |
| { | |
| char macStr[18]; | |
| snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X", | |
| mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); | |
| //Serial.printf("Last Packet Recv from: %s\n", macStr); | |
| //Serial.printf("Last Packet Recv Time=%d,Datalength=%d \n\r", millis(),data_len); | |
| //Serial.println(data_len); | |
| if(data_len==172) | |
| { | |
| navflag=1; | |
| N172_1=N172; | |
| N172++; | |
| //Serial.printf("Last Packet Recv from: %s\n", macStr); | |
| //Serial.printf("Last Packet Recv n=%d,Time=%d,Datalength=%d \n\r",n, millis(),data_len); | |
| /* | |
| Serial.print("Rcved data time="); | |
| Serial.println(millis()); | |
| for (i=0;i<data_len;i++) | |
| { | |
| Serial.print(data[i],HEX); | |
| Serial.print(","); | |
| } | |
| Serial.println(); | |
| */ | |
| } | |
| else | |
| { | |
| monFlag=1; | |
| navflag=0; | |
| } | |
| //================================================================================================= | |
| //-------------------------------------------------------------------------------------------- | |
| //========================電源オンorRESET直後に自動的にSD File OPEN firstflag==1 ======================== | |
| //ファイル名用のタイムスタンプ作成 timestamp STRING生成-------------------------------------------------------------------- | |
| sNo=6;//PVTヘッダー部オフセット 6byte | |
| imonth=int(data[sNo+6]);//12byte目 | |
| iday=int(data[sNo+7]);//13byte目 | |
| ihour=int(data[sNo+8])+9;//14byte目 | |
| if (ihour>24) | |
| { | |
| ihour=ihour-24; | |
| iday=iday+1; | |
| } | |
| imin=int(data[sNo+9]);//15byte目 | |
| isec=int(data[sNo+10]);//16byte目 | |
| timestr=String(imonth)+"-"+String(iday)+"-"+String(ihour)+"-"+String(imin)+"-"+String(isec); | |
| //---------------------------------------- | |
| //-----Filename make and FileOpen--------------------------- | |
| if(firstFlag==1)//電源オン RESET直後 firstFlag=1 | |
| { | |
| Serial.print("======firstFlag=1 OPEN FILE====="); | |
| stime="/"+timestr+".txt";//timestr YY-MM-HH-MM-SS | |
| Serial.print("OPEN SD fname="); | |
| Serial.println(stime); | |
| //file = SD.open(stime, FILE_APPEND);//ファイルの追加書き込みOPEN-CLOSE後再度OPENして追加書き込みできる | |
| openflag=1; | |
| firstFlag=0; | |
| if(!file){ | |
| Serial.println("Failed to open file for writing"); | |
| return; | |
| openflag=0; | |
| firstFlag=1; | |
| sdataN=0; | |
| } | |
| } | |
| //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
| //-------------------------------------------------- | |
| //Display Pameter Claculation & Plot | |
| //BOX 1 Log Color Change---------------- | |
| if (sdFlag==0x01 )//Logging ON | |
| { | |
| Col1=0xF81F; | |
| Col2=0xF81F; | |
| } | |
| else if (sdFlag==0x00)//Loggin OFF | |
| { | |
| Col1=0xffff; | |
| Col2=0xFFE0; | |
| tim=0; | |
| } | |
| // tftSprite.setTextSize(1); | |
| // Serial.print("ch="); | |
| // Serial.print(ch); | |
| //Serial.print("Nextchflag="); | |
| // Serial.println(Nextchflag); | |
| //chN | |
| chN++; | |
| chN=chN%40; | |
| totalN++; | |
| //PVT iTow U4 | |
| int sNo=6;//Payload startNo | |
| //iTow[chN]=B2L(data[sNo+3],data[sNo+2],data[sNo+1],data[sNo]);//headMot上位ー＞下位 ofset64+6 | |
| numSV[chN]=data[sNo+23]; | |
| Long[chN]=B2L(data[sNo+27],data[sNo+26],data[sNo+25],data[sNo+24]);//Longtitude上位ー＞下位 | |
| Long[chN]=Long[chN];//%100000; | |
| Lati[chN]=B2L(data[sNo+31],data[sNo+30],data[sNo+29],data[sNo+28]);//latitude 上位ー＞下位 | |
| Lati[chN]=Lati[chN];//%100000; | |
| //Seah[chN]=B2L(data[sNo+35],data[sNo+34],data[sNo+33],data[sNo+32]);//headMot上位ー＞下位 ofset64+6 | |
| //Seah[chN]=Seah[chN];//*0.001;//mm=>m | |
| Acc2d[chN]=B2L(data[sNo+43],data[sNo+42],data[sNo+41],data[sNo+40]);//Accuracy 2D 上位ー＞下位 | |
| Acc3d[chN]=B2L(data[sNo+47],data[sNo+46],data[sNo+45],data[sNo+44]);//Accuracy 2D 上位ー＞下位 | |
| //RELPOSNED | |
| long relN_1=relN[chN]; | |
| long relE_1=relN[chN]; | |
| long relD_1=relN[chN]; | |
| relN[chN]=B2L(data[sNo+111],data[sNo+110],data[sNo+109],data[sNo+108]); | |
| relE[chN]=B2L(data[sNo+115],data[sNo+114],data[sNo+113],data[sNo+112]); | |
| relD[chN]=B2L(data[sNo+119],data[sNo+118],data[sNo+117],data[sNo+116]); | |
| long Len_1=Length[chN]; | |
| Length[chN]=B2L(data[sNo+123],data[sNo+122],data[sNo+121],data[sNo+120]); | |
| Head[chN]=B2L(data[sNo+127],data[sNo+126],data[sNo+125],data[sNo+124]); | |
| Head[chN]=int(Head[chN]/100000); | |
| //reserved 61,60,59,58バイトある | |
| long relHPN_1=relHPN[chN]; | |
| long relHPE_1=relHPE[chN]; | |
| long relHPD_1=relHPD[chN]; | |
| relHPN[chN]=B2sb(data[sNo+132]); | |
| relHPE[chN]=B2sb(data[sNo+133]); | |
| relHPD[chN]=B2sb(data[sNo+134]); | |
| relHPLen[chN]=B2sb(data[sNo+135]); | |
| accN[chN]=B2L(data[sNo+139],data[sNo+138],data[sNo+137],data[sNo+136]); | |
| accE[chN]=B2L(data[sNo+143],data[sNo+142],data[sNo+141],data[sNo+140]); | |
| accD[chN]=B2L(data[sNo+147],data[sNo+146],data[sNo+145],data[sNo+144]); | |
| accLen[chN]=B2L(data[sNo+151],data[sNo+150],data[sNo+149],data[sNo+148]); | |
| float accNf=float(accN[chN])/100; | |
| float accEf=float(accE[chN])/100; | |
| float accDf=float(accD[chN])/100; | |
| float accLenf=float(accLen[chN])/100; | |
| //--=Error 回避------------------------------- | |
| if (Length[chN]>2000||accLenf>100||accNf>100||accEf>100||accDf >100) | |
| { | |
| Serial.print("======Data Iregulare Occured ====:time="); | |
| Serial.println(millis()); | |
| Length[chN]=Len_1;// valid data change | |
| relN[chN]=relN[chN_1]; | |
| relE[chN]=relE[chN_1]; | |
| relD[chN]=relD[chN_1]; | |
| relHPN[chN]=relHPN[chN_1]; | |
| relHPE[chN]=relHPE[chN_1]; | |
| relHPD[chN]=relHPD[chN_1]; | |
| for (i=0;i<data_len;i++) | |
| { | |
| Serial.print(data[i],HEX); | |
| Serial.print(","); | |
| } | |
| Serial.println(); | |
| } | |
| //relP 浮動小数点2桁精度変換 | |
| relNf[chN]=relN[chN]+relHPN[chN]*0.01; | |
| relEf[chN]=relE[chN]+relHPE[chN]*0.01; | |
| relDf[chN]=relD[chN]+relHPD[chN]*0.01; | |
| Lengthf[chN]=Length[chN]+relHPLen[chN]*0.01; | |
| //-----Averaging---------- | |
| int an=20; | |
| if (totalN>an) | |
| { | |
| relNfsum=0; | |
| relEfsum=0; | |
| relDfsum= 0; | |
| Lengthfsum=0; | |
| for (i=0;i<an;i++) | |
| { | |
| relNfsum=relNfsum+relNf[i]; | |
| relEfsum=relEfsum+relEf[i]; | |
| relDfsum= relDfsum+relDf[i]; | |
| Lengthfsum=Lengthfsum+Lengthf[i]; | |
| }//end for | |
| relNave=relNfsum/an; | |
| relEave=relEfsum/an; | |
| relDave=relDfsum/an; | |
| Lengthave=Lengthfsum/an; | |
| //STDEV | |
| relNfsum=0; | |
| relEfsum=0; | |
| relDfsum= 0; | |
| Lengthfsum=0; | |
| for (i=0;i<an;i++) | |
| { | |
| relNfsum=relNfsum+(relNf[chN]-relNave)*(relNf[chN]-relNave); | |
| relEfsum=relEfsum+(relEf[chN]-relEave)*(relEf[chN]-relEave); | |
| relDfsum=relDfsum+(relDf[chN]-relDave)*(relDf[chN]-relDave); | |
| Lengthfsum=Lengthfsum+(Length[chN]-Lengthave)*(Length[chN]-Lengthave); | |
| } | |
| relNstd=relNfsum/an*2;//2sigma | |
| relEstd=relEfsum/an*2;//2sigma | |
| relDstd=relDfsum/an*2;//2sigma | |
| Lengthstd=Lengthfsum/an*2;//2sigma | |
| } //averaging | |
| //********************************************************************************************************************************* | |
| //^^^^^^^^^^^^^^^^^^500msec period Display^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
| //********************************************************************************************************************************* | |
| if (N172%8==0 ) // one Display time at 500msec 2chx4 | |
| { | |
| chN_1=chN;//Valid data No | |
| //=====Sprite draw==================== | |
| coff=20; | |
| tftSprite.fillRect(0,0,320,210,BLACK); | |
| tftSprite.setTextSize(2); | |
| tftSprite.setTextColor(Col2);//YELLOW Text | |
| //Serial.println(timestr); | |
| tftSprite.setCursor(0,0); | |
| tftSprite.print(timestr); | |
| tftSprite.printf("[sdN=%d]\r\n",sdataN);//時刻表示 | |
| tftSprite.setCursor(0,2+coff); | |
| tftSprite.printf("Acc2d:%2d,Acc3d:%2d[%2d]\r\n",Acc2d[chN],Acc3d[chN],numSV[chN]);//RTK Acc表示 | |
| tftSprite.setCursor(0,20+coff); | |
| //--Length | |
| //tftSprite.printf("Len=%2.1f[%2.1f]\r\n",Lengthf[chN],accLenf); | |
| tftSprite.printf("Len=%2.1f[%2.1f],%d\r\n",Lengthave,Lengthstd); | |
| //--relN | |
| tftSprite.setCursor(0,40+coff); | |
| //tftSprite.printf("relN=%2.1f[%2.1f]\r\n",relNf[chN],accNf); | |
| tftSprite.printf("relN=%2.1f[%2.1f]\r\n",relNave,relNstd); | |
| //--relE | |
| tftSprite.setCursor(0,60+coff); | |
| //tftSprite.printf("relE=%2.1f[%2.1f]\r\n",relEf[chN],accEf); | |
| tftSprite.printf("relE=%2.1f[%2.1f]\r\n",relEave,relEstd); | |
| //--relD | |
| tftSprite.setCursor(0,80+coff); | |
| //tftSprite.printf("relD=%2.1f[%2.1f]\r\n",relDf[chN],accDf); | |
| tftSprite.printf("relD=%2.1f[%2.1f]\r\n",relDave,relDstd); | |
| tftSprite.setCursor(0,100+coff); | |
| tftSprite.printf("Long=%2d\r\n",Long[chN]); | |
| tftSprite.setCursor(0,120+coff); | |
| tftSprite.printf("Lati=%2d\r\n",Lati[chN]); | |
| tftSprite.setCursor(0,140+coff); | |
| tftSprite.printf("Head=%2d\r\n",Head[chN]); | |
| //------IMU------------------- | |
| M5.IMU.getAccelData(&accX, &accY, &accZ); | |
| tftSprite.setCursor(0,160+coff); | |
| tftSprite.printf("accXYZ=%5.2f,%5.2f,%5.2f\r\n",accX,accY,accZ); | |
| tftSprite.pushSprite(0, 0); | |
| }//n% 間引き | |
| }//ESP RCV | |
| //============================================================================================= | |
| //================================SET UP======================================================= | |
| //====================M5Stack LCD 320x240================================================== | |
| void setup() { | |
| M5.begin(); | |
| M5.IMU.Init(); | |
| M5.Buttons.addHandler(eventDisplay, E_ALL - E_MOVE); | |
| ///---SD initialize------------ | |
| // Start SD card | |
| if (!SD.begin()) | |
| { | |
| // tftSprite.println("ERROR: SD CARD."); | |
| } | |
| monFlag=0; | |
| M5.Lcd.fillScreen(BLACK); | |
| M5.Lcd.setRotation(1); // tftSprite.print("ESP-NOW Test\n"); | |
| M5.Lcd.print("----SetUP-----\n"); | |
| tftSprite.createSprite(M5.Lcd.width(),210);// M5.Lcd.height()); | |
| // DisIMUbuff.createSprite(320, 40); | |
| // ESP-NOW初期化 | |
| WiFi.mode(WIFI_STA); | |
| WiFi.disconnect(); | |
| if (esp_now_init() == ESP_OK) { | |
| Serial.println("ESPNow Init Success"); | |
| //tftSprite.print("ESPNow Init Success\n"); | |
| } else { | |
| Serial.println("ESPNow Init Failed"); | |
| //tftSprite.print("ESPNow Init Failed\n"); | |
| ESP.restart(); | |
| } | |
| // マルチキャスト用Slave登録 | |
| memset(&slave, 0, sizeof(slave)); | |
| for (int i = 0; i < 6; ++i) { | |
| slave.peer_addr[i] = (uint8_t)0xff; | |
| } | |
| esp_err_t addStatus = esp_now_add_peer(&slave); | |
| if (addStatus == ESP_OK) { | |
| // Pair success | |
| Serial.println("Pair success"); | |
| } | |
| // ESP-NOWコールバック登録 | |
| esp_now_register_send_cb(OnDataSent); | |
| esp_now_register_recv_cb(OnDataRecv); | |
| //MENU--------------------- | |
| M5.Lcd.setTextSize(2); | |
| M5.Lcd.fillRect(10,210,240,30,BLACK); | |
| M5.Lcd.setCursor(10,210); | |
| M5.Lcd.setTextColor(WHITE); | |
| M5.Lcd.print("A:MonOn"); | |
| /* | |
| M5.Lcd.setCursor(130,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Raw"); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:Ave"); | |
| */ | |
| /* | |
| M5.Lcd.setTextSize(2); | |
| M5.Lcd.setCursor(10,210); | |
| M5.Lcd.setTextColor(WHITE); | |
| M5.Lcd.print("A:MonOn"); | |
| M5.Lcd.setCursor(130,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Save"); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:File"); | |
| */ | |
| //------------------------- | |
| //Monitor ON send | |
| //cdata[0] =0x6d;//'m' mFlag=1or0 Monitor Start | |
| //initFlag=1;//init R zero level Long0,Lati0 Fix | |
| //esp_err_t result = esp_now_send(slave.peer_addr, cdata, 1); | |
| sdFlag_1=0; | |
| Monitormode=1; | |
| } | |
| void loop() { | |
| M5.update(); | |
| // Aボタンを押したらHaltしてAverageとstdevを表示 再度押すとHalt解除 --------------------------------------------------------------------- | |
| if ( M5.BtnA.isPressed() ) {// buttonA=>Start SD log | |
| Serial.println("Push Button A"); | |
| vibration(); | |
| if(nc%2==0)//トグル | |
| { | |
| nc++; | |
| //Monitor ON send | |
| cdata[0] =0x6d;//'m' mFlag=Toggle Monitor Start | |
| //initFlag=1;//init R zero level Long0,Lati0 Fix | |
| esp_err_t result = esp_now_send(slave.peer_addr, cdata, 1); | |
| M5.Lcd.setTextSize(2); | |
| M5.Lcd.fillRect(10,210,320,30,BLACK); | |
| M5.Lcd.setCursor(10,210); | |
| M5.Lcd.setTextColor(BLUE); | |
| M5.Lcd.print("A:Halt"); | |
| M5.Lcd.setCursor(110,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Save_"); | |
| M5.Lcd.print(sdataN); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:List"); | |
| delay(200); | |
| } | |
| else | |
| { | |
| nc++; | |
| //Monitor ON send | |
| cdata[0] =0x6c;//l' mFlag=Toggle Monitor Start | |
| //initFlag=1;//init R zero level Long0,Lati0 Fix | |
| esp_err_t result = esp_now_send(slave.peer_addr, cdata, 1); | |
| M5.Lcd.setTextSize(2); | |
| M5.Lcd.fillRect(10,210,320,30,BLACK); | |
| M5.Lcd.setCursor(10,210); | |
| M5.Lcd.setTextColor(WHITE); | |
| M5.Lcd.print("A:MonOn"); | |
| /* | |
| M5.Lcd.setCursor(130,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Raw"); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:Ave"); | |
| */ | |
| delay(200); | |
| } | |
| } | |
| delay(1); | |
| //Bボタンを押したら送信 Data Save--------------------------------------------------------------------- | |
| if (M5.BtnB.isPressed()) { | |
| vibration(); | |
| if(Listmode==0) | |
| { | |
| sdflag=1; | |
| //--------SD data save------------------------ | |
| file = SD.open(stime, FILE_APPEND);//ファイルの追加書き込みOPEN-CLOSE後再度OPENして追加書き込みできる | |
| sdataN++; | |
| strTotal=String(sdataN)+":"+String(relNave,1)+","+String(relEave,1)+","+String(relDave,1)+","+String(Lengthave,1)+","+String(relNstd,1)+","+String(relEstd,1)+","+String(relDstd,1)+","+String(Lengthstd,1)+","; | |
| strTotal=strTotal+String(Long[chN])+","+String(Lati[chN])+","+timestr; | |
| Serial.println(strTotal); | |
| file.println(strTotal); | |
| file.close(); | |
| sdflag=0; | |
| M5.Lcd.fillRect(120,210,110,30,BLACK); | |
| M5.Lcd.setCursor(110,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Save_"); | |
| M5.Lcd.print(sdataN); | |
| Serial.println("File Closed"); | |
| delay(500);//チャタリング防止 | |
| } | |
| else//Dir | |
| { | |
| printDirectory(file, 0); | |
| } | |
| }//BtnB end | |
| //Cボタンを押したら送信 --------------------------------------------------------------------- | |
| if (M5.BtnC.isPressed()) { | |
| vibration(); | |
| tftSprite.fillRect(0,0,320,210,BLACK); | |
| tftSprite.pushSprite(0, 0); | |
| Listpara(); | |
| Listmode=1; | |
| //Listモード開始 | |
| M5.Lcd.setTextSize(2); | |
| M5.Lcd.fillRect(10,210,320,30,BLACK); | |
| M5.Lcd.setCursor(10,210); | |
| M5.Lcd.setTextColor(BLUE); | |
| M5.Lcd.print(""); | |
| M5.Lcd.setCursor(110,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Dir"); | |
| //M5.Lcd.print(sdataN); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:Exit"); | |
| delay(400); | |
| }//ボタンC end | |
| }//loop end | |
| // バイブレーション実行関数 | |
| void vibration() { | |
| power.SetLDOEnable(3, true); // 3番をtrueにしてバイブレーション開始 | |
| delay(200); // バイブレーションの長さ（ms）はお好みで調整 | |
| power.SetLDOEnable(3, false); // 3番をfalseにしてバイブレーション修了 | |
| } | |
| void eventDisplay(Event& e) { | |
| Serial.println("=================void eventDiplay============="); | |
| //Serial.printf("%-12s finger%d %-18s (%3d, %3d) --> (%3d, %3d) ",e.typeName(), e.finger, e.objName(), e.from.x, e.from.y,e.to.x, e.to.y); | |
| //Serial.printf("( dir %d deg, dist %d, %d ms )\n", e.direction(),e.distance(), e.duration); | |
| objNm=e.objName(); | |
| //M5.Lcd.printf("%-18s\r\n",e.objName()); | |
| Serial.printf("%-18s\r\n",e.objName()); | |
| //M5.Lcd.printf("[%-18s]\r\n", objNm); | |
| Serial.printf("objNm=%-18s,length=%d\n\r",objNm,objNm.length()); | |
| if(objNm=="swipe up") | |
| { | |
| Serial.printf(">>>[%-18s]\r\n", objNm); | |
| M5.Lcd.fillRect(0,210,320,30,BLACK); | |
| M5.Lcd.setCursor(0,210); | |
| M5.Lcd.setTextColor(WHITE); | |
| M5.Lcd.print("Up"); | |
| M5.Lcd.setCursor(110,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Dir"); | |
| //M5.Lcd.print(sdataN); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:Exit"); | |
| delay(200); | |
| } | |
| if(objNm=="swipe down") | |
| { | |
| Serial.printf(">>>[%-18s]\r\n", objNm); | |
| M5.Lcd.fillRect(0,210,320,30,BLACK); | |
| M5.Lcd.setCursor(0,210); | |
| M5.Lcd.setTextColor(WHITE); | |
| M5.Lcd.print("Down"); | |
| M5.Lcd.setCursor(110,210); | |
| M5.Lcd.setTextColor(YELLOW); | |
| M5.Lcd.print("B:Dir"); | |
| //M5.Lcd.print(sdataN); | |
| M5.Lcd.setCursor(235,210); | |
| M5.Lcd.setTextColor(RED); | |
| M5.Lcd.print("C:Exit"); | |
| delay(200); | |
| } | |
| }//eventDisplaｙend | |
| String readFile(String fnm ) | |
| { | |
| String str; | |
| file = SD.open(fnm, FILE_READ);//ファイルの読み込み | |
| if(file){ | |
| //---1byteずつ読み込んだ文字を結合 | |
| while (file.available()) { | |
| str += char(file.read()); | |
| } | |
| } else{ | |
| Serial.println(" error..."); | |
| } | |
| //---ファイルを閉じる | |
| //Serial.println(str); | |
| file.close(); | |
| return str; | |
| } | |
| void Listpara() | |
| { | |
| stime="9-26-15-27-10.txt";//debug | |
| Serial.print("--Listpara-------Read stime="); | |
| Serial.println(stime); | |
| datastr=readFile(stime); | |
| Serial.print("datastr="); | |
| Serial.println(datastr); | |
| int arry_len=splitCRLF(datastr,datastrArry); | |
| Serial.println("------split finished-------"); | |
| tftSprite.fillRect(0,0,320,210,BLACK); | |
| //M5.Lcd.fillRect(0,0,320,210,BLACK); | |
| //M5.Lcd.setTextSize(2); | |
| //M5.Lcd.setCursor(0,0); | |
| tftSprite.setTextSize(2); | |
| tftSprite.setTextColor(YELLOW);//YELLOW Text | |
| //---Header print--- | |
| tftSprite.setCursor(0,0); | |
| tftSprite.print("N:"); | |
| tftSprite.setCursor(20,0); | |
| tftSprite.print("relN"); | |
| tftSprite.setCursor(80,0); | |
| tftSprite.print(",relE"); | |
| tftSprite.setCursor(140,0); | |
| tftSprite.print(",relD"); | |
| tftSprite.setCursor(200,0); | |
| tftSprite.print(",Len"); | |
| //============================== | |
| tftSprite.setTextColor(WHITE);//WHITE Text | |
| int rowN; | |
| if (arry_len>9) | |
| { | |
| rowN=9; | |
| } | |
| else | |
| { | |
| rowN=arry_len; | |
| } | |
| for(i=0;i<rowN;i++) | |
| { | |
| //Serial.print("datastrArry["); | |
| //Serial.print(i); | |
| //Serial.print("]="); | |
| //Serial.println(datastrArry[i]); | |
| if (i==0) | |
| { | |
| bufstr[i]=datastrArry[i].substring(0,21); | |
| } | |
| else | |
| { | |
| bufstr[i]=datastrArry[i].substring(1,22); | |
| } | |
| //Serial.print(i); | |
| //Serial.print(":bufstr[] length="); | |
| //Serial.print(bufstr[i].length()); | |
| //Serial.print(":bufstr="); | |
| //Serial.println(bufstr[i]); | |
| //M5.Lcd.println(bufstr); | |
| tftSprite.setCursor(0,(i+1)*20); | |
| tftSprite.print(bufstr[i]); | |
| } | |
| tftSprite.pushSprite(0, 0); | |
| } | |
| int splitCRLF(String data, String *dst){ | |
| int index = 0; | |
| int arraySize = (sizeof(data)/sizeof((data)[0])); | |
| int datalength = data.length(); | |
| //Serial.print("split:length="); | |
| //Serial.println(datalength); | |
| for (int i = 0; i < datalength; i++) { | |
| char tmp = data.charAt(i); | |
| char tmp_1 = data.charAt(i+1); | |
| //if ( tmp == delimiter ) { | |
| if(tmp==0x0D && tmp_1==0x0A && index<(arraySize-1)){ | |
| index++; | |
| tmp=data.charAt(i+2); | |
| i=i+1; | |
| //if ( index > (arraySize - 1)) return -1; | |
| } | |
| dst[index] += tmp; | |
| //Serial.print("dst["); | |
| //Serial.print(index); | |
| //Serial.print("]="); | |
| //Serial.println(dst[index]); | |
| } | |
| return (index + 1); | |
| } | |
| int split(String data, char delimiter, String *dst){ | |
| int index = 0; | |
| int arraySize = (sizeof(data)/sizeof((data)[0])); | |
| int datalength = data.length(); | |
| Serial.print("split:length="); | |
| Serial.println(datalength); | |
| for (int i = 0; i < datalength; i++) { | |
| char tmp = data.charAt(i); | |
| if ( tmp == delimiter ) { | |
| index++; | |
| if ( index > (arraySize - 1)) return -1; | |
| } | |
| else dst[index] += tmp; | |
| Serial.print("dst["); | |
| Serial.print(index); | |
| Serial.print("]="); | |
| Serial.println(dst[index]); | |
| } | |
| return (index + 1); | |
| } | |
| void printDirectory(File dir, int numTabs) { | |
| while(true) { | |
| File entry = dir.openNextFile(); | |
| if (! entry) { | |
| dir.rewindDirectory(); | |
| break; | |
| } | |
| for (uint8_t i=0; i<numTabs; i++) { | |
| M5.Lcd.print('\t'); | |
| Serial.print('\t'); | |
| } | |
| M5.Lcd.print(entry.name()); | |
| if (entry.isDirectory()) { | |
| M5.Lcd.println("/"); | |
| Serial.println("/"); | |
| printDirectory(entry, numTabs+1); | |
| } else { | |
| M5.Lcd.println("\t\t"); | |
| Serial.println("\t\t"); | |
| //M5.Lcd.println(entry.size(), DEC); | |
| } | |
| } | |
| } | |
| /* | |
| void setup() { | |
| // 文字列 | |
| String cmd = "on,off,on"; | |
| // 分割数 = 分割処理(文字列, 区切り文字, 配列) | |
| int index = split(cmd, ',', cmds); | |
| // 結果表示 | |
| Serial.begin(9600); | |
| for(int i = 0; i < index; i++){ | |
| Serial.println(cmds[i]); | |
| } | |
| } | |
| */ | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment