Hi everyone!
I’m working in a proect where i use a GPS, and 1 IMU. What i want to do is to take 2 sentences from the GPS ($GPGGA, $GPRMC), then i want to take the GPRMC sentence and replace the CoG Value(Course over Ground) and replace it for the Yaw reading of my IMU. Later, i want to send the GPGGA sentence through serial as i get it from the gps and in a new line print the GPRMC sentence with the CoG from The IMU. But i looks like im getting a buffer overflow and the terminal gets stucked after printing some lines.
I hope my problem is well explained…
Here is my code:
#include <ADXL345.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <bma180.h>
#include <MS561101BA.h>
//#define DEBUG
#include "DebugUtils.h"
#include "FreeIMU.h"
#include "CommunicationUtils.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU60X0.h"
#include <SPI.h>
#include <EEPROM.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
float ypr[3],v8;
char inputString[1024],iS[1024],iS2[1024],str[1024],v8v[20];
char *v0,*v1,*v2,*v3,*v4,*v5,*v6,*v7,*v9,*v10,*v01,*v02,*u,*y;
int i = 0,chk=0;
boolean stringComplete = false;
FreeIMU my3IMU = FreeIMU();
void setup() {
Serial.begin(115200);
Serial3.begin(57600); //GPS
delay(50);
Wire.begin();
my3IMU.init(0x69,false); //initialize IMU i2c comm
}
void loop() {
my3IMU.getYawPitchRoll(ypr); //Read Yaw value from IMU
if (stringComplete) {
if(inputString [0]== '
Thanks!!!
&& inputString [1]== ‘G’ && inputString [2]== ‘P’ && inputString [3]== ‘G’ && inputString [4]== ‘G’ && inputString [5]== ‘A’){
Serial.print(inputString); //Prints GPGGA Sentence as i get it from GPS
// clear the string:
inputString[0] = ‘\0’;
i=0;
stringComplete = false;
}
if(inputString [0]== ’
Thanks!!!!!
&& inputString [1]== 'G' && inputString [2]== 'P' && inputString [3]== 'R' && inputString [4]== 'M' && inputString [5]== 'C'){
iS[0]='\0';
strcpy(iS,inputString); //Copy String from GPS to a new string
v0= strtok_r(iS,",",&u); //Read Tokens from GPRMC sentence
v1= strtok_r(NULL,",",&u);
v2= strtok_r(NULL,",",&u);
v3= strtok_r(NULL,",",&u);
v4= strtok_r(NULL,",",&u);
v5= strtok_r(NULL,",",&u);
v6= strtok_r(NULL,",",&u);
v7= strtok_r(NULL,",",&u);
strtok_r(NULL,",",&u);
v8=posDegrees(ypr[0]); //Replace CoG value for IMU data
v9= strtok_r(NULL,",",&u);
v10= strtok_r(NULL,",",&u);
strcpy(str,v0); //Rebuild GPRMC Sentence with CoG value from IMU
strcat(str,",");
strcat(str,v1);
strcat(str,",");
strcat(str,v2);
strcat(str,",");
strcat(str,v3);
strcat(str,",");
strcat(str,v4);
strcat(str,",");
strcat(str,v5);
strcat(str,",");
strcat(str,v6);
strcat(str,",");
strcat(str,v7);
strcat(str,",");
floatToString(v8v,v8,2,4);
strcat(str,v8v);
strcat(str,",");
strcat(str,v9);
strcat(str,",");
strcat(str,v10);
chk=getChkSum(str); //Calculate new checksum with imu data
strcpy(iS2,str);
v01=strtok_r(iS2,"*",&y);
Serial.print(v01); //prints GPRMC Sentence
Serial.print("*");
Serial.println(chk,HEX);
inputString[0] = '\0'; //Empty string buffer for new sentence
i=0;
stringComplete = false;
}
}
}
void serialEvent3() {
while (Serial3.available()) {
char inChar = (char)Serial3.read();
inputString[i] = inChar;
i++;
if (inChar == '\n') {
stringComplete = true;
inputString[i] ='\0';
}
}
}
// Calculates the checksum for a given string
// returns as integer
int getChkSum(char *string) {
int i;
int XOR;
int c;
// Calculate checksum ignoring any
Thanks!!!
s in the string
for (XOR = 0, i = 0; i < strlen(string); i++) {
c = (unsigned char)string[i];
if (c == ‘*’) break;
if (c != ’
Thanks!!!!!
) XOR ^= c;
}
return XOR;
}
char * floatToString(char * outstr, double val, byte precision, byte widthp){
char temp[16]; //increase this if you need more digits than 15
byte i;
temp[0]='\0';
outstr[0]='\0';
if(val < 0.0){
strcpy(outstr,"-\0"); //print "-" sign
val *= -1;
}
if( precision == 0) {
strcat(outstr, ltoa(round(val),temp,10)); //prints the int part
}
else {
unsigned long frac, mult = 1;
byte padding = precision-1;
while (precision--)
mult *= 10;
val += 0.5/(float)mult; // compute rounding factor
strcat(outstr, ltoa(floor(val),temp,10)); //prints the integer part without rounding
strcat(outstr, ".\0"); // print the decimal point
frac = (val - floor(val)) * mult;
unsigned long frac1 = frac;
while(frac1 /= 10)
padding--;
while(padding--)
strcat(outstr,"0\0"); // print padding zeros
strcat(outstr,ltoa(frac,temp,10)); // print fraction part
}
// generate width space padding
if ((widthp != 0)&&(widthp >= strlen(outstr))){
byte J=0;
J = widthp - strlen(outstr);
for (i=0; i< J; i++) {
temp[i] = ' ';
}
temp[i++] = '\0';
strcat(temp,outstr);
strcpy(outstr,temp);
}
return outstr;
}
float posDegrees(float v){
float r;
if (v>=0 && v<=180){
r=v;
}
if (v<0 && v>=-180){
r=v+360;
}
return r;
}
Thanks!!!
