Проект C++ Arduino с Ublox NEO6MV2 GPS и компасом HMC5883L

Я хочу использовать модуль GPS и датчик компаса вместе и регистрировать информацию с помощью SD-карты. Я закончил аппаратную часть, но мой код не работает должным образом. Я могу регистрировать данные GPS на SD-карте, но я не могу регистрировать данные GPS с данными направления компаса в этом формате => дата-время+долгота+широта+скорость+курс..

Я думаю, что по какой-то причине цикл прерывается и останавливается после первого запуска, но я не мог понять, почему.

#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <SD.h>
#include <Wire.h>
#include <HMC5883L.h>

HMC5883L compass;

TinyGPS gps;
static char dtostrfbuffer[20];
int CS = 10;
int LED = 13;
int SD_LED = A0;
int GPS_LED = A1;
int Blink_LED = 6;
SoftwareSerial mySerial(2 , 3); // RX, TX


String SD_date_time = "";
String SD_lat = "";
String SD_lon = "";
String SD_spd = "";
String SD_heading = "";
String dataString ="";

static void gpsdump(TinyGPS &gps);
static bool feedgps();


void setup()
{
  pinMode(CS, OUTPUT);  //Chip Select Pin for the SD Card
  pinMode(LED, OUTPUT);  //LED Indicator

  //Serial interfaces
  Serial.begin(9600);
  mySerial.begin(9600);
   Wire.begin();

  //Connect to the SD Card
  if(!SD.begin(CS))
  {
    Serial.println("card failure");
    return;
  }


 compass = HMC5883L(); //new instance of HMC5883L library
 setupHMC5883L(); //setup the HMC5883L

}


void loop()
{
  bool newdata = false;
  unsigned long start = millis();

  // Every 5 second we print an update
  while (millis() - start < 5000)
  {
    if (feedgps())
      newdata = true;
  }
  gpsdump(gps);
  int intHead = ReadCompass();
  SD_heading = String(intHead); 
  dataString = SD_date_time + "," + SD_lat + "," + SD_lon + "," + SD_spd + "," + SD_heading;

  //Open the Data CSV File
  File dataFile = SD.open("LOG.csv", FILE_WRITE);
  if (dataFile)
  {
    dataFile.println(dataString);
    Serial.print("SD card writing:");
    Serial.println(dataString);
    dataFile.close();
    digitalWrite(SD_LED, HIGH);      //if sd works led is on

  }
  else
  {
    Serial.println("\ncouldn't open LOG file!");
  }
}


int ReadCompass() 
{
  int intHead;
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();

  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;

  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 

  intHead= int (headingDegrees);
  return intHead;
  delay (200);
}

//GPS Reading Function  
static void gpsdump(TinyGPS &gps)
{
  float flat, flon;
  unsigned long age, date, time, chars = 0;
  unsigned short sentences = 0, failed = 0;

  int year;
  byte month, day, hour, minute, second, hundredths;


  gps.f_get_position(&flat, &flon, &age); 
  gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
  gps.stats(&chars, &sentences, &failed);
  float fmps = gps.f_speed_mps(); // speed in m/sec

  if (age == TinyGPS::GPS_INVALID_AGE)
  {
    Serial.println("no GPS connection");

    SD_lat = "invalid";
    SD_lon = "invalid";
    SD_date_time = "invalid";

    Serial.print("Age:"); 
    Serial.println(age);
    Serial.print("char count:"); 
    Serial.println(chars);
    Serial.print("valid NMEA sentences"); 
    Serial.println(sentences);
    Serial.print("invalid NMEA sentences"); 
    Serial.println(failed);
    digitalWrite(GPS_LED, HIGH);                //if GPS doesn't get fix led is on
  }
  else
  {

    Serial.println("GPS connection is available ");

    digitalWrite(LED, HIGH);          //GPS fix exists

    SD_lat = dtostrf(flat,9,5,dtostrfbuffer);
    SD_lon = dtostrf(flon,10,5,dtostrfbuffer);
    SD_spd = dtostrf(fmps,5,2,dtostrfbuffer);

    char sz[32];
    sprintf(sz, "%02d/%02d/%02d %02d:%02d:%02d   ",
    day, month, year, hour+2, minute, second);
    Serial.print(sz);
    Serial.println(SD_lat);
    Serial.println(SD_lon);
    Serial.print("Hiz:"); 
    Serial.println(SD_spd);
    Serial.print("Age:"); 
    Serial.println(age);
    Serial.print("char count:"); 
    Serial.println(chars);
    Serial.print("valid NMEA sentences"); 
    Serial.println(sentences);
    Serial.print("invalid NMEA sentences:"); 
    Serial.println(failed);
    SD_date_time = sz;
  }
  Serial.println();
}

static bool feedgps()
{
  while (mySerial.available())
  {
    if (gps.encode(mySerial.read()))
      return true;
  }
  return false;
}


void setupHMC5883L(){
 //Setup the HMC5883L, and check for errors
 int error; 
 error = compass.SetScale(1.3); //Set the scale of the compass.
 if(error != 0) Serial.println(compass.GetErrorText(error)); //check if there is an error, and print if so

 error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
 if(error != 0) Serial.println(compass.GetErrorText(error)); //check if there is an error, and print if so
}