commit d0baf9d0e322d4d72c5345a0fff942332a1d758f Author: Luis Rodil-Fernandez Date: Fri Mar 4 18:05:55 2022 +0100 first commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..f2ae0c6 --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +## Seacamp experiments + + diff --git a/m5__wave-sampler-gps-imu/GPSAnalyse.cpp b/m5__wave-sampler-gps-imu/GPSAnalyse.cpp new file mode 100644 index 0000000..b8ab6de --- /dev/null +++ b/m5__wave-sampler-gps-imu/GPSAnalyse.cpp @@ -0,0 +1,280 @@ +#include "GPSAnalyse.h" + +GPSAnalyse::GPSAnalyse() +{ + _GPS_Str.clear(); +} + + +void GPSAnalyse::setSerialPtr(HardwareSerial &serial) +{ + _serial = &serial; + _xSemaphore = xSemaphoreCreateMutex(); +} + +GPSAnalyse::~GPSAnalyse() +{ + +} + +void GPSAnalyse::run(void *data) +{ + GPSReadBuff = (char*)calloc(1024,sizeof(char)); + Serial.println("GPS Task"); + while (1) + { + if (_serial->available()) + { + + memset(GPSReadBuff, 1024, sizeof(char)); + _serial->readBytes(GPSReadBuff, _serial->available()); + _GPS_Str.concat(GPSReadBuff); + xSemaphoreTake(_xSemaphore, portMAX_DELAY); + Analyse(); + xSemaphoreGive(_xSemaphore); + delay(1); + } + else + { + delay(5); + } + } +} + +void GPSAnalyse::upDate() +{ + xSemaphoreTake(_xSemaphore, portMAX_DELAY); + memcpy(&s_GNRMC,&_s_GNRMC,sizeof(GNRMC_t)); + memcpy(&s_GNGAS,&_s_GNGAS,sizeof(GNGSA_t)); + memcpy(&s_GPGSV,&_s_GPGSV,sizeof(GPGSV_t)); + xSemaphoreGive(_xSemaphore); +} + +void GPSAnalyse::AnaGPRMC(String str) +{ + str = str.substring(str.indexOf("GNRMC"), str.length()); + if (str.indexOf('*') != -1) + { + int indexsub = str.indexOf('*'); + String sumstr = str.substring(indexsub + 1, str.length()); + str = str.substring(0, indexsub); + //Serial.println(sumstr); + } + + int index = 0, last_index = 0, pamindex = 1; + + while (str.indexOf(',', index) != -1) + { + index = str.indexOf(',', index + 1); + last_index = str.indexOf(',', index + 1); + //Serial.printf("index:%d,%d\n", index, last_index); + if (index != -1) + { + last_index = (last_index == -1) ? str.length() : last_index; + if ((last_index - index) > 1) + { + String pamstr = str.substring(index + 1, last_index); +#ifdef DEBUG_GPS + Serial.printf("%d:", pamindex); + Serial.println(pamstr); +#endif + _s_GNRMC.pamstr[pamindex] = pamstr; + } + else + { + _s_GNRMC.pamstr[pamindex].clear(); + } + + pamindex++; + } + } + + _s_GNRMC.Utc = _s_GNRMC.pamstr[1]; + if (_s_GNRMC.pamstr[2].length() != 0) + { + _s_GNRMC.State = _s_GNRMC.pamstr[2].charAt(0); + } + + { + _s_GNRMC.Latitude = _s_GNRMC.pamstr[3].substring(0, 2).toDouble() + _s_GNRMC.pamstr[3].substring(2, 4).toDouble() / 60 + _s_GNRMC.pamstr[3].substring(5, 9).toDouble() / 600000; + } + //_s_GNRMC.Latitude = _s_GNRMC.pamstr[3].toDouble(); + _s_GNRMC.LatitudeMark = _s_GNRMC.pamstr[4].charAt(0); + { + _s_GNRMC.Longitude = _s_GNRMC.pamstr[5].substring(0, 3).toDouble() + _s_GNRMC.pamstr[5].substring(3, 5).toDouble() / 60 + _s_GNRMC.pamstr[5].substring(6, 10).toDouble() / 600000; + } + //_s_GNRMC.Longitude = _s_GNRMC.pamstr[5].toDouble(); + _s_GNRMC.LongitudeMark = _s_GNRMC.pamstr[6].charAt(0); + _s_GNRMC.TrackSpeed = _s_GNRMC.pamstr[7].toFloat(); + _s_GNRMC.TrackAngle = _s_GNRMC.pamstr[8].toFloat(); + _s_GNRMC.Date = _s_GNRMC.pamstr[9]; + _s_GNRMC.Magnetic = _s_GNRMC.pamstr[10].toFloat(); + _s_GNRMC.Declination = _s_GNRMC.pamstr[11].charAt(0); + _s_GNRMC.mode = _s_GNRMC.pamstr[12].charAt(0); +} + +void GPSAnalyse::AnaGNGAS(String str) +{ + str = str.substring(str.indexOf("GNGSA"), str.length()); + + if (str.indexOf('*') != -1) + { + int indexsub = str.indexOf('*'); + String sumstr = str.substring(indexsub + 1, str.length()); + str = str.substring(0, indexsub); + //Serial.println(sumstr); + } + + int index = 0, last_index = 0, pamindex = 1; + while (str.indexOf(',', index) != -1) + { + index = str.indexOf(',', index + 1); + last_index = str.indexOf(',', index + 1); + if (index != -1) + { + last_index = (last_index == -1) ? str.length() : last_index; + if ((last_index - index) > 1) + { + String pamstr = str.substring(index + 1, last_index); +#ifdef DEBUG_GPS + Serial.printf("%d:", pamindex); + Serial.println(pamstr); +#endif + _s_GNGAS.pamstr[pamindex] = pamstr; + } + else + { + _s_GNGAS.pamstr[pamindex].clear(); + } + pamindex++; + } + } + + _s_GNGAS.mode2 = _s_GNGAS.pamstr[1].charAt(0); + _s_GNGAS.mode1 = _s_GNGAS.pamstr[2].toInt(); + for (size_t i = 0; i < 12; i++) + { + _s_GNGAS.PINMap[i] = _s_GNGAS.pamstr[3 + i].toInt(); + } + _s_GNGAS.PDOP = _s_GNGAS.pamstr[15].toFloat(); + _s_GNGAS.HDOP = _s_GNGAS.pamstr[16].toFloat(); + _s_GNGAS.VDOP = _s_GNGAS.pamstr[17].toFloat(); +} + +void GPSAnalyse::AnaGPGSV(String str) +{ + //Serial.println(str); + str = str.substring(str.indexOf("GPGSV"), str.length()); + + if (str.indexOf('*') != -1) + { + int indexsub = str.indexOf('*'); + String sumstr = str.substring(indexsub + 1, str.length()); + str = str.substring(0, indexsub); + //Serial.println(sumstr); + } + + int index = 0, last_index = 0, pamindex = 1; + while (str.indexOf(',', index) != -1) + { + index = str.indexOf(',', index + 1); + last_index = str.indexOf(',', index + 1); + if (index != -1) + { + last_index = (last_index == -1) ? str.length() : last_index; + if ((last_index - index) > 1) + { + String pamstr = str.substring(index + 1, last_index); +#ifdef DEBUG_GPS + Serial.printf("%d:", pamindex); + Serial.println(pamstr); +#endif + _s_GPGSV.pamstr[pamindex] = pamstr; + } + else + { + _s_GPGSV.pamstr[pamindex].clear(); + } + pamindex++; + } + } + int SatelliteSize = (pamindex - 4) / 4; + //Serial.printf("Number%d\n", SatelliteSize); + + _s_GPGSV.size = _s_GPGSV.pamstr[1].toInt(); + _s_GPGSV.Number = _s_GPGSV.pamstr[2].toInt(); + _s_GPGSV.SatelliteSize = _s_GPGSV.pamstr[3].toInt(); + + if (_s_GPGSV.Number == 1) + { + for (size_t i = 0; i < 32; i++) + { + _s_GPGSV.Satellite[i].flag = false; + } + } + for (size_t i = 0; i < SatelliteSize; i++) + { + int id = _s_GPGSV.pamstr[4 + (i * 4) + 0].toInt(); + if( id >= 32 ) continue; + if(( 7 + (i * 4)) > 50 ) + { + break; + } + _s_GPGSV.Satellite[id].elevation = _s_GPGSV.pamstr[4 + (i * 4) + 1].toInt(); + _s_GPGSV.Satellite[id].Azimuth = _s_GPGSV.pamstr[4 + (i * 4) + 2].toInt(); + _s_GPGSV.Satellite[id].SNR = (_s_GPGSV.pamstr[4 + (i * 4) + 3].length() == 0) ? -1 : _s_GPGSV.pamstr[4 + (i * 4) + 3].toInt(); + _s_GPGSV.Satellite[id].flag = true; + } + + if (_s_GPGSV.Number == _s_GPGSV.size) + { + for (size_t i = 0; i < 32; i++) + { + if (_s_GPGSV.Satellite[i].flag == true) + { +#ifdef DEBUG_GPS + Serial.printf("ID %d:%d,%d,%d\n", i, _s_GPGSV.Satellite[i].elevation, _s_GPGSV.Satellite[i].Azimuth, _s_GPGSV.Satellite[i].SNR); +#endif + } + } + } +} + +void GPSAnalyse::Analyse() +{ + while (_GPS_Str.indexOf('\r') != -1) + { + int index = _GPS_Str.indexOf('\r'); + String str = _GPS_Str.substring(0, index); + _GPS_Str = _GPS_Str.substring(index + 3, _GPS_Str.length()); + + str.trim(); + + //Serial.println(str); + + if (str.indexOf("GNRMC") != -1) + { + AnaGPRMC(str); + } + else if (str.indexOf("GPVTG") != -1) + { + } + else if (str.indexOf("GPGGA") != -1) + { + + } + else if (str.indexOf("GNGSA") != -1) + { + //Serial.print("GNGSA:"); + //AnaGNGAS(str); + } + else if (str.indexOf("GPGSV") != -1) + { + //Serial.print("GPGSV:"); + AnaGPGSV(str); + } + else if (str.indexOf("GPGLL") != -1) + { + } + } +} diff --git a/m5__wave-sampler-gps-imu/GPSAnalyse.h b/m5__wave-sampler-gps-imu/GPSAnalyse.h new file mode 100644 index 0000000..831fcd5 --- /dev/null +++ b/m5__wave-sampler-gps-imu/GPSAnalyse.h @@ -0,0 +1,117 @@ +#ifndef _GPSANALYSE_H_ +#define _GPSANALYSE_H_ + +#include +#include "utility/Task.h" +#include +#include + +//#define DEBUG_GPS + +typedef struct GNRMC +{ + String pamstr[13]; + String Utc; //1 + char State; //2 + double Latitude; //3 + char LatitudeMark; //4 + double Longitude; //5 + char LongitudeMark; //6 + float TrackSpeed; //7 + float TrackAngle; //8 + String Date; //9 + float Magnetic; //10 + char Declination; //11 + int mode; //12 + String Sum; +}GNRMC_t; + + +typedef struct GNGSA +{ + String pamstr[50]; + char mode2; + int mode1; + int PINMap[12]; + float PDOP; + float HDOP; + float VDOP; + String Sum; + +}GNGSA_t; + +typedef struct GPSSatellite +{ + bool flag; + int id; + int elevation; + int Azimuth; + int SNR; +}GPSSatellite_t; + +typedef struct GPGSV +{ + String pamstr[128]; + int size; + int Number; + int SatelliteSize; + GPSSatellite_t Satellite[32]; + String sum; +}GPGSV_t; + +/* +typedef struct GPGGA +{ +}GPGGA_t; + +typedef struct GPGGA +{ +}GPGGA_t; + + +typedef struct GPGLL +{ + +}GPGLL_t; +*/ + +class GPSAnalyse : public Task +{ +private: + + /* data */ + String _GPS_Str; + HardwareSerial *_serial; + SemaphoreHandle_t _xSemaphore = NULL; + char *GPSReadBuff; + + void Analyse(); + + void AnaGPRMC(String str); + void AnaGNGAS(String str); + void AnaGPGSV(String str); + + void run(void *data); + + GNRMC_t _s_GNRMC; + GNGSA_t _s_GNGAS; + GPGSV_t _s_GPGSV; + +public: + GPSAnalyse(); + ~GPSAnalyse(); + void setSerialPtr(HardwareSerial &serial); + void upDate(); + + + GNRMC_t s_GNRMC; + GNGSA_t s_GNGAS; + GPGSV_t s_GPGSV; +}; + + + + + + +#endif diff --git a/m5__wave-sampler-gps-imu/m5__wave-sampler-gps-imu.ino b/m5__wave-sampler-gps-imu/m5__wave-sampler-gps-imu.ino new file mode 100644 index 0000000..a1b5329 --- /dev/null +++ b/m5__wave-sampler-gps-imu/m5__wave-sampler-gps-imu.ino @@ -0,0 +1,171 @@ +#undef DEBUG_GPS + +#include "M5Atom.h" +#include "GPSAnalyse.h" +#include +#include "FS.h" +#include +#include + +GPSAnalyse GPS; +Metro banggps(2000); +Metro bangimu(20); +Metro banglog(1000); + +const char outfilename[] = "/wave-sample.csv"; +File of; + +typedef struct sGps { + float lat; + float lon; + String utc; +} tGps; + +typedef struct sImu { + // accelerometer + float aX = 0.0f; + float aY = 0.0f; + float aZ = 0.0f; + + // gyroscope + float gX = 0.0f; + float gY = 0.0f; + float gZ = 0.0f; + + // angles + float pitch = 0.0f; + float roll = 0.0f; + float yaw = 0.0f; + + float temp = 0.0f; +} tImu; + +unsigned long mselapsed; +tGps gpssample; +tImu imusample; + +bool writeSample(String filename) { + of = SD.open(filename, FILE_APPEND); + if(of) { + of.print( mselapsed ); + of.print(", "); + of.print(gpssample.lat); + of.print(", "); + of.print(gpssample.lon); + of.print(", "); + of.print(gpssample.utc); + of.print(", "); + of.print(imusample.aX); + of.print(", "); + of.print(imusample.aY); + of.print(", "); + of.print(imusample.aZ); + of.print(", "); + of.print(imusample.gX); + of.print(", "); + of.print(imusample.gY); + of.print(", "); + of.print(imusample.gZ); + of.print(", "); + of.print(imusample.pitch); + of.print(", "); + of.print(imusample.roll); + of.print(", "); + of.print(imusample.yaw); + of.print(", "); + of.print(imusample.temp); + of.println(); // EOL + of.close(); + } else { + return false; + } + return true; +} + +void setup(){ + M5.begin(true, false, true); + + M5.IMU.Init(); + + SPI.begin(23,33,19,-1); + if(!SD.begin(-1, SPI, 40000000)){ + Serial.println("initialization failed!"); + } + sdcard_type_t Type = SD.cardType(); + + Serial.printf("SDCard Type = %d \r\n",Type); + Serial.printf("SDCard Size = %d \r\n" , (int)(SD.cardSize()/1024/1024)); + + Serial1.begin(9600,SERIAL_8N1,22,-1); + GPS.setTaskName("GPS"); + GPS.setTaskPriority(2); + GPS.setSerialPtr(Serial1); + GPS.start(); + + M5.dis.fillpix(0x000000); +} + +void gps_loop() { + GPS.upDate(); + gpssample.lat = GPS.s_GNRMC.Latitude; + gpssample.lon = GPS.s_GNRMC.Longitude; + gpssample.utc = GPS.s_GNRMC.Utc; +} + +void imu_loop() { + //Stores the triaxial gyroscope data of the inertial sensor to the relevant variable + M5.IMU.getGyroData(&imusample.gX,&imusample.gY,&imusample.gZ); + M5.IMU.getAccelData(&imusample.aX,&imusample.aY,&imusample.aZ); // accelero + M5.IMU.getAhrsData(&imusample.pitch,&imusample.roll,&imusample.yaw); // attitude + M5.IMU.getTempData(&imusample.temp); // temperature +} + +void debugSample() { + Serial.print( mselapsed ); + Serial.print(", "); + Serial.print(gpssample.lat); + Serial.print(", "); + Serial.print(gpssample.lon); + Serial.print(", "); + Serial.print(gpssample.utc); + Serial.print(", "); + Serial.print(imusample.aX); + Serial.print(", "); + Serial.print(imusample.aY); + Serial.print(", "); + Serial.print(imusample.aZ); + Serial.print(", "); + Serial.print(imusample.gX); + Serial.print(", "); + Serial.print(imusample.gY); + Serial.print(", "); + Serial.print(imusample.gZ); + Serial.print(", "); + Serial.print(imusample.pitch); + Serial.print(", "); + Serial.print(imusample.roll); + Serial.print(", "); + Serial.print(imusample.yaw); + Serial.print(", "); + Serial.print(imusample.temp); + Serial.println(); // EOL +} + +void loop() { + M5.dis.fillpix(0x000000); + + if(banggps.check()) { + gps_loop(); + } + + if(bangimu.check()) { + imu_loop(); +// writeSample( outfilename ); + } + + if(banglog.check()) { + debugSample(); + } + + mselapsed = millis(); +}