Ibus telemetry with gps neo 6m

[olah951]

HelloUse code tags to format code for the forum
I have an arduino nano that transmits data via ibus to a flysky i6 radio control. I use a gps neo 6m that sends data to the radio control, I can save several points in the eeprom and read again with a certain button. The problem is that I don't have the correct distance displayed from home point 0 to the selected point. Can someone help me? Thank you

#include <iBUSTelemetry.h>
#include <EEPROM.h>
#include <TinyGPS++.h>

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

const int ledPin = 2;
const int led2Pin = 8;
const int buzzerPin = 10; // Pinul pentru buzzer
const int swaPin = 3; // Pin pentru SWA
const int swbPin = 7; // Pin pentru SWB
const int swcPin = 5; // Pin pentru SWC
const int swdPin = 6; // Pin pentru SWD

#define TRIG 11 // Module pins
#define ECHO 12 

TinyGPSPlus gps;  // DHLWSH GPS

#define UPDATE_INTERVAL 100  // 

iBUSTelemetry telemetry(9);  //

uint32_t prevMillis = 0;  //
volatile uint8_t countSatelite = 0;

const int maxSavedPoints = 10;
int currentPointIndex = 0; // Indexul punctului curent

struct GPSPoint {
  double latitude;
  double longitude;
};

GPSPoint savedPoints[maxSavedPoints];

// Adăugare pentru punctul Home
GPSPoint homePoint = {0.0, 0.0}; // Coordonate inițiale pentru punctul Home
double startpoint_lat = -4.409;
double startpoint_lon = +4.678;
double TXdistanceRX = 7, i = 6;
long heading = 0; // modificare: initializam heading cu 0 pentru a incepe de la 0
double offsetX = 0.0, offsetY = 0.0, offsetZ = 0.0;

void setup() {
  pinMode(ledPin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(buzzerPin, OUTPUT); // Configurează pinul buzzerului ca ieșire
  pinMode(TRIG, OUTPUT); // Initializing Trigger Output and Echo Input 
  pinMode(ECHO, INPUT_PULLUP);
  pinMode(swcPin, INPUT_PULLUP);
  pinMode(swdPin, INPUT_PULLUP);

  Serial.begin(9600);
  Serial.println("HMC5883 Magnetometer Test");
  Serial.println("");

  /* Initialise the sensor */
  if (!mag.begin()) {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while (1);
  }

  telemetry.begin();
  // sensor definition
  telemetry.addSensor(0x01);  // temp
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_ALT);
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LAT);
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LON);
  telemetry.addSensor(0x14);  // distance
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_STATUS);  // satellite status
  telemetry.addSensor(0x13);                       // speed m/s
  telemetry.addSensor(0x7e);                       // Speed in km/hr
  telemetry.addSensor(0x7e);           // GPS Full
  telemetry.addSensor(IBUS_MEAS_TYPE_CMP_HEAD); // Heading
  telemetry.addSensor(IBUS_MEAS_TYPE_COG);// course over ground
  telemetry.addSensor(IBUS_MEAS_TYPE_ACC_X); // Acc X
  telemetry.addSensor(IBUS_MEAS_TYPE_ACC_Y); // Acc Y
  telemetry.addSensor(IBUS_MEAS_TYPE_ACC_Z); // Acc Z
  telemetry.addSensor(0x85); // Address 0x84 for displaying current point index
  telemetry.addSensor(0x07); // Address 0x84 for displaying current point index

  calibrateCompass();
}

void loop() {
  updateValues();
  telemetry.run();
}

bool inited = false;
int readNo = 0;
double latitude = 0, longitude = 0, destLat = 0, destLong = 0;
double X = 0, Y = 0, Z = 0;
bool isSatelliteFound = false;

void compassHeadingToDestination(double currentLat, double currentLon, double destLat, double destLon) {
  double dLon = destLon - currentLon;
  double y = sin(dLon) * cos(destLat);
  double x = cos(currentLat) * sin(destLat) - sin(currentLat) * cos(destLat) * cos(dLon);
  double brng = atan2(y, x);
  brng = fmod((brng + 2 * M_PI), (2 * M_PI)); // Asigurați-vă că unghiul este întotdeauna pozitiv
  brng = brng * 180 / M_PI;

  double headingToDest = brng;
  if (headingToDest < 0) {
    headingToDest += 360;
  }

  telemetry.setSensorValue(9, headingToDest);
}

int readDistance() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2); 
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(20); 
  digitalWrite(TRIG, LOW);
  float distance = pulseIn(ECHO, HIGH, 26000);
  distance = distance / 58;
  return distance;
}

// Funcție pentru salvarea locației GPS în EEPROM
void saveLocation(int index) {
  if (index < 0 || index >= maxSavedPoints) {
    Serial.println("Index invalid pentru salvare!");
    return;
  }
  int addr = index * sizeof(GPSPoint);
  EEPROM.put(addr, savedPoints[index]);
  Serial.print("Locație salvată la indexul: ");
  Serial.println(index);
}

// Funcție pentru salvarea punctului Home
void saveHomePoint() {
  homePoint.latitude = gps.location.lat();
  homePoint.longitude = gps.location.lng();
  EEPROM.put(0, homePoint);
  Serial.println("Punctul Home a fost salvat!");
}

// Funcție pentru recuperarea locației GPS din EEPROM
void updateValues() {
  uint32_t currMillis = millis();

  if (Serial.available()) {
    gps.encode(Serial.read());
  }
  if (!inited) {
    if (gps.location.isValid()) {
      startpoint_lat = gps.location.lat();
      startpoint_lon = gps.location.lng();
      inited = true;
      readNo = 0;
    }
  } else {
    TXdistanceRX = 3;
  }

  if (currMillis - prevMillis >= UPDATE_INTERVAL) {
    prevMillis = currMillis;
    sensors_event_t event;

    mag.getEvent(&event);

    X = event.magnetic.x - offsetX;
    Y = event.magnetic.y - offsetY;
    Z = event.magnetic.z - offsetZ;
    float heading = atan2(event.magnetic.y, event.magnetic.x);
    float declinationAngle = 0.22;
    heading += declinationAngle;

    if (heading < 0)
      heading += 2 * PI;
    if (heading > 2 * PI)
      heading -= 2 * PI;

    float headingDegrees = heading * 180 / M_PI;
    int headDir = (int)headingDegrees;

    if (headingDegrees < 0)
      headingDegrees += 360;

    float sonicDistance = readDistance();

    telemetry.setSensorValue(1, (i * 10));
    telemetry.setSensorValue(2, sonicDistance);
    telemetry.setSensorValue(3, ((latitude = gps.location.lat() ) * 10000000));
    telemetry.setSensorValue(4, ((longitude = gps.location.lng() ) * 10000000));

    if (gps.satellites.isValid()) {
      telemetry.setSensorValue(6, min(gps.satellites.value(), 25));
    } else {
      telemetry.setSensorValue(6, 0);
    }

    telemetry.setSensorValue(7, (gps.speed.value()));
    telemetry.setSensorValueFP(8, gps.speed.kmph());
    telemetry.setSensorValue(9, headDir);
    telemetry.setSensorValue(10, TinyGPSPlus::courseTo(latitude, longitude, destLat, destLong));
    telemetry.setSensorValue(11, X);
    telemetry.setSensorValue(12, Y);
    telemetry.setSensorValue(13, Z);
    telemetry.setSensorValue(0x85, currentPointIndex + 1);
    telemetry.setSensorValue(0x0f, currentPointIndex + 1);

    i += 0.1;
    if (i > 50)
      i = 0;

    if (gps.satellites.value() >= 4 && !isSatelliteFound) {
      isSatelliteFound = true;
      tone(buzzerPin, 1000, 1500);
      delay(1500);
      noTone(buzzerPin);
    }

    int swaVal = pulseIn(swaPin, HIGH, 30000);

    if (swaVal > 1500) {
      digitalWrite(ledPin, HIGH);
      saveLocation(currentPointIndex);
    } else {
      digitalWrite(ledPin, LOW);
    }

    int swbVal = pulseIn(swbPin, HIGH, 30000);

    if (swbVal > 1500) {
      digitalWrite(led2Pin, HIGH);
      getSavedLocation(currentPointIndex);
      compassHeadingToDestination(latitude, longitude, destLat, destLong);
      TXdistanceRX = TinyGPSPlus::distanceBetween(gps.location.lat(), gps.location.lng(), destLat, destLong);
      telemetry.setSensorValue(5, TXdistanceRX);
    } else {
      digitalWrite(led2Pin, LOW);
    }

    // Modificare pentru SWC și SWD pe pini digitali
    static unsigned long lastDebounceTimeSWC = 0;
    static unsigned long lastDebounceTimeSWD = 0;
    const unsigned long debounceDelay = 200; // Întârziere pentru debounce

    if (digitalRead(swcPin) == LOW && millis() - lastDebounceTimeSWC > debounceDelay) {
      currentPointIndex = (currentPointIndex + 1) % maxSavedPoints;
      lastDebounceTimeSWC = millis();
      Serial.print("Punct curent: ");
      Serial.println(currentPointIndex + 1);
    }

    if (digitalRead(swdPin) == LOW && millis() - lastDebounceTimeSWD > debounceDelay) {
      currentPointIndex = (currentPointIndex - 1 + maxSavedPoints) % maxSavedPoints;
      lastDebounceTimeSWD = millis();
      Serial.print("Punct curent: ");
      Serial.println(currentPointIndex + 1);
    }
  }
}

void getSavedLocation(int index) {
  if (index < 0 || index >= maxSavedPoints) {
    Serial.println("Index invalid pentru recuperare!");
    return;
  }
  int addr = index * sizeof(GPSPoint);
  EEPROM.get(addr, savedPoints[index]);
  destLat = savedPoints[index].latitude;
  destLong = savedPoints[index].longitude;
  Serial.print("Locație încărcată de la indexul: ");
  Serial.println(index);
  Serial.print("Latitudine: ");
  Serial.println(destLat, 6);
  Serial.print("Longitudine: ");
  Serial.println(destLong, 6);
}

void calibrateCompass() {
  Serial.println("Începeți calibrarea busolei. Vă rugăm să rotiți și să mișcați senzorul lent în toate direcțiile.");

  float minX = 1000, minY = 1000, minZ = 1000;
  float maxX = -1000, maxY = -1000, maxZ = -1000;

  for (int i = 0; i < 1000; i++) {
    sensors_event_t event;
    mag.getEvent(&event);

    minX = min(minX, event.magnetic.x);
    minY = min(minY, event.magnetic.y);
    minZ = min(minZ, event.magnetic.z);
    maxX = max(maxX, event.magnetic.x);
    maxY = max(maxY, event.magnetic.y);
    maxZ = max(maxZ, event.magnetic.z);
  }

  offsetX = (maxX + minX) / 2;
  offsetY = (maxY + minY) / 2;
  offsetZ = (maxZ + minZ) / 2;

  Serial.println("Calibrare completă.");
}