Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/M5StickC_STA22_ESP-NOW_Bugfix_T4-rev10283_rev09.ino Secret

Created May 9, 2022 16:34
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/8acbe6b97e859fa7a5d9dee706de1105 to your computer and use it in GitHub Desktop.
Save dj1711572002/8acbe6b97e859fa7a5d9dee706de1105 to your computer and use it in GitHub Desktop.
Download ZIP
M5StickC ArduinoIDE RTK Alarm monitor system
Raw
M5StickC_STA22_ESP-NOW_Bugfix_T4-rev10283_rev09.ino
//#include <M5Core2.h>
//#include <M5Stack.h>
#include <M5StickC.h>
#include <esp_now.h>
#include <WiFi.h>
#include "SD.h"
#include "FS.h"
#include "SPI.h"
esp_now_peer_info_t slave;
File file;
File f;
File root;
//---BUZZER---------------
#define buzzer_pin 26
int freq = 4000;//音の高さ
int ledChannel = 0;//PWM CH
int resolution = 10;// PWM bit数10bit=1024
int vol=512;//Volume PWM Duty50% (512/1024)
//--general parameters--
int i,j,k;
uint8_t f9pdata[172];//ESP-Now Rcv data
uint8_t f9pdataL[172];// SD Loop data
uint8_t f9pdata_1[172];
volatile uint8_t rcvflag;
//------data value -------
int itow,itow_1;//4byte
volatile uint8_t flags,flags_1;//1byte
uint8_t flagsL;//loop data for Buzzer
int numSV,numSV_1;//1byte
int hacc,hacc_1;//2byte
int fixtime,fixtime_1;//2byte
//----SD parameters----
int readflag;
String stime;//filename
uint8_t count;
volatile uint8_t iniflag;
int epochN,epochN_1;
//-----calibration mode-----------------------
int calicount;
double rrN,rrE,rrD,rrL;
double rrN_1,rrE_1,rrD_1,rrL_1;
volatile uint8_t aveflag;
double odosum,odosum_1;
//----------------------------------------------------------------------------------------
//--------LCD--------
volatile double oddsum;
volatile int btN;//Buttton counter
int coff;
uint16_t Col1,Col2;
//ESP-NOW
uint8_t com[1];
//ブザーSub 　(flag値,周波数,Duty,駆動時間,休止時間）　
void Abeep(uint8_t flag,int vol){
int tb,tw;//time Beep,time wait
if (flag<67){
flag=1;
}
switch(flag) {
case 131:
freq= int(3000);
ledcSetup(ledChannel, freq, resolution); //PWM CH0,Freuency4000,10Bit
tb=20;
tw=1000;
//Serial.printf("flag=%d,freq=%d,vol=%d\n\r",flag,freq,vol);
break;
case 67:
freq= int(1000);
ledcSetup(ledChannel, freq, resolution); //PWM CH0,Freuency4000,10Bit
tb=400;
tw=1000;
//Serial.printf("flag=%d,freq=%d,vol=%d\n\r",flag,freq,vol);
break;
case 1:
freq= int(200);
ledcSetup(ledChannel, freq, resolution); //PWM CH0,Freuency4000,10Bit
tb=700;
tw=1000;
//Serial.printf("flag=%d,freq=%d,vol=%d\n\r",flag,freq,vol);
break;
}
//===========Beep Action===================================
ledcWrite(0, vol); //Output PWM START at Volume vol
delay(tb);
ledcWrite(0, 0); //Output PWM. STOP at Volume 0
delay(tw);
//==========================================================
}// Abeep end
// ===========BYTE TO DOUBLE=========================
double by2Todbl(uint8_t l,uint8_t u){
double result;
if( u & 0x80){
int i=(l+u *256)-65536;
result=(double)i;
}
else{
result=(double)(l+u*256);
}
return result;
}// byTodbl end
double by4Todbl(uint8_t ll,uint8_t l,uint8_t u,uint8_t uu){
double result;
if( uu & 0x80){
uint32_t i1=ll;
uint32_t i2=l<<8;
uint32_t i3=u<<16;
uint32_t i4=uu<<24;
uint32_t i5=i1 | i2 | i3| i4;
long i6=(long)i5;//-4294967296;
//Serial.printf("by4TOdble:i1=%x,i2=%x,i3=%x,i4=%x,i5(sum)=%x,i6(Result)=%d\n\r",i1,i2,i3,i4,i5,i6);
result=(double)i6;
}
else{
result=(double)(ll+l*256+u*256*256+uu*256*256*256);
}
return result;
}// by4Todbl end
//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;
}
//-------------
HMS=String(JHOUR)+":"+String(MIN)+":"+String(SEC);
return HMS;
}//itowToHMS end=====================================================
// 送信コールバック
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");
// 画面にも描画
Serial.print("Last Packet Sent to: \n ");
Serial.println(macStr);
Serial.print("Last Packet Send Status: \n ");
Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}
// 受信コールバック
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 Data(%d): ", data_len);
//LCD SET
//M5.Lcd.fillRect(0,0,80,15,BLACK);
// M5.Lcd.setTextSize(2);
// M5.Lcd.setCursor(0,00);
count=data_len;
//if(data_len<5){
//Serial.printf("%c,%c,%c,%c\n\r",data[0],data[1],data[2],data[3]);
//M5.Lcd.fillRect(0,0,160,80,BLACK);
// M5.Lcd.setTextColor(YELLOW);//YELLOW Text
// M5.Lcd.setTextSize(2);
//M5.Lcd.setCursor(0,0);
//M5.Lcd.printf("%c%c%c%c OK");//RTK Acc/Dop表示
//iniflag=1;
// }
//Serial.print("f9pdata[]=");
//if(data_len>5){
for ( int i = 0 ; i < data_len ; i++ ) {
f9pdata[i]=data[i];
//Serial.print(f9pdata[i]);
//Serial.print(",");
}
//Serial.println(millis());
rcvflag=1;
itow=f9pdata[0]+f9pdata[1]*256+f9pdata[2]*256*256+f9pdata[3]*256*256*256;
Serial.printf("---------itow=%d,ESP-RCV=%dbyte,millis=%d\n\r",itow,data_len,millis());
//Serial.println(millis());
itow_1=f9pdata_1[0]+f9pdata_1[1]*256+f9pdata_1[2]*256*256+f9pdata_1[3]*256*256*256;
flags_1=flags;//f9pdata_1[4];
//numSV_1=f9pdata_1[5];
//hacc_1=f9pdata_1[6]+f9pdata_1[7]*256;
//fixtime_1=f9pdata_1[8]+f9pdata_1[9];
flags=f9pdata[4];
numSV=f9pdata[5];
hacc=f9pdata[6]+f9pdata[7]*256;
fixtime=f9pdata[8]+f9pdata[9]*256;
double fixpercent=(double)(((double)f9pdata[10]+f9pdata[11]*256)/10);
int epNo=f9pdata[12]+f9pdata[13]*256;
//Serial.printf("---epNo=%d---\n\r",epNo);
int miscount1=f9pdata[14];
double pDOP=((double)f9pdata[15]+f9pdata[16]*256)/100;
if(epNo%4==0){
Serial.printf("calicount=%d\n\r",calicount);
String itowHMS=itowToHMS(itow);
//Serial.printf("epNo=%d,itow=%d,itowHMS=%s,millis=%d=======\n\r",epNo,itow,itowHMS,millis());
//---------------現在相対座標 4Byte-------------------------
rrN_1=rrN;
rrE_1=rrE;
rrD_1=rrD;
rrL_1=rrL;
rrN=by4Todbl(f9pdata[17],f9pdata[18],f9pdata[19],f9pdata[20])*0.01;//
rrE=by4Todbl(f9pdata[21],f9pdata[22],f9pdata[23],f9pdata[24])*0.01;//
rrD=by4Todbl(f9pdata[25],f9pdata[26],f9pdata[27],f9pdata[28])*0.01;//
rrL=sqrt(pow(rrN,2)+pow(rrE,2)+pow(rrD,2));
//----------------MovingBase Head-------------------------
double mHead=by4Todbl(f9pdata[29],f9pdata[30],f9pdata[31],f9pdata[32])*0.00001;
//------------Calibration Average ----------------
double rNaveN=by4Todbl(f9pdata[33],f9pdata[34],f9pdata[35],f9pdata[36])*0.01;
double rEaveN=by4Todbl(f9pdata[37],f9pdata[38],f9pdata[39],f9pdata[40])*0.01;
double rDaveN=by4Todbl(f9pdata[41],f9pdata[42],f9pdata[43],f9pdata[44])*0.01;
double rLaveN=by4Todbl(f9pdata[45],f9pdata[46],f9pdata[47],f9pdata[48])*0.01;
//------------Calibration standard Deviation-----------------------------
double rNstd=by2Todbl(f9pdata[49],f9pdata[50])*0.01;
double rEstd=by2Todbl(f9pdata[51],f9pdata[52])*0.01;
double rDstd=by2Todbl(f9pdata[53],f9pdata[54])*0.01;
double rLstd=by2Todbl(f9pdata[55],f9pdata[56])*0.01;
//------------Calibration Counter-----------------------------
int avecount=(int)f9pdata[57]+f9pdata[58]*256;
if(avecount>=490){
aveflag=1;
odosum=0;
}
Serial.printf("--------avecount=%d,f9p[51]=%x,f9p[52]=%x,aveflag=%d-------\n\r",avecount,f9pdata[51],f9pdata[52],aveflag);
//----------ODO Caliculation-------------------------------------------
if(aveflag==1){//START ODO sum from Calibration Finished time
double odosqr=pow(rrN-rrN_1,2)+pow(rrE-rrE_1,2)+pow(rrD-rrD_1,2);
odosum_1=odosum;
odosum=odosum+sqrt(odosqr);
Serial.printf("odosum=%5.2f\n\r",odosum);
if(abs(rrL-rrL_1)<5){//移動距離5cm以下は静止
odosum=odosum_1;
}
}
// Serial.printf("------------------------------itow=%d---------------------------------------\n\r",itow);
//Serial.printf("it=%d,flg=%d,nSV=%d,acc=%d,tim=%d,F%%=%3.1f,epN=%d,mis=%d,Dop=%2.2f\n\r",itow,flags,numSV,hacc,fixtime,fixpercent,epNo,miscount1,pDOP);
//Serial.printf("rrN=%5.2f,rrE=%5.2f,rrD=%5.2f,Hed=%3.5f,Nav=%5.2f,Eav=%5.2f,Dav=%5.2f,Lav=%5.2f\n\r",rrN,rrE,rrD,mHead,rNaveN,rEaveN,rDaveN,rLaveN);
//Serial.printf(",Nstd=%5.2f,Estd=%5.2f,Dstd=%5.2f,Lstd=%5.2f,avecount=%d \n\r",rNstd,rEstd,rDstd,rLstd,avecount);
//===M%Stack Basic LCD=============================
coff=0;
M5.Lcd.fillRect(0,0,160,80,BLACK);
M5.Lcd.setTextColor(YELLOW);//YELLOW Text
if(btN%3==0){
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0,0);
M5.Lcd.printf("%s/%3.1f",itowHMS,pDOP);//RTK Acc/Dop表示
M5.Lcd.setCursor(0,20+coff);
M5.Lcd.printf("[%d]%dmm",flags,hacc);//
M5.Lcd.setCursor(0,40+coff);
M5.Lcd.printf("Fix:%d sec",fixtime);//fixtime
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("Fix:%3.1f%%",fixpercent);//fixpercent
}
if(btN%3==1){
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0,0+coff);
M5.Lcd.printf("Ep%d/%d",miscount1,epNo);//Misscount/epNo
M5.Lcd.setCursor(0,20+coff);
M5.Lcd.printf("BL:%2.1fkm/%2.2f",rLaveN/100000,(rLstd*3));//基線長＋３シグマ
M5.Lcd.setCursor(0,40+coff);
M5.Lcd.printf("L=%5.2fcm",rrL);//L distance
if(avecount>0 && avecount<501){
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("aveC=%d",avecount);//Calibrationmode counter
}
if(aveflag==1){//ODD Meter
if(abs(odosum)<999.99 ){//cm [999.99cm未満]
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("OD=%3.2fcm",odosum);//ODO cm
}
else if(abs(odosum) >999.99 && abs(odosum)<100000){ //m [10.0000m-999.9999m範囲]
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("OD=%3.4fm",odosum*0.01);//ODO m
}
else{//km [1.00000km以上99.9999km以下]
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("OD=%2.5fkm",odosum*0.00001);//ODO km
}
}//if aveflag==1 end
}// Screen2 end
if(btN%3==2){
M5.Lcd.setTextSize(2);
//相対位置99km999m99.99cm(99.99999[99]km) 7文字以内表示する
//rrN------------------------------------------------
if(abs(rrN)<999.99 ){//cm [999.99cm未満]
M5.Lcd.setCursor(0,0+coff);
M5.Lcd.printf("N=% 3.2f/%2.2f",rrN,rNstd*3);//N相対座標＋３シグマ
}
else if(abs(rNaveN) >999.99 && abs(rrN)<100000){ //m [10.0000m-999.9999m範囲]
M5.Lcd.setCursor(0,0+coff);
M5.Lcd.printf("N=% 3.4fm",rrN*0.01);//999.9999[99]m
}
else{//km [1.00000km以上99.9999km以下]
M5.Lcd.setCursor(0,0+coff);
M5.Lcd.printf("N=% 2.5fkm",rrN*0.00001);//99.99999[99]km
}
//rrE------------------------------------------------
if(abs(rrE)<999.99 ){//cm [999.99cm未満]
M5.Lcd.setCursor(0,20+coff);
M5.Lcd.printf("E=% 3.2f/%2.2f",rrE,rEstd*3);//E相対座標＋３シグマ
}
else if(abs(rrE) >999.99 && abs(rrE)<100000){ //m [10.0000m-999.9999m範囲]
M5.Lcd.setCursor(0,20+coff);
M5.Lcd.printf("E=% 3.4fm",rrE*0.01);//999.9999[99]m
}
else{//km [1.00000km以上99.9999km以下]
M5.Lcd.setCursor(0,20+coff);
M5.Lcd.printf("E=% 2.5fkm",rrE*0.00001);//99.99999[99]km
}
//rrD------------------------------------------------
if(abs(rrD)<999.99 ){//cm [999.99cm未満]
M5.Lcd.setCursor(0,40+coff);
M5.Lcd.printf("D=% 3.2f/%2.2f",rrD,rDstd*3);//D相対座標＋３シグマ
}
else if(abs(rrD) >999.99 && abs(rrD)<100000){ //m [10.0000m-999.9999m範囲]
M5.Lcd.setCursor(0,40+coff);
M5.Lcd.printf("D=% 3.4fm",rrD*0.01);//999.9999[99]m
}
else{//km [1.00000km以上99.9999km以下]
M5.Lcd.setCursor(0,40+coff);
M5.Lcd.printf("D=% 2.5fkm",rrD*0.00001);//99.99999[99]km
}
//Head-------------------------------------------------------
M5.Lcd.setCursor(0,60+coff);
M5.Lcd.printf("Head:%3.1fdeg",mHead);//Heading
}//screen3 end
}//if epNo%4==0 end
}// Rcv end
void setup() {
M5.begin();
//---LCD SET UP---
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setRotation(3); // tftSprite.print("ESP-NOW Test\n");
M5.Lcd.setTextFont(1);
M5.Lcd.setTextSize(2);
M5.Lcd.print("----Push M5Atom Restart-----");
//tftSprite.createSprite(M5.Lcd.width(),210);// M5.Lcd.height());
///---SD initialize------------
// Start SD card
if (!SD.begin())
{
// tftSprite.println("ERROR: SD CARD.");
Serial.print("SD begin ERROR ");
}
stime="/ESPdata.txt";
file = SD.open(stime, FILE_APPEND);
String strTotal="1234567890";
file.println(strTotal);
file.close();
//Serial.println(strTotal);
//listDir(SD, "/", 0);
Serial.print("ESP-NOW Test\n");
//MAC address GET-----------
uint8_t mac3[6];
esp_read_mac(mac3, ESP_MAC_WIFI_STA);
Serial.printf("[Wi-Fi Station] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", mac3[0], mac3[1], mac3[2], mac3[3], mac3[4], mac3[5]);
// ESP-NOW初期化
WiFi.mode(WIFI_STA);
WiFi.disconnect();
if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success");
Serial.print("ESPNow Init Success\n");
} else {
Serial.println("ESPNow Init Failed");
Serial.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);
count=0;
epochN=0;
//==BUZZER=================
ledcSetup(ledChannel, freq, resolution); //PWM CH0,Freuency4000,10Bit
ledcAttachPin(buzzer_pin, ledChannel); //PWM Pin26,PWM CH0
//ledcWrite(ledChannel, vol); //PWM Duty
btN=0;
calicount=0;
//
iniflag=0;
odosum=0;
}
//============LOOP========================================
void loop() {
M5.update();
if(M5.BtnA.wasPressed()){
btN++;
}
//flagsL=f9pdataL[4];
//Serial.printf("loop in time=%d,flagsL=%d\n\r",millis(),flags);
Abeep(flags,860);
if(M5.BtnB.isPressed()){
com[0]='R';
esp_err_t result = esp_now_send(slave.peer_addr,com, 1);//'R' Broadcast
}
}
//===========================================================================================
//===========LOOP END========================================================================
//===========================================================================================
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment