Hey all!
Long time lurker but first time poster. I’ve created an app I’m going to use for logging data from my motorcycle for review later.
Everything seems to be working well expect for the speed. It seems to stick to one speed for a long time and updates very, very slowly. I am trying to determine if this is an issue with my refresh rate and the speed at which TinyGPSPlus operates, or something else.
I set my refresh rate at 10Hz and am only allowing for the RMC string to speed things up.
If anyone could provide any guidance, that would be amazing!
Thanks in advance!
#include <SdFat.h>
#include <TinyGPS++.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "RTPressure.h"
#include "CalLib.h"
#include <EEPROM.h>
#include <math.h>
#define SD_CHIP_SELECT 53
#define ss Serial1
#define LOG_COLUMN_COUNT 8
void doSomeWork();
SdFat sdcard;
SdFile logfile;
RTIMU *imu; // the IMU object
RTPressure *pressure; // the pressure object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
RTVector3 vec; // the vector object
TinyGPSPlus gps;
const char *log_col_names[LOG_COLUMN_COUNT] = {
"timer", "time", "latitude", "longitude", "mph", "roll", "pitch", "heading"
}; // log_col_names is printed at the top of the file.
unsigned long lastDisplay;
unsigned long lastRate;
int counter = 0;
const int UTC_offset = -8; // Pacific Standard Time
char filename[15];
void setup()
{
int errcode;
Serial.begin(115200);
ss.begin(9600);
ss.println(F("$PMTK251,38400*27")); // 38400 baud
ss.end();
ss.begin(38400);
delay(5000);
ss.println(F("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28")); // RMC and GGA
ss.println(F("$PMTK220,100*2F")); // 10 Hz
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
pressure = RTPressure::createPressure(&settings); // create the pressure sensor
if (pressure == 0) {
Serial.println("No pressure sensor has been configured - terminating");
while (1) ;
}
Serial.print("ArduinoIMU10 starting using IMU "); Serial.print(imu->IMUName());
Serial.print(", pressure sensor "); Serial.println(pressure->pressureName());
if ((errcode = imu->IMUInit()) < 0) {
Serial.print("Failed to init IMU: "); Serial.println(errcode);
}
if ((errcode = pressure->pressureInit()) < 0) {
Serial.print("Failed to init pressure sensor: "); Serial.println(errcode);
}
if (imu->getCalibrationValid())
Serial.println("Using compass calibration");
else
Serial.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
// Slerp power controls the fusion and can be between 0 and 1
// 0 means that only gyros are used, 1 means that only accels/compass are used
// In-between gives the fusion mix.
fusion.setSlerpPower(0.50);
// use of sensors in the fusion algorithm can be controlled here
// change any of these to false to disable that sensor
fusion.setGyroEnable(true);
fusion.setAccelEnable(true);
fusion.setCompassEnable(true);
delay(500);
initializeSD();
delay(500);
printHeader(filename);
}
void loop()
{
while (ss.available() > 0) {
if (gps.encode(ss.read())) {
doSomeWork();
}
}
}
void doSomeWork()
{
unsigned long now = millis();
unsigned long delta;
float latestPressure;
float latestTemperature;
int loopCount = 1;
while (imu->IMURead()) { // get the latest data if ready yet
// this flushes remaining data in case we are falling behind
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
vec = fusion.getFusionPose();
}
//Logs all the things
logfile.open(filename, O_CREAT | O_WRITE | O_APPEND);
logfile.print( counter );
logfile.print(',');
logfile.print( gps.time.hour() );
logfile.print(':');
logfile.print( gps.time.minute() );
logfile.print(':');
logfile.print( gps.time.second() );
logfile.print(',');
logfile.print( gps.location.lat(), 6 );
logfile.print(',');
logfile.print( gps.location.lng(), 6 );
logfile.print(',');
logfile.print( gps.speed.mph(), 1 );
logfile.print(',');
logfile.print(vec.x() * (180.0 / M_PI));
logfile.print(',');
logfile.print(vec.y() * (180.0 / M_PI));
logfile.print(',');
logfile.println(vec.z() * (180.0 / M_PI));
/*
//prints all the things
Serial.print( counter );
Serial.print(',');
Serial.print( gps.time.hour() );
Serial.print(':');
Serial.print( gps.time.minute() );
Serial.print(':');
Serial.print( gps.time.second() );
Serial.print(',');
Serial.print( gps.location.lat(), 6 );
Serial.print(',');
Serial.print( gps.location.lng(), 6 );
Serial.print(',');
Serial.print( gps.speed.mph(), 1 );
Serial.print(',');
Serial.print( vec.x() * (180.0 / M_PI) );
Serial.print(',');
Serial.print( vec.y() * (180.0 / M_PI) );
Serial.print(',');
Serial.println( vec.z() * (180.0 / M_PI) );
*/
logfile.close(); // close the file
counter++;
}
void printHeader(char f[15])
{
if ( ! logfile.open(f, O_CREAT | O_WRITE | O_EXCL) ) {
int i = 0;
for (; i < LOG_COLUMN_COUNT; i++)
{
logfile.print(log_col_names[i]);
if (i < LOG_COLUMN_COUNT - 1) // If it's anything but the last column
logfile.print(','); // print a comma
else // If it's the last column
logfile.println(); // print a new line
}
}
logfile.close(); // close the file
}
void initializeSD()
{
Serial.print(F("\n\nInitializing SD card..."));
if (!sdcard.begin(SD_CHIP_SELECT, SPI_FULL_SPEED)) {
Serial.println(F("Card init. failed!"));
while (true) {}
}
Serial.println(F("done."));
Serial.print(F("Opening file..."));
strcpy(filename, "GPSLOG00.CSV");
for (uint8_t i = 0; i < 100; i++) {
filename[6] = '0' + i / 10;
filename[7] = '0' + i % 10;
if (! sdcard.exists(filename)) {
break;
}
}
if ( ! logfile.open(filename, O_CREAT | O_WRITE | O_EXCL) ) {
Serial.print("Couldnt create ");
Serial.println(filename);
while (true) {}
}
Serial.print(" Writing to ");
Serial.println(filename);
}
