Hi all,
I'm pretty sure I'm missing something quite obvious here but I've not been able to sort the issue out for a while.
Here's the code, edited to show it in it's entirety.
#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <HMC5883L.h>
#include <Math.h>
HMC5883L mag;
int16_t mx, my, mz;
int rud;
int rud2;
long lat,lon;
long currentWPlon, currentWPlat;
long latDiff, lonDiff, angDiff;
TinyGPS gps;
void setup() {
Wire.begin();
mag.initialize();
Serial.begin(38400);
Serial1.begin(38400);
currentWPlon = -30845703;
currentWPlat = 12609874;
}
void loop() {
while(Serial1.available()){
if(gps.encode(Serial1.read())){
gps.get_position(&lat,&lon);
float speed = gps.f_speed_kmph();
Serial.print("Position: ");
Serial.print("Latitude: ");
Serial.print(lat);
Serial.print("\t");
Serial.print("Longitude: ");
Serial.print(lon);
Serial.print("\t");
Serial.print("Speed: ");
Serial.print(speed);
Serial.println(" km/h");
latDiff = abs(lat-currentWPlat);
Serial.print("latDiff: ");
Serial.print(latDiff);
Serial.print("\t");
lonDiff = abs(lon-currentWPlon);
Serial.print("lonDiff: ");
Serial.print(lonDiff);
Serial.print("\t");
angDiff = atan(latDiff/lonDiff);
Serial.print("Angle Difference: ");
Serial.println(angDiff * 180/M_PI);
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);
// display tab-separated gyro x/y/z values
Serial.print("mag:\t");
Serial.print(mx); Serial.print("\t");
Serial.print(my); Serial.print("\t");
Serial.print(mz); Serial.print("\t");
// To calculate heading in degrees. Where 0 degree indicates North
float rud = atan2(my, mx);
if(rud < 0)
rud += 2 * M_PI;
Serial.print("heading:\t");
rud2 = (rud * 180/M_PI);
Serial.println(rud2);
//Serial.println(rud2);
// blink LED to indicate activity
// blinkState = !blinkState;
// digitalWrite(LED_PIN, blinkState);
delay(2000);
}
}
}
The problem is that when the angDiff value is printed, it shows as 0 every time it loops, any idea how I can fix this?
The values of lonDiff and latDiff are correct, which leads me to believe the issue must purely be in my use of the atan function.
The values of currentWPlon and currentWPlat are hard coded in at the moment, and oddly enough, when I mistakenly entered decimal points into their values when there should be none (making them 1,000,000 times smaller) an angDiff value of 57.30 was shown every loop.
I'd greatly appreciate any help.
Thanks