Help with code structure GPS and Tilt Compass

Hi guys,

See my code below

#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <math.h>
#include <Wire.h>
#include <LSM303.h>

TinyGPS gps;
LSM303 compass;

float x_offset = 12129.797128;
float y_offset = -2836.707283;
float z_offset =  6202.044569;
const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;
int i = 0;

SoftwareSerial ss(10,11);  

// Store our compass as a variable.

float Coursedegrees, Courserads;


void setup() {
 
  Serial.begin(9600);
  ss.begin(9600);
  Wire.begin();
compass.init();
compass.enableDefault();
  
 Coursedegrees = getheading ();
 //Converts into useable radians
 Courserads = Coursedegrees * PI/180;
 //converts for display into an interger
 
}

void loop() {
  
float lat, lon, latrad, lonrad;
int i, hdng;
float lat1, lon1;
float brngdegrees, brngrad;
int d, R;  
  d = 1000;   //set distance
  R = 6371000; //radius of earth
 
//  for (i==1; i++)

//Calculating inital waypoints from current location.
 // { 
        //Initating GPS
while(ss.available()){ // check for gps data
   if(gps.encode(ss.read())){ // encode gps data
      gps.f_get_position(&lat,&lon); // get latitude and longitude
      
 //Calls the getcompassbrng function to continuously calculate the reading on the compass during travel 
 //brngdegrees = getcompassbrng ();

 //Converts to int to use in differences math for PID loop 
//int compass_heading = brngdegrees;

 for (i=1; i>0; i++) {
       // display position
    Serial.println();
    Serial.print("Current Position: ");
    Serial.print("lat: ");Serial.print(lat,6);Serial.print(" ");// print latitude
    Serial.print("lon: ");Serial.println(lon,6); // print longitude

    //Calculating the corodinates for the next waypoint at given distance d. Used a flat earth model,
    //as distances never exceed 2 km. 

 latrad=lat*(PI/180);
 lonrad=lon*(PI/180);

    lat1 = ((cos(Courserads)*d)/111111)+ lat;
    lon1 =(((d*sin(Courserads))/cos(Courserads))/111111) + lon;
    
    Serial.println ();
    Serial.print("Waypoint Lat: ");Serial.print(lat1,6);Serial.print(" ");// print new latitude
    Serial.print("Waypoint Lon: ");Serial.println(lon1,6); // print new longitude
    Serial.println ();
    
 }

 
    
hdng = TinyGPS::course_to(lat, lon, lat1, lon1);
brngdegrees = getheading ();
int compass_heading = brngdegrees;


   //Next for loop will run the main code, 
  

Serial.print ("Initial Course: "); Serial.print(Coursedegrees);
Serial.println ();

Serial.print ("Bearing to Waypoint: "); Serial.print(hdng);
Serial.println();
Serial.print ("Compass bearing: "); Serial.print(compass_heading);
Serial.println();



 
//Runs dealy between next read

 delay(1000);
 
  }
}
}

int getheading ()
{
compass.read();
float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;
//float Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal;
//float Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal;
// Accelerometer calibration
Xa_off = compass.a.x/16.0 -40.706689; //X-axis combined bias (Non calibrated data - bias)
Ya_off = compass.a.y/16.0 -17.265535; //Y-axis combined bias (Default: substracting bias)
Za_off = compass.a.z/16.0 -36.354047; //Z-axis combined bias

Xa_cal =  0.971172*Xa_off  -0.002819*Ya_off - 0.004151*Za_off; //X-axis correction for combined scale factors (Default: positive factors)
Ya_cal =  -0.002819*Xa_off + 0.961651*Ya_off + 0.002947*Za_off; //Y-axis correction for combined scale factors
Za_cal = -0.0041517*Xa_off - 0.002947*Ya_off + 0.956406*Za_off;

// Magnetometer calibration
Xm_off = compass.m.x*(100000.0/1100.0) + x_offset; //X-axis combined bias (Non calibrated data - bias)
Ym_off = compass.m.y*(100000.0/1100.0) - y_offset; //Y-axis combined bias (Default: substracting bias)
Zm_off = compass.m.z*(100000.0/980.0 ) + z_offset; //Z-axis combined bias

Xm_cal = 0.000865*Xm_off + 0.000021*Ym_off - 0.000002*Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
Ym_cal =  0.000021*Xm_off + 0.000909*Ym_off - 0.0000074*Zm_off; //Y-axis correction for combined scale factors
Zm_cal =  -0.000002*Xm_off - 0.000007*Ym_off + 0.000831*Zm_off; //Z-axis correction for combined scale factors

// Low-Pass filter accelerometer
fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
fZa = Za_cal * alpha + (fZa * (1.0 - alpha));

// Low-Pass filter magnetometer
fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));

// Pitch and roll
roll  = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;

// Tilt compensated magnetic sensor measurements
fXm_comp = fXm*cos(pitch)+fZm*sin(pitch);
fYm_comp = fXm*sin(roll)*sin(pitch)+fYm*cos(roll)-fZm*sin(roll)*cos(pitch);

// Arctangent of y/x
Heading = (atan2(fYm_comp,fXm_comp)*180.0)/M_PI;
if (Heading < 0)
Heading += 360;
 return Heading;
}

As it stands I am trying to get the compass to read once, store and use to calculate the waypoint as per the code above.

However, I obviously only want to do this step once each time the arduino is switched on. I thought the for loop with a counter would work, however it stays in a loop only outputting the current position and waypoint position.

I want to do this step once, then continuously do the ‘TinyGPS course’ and then compare this with my compass reading to work a servo.

Im sure its something easily fixed, that I have overlooked, however it has me stumped at the moment.
Cheers guys,

Things that should only happen when the Arduino is started should go in setup().

I can’t make sense of your code.
You have

//brngdegrees = getcompassbrng ();

but there is no function called getcompassbrng() in your code.

If you want to have some code in loop() that only works once you just need a variable that records whether it has happened. For example

if (compassRead == false) {
    readCompass();
    compassRead = true;
}

…R

Hi Robin,

Apologies for the nonsense in the code, I had commented that out, and forgot to remove it before I posted it.

I had thought about putting the waypoint calculation in the setup loop, however, everytime I tried it was spitting out errors.

I tidied up the code.

#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <math.h>
#include <Wire.h>
#include <LSM303.h>

TinyGPS gps;
LSM303 compass;

float x_offset = 12129.797128;
float y_offset = -2836.707283;
float z_offset =  6202.044569;
const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;
int i = 0;

SoftwareSerial ss(10,11);  

float Coursedegrees, Courserads;

void setup() {
 
  Serial.begin(9600);
  ss.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  
 Coursedegrees = getheading ();
 //Converts into useable radians
 Courserads = Coursedegrees * PI/180;
 
}

void loop() {
  
float lat, lon, latrad, lonrad;
int i, hdng;
float lat1, lon1;
float brngdegrees, brngrad;
int d, R;  
  d = 1000;   //set distance
  R = 6371000; //radius of earth
 
//  for (i==1; i++)

//Calculating inital waypoints from current location.
 // { 
        //Initating GPS
while(ss.available()){ // check for gps data
   if(gps.encode(ss.read())){ // encode gps data
      gps.f_get_position(&lat,&lon); // get latitude and longitude
      
//Calculates the waypoint given the start position (I only want  this part of the code to run once
 for (i=1; i>0; i++) {
       // display position
    Serial.println();
    Serial.print("Current Position: ");
    Serial.print("lat: ");Serial.print(lat,6);Serial.print(" ");// print latitude
    Serial.print("lon: ");Serial.println(lon,6); // print longitude

    //Calculating the corodinates for the next waypoint at given distance d. Used a flat earth model,
    //as distances never exceed 2 km. 

 latrad=lat*(PI/180);
 lonrad=lon*(PI/180);

    lat1 = ((cos(Courserads)*d)/111111)+ lat;
    lon1 =(((d*sin(Courserads))/cos(Courserads))/111111) + lon;
    
    Serial.println ();
    Serial.print("Waypoint Lat: ");Serial.print(lat1,6);Serial.print(" ");// print new latitude
    Serial.print("Waypoint Lon: ");Serial.println(lon1,6); // print new longitude
    Serial.println ();
    
 }

 
 //This part of the code I would like to run continuously, to work the servo (not added in yet).

hdng = TinyGPS::course_to(lat, lon, lat1, lon1);
brngdegrees = getheading ();
int compass_heading = brngdegrees;


Serial.print ("Initial Course: "); Serial.print(Coursedegrees);
Serial.println ();

Serial.print ("Bearing to Waypoint: "); Serial.print(hdng);
Serial.println();
Serial.print ("Compass bearing: "); Serial.print(compass_heading);
Serial.println();


//Runs dealy between next read

 delay(1000);
 
  }
}
}

int getheading ()
{
compass.read();
float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;
//float Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal;
//float Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal;
// Accelerometer calibration
Xa_off = compass.a.x/16.0 -40.706689; //X-axis combined bias (Non calibrated data - bias)
Ya_off = compass.a.y/16.0 -17.265535; //Y-axis combined bias (Default: substracting bias)
Za_off = compass.a.z/16.0 -36.354047; //Z-axis combined bias

Xa_cal =  0.971172*Xa_off  -0.002819*Ya_off - 0.004151*Za_off; //X-axis correction for combined scale factors (Default: positive factors)
Ya_cal =  -0.002819*Xa_off + 0.961651*Ya_off + 0.002947*Za_off; //Y-axis correction for combined scale factors
Za_cal = -0.0041517*Xa_off - 0.002947*Ya_off + 0.956406*Za_off;

// Magnetometer calibration
Xm_off = compass.m.x*(100000.0/1100.0) + x_offset; //X-axis combined bias (Non calibrated data - bias)
Ym_off = compass.m.y*(100000.0/1100.0) - y_offset; //Y-axis combined bias (Default: substracting bias)
Zm_off = compass.m.z*(100000.0/980.0 ) + z_offset; //Z-axis combined bias

Xm_cal = 0.000865*Xm_off + 0.000021*Ym_off - 0.000002*Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
Ym_cal =  0.000021*Xm_off + 0.000909*Ym_off - 0.0000074*Zm_off; //Y-axis correction for combined scale factors
Zm_cal =  -0.000002*Xm_off - 0.000007*Ym_off + 0.000831*Zm_off; //Z-axis correction for combined scale factors

// Low-Pass filter accelerometer
fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
fZa = Za_cal * alpha + (fZa * (1.0 - alpha));

// Low-Pass filter magnetometer
fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));

// Pitch and roll
roll  = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;

// Tilt compensated magnetic sensor measurements
fXm_comp = fXm*cos(pitch)+fZm*sin(pitch);
fYm_comp = fXm*sin(roll)*sin(pitch)+fYm*cos(roll)-fZm*sin(roll)*cos(pitch);

// Arctangent of y/x
Heading = (atan2(fYm_comp,fXm_comp)*180.0)/M_PI;
if (Heading < 0)
Heading += 360;
 return Heading;
}

Your waypoint calculation does not look to be correct. In particular, the distance spanned by 1 degree of longitude depends on the latitude.

You might check against the calculator and formulas presented on this site.

@ Jremington,

I have double checked this calculations, and initially I had tried working with the formulae on the above site. However, because I am only dealing with short distances, I thought the formulae overkill.

Hence I have gone with a more basic calculation. As it stands through trials, the calculations predict the new waypoint quite accurately, through different locations in New Zealand.

How can you have an int with a value of over 6 million? Radius.

Good point. However I don’t use R anymore. It’s the remnants of the old code.

Zac

zacl79: I tidied up the code.

It still gets an 'F' for poor formatting :)

In any case, which is the piece that should only run once ?

You would make it much easier to manage the flow of your program if you break it into small single-purpose functions rather than bundle all the code into loop()

Have a look at planning and implementing a program.

...R

As it stands through trials, the calculations predict the new waypoint quite accurately, through different locations in New Zealand.

That is probably accidental, because the formula is clearly incorrect. One line needs to be changed to include the latitude correction, for example:

    lon1 =(((d*sin(Courserads))/cos(Latituderads))/111111) + lon;