boringrgb
/
seacamp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

first commit

master
Luis Rodil-Fernandez 3 years ago
commit
d0baf9d0e3
4 changed files with 571 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      README.md
  2. 280
      m5__wave-sampler-gps-imu/GPSAnalyse.cpp
  3. 117
      m5__wave-sampler-gps-imu/GPSAnalyse.h
  4. 171
      m5__wave-sampler-gps-imu/m5__wave-sampler-gps-imu.ino

3
README.md
View File

@ -0,0 +1,3 @@
## Seacamp experiments

280
m5__wave-sampler-gps-imu/GPSAnalyse.cpp
View File

@ -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)
{
}
}
}

117
m5__wave-sampler-gps-imu/GPSAnalyse.h
View File

@ -0,0 +1,117 @@
#ifndef _GPSANALYSE_H_
#define _GPSANALYSE_H_
#include <Arduino.h>
#include "utility/Task.h"
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
//#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

171
m5__wave-sampler-gps-imu/m5__wave-sampler-gps-imu.ino
View File

@ -0,0 +1,171 @@
#undef DEBUG_GPS
#include "M5Atom.h"
#include "GPSAnalyse.h"
#include <SPI.h>
#include "FS.h"
#include <SD.h>
#include <Metro.h>
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();
}
Write Preview
Loading…
Cancel
Save