GPS ... distance calculation

Other Hardware Sensors
1

GPS ... distance calculation.

Hello!
I did the editing from the following link ...

I used Neo6M GPS.
Out of curiosity, I saved the location and without changing the GPS position, I started to calculate the distance between the saved position and the current position, it would be normal that if the GPS position is the same distance displayed to be the same, not to change. But this is changing, growing, as if GPS is changing its position.
Question ... Is such behavior normal?
If not ... what could be the cause?
Thank you in advance.
Sorry for any misunderstandings. Google Translate.

2

That depends on your code, which you have not posted

What if your code is printing the total distance travelled since starting ? Then, even if the GPS unit is not moving, as it will not report exactly the same current position each time then the total distance travelled will change

Please post you test code that shows the problem

3

Hello
The postition calculated is a little bit "noisy" due to the current postition of the used GPS satellites. Check the usage of a Kalmanfilter to reduce the noise.
Have a nice day and enjoy coding in C++.

4

Small changes in position, 3M or sometimes more, are normal behaviour for a GPS.

So what position or distance changes are you seeing ?

5 
/*  Приемник для машины с автопилотом
 *    канал RC FPV Aviator
*/


#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
RF24 radio(9, 10); // "создать" модуль на пинах 9 и 10 Для Уно
byte address[][6] = {"1Node", "2Node", "3Node", "4Node", "5Node", "6Node"}; //возможные номера труб


int recieved_data[4]; // массив принятых данных
double telemetry[4];   //массив для обратной передачи телеметрии

//СТРОКИ ДЛЯ КОМПАСА
#include "DFRobot_BNO055.h"
DFRobot_BNO055 mpu;
double compass_yaw; //курс
//double compass_roll; //крен
//double compass_pitch; //тангаж
//**************************************

//СТРОКИ ДЛЯ GPS МОДУЛЯ
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
static const int RXPin = 4, TXPin = 2;
static const uint32_t GPSBaud = 9600;
TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);
unsigned long last = 0UL;
//**************************************

//ПЕРЕМЕННЫЕ ДЛЯ GPS
double lalitude_moy;
double longitude_moy;
long lalitude_nach;
long lalitude_prod;
long longitude_nach;
long longitude_prod;
byte day_moy;
byte month_moy;
int year_moy;
byte hour_moy;
byte minute_moy;
byte second_moy;
float speed_kmph_moy;
float speed_mps_moy;
double course_moy;
long visota_moy;
long sputniki_moy;
float hdop_moy;
//****************************************

void setup() {
//  Serial.begin(9600); //открываем порт для связи с ПК

//Строки для компаса. Если компас не работает, то программа не продолжит работу.
   while (!mpu.init())
   {
     //Serial.println("ERROR! Unable to initialize the chip!");
     delay(50);
   }
//   mpu.setMode(mpu.eNORMAL_POWER_MODE, mpu.eFASTEST_MODE);
   delay(100);
   //Serial.println("Read euler angles...");
//******************

//Строки для радиомодуля
  radio.begin(); //активировать модуль
  //radio.setAutoAck(0);         //режим подтверждения приёма, 1 вкл 0 выкл
  //вставка для обратной связи
//  radio.setAutoAck(1);         // режим подтверждения приёма, 1 вкл 0 выкл
//  radio.setRetries(0, 15);     // (время между попыткой достучаться, число попыток)
  radio.enableAckPayload();    // разрешить отсылку данных в ответ на входящий сигнал
//  radio.setPayloadSize(32);    // размер пакета, байт
  //конец вставки для обратной связи
  radio.openReadingPipe(1, address[0]);     //хотим слушать трубу 0
  radio.setChannel(0x60);  //выбираем канал (в котором нет шумов!)
  radio.setPALevel (RF24_PA_HIGH); //уровень мощности передатчика. На выбор RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX
  radio.setDataRate (RF24_250KBPS); //скорость обмена. На выбор RF24_2MBPS, RF24_1MBPS, RF24_250KBPS
  //должна быть одинакова на приёмнике и передатчике!
  //при самой низкой скорости имеем самую высокую чувствительность и дальность!!
  radio.powerUp(); //начать работу
  radio.startListening();  //начинаем слушать эфир, мы приёмный модуль
//**********************

//СТРОКИ ДЛЯ GPS  
  ss.begin(GPSBaud);
//****************************************

pinMode(8, OUTPUT); //пин для сервопривода
  
}

void loop() {

//ПОДПРОГРАММА ЧТЕНИЯ ДАННЫХ С КОМПАСА
  mpu.readEuler();  ///* read euler angle */ чтение углов
    compass_yaw = mpu.EulerAngles.x; //определение курса
    //compass_roll = mpu.EulerAngles.y; //определение крена
    //compass_pitch = mpu.EulerAngles.z; //определение тангажа
//************************************
  
//ПОДПРОГРАММА ЧТЕНИЯ ДАННЫХ С GPS МОДУЛЯ И ВНЕСЕНИЯ ИХ В ПЕРЕМЕННЫЕ
  // Dispatch incoming characters
  while (ss.available() > 0)
    gps.encode(ss.read());
  if (gps.location.isUpdated())
  {
//lalitude_nach = gps.location.rawLat().deg;
//lalitude_prod = (gps.location.rawLat().billionths);// /1000;
//longitude_nach = gps.location.rawLng().deg;
//longitude_prod = (gps.location.rawLng().billionths);// /1000;
lalitude_moy = gps.location.lat();
longitude_moy = gps.location.lng();
  }
  else if (gps.date.isUpdated())
  {
day_moy = gps.date.day();
month_moy = gps.date.month();
year_moy = gps.date.year();
  }
  else if (gps.time.isUpdated())
  {
hour_moy = gps.time.hour();
minute_moy = gps.time.minute();
second_moy = gps.time.second();
  }
  else if (gps.speed.isUpdated())
  {
 speed_kmph_moy = gps.speed.kmph();
 speed_mps_moy = gps.speed.mps();
  }
  else if (gps.course.isUpdated())
  {
course_moy = gps.course.deg();//КУРС
  }
  else if (gps.altitude.isUpdated())
  {
visota_moy = gps.altitude.meters();//ВЫСОТА
  }
  else if (gps.satellites.isUpdated())
  {
sputniki_moy = gps.satellites.value();
  }
  else if (gps.hdop.isUpdated())
  {
hdop_moy = gps.hdop.hdop();
  }
//**************************************************************

//СОЗДАНИЕ МАССИВА С ДАННЫМИ GPS  
telemetry[0] = lalitude_moy;
telemetry[1] = longitude_moy;
//lalitude_nach; //
//lalitude_prod; //
//longitude_nach;; //
//longitude_prod; //
//byte day_moy;
//byte month_moy;
//int year_moy;
//byte hour_moy;
//byte minute_moy;
//byte second_moy;
//speed_kmph_moy;
//float speed_mps_moy;
//telemetry[3] = course_moy; //КУРС С ЖПС МОДУЛЯ
telemetry[3] = compass_yaw; //КУРС С КОМПАСА
//visota_moy;
telemetry[2] = sputniki_moy;
//float hdop_moy;
//****************************************************

//Строки для радиомодуля
  byte pipeNo;
//  while ( radio.available(&pipeNo)) {  // слушаем эфир со всех труб
//    radio.read( &recieved_data, sizeof(recieved_data) );         // чиатем входящий сигнал
  //вставка для приема с обратной связью
  while (radio.available(&pipeNo)) {                                 // слушаем эфир
    radio.read( &recieved_data, sizeof(recieved_data));              // чиатем входящий сигнал
    // формируем пакет данных телеметрии (напряжение АКБ, скорость, температура...)
//    telemetry[0] = 20; //telemetr_d1 = telemetr_d1 + 1;
//    telemetry[1] = 21; //telemetr_d2 = telemetr_d2 - 1;

//    radio.powerUp(); // включить передатчик
    
    radio.writeAckPayload(pipeNo, &telemetry, sizeof(telemetry));    // отправляем пакет телеметрии
    
//    radio.powerDown(); // выключить передатчик
  //конец вставки приема с обратной связью
byte motor_l = recieved_data[0];
byte motor_r = recieved_data[1];
byte motor_z = recieved_data[3];

if ( motor_l >0 || motor_r >0 ){//если передние моторы больше ноля, то задний ход выключается
  motor_z = 0;    
}
if (motor_z > 0){//если задний ход больше ноля, то передний ход выключается 
  motor_l = 0;
  motor_r = 0;
}
      analogWrite(5,motor_l);
      analogWrite(6,motor_r);
      digitalWrite(8,recieved_data[2]); //сигнал на сервопривод
      analogWrite(3,motor_z); //задний ход      
    
    //myservo2.writeMicroseconds(recieved_data[1]); // повернуть серво на угол 0..180
    // значение получено с 1 элемента массива
    //myservo3.writeMicroseconds(recieved_data[2]); // повернуть серво на угол 0..180
    // значение получено с 1 элемента массива
  }

//Serial.println(course_moy);
  
//**********************

//Строки для файлсейва
    unsigned long started_waiting_at = millis();
while ( ! radio.available() ){                             // FILESAVE
      if (millis() - started_waiting_at > 700 && millis() - started_waiting_at < 5000){   //если связь прерывается от 800мс до 5000мс происходит остановка мтора        
      analogWrite(5,0);
      analogWrite(6,0);         
      analogWrite(3,0);
//         myservo3.writeMicroseconds(800);
//         myservo3.detach();
      }
//      if (millis() - started_waiting_at > 5000){  //через 5 секунд пин отключается         
//         myservo3.detach();
//      }
//Serial.print(sputniki_moy); Serial.print("     "); Serial.print(recieved_data[0]); Serial.print("     "); Serial.print(recieved_data[1]); Serial.print("     КУРС КОМПАС"); Serial.println(compass_yaw);         
}
//*********************
}
6

Thanks for the answers ... Sorry for the code ... that's it.

7

The distance changes ..from 0.5m ..to 50-70m meters.

8

Thanks for the hints.

9

That can be a symptom of a GPS of a very poor or faulty antenna.

10

I didn't mention it.
The installation is on the balcony and receives 8-10 satellites,
can this be the cause?

11

Now I also found the TX part where the calculation is done ...

//Для расчета курса и направления
  #include <TinyGPS++.h>
//*******************************

//Variabile EEPROM
#include <EEPROM.h>
        double read_Latitude_EEPROM  //переменная для чтения из памяти общая для расчетов пути
                                              
        double read_Longitude_EEPROM  //переменная для чтения из памяти общая для расчетов пути     
        double read_Latitude_EEPROM_p1; //переменная для чтения из памяти
                                
        double read_Longitude_EEPROM_p1; //переменная для чтения из памяти
        double read_Latitude_EEPROM_p2; //переменная для чтения из памяти
        double read_Longitude_EEPROM_p2; //переменная для чтения из памяти       
        double read_Latitude_EEPROM_p3; //переменная для чтения из памяти     добавил для увеличения количества точек сохранения 
                       
        double read_Longitude_EEPROM_p3; //переменная для чтения из памяти     добавил для увеличения количества точек сохранения 
        double read_Latitude_EEPROM_p4; //переменная для чтения из памяти     добавил для увеличения количества точек сохранения 
        double read_Longitude_EEPROM_p4; //переменная для чтения из памяти     добавил для увеличения количества точек сохранения 




         
//**************************

//Для вывода значений на экран
// include the library code:
#include <LiquidCrystal.h>
// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 8, en = 7, d4 = 6, d5 = 5, d6 = 3, d7 = 4;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
//***********************

//Для радиомодуля
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
RF24 radio(9, 10); // "создать" модуль на пинах 9 и 10 Для Уно
byte address[][6] = {"1Node", "2Node", "3Node", "4Node", "5Node", "6Node"}; //возможные номера труб
                                                                 
//****************

byte pin_potent1 = 2; // потенциометр на 0 аналоговом - 2 канал пульта
byte pin_potent2 = 3; // потенциометр на 1 аналоговом - 1 канал пульта

int transmit_data[4]; // массив, хранящий передаваемые данные
                 
double telemetry[4];  // массив принятых от приёмника данных телеметрии
                  
byte rssi;           
int trnsmtd_pack = 1, failed_pack;
unsigned long RSSI_timer;

//VARIABILE PENTRU GPS
//long lalitude_nach;
double lalitude_prod;
//long longitude_nach;
double longitude_prod;
//byte day_moy;
//byte month_moy;
//int year_moy;
//byte hour_moy;
//byte minute_moy;
//byte second_moy;
//long speed_kmph_moy;
//float speed_mps_moy;
double course_moy;
//long visota_moy;
double sputniki_moy;
//float hdop_moy;
//****************************************

//для рассчета направления движения
float koef_asimut; 
//*********************************

//Для автопилота
byte autopilot_on = 0; // начальное положение автопилота выключенное
byte avtotormoz_l = 0; //автотормоз при автопилоте для левого мотора
byte avtotormoz_r = 0; //автотормоз при автопилоте для правого мотора
byte avtotormoz = 50; // ИЗМЕНЯЕМОЕ ЗНАЧЕНИЕ АВТОТОРМОЗА ПРИ АВТОПИЛОТЕ
//**************

//переменные для моторов с разнотягом
int motor_l; //переменная шим левого мотора
int motor_r; //переменная шим правого мотора
int motor_z; //переменная шим заднего хода обоих моторов
//********************************

  byte punkt_menu = 1; //выбор экрана меню

  int sbros = 0; //начальное положение кнопки сброса
                 //RESET
void setup() {
  
//Для вывода значений на экран
  lcd.begin(16, 2);


//****************************

  
//  Serial.begin(9600); //открываем порт для связи с ПК

//Строки для радиомодуля
  radio.begin(); //активировать модуль
//  radio.setAutoAck(0);         //режим подтверждения приёма, 1 вкл 0 выкл закомментировано для вставки обратной связи
  //вставка для обратной связи
  radio.setAutoAck(1);        // режим подтверждения приёма, 1 вкл 0 выкл---------------------------------------------
//  radio.setRetries(0, 15);    // (время между попыткой достучаться, число попыток)------------------------------------
  radio.enableAckPayload();   // разрешить отсылку данных в ответ на входящий сигнал----------------------------------
//  radio.setPayloadSize(32);   // размер пакета, в байтах--------------------------------------------------------------  
  //конец вставки для обратной связи  
  radio.openWritingPipe(address[0]);   //мы - труба 0, открываем канал для передачи данных
  radio.setChannel(0x60);  //выбираем канал (в котором нет шумов!)
  radio.setPALevel (RF24_PA_HIGH); //уровень мощности передатчика. На выбор RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX
  radio.setDataRate (RF24_250KBPS); //скорость обмена. На выбор RF24_2MBPS, RF24_1MBPS, RF24_250KBPS
  //должна быть одинакова на приёмнике и передатчике!
  //при самой низкой скорости имеем самую высокую чувствительность и дальность!!
  radio.powerUp(); //начать работу
  radio.stopListening();  //не слушаем радиоэфир, мы передатчик
//***********************
  
}

void loop() {

if (autopilot_on == 0) {; // если автопилот выключен, то автотормоза обнуляются
byte avtotormoz_l = 0; //автотормоз при автопилоте для левого мотора
byte avtotormoz_r = 0; //автотормоз при автопилоте для правого мотора
}

int in_ch1 = analogRead(pin_potent1); //чтение аналоговых пинов
int in_ch2 = analogRead(pin_potent2); //чтение аналоговых пинов

//Подпрограмма "Если джойстик газа вверх, то движение вперед" 
if (in_ch1 <= 504) {

in_ch1 = constrain(in_ch1,0,504); //ограничение канала газа от середины до верха
in_ch1 = map(in_ch1, 0, 504, 255-0, 0); //масштабирование канала газа в шим формат. МИНУСУЕТСЯ ПРОЦЕНТ ОБЩЕГО ХОДА. расходы газа есть еще в строках ограничения отдельных моторов (ниже).

int in_ch2_l = constrain(in_ch2,508,1023); //делаю ограничения по сторонам
int in_ch2_r = constrain(in_ch2,0,506); 

in_ch2_l = (map(in_ch2_l, 507, 1023, 0, 255))/3; //масштабирую стороны от 0 до 255. ПОСЛЕ ДРОБИ РАСХОДЫ РУЛЕНИЯ
in_ch2_r = (map(in_ch2_r, 506, 0, 0, 255))/3; //масштабирую стороны от 0 до 255. ПОСЛЕ ДРОБИ РАСХОДЫ РУЛЕНИЯ

//формулы для моторов с разнотягом
motor_l = in_ch1 - in_ch2_l + in_ch2_r - 15; // -15 ЭТО ОБЩИЙ ТРИММЕР ХОДА ПРЯМО
motor_r = in_ch1 - in_ch2_r + in_ch2_l;
//********************************

motor_l = constrain(motor_l,0,255-avtotormoz_l-0); //ограничиваю цифры моторов. МИНУСУЕТСЯ ПРОЦЕНТ ОБЩЕГО ХОДА И АВТОТОРМОЗ АВТОПИЛОТА. расходы газа есть еще в масштабировании канала газа (выше). 
motor_r = constrain(motor_r,0,255-avtotormoz_r-0); //ограничиваю цифры моторов. МИНУСУЕТСЯ ПРОЦЕНТ ОБЩЕГО ХОДА И АВТОТОРМОЗ АВТОПИЛОТА. расходы газа есть еще в масштабировании канала газа (выше).
motor_z = 0; //канал задней скорости отключен
} 
//Конец подпрограммы "Если джойстик газа вверх, то движение вперед"

//Подпрограмма "Если джойстик газа вниз, то движение назад" 
if (in_ch1 >= 510) {
in_ch1 = constrain(in_ch1,510,1023); //ограничение канала газа от середины до низа
motor_z = map(in_ch1, 510, 1023, 0, 255-0); //масштабирование канала газа заднего хода в шим формат. МИНУСУЕТСЯ РАСХОД ОБЩЕГО ХОДА.
motor_l = 0; //передний левый мотор стоит
motor_r = 0; //передний правый мотор стоит
}
//Конец подпрограммы "Если джойстик газа вниз, то движение назад"

//Создание массива для передачи
transmit_data[0] = motor_l; //канал передней скорости левого мотора
transmit_data[1] = motor_r; //канал передней скорости правого мотора
transmit_data[2] = sbros; // канал сброса груза
transmit_data[3] = motor_z; //канал задней скорости на оба мотора

//Строки для радиомодуля
    radio.powerUp(); // включить передатчик
  //radio.write(&transmit_data, sizeof(transmit_data)); // отправить по радио

  //вставка отправки с обратной связью и RSSI
  if (radio.write(&transmit_data, sizeof(transmit_data))) {    // отправка пакета transmit_data
    trnsmtd_pack++;
    if (!radio.available()) {                                  // если получаем пустой ответ
    } else {
      while (radio.available() ) {                    // если в ответе что-то есть
        radio.read(&telemetry, sizeof(telemetry));    // читаем
        // получили забитый данными массив telemetry ответа от приёмника
      }
    }
  } else {
    failed_pack++;
  }
  if (millis() - RSSI_timer > 1000) {    // таймер RSSI
    // расчёт качества связи (0 - 100%) на основе числа ошибок и числа успешных передач
    rssi = (1 - ((float)failed_pack / trnsmtd_pack)) * 100;
    // сбросить значения
    failed_pack = 0;
    trnsmtd_pack = 0;
    RSSI_timer = millis();
  }
  //конец вставки передачи с обратной связью и RSSI
    radio.powerDown(); // выключить передатчик
//**********************

//Для расчета курса и направления
// Pentru a calcula cursul și direcția
      double distanceToLondon =
        TinyGPSPlus::distanceBetween(
          telemetry[0],
          telemetry[1],
          read_Latitude_EEPROM, 
          read_Longitude_EEPROM);
      double courseToLondon =
        TinyGPSPlus::courseTo(
          telemetry[0],
          telemetry[1],
          read_Latitude_EEPROM, 
          read_Longitude_EEPROM);
//      Serial.print(distanceToLondon, 2);
//      Serial.print(" m   Course-to=");
//      Serial.println(courseToLondon, 0);

//*******************************

//Для вывода значений на экран
// Pentru a afișa valorile pe ecran

//Выбор экрана меню
      if (analogRead(1)>1000 && analogRead(0)<1000) {
        punkt_menu = punkt_menu+1;
        if (punkt_menu >= 13) {
          punkt_menu = 1;
       }
      }
        lcd.clear(); 
        lcd.setCursor(0, 0); lcd.print(punkt_menu);
        lcd.setCursor(0, 1); lcd.print(rssi);        

//Экран №1
if (punkt_menu == 1){
  lcd.setCursor(2, 0); lcd.print("RC FPV Aviator");
  lcd.setCursor(3, 1); lcd.print("  Autopilot");  
}
//Экран №0
if (punkt_menu == 0){
//  lcd.setCursor(2, 0); lcd.print("*");  

//  lcd.setCursor(3, 0); lcd.print(read_Latitude_EEPROM, 6); lcd.print(" "); lcd.print(read_Longitude_EEPROM, 6);

  //верхняя строка с координатами
  lcd.setCursor(7, 0); lcd.print(read_Longitude_EEPROM, 6);
  lcd.setCursor(0, 0); lcd.print(read_Latitude_EEPROM, 6); lcd.print(" "); 
  lcd.setCursor(0, 0); lcd.print(punkt_menu);     
  lcd.setCursor(1, 0); lcd.print(" *");   
  //----------------------------- 
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(telemetry[1], 6);
  lcd.setCursor(0, 1);  lcd.print(telemetry[0], 6); lcd.print(" "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print(" ");
  //-----------------------------
}
//Экран №2
if (punkt_menu == 2){

        if (analogRead(1)>1000 && analogRead(0)>1000){ //если нажимаются клавиши правая и левая, то значение включения автопилота меняется
        autopilot_on = !autopilot_on;
        }
        //Подпрограмма расчета направления движения
        course_moy = telemetry[3]; //присваиваются переменной действующего курса данные с телеметрии
        koef_asimut = (course_moy - courseToLondon)/180; //дельта курсов делится на 180. если больше 1-направо, если меньше 1-налево
        if (koef_asimut > 1.005){
            lcd.setCursor(4, 0); lcd.print(">>>>>>>"); lcd.setCursor(13, 0); lcd.print(course_moy,0); //показывает действующий курс
                if (autopilot_on == 1){ //Если автопилот включен, то
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = avtotormoz; //то тормозит правый мотор
                    lcd.setCursor(4, 0); lcd.print(">>AUTO>>");
                }else{
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит                  
                }
        }
        if (koef_asimut < -0.005){
            lcd.setCursor(4, 0); lcd.print(">>>>>>>"); lcd.setCursor(13, 0); lcd.print(course_moy,0); //показывает действующий курс
                if (autopilot_on == 1){ //Если автопилот включен, то
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = avtotormoz; //то тормозит правый мотор
                    lcd.setCursor(4, 0); lcd.print(">>AUTO>>");
                }else{
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит                  
                }
        }        
        if (koef_asimut > 0.005 && koef_asimut < 0.994){
            lcd.setCursor(4, 0); lcd.print("<<<<<<<"); lcd.setCursor(13, 0); lcd.print(course_moy,0); //показывает действующий курс
                if (autopilot_on == 1){ //Если автопилот включен, то
                    avtotormoz_l = avtotormoz+10; //то тормозит левый мотор // +10 ЭТО УСКОРЕНИЕ РУЛЕНИЯ В ЛЕВО ПРИ АВТОПИЛОТЕ. БЕЗ НЕГО ВЛЕВО РУЛИТ МЕДЛЕННЕЕ ЧЕМ В ПРАВО!!!!!!!!!
                    avtotormoz_r = 0; //правый мотор не тормозит
                    lcd.setCursor(4, 0); lcd.print("<<AUTO<<");
                }else{
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит                  
                }
        }
        if (koef_asimut >= 0.994 && koef_asimut <= 1.005){
            lcd.setCursor(4, 0); lcd.print("<<< >>>"); lcd.setCursor(13, 0); lcd.print(course_moy,0); //показывает действующий курс
                if (autopilot_on == 1){ //Если автопилот включен, то
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = avtotormoz; //то тормозит правый мотор
                    lcd.setCursor(4, 0); lcd.print(">>AUTO>>");
                }else{
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит                  
                }
        }
        if (koef_asimut >= -0.005 && koef_asimut <= 0.005){
            lcd.setCursor(4, 0); lcd.print(">>>o<<<"); lcd.setCursor(13, 0); lcd.print(course_moy,0); //показывает действующий курс
                if (autopilot_on == 1){ //Если автопилот включен, то
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит
                    lcd.setCursor(4, 0); lcd.print(">>AUTO<<");
                }else{
                    avtotormoz_l = 0; //левый мотор не тормозит
                    avtotormoz_r = 0; //правый мотор не тормозит                  
                }
        }
                
        //*****************************************
  //lcd.setCursor(4, 0); lcd.print("Course:"); lcd.setCursor(13, 0); lcd.print(telemetry[3],0); //показывает действующий курс
  lcd.setCursor(4, 1); lcd.print(distanceToLondon, 2); lcd.print("m" ); lcd.setCursor(12, 1); lcd.print(" "); lcd.print(courseToLondon, 0); //показывает расстояние и курс до нужной точки
}

//Экран №3
if (punkt_menu == 3){
  lcd.setCursor(4, 0); lcd.print("SBROS???"); //RESET
  lcd.setCursor(7, 1); lcd.print(sbros); 
    if (analogRead(1)>1000 && analogRead(0)>1000){
        sbros = 1;
      }else{
        sbros = 0;
      }
}

//Экран №4
if (punkt_menu == 4){
  lcd.setCursor(4, 0); lcd.print("Satellites:");
  lcd.setCursor(4, 1); lcd.print(telemetry[2],0); 
}
//Экран №5
if (punkt_menu == 5){
  lcd.setCursor(3, 0); lcd.print("Save point 1?");
  //нижняя строка с координатами
  // linia de jos cu coordonatele
  lcd.setCursor(7, 1);  lcd.print(telemetry[1], 6);
  lcd.setCursor(0, 1);  lcd.print(telemetry[0], 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------
  if (analogRead(1)>1000 && analogRead(0)>1000){
        EEPROM_double_write (0, telemetry[0]); //запись в память. обращение к функции
                                               // scrie în memorie. apel de funcție---adresa 0
        EEPROM_double_write (10, telemetry[1]); //запись в памяь. обращение к функции
                                               // scrie în memorie. apel de funcție---adresa 10
  }
}
//Экран №6
if (punkt_menu == 6){
  lcd.setCursor(3, 0); lcd.print("Read point 1?");
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(read_Longitude_EEPROM_p1, 6);
  lcd.setCursor(0, 1);  lcd.print(read_Latitude_EEPROM_p1, 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------
  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        read_Latitude_EEPROM_p1 = EEPROM_double_read(0); //чтение из памяти. обращение к функции
        read_Longitude_EEPROM_p1 = EEPROM_double_read(10); //чтение из памяти. обращение к функции
        read_Latitude_EEPROM = read_Latitude_EEPROM_p1;// для расчетов пути применяется именно это значение
        read_Longitude_EEPROM = read_Longitude_EEPROM_p1;// для расчетов пути применяется именно это значение
  }
}
//Экран №7
if (punkt_menu == 7){
//  lcd.setCursor(2, 1); lcd.print(" ");  
  lcd.setCursor(3, 0); lcd.print("Save point 2?");
//  lcd.setCursor(3, 1); lcd.print(telemetry[0]/1000); lcd.print(" "); lcd.print(telemetry[1]/1000);
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(telemetry[1], 6);
  lcd.setCursor(0, 1);  lcd.print(telemetry[0], 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        EEPROM_double_write (20, telemetry[0]); //запись в память. обращение к функции
        EEPROM_double_write (30, telemetry[1]); //запись в памяь. обращение к функции
  }
}
//Экран №8
if (punkt_menu == 8){
//  lcd.setCursor(2, 1); lcd.print(" ");  
  lcd.setCursor(3, 0); lcd.print("Read point 2?");
//  lcd.setCursor(3, 1); lcd.print(read_Latitude_EEPROM_p2/1000); lcd.print(" "); lcd.print(read_Longitude_EEPROM_p2/1000);
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(read_Longitude_EEPROM_p2, 6);
  lcd.setCursor(0, 1);  lcd.print(read_Latitude_EEPROM_p2, 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        read_Latitude_EEPROM_p2 = EEPROM_double_read(20); //чтение из памяти. обращение к функции
        read_Longitude_EEPROM_p2 = EEPROM_double_read(30); //чтение из памяти. обращение к функции
        read_Latitude_EEPROM = read_Latitude_EEPROM_p2;// для расчетов пути применяется именно это значение
        read_Longitude_EEPROM = read_Longitude_EEPROM_p2;// для расчетов пути применяется именно это значение     
  }
}
//Далее для увеличения количества сохраняемых точек с 2-х до 4-х добавляю еще 4 экрана меню, аналогичные для первых двух точек, но изменяю параметры памяти и названия переменных


//Экран №9
if (punkt_menu == 9){
  lcd.setCursor(3, 0); lcd.print("Save point 3?");
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(telemetry[1], 6);
  lcd.setCursor(0, 1);  lcd.print(telemetry[0], 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------
  if (analogRead(1)>1000 && analogRead(0)>1000){
        EEPROM_double_write (40, telemetry[0]); //запись в память. обращение к функции
        EEPROM_double_write (50, telemetry[1]); //запись в памяь. обращение к функции
  }
}
//Экран №10
if (punkt_menu == 10){
  lcd.setCursor(3, 0); lcd.print("Read point 3?");
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(read_Longitude_EEPROM_p3, 6);
  lcd.setCursor(0, 1);  lcd.print(read_Latitude_EEPROM_p3, 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------
  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        read_Latitude_EEPROM_p3 = EEPROM_double_read(40); //чтение из памяти. обращение к функции
        read_Longitude_EEPROM_p3 = EEPROM_double_read(50); //чтение из памяти. обращение к функции
        read_Latitude_EEPROM = read_Latitude_EEPROM_p3;// для расчетов пути применяется именно это значение
        read_Longitude_EEPROM = read_Longitude_EEPROM_p3;// для расчетов пути применяется именно это значение
  }
}
//Экран №11
if (punkt_menu == 11){
//  lcd.setCursor(2, 1); lcd.print(" ");  
  lcd.setCursor(3, 0); lcd.print("Save point 4?");
//  lcd.setCursor(3, 1); lcd.print(telemetry[0]/1000); lcd.print(" "); lcd.print(telemetry[1]/1000);
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(telemetry[1], 6);
  lcd.setCursor(0, 1);  lcd.print(telemetry[0], 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        EEPROM_double_write (60, telemetry[0]); //запись в память. обращение к функции
        EEPROM_double_write (70, telemetry[1]); //запись в памяь. обращение к функции
  }
}
//Экран №12
if (punkt_menu == 12){
//  lcd.setCursor(2, 1); lcd.print(" ");  
  lcd.setCursor(3, 0); lcd.print("Read point 4?");
//  lcd.setCursor(3, 1); lcd.print(read_Latitude_EEPROM_p2/1000); lcd.print(" "); lcd.print(read_Longitude_EEPROM_p2/1000);
  //нижняя строка с координатами
  lcd.setCursor(7, 1);  lcd.print(read_Longitude_EEPROM_p4, 6);
  lcd.setCursor(0, 1);  lcd.print(read_Latitude_EEPROM_p4, 6); lcd.print("    "); 
  lcd.setCursor(0, 1); lcd.print(rssi);     
  lcd.setCursor(2, 1); lcd.print("    ");    
  //----------------------------  
  if (analogRead(1)>1000 && analogRead(0)>1000){
        read_Latitude_EEPROM_p4 = EEPROM_double_read(60); //чтение из памяти. обращение к функции
        read_Longitude_EEPROM_p4 = EEPROM_double_read(70); //чтение из памяти. обращение к функции
        read_Latitude_EEPROM = read_Latitude_EEPROM_p4;// для расчетов пути применяется именно это значение
        read_Longitude_EEPROM = read_Longitude_EEPROM_p4;// для расчетов пути применяется именно это значение     
  }
}

 delay (100);
//****************************    
    
//    Serial.print(in_ch1); Serial.print("    "); Serial.print(in_ch2); Serial.print("    l_"); Serial.print(in_ch2_l); Serial.print("    r_"); Serial.print(in_ch2_r); 
//    Serial.print("        M_l_"); Serial.print(motor_l); Serial.print("    M_r_"); Serial.print(motor_r); 
//    Serial.print("          "); Serial.print(analogRead(pin_potent1)); Serial.print("   "); Serial.println(analogRead(pin_potent2)); 
//    Serial.print("          "); Serial.print(telemetry[0]); Serial.print("   "); Serial.print(telemetry[1]); Serial.print("   "); Serial.println(rssi); 

} //КОНЕЦ LOOP ***********************


// Функция записи в EEPROM
// Scrieți funcția în EEPROM
void EEPROM_double_write(int addr, double num) {
  byte raw[4];
  (double&)raw = num;
  for(byte i = 0; i < 4; i++) EEPROM.write(addr+i, raw[i]);
}
//*************************

// Функция чтения из EEPROM
// Citiți funcția din EEPROM
double EEPROM_double_read(int addr) {   
  byte raw[4];
  for(byte i = 0; i < 4; i++) raw[i] = EEPROM.read(addr+i);
  double &num = (double&)raw;
  return num;
}
//**************************
12

GPSs receive very weak signals, so if there is a relativly powerful radio transmitter nearby, that can cause problems also.

14

OK ... I have to try ... spring, in the open field

15

Hello ! I'll be back for some clarification.
DFRobot_BNO055 is also used in the above code
which has a magnetometer included.
My question is: how many axes should be used and if the X axis is not wrong in the code (compass_yaw = mpu.EulerAngles.x;), is there no need for the Z axis? ...
Thanks .
Sorry .... Google Translate ..

16

Hi,
Are you calculating distance on using the controller, or using the GPS to do it for you?

Thanks.. Tom... :grinning: :+1: :coffee: :australia:

17

is not wrong in the code (compass_yaw = mpu.EulerAngles.x;), is there no need for the Z axis? ...
Thanks .

Thanks to J Remington, for the directions given in another similar post ...
3d compass tilt correction

18

Hello!
Regarding the Tx code, I come up with a question ...
How do I set a pin to output the value - koef_asimut - which I can read with another arduino to control a servo.
This is because I have tried and seen many problems with the influence of GPS -Servo.
I use Mega 2560
Thank you in advance.
Forgive google translation.