Как подключить к Arduino эхолот Humminbird?

можно.

 

Если ваш эхолот выдает RS-232, то данная схема уже правильно проинвертирует сигнал (там +12 это 0, а -12 это 1). А если на выходе эхолота TTL-USART, то его наверняка можно подавать прямо на ногу ардуины (лучше через резистор в 1 килоом). Кракозябры на экране - несоответствие скорости (четности).

 

С humminbird matrix 47 3d сигнал 5v надо инвертировать. 100% будут кракозябры! Проверено на практике!

 

Добрый день, уважаемое сообщество форума!
Хочу поделиться с Вами некоторыми итогами по работе над автопилотом по глубине и компасу для троллинга на катере.
Как я уже упоминал ранее в постах, я приобрел «чудо Советского машиностроения» - Ветерок 8.
https://ibb.co/hB1T7XZ
https://ibb.co/FKVC3LS
Мотор живучий, но не имеет заднего хода, а только включается редуктор на ход вперед и отключается специальным рычажком с регулируемым усилием, что хорошо для установки на переключение редуктора китайским 20 кг серводвигателем.
https://ibb.co/MBtBtPH
Кстати, о надежности китайских «прибамбасов», у первой сервы от вибрации и плохой пропайки отвалился проводок и замкнул на металлический корпус оной. Результат – вся электроника сервы выгорела, издавая резкий химический запах. Пришлось заменить, проверив качество пайки и изолировать корпус от Ветерка.
https://ibb.co/88pjSbD
Установил: электростартер, заменив венец на маховике;
https://ibb.co/x3hCWck
https://ibb.co/Wc5tqN7
кнопки СТАРТ и СТОП на двигатель и на пульте;
на управление оборотами такая же 20 кг китайская серва с управлением с пульта;
на обмотку подсветки повесил диодный мост и стабилизатор напряжения на LT 1084 для подзарядки АКБ или хотя бы частичной подпитки всей электросистемы, хотя в бортовой сети и использую АКБ 74Ah;
https://ibb.co/Wxr5br3
заменил родные катушки зажигания на Хонда Дио, что освободило место для установки серводвигателя для переключения редуктора.
https://ibb.co/j51QDZ5
https://ibb.co/HPZCLGR
https://ibb.co/71XFqV7
Изготовил мотр-редуктор для управления докаткой (Ветерок-8) из редуктора старой (одна из ошибок) дрели и «дворника» от Нивы с конечными выключателями, спрятав все это в «скворечник».
https://ibb.co/fkx80mZ
От конечников при их срабатывании сигнал поступает на соответствующие каждому конечнику светодиоды на пульте управления.
https://ibb.co/MsSHhvN
https://ibb.co/jTG7gLb
https://ibb.co/KsbxQRJ
https://ibb.co/JHLcPTr
Из подручных материалов изготовил пульт управления, использовав П-образный профиль для гипсокартона и оцинкованный лист. Для того, чтобы во время пуска электорстартера, посадка напряжения не влияла на работу эхолота и пульта управления, запитал их от бортового АКБ через цепочку диод 10А- лампу Н1 55вт- С=1фарада, что сохраняет работоспособность электроники в течении 10-15 сек.
https://ibb.co/JpCb6pM
https://ibb.co/sjbzRQ0
https://ibb.co/w7dBXny
https://ibb.co/0yrJ1GH
https://ibb.co/Tw740yf
https://ibb.co/t40nMcw
Система для тех целей, которые были первично описаны выше, пригодна, работает, но не надо думать, что это действительно автопилот, это скорее помощник рыбака в удержании глубины и курса. Если сравнивать работу автопилота по глубине и по компасу, то на компасе "автопилотирование" производится более плавно из-за более чувствительной реакции исполнительного механизма вслед за отклонением от курса, т.е. чем больше отклонение, тем на больший угол поворачивается исполнительный механизм. По глубине же в окончании скетча были заложены только предельные отклонения механизма, что «не есть хорошо!». Пришлось повозиться с настройкой угла поворота двигателя. Надо было изменять алгоритм работы конечного каскада, вводя промежуточные положения для более плавной работы всей системы. Докатка навешена на транец справой стороны катера, что тоже влияет на управление - налево поворачивал гораздо резвее, чем направо, поэтому задаются соответственно разные углы поворота мотор-редуктору. 80% всего затраченного времени ушло на механическую часть, слесарно-сварочные работы и отладку конечного каскада, с начала в гараже, затем на воде . Были допущены некоторые ошибки, которые пришлось исправлять в ходе испытаний. В частности увеличив длину рычага на мотор-редукторе и максимально уменьшив длину рычага румпеля получил хороший угол поворота докатки, но и большой люфт всей рычажной системы. Вроде бы не большой люфт в мотор-редукторе из-за старых шестеренок в результате оказался губительным для всего автопилота. Пришлось переделывать, но до конца не смог избавиться от люфта. Нужен актуатор! И ещё, мала скорость поворота от крайнего левого положения до крайнего правого. Может кто скинет ссылочку на актуатор с ходом 30-40см и временем выдвижения 3-4сек., если вообще есть такие. При использовании этого автопилота надо знать дно в районе рыбалки. При пересечении русла впадающей речки, автопилот теряется и может уйти не туда, куда нужно. В этом случае переключаюсь на автопилот по компасу. Он собран на Nano. Об этом я тоже ранее писал, но при качке компас работает не стабильно. Надо его с MPU6050 соединить. Если кто-то может помочь, буду очень признателен.

Код (C++):

/*Пример скетча для управления  катером через компас
   подробнее -  https://lihoy-lj.livejournal.com/831.html


*/
//только с серво, без всего лишнего

#include <Servo.h>      
#include <Wire.h>            // библиотека I2C (стандартная)
#include <MechaQMC5883.h>    //  библиотека компаса https://yadi.sk/d/RF0VF6tA3Xvfjb  

#define    B 10

byte angle = 97;              // устанавливаем начальный угол сервы, при котором перо руля будет параллельно движению
int vector = 0;             // устанавливаем начальный курс для старта, если поставить 0 платформа не тронется до нажатия кнопки D2
MechaQMC5883 qmc;             // The CompassLib object
Servo servo1;
/* блок функций управления моторами*/
void motor_off()             //---Выключить моторы
{ }

void setup() {
  Serial.begin(4800);
  Wire.begin();

  pinMode(B, OUTPUT);
  pinMode(12, INPUT);     // назначаем пин кнопки D12(было D2)
 
  pinMode(13, OUTPUT);   // подключаем встроенный светодиод
  servo1.attach(B);      // назначаем пин D10 сервоприводу
  qmc.init(); // инициируем компас
  qmc.setMode(Mode_Continuous, ODR_10Hz, RNG_8G, OSR_512); // устанавливаем параметры работы компаса
}

void loop() {

  /* блок работы компаса*/
  int x, y, z;
  qmc.read(&x, &y, &z);
  float heading = atan2(y, x);
  float declinationAngle = 6.7;// значение магнитного склонения, искать в справочнике для своей местности
  heading += declinationAngle;
  if (heading < 0)
    heading += 2 * PI;
  if (heading > 2 * PI)
    heading -= 2 * PI;
  int azimuth = heading * 180 / M_PI;

  /* блок работы кнопки: при нажатии запоминает текущий курс*/
  if (vector == 0) {       // ждем нажатия кнопки (задаем курс)
    motor_off();
  }
  if (digitalRead(12) == HIGH) { // было (2)
    vector = azimuth;
    digitalWrite(13, HIGH);
  }
  else {
    digitalWrite(13, LOW);
  }
  /*создание переменной для управления моторами */
  int g = azimuth - vector; // измеряем величину и направление отклонения от курса
  while (g >= 180)
    g -= 360;
  while (g < -180)
    g += 360;
  int s = fabs (g) * 2 ;         // переменная угла поворота сервы  от G
  s = constrain(s, 1, 30); // ограничиваем значения S диапазоном поворота руля (49* определен экспериментально)

  /* блок управления моторами*/

  if (g == 0) {
    servo1.write(angle);         //---Полный вперед
  }
  if (g < 0) {
    servo1.write(angle + s);    //---Установка влево
  }
  if (g > 0) {
    servo1.write(angle - s);    //---Установка вправо
  }

  // блок диагностики работы программы

  Serial.print("AZ: ");
  Serial.print(azimuth);
  Serial.print("   ");

  Serial.print("vect: ");
  Serial.print(vector);
  Serial.print("    ");

  Serial.print("G: ");
  Serial.print(g);
  Serial.print("    ");

  Serial.print("S: ");
  Serial.print(s);
  Serial.print("   ");

  Serial.print("angle: ");
  Serial.print(servo1.read());
  Serial.print("   ");

  Serial.println();
  delay (800); // задержка цикла
}

Еще столкнулся с таким результатом испытаний, как зависимость амплитуды синусоиды при движении от ширины задаваемой глубины и она резко возрастает при разности заданных глубин более 1 метра. А если трансюдер стоит на корме, а не на носу катера, то вообще "кранты". На крайних испытаниях автопилота вышел из строя главный двигатель катера – пробило прокладку головки цилиндра, но даже это не испортило рыбалку. Весь день использовал докатку Ветерок как на троллинге, так и возвращение к месту спуска на воду. Работа по автопилоту продолжается…
Люблю рыбалку летом...
https://ibb.co/Gd7h1Jk
https://ibb.co/Bf2Ltft
https://ibb.co/qD4MztD

 

улов знатный))))

 

Рост 150 см, вес 32 кг. Раньше были и под 60 кг.

 

Это чё за конь? Я таких отродясь не видел.

 

Конь - это жерех, или "шелеспер", а здесь сом.

 

Добрый вечер, уважаемое сообщество форума!
Лето закончилось, хотя по хорошему у нас оно и не начиналось.
Можно продолжить работу над проектом.
Магнетометр QMC5883 заменил на HMC5883L и подцепил ADXL 345, но получилось как-то корявенько.

Код (C++):

// с компенсацией наклона компаса

#include <PWMServo.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <ADXL345.h>

HMC5883L compass;
ADXL345 accelerometer;

float heading1;
float heading2;

#define    B 10

byte angle = 89;              //опытным путем устанавливаем начальный угол сервы,
                              //при котором перо руля будет параллельно движению

int vector = 0;             // устанавливаем начальный курс для старта,
                            //ждем нажатия кнопки D12

PWMServo servo1;


void motor_off()             //
  {}


void setup()
{
  Serial.begin(9600);

  // Initialize ADXL345

  if (!accelerometer.begin())
  {
   // delay(500);
  }

  accelerometer.setRange(ADXL345_RANGE_2G);

  // Initialize HMC5883L
  Serial.println("Initialize HMC5883L");

  while (!compass.begin())
  {
   // Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
  // delay(500);
  }

  pinMode(B, OUTPUT);
  pinMode(12, INPUT);               // назначаем пин кнопки D12

  pinMode(13, OUTPUT);              // подключаем встроенный светодиод
  servo1.attach(SERVO_PIN_B);       // назначаем пин D10 сервоприводу


  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(0, 0);  
}



// No tilt compensation

float noTiltCompensate(Vector mag)
{
  float heading = atan2(mag.YAxis, mag.XAxis);
  return heading;
}
// Tilt compensation

float tiltCompensate(Vector mag, Vector normAccel)
{
  // Pitch & Roll

  float roll;
  float pitch;

  roll = asin(normAccel.YAxis);
  pitch = asin(-normAccel.XAxis);

  if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78)
  {
    return -1000;
  }



  // Some of these are used twice, so rather than computing
  //  them twice in the algorithem we precompute them before hand.

  float cosRoll = cos(roll);
  float sinRoll = sin(roll);
  float cosPitch = cos(pitch);
  float sinPitch = sin(pitch);

  // Tilt compensation

  float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
  float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

  float heading = atan2(Yh, Xh);

  return heading;
}



   // Correct angle

float correctAngle(float heading)
{
  if (heading < 0) { heading += 2 * PI; }
  if (heading > 2 * PI) { heading -= 2 * PI; }

  return heading;
}

void loop()
{
  // Read vectors

  Vector mag = compass.readNormalize();
  Vector acc = accelerometer.readScaled();

  // Calculate headings

  heading1 = noTiltCompensate(mag);
  heading2 = tiltCompensate(mag, acc);

  if (heading2 == -1000)
  {
    heading2 = heading1;
  }


  // Formula: (deg + (min / 60.0)) / (180 / M_PI);

  float declinationAngle = (6.7 + (26.0 / 60.0)) / (180 / M_PI);
  heading1 += declinationAngle;
  heading2 += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg

  heading1 = correctAngle(heading1);
  heading2 = correctAngle(heading2);

  // Convert to degrees-----------------------------------
  heading1 = heading1 * 180/M_PI;
  heading2 = heading2 * 180/M_PI;
 
  if (heading2 < 0)
  {
      heading2 += 2 * PI;
  }

  if (heading2 > 2 * PI)
  {
      heading2 -= 2 * PI;
  }

  // Convert to degrees


  int azimuth = heading2;         //

  //---------------------------------------------------------

  /* блок работы кнопки: при нажатии запоминает текущий курс*/
  if (vector == 0)                // ждем нажатия кнопки (задаем курс)
  {
      motor_off();
  }
  if (digitalRead(12) == HIGH)
  {
    vector = azimuth;             // Если кнопка была нажата
    digitalWrite(13, HIGH);
  }
  else
  {
    digitalWrite(13, LOW);
  }

  //создание переменной для управления моторами

  int g = azimuth - vector;       // измеряем величину и направление отклонения от курса
  while (g >= 180)
    g -= 360;
  while (g < -180)
    g += 360;
  int s = fabs (g) * 4 ;         // переменная угла поворота сервы  от G
  s = constrain(s, 1, 35);       // ограничиваем значения S диапазоном поворота руля (35* определен экспериментально)

  /* блок управления мотор-редуктором */

  if (g == 0) {
    servo1.write(angle);         //---Полный вперед
  }
  if (g < 0) {
    servo1.write(angle + s);    //---Установка влево
  }
  if (g > 0) {
    servo1.write(angle - s);    //---Установка вправо
  }

  //----------------------------------------------------
  // блок диагностики работы программы

///*
  Serial.print("h1: ");
  Serial.print(heading1);
  Serial.print("   ");


  Serial.print("h2: ");
  Serial.print(heading2);
  Serial.print("   ");
// */

  Serial.print("AZ: ");
  Serial.print(azimuth);
  Serial.print("   ");

  Serial.print("vect: ");
  Serial.print(vector);
  Serial.print("    ");

  Serial.print("G: ");
  Serial.print(g);
  Serial.print("    ");

  Serial.print("S: ");
  Serial.print(s);
  Serial.print("   ");

  Serial.print("angle: ");
  Serial.print(servo1.read());
  Serial.print("   ");

  Serial.println();

  delay(200);
}

Работает, но как-то не очень хорошо, при том, что работает, когда развернут на 180 гр. и compass.setOffset(0, 0).
Если вставить калибровочные данные компаса, то стабилизации вообще нет - работает хуже, чем без акселерометра. Тангаж в норме, крен налево стабилизирует терпимо, а вот крен направо - вообще никак. Что не так?

 

без правильной калибровки магнитометра и акселя работать не будет

 

Магнетометр откалиброван и отдельно от акселя показывает север при сравнении с "образцовым" навигатором ЭТРЕКС почти в ноль. ADXL345 тоже калибровал, но есть Вопрос: куда в этом коде вставлять поправочные коэффициенты ADXL345?
Sensor readings with offsets: 0 0 16390 0 0 0

Your offsets: -2461 -113 1262 194 45 -44

 

Каким образом получили калибровочные поправки? В коде вижу что для компаса поправки 0, для акселя вообще не вызывались. Откройте библиотеки, посмотрите на функции со словом offset - это и есть установка калибровок.

 

[QUOTESensor readings with offsets: 0 0 16390 0 0 0

Your offsets: -2461 -113 1262 194 45 -44][/QUOTE]
это калибровка ADXL345
<<Данные печатаются как: acelX acelY acelZ giroX giroY giroZ Убедитесь, что показания вашего датчика близки к 0 0 16384 0 0 0 . Если калибровка прошла успешно, запишите свои смещения, чтобы вы могли установить их в своих проектах>>
Для магнетометра compass.setOffset (-210, -236)
Как я уже писал выше, компас хорошо откалиброван, но куда подставлять калибровочные данные акселерометра
(-2461, -113, 1262) не соображу.

 

Разговор слепого с глухим. Еще раз спрошу - как проводилась калибровка? Откуда эти коэффициенты? По каким признакам определили, что все хорошо скалибровано? Говорит ли что-нибудь MagMaster ?
Но если есть полная уверенность в непогрешимости настройки магнитометра, то вот первая попавшаяся ссылка по поводу калибровки акселя https://howtomechatronics.com/tutor...ation-with-arduino-and-adxl345-accelerometer/

 

Это я читал... Там скетч не компилируется.

 

https://howtomechatronics.com/tutor...ation-with-arduino-and-adxl345-accelerometer/

Код (C++):

/*
    Arduino and ADXL345 Accelerometer - 3D Visualization Example
     by Dejan, https://howtomechatronics.com
*/
#include <Wire.h>  // Wire library - used for I2C communication
int ADXL345 = 0x53; // The ADXL345 sensor I2C address
float X_out, Y_out, Z_out;  // Outputs
float roll,pitch,rollF,pitchF=0;
void setup() {
  Serial.begin(9600); // Initiate serial communication for printing the results on the Serial monitor
  Wire.begin(); // Initiate the Wire library
  // Set ADXL345 in measuring mode
  Wire.beginTransmission(ADXL345); // Start communicating with the device
  Wire.write(0x2D); // Access/ talk to POWER_CTL Register - 0x2D
  // Enable measurement
  Wire.write(8); // Bit D3 High for measuring enable (8dec -> 0000 1000 binary)
  Wire.endTransmission();
  delay(10);
  //Off-set Calibration
  //X-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1E);
  Wire.write(0);
  Wire.endTransmission();
  delay(10);
  //Y-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1F);
  Wire.write(-1);
  Wire.endTransmission();
  delay(10);
  //Z-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x20);
  Wire.write(-4);
  Wire.endTransmission();
  delay(10);
}
void loop()
  // === Read acceleromter data === //
  Wire.beginTransmission(ADXL345);
  Wire.write(0x32);
  Wire.endTransmission(false);
  Wire.requestFrom(ADXL345, 6, true);
  X_out = ( Wire.read() | Wire.read() << 8);
  X_out = X_out / 256;
  Y_out = ( Wire.read() | Wire.read() << 8);
  Y_out = Y_out /
  Z_out = ( Wire.read() | Wire.read() << 8); // Z-axis value
  Z_out = Z_out / 256;
  // Calculate Roll and Pitch (rotation around X-axis, rotation around Y-axis)
  roll = atan(Y_out / sqrt(pow(X_out, 2) + pow(Z_out, 2))) * 180 / PI;
  pitch = atan(-1 * X_out / sqrt(pow(Y_out, 2) + pow(Z_out, 2))) * 180 / PI;
  // Low-pass filter
  rollF = 0.94 * rollF + 0.06
  pitchF = 0.94 * pitchF + 0.06 * pitch;
  Serial.print(rollF);
  Serial.print("/");
  Serial.println(pitchF);
}

Выделяет
Wire.beginTransmission(ADXL345);
в

Код (C++):

void loop()
  // === Read acceleromter data === //
[B]  Wire.beginTransmission(ADXL345);[/B]
  Wire.write(0x32);
  Wire.endTransmission(false);
  Wire.requestFrom(ADXL345, 6, true);

пишет- ошибка
exit status 1
expected initializer before 'Wire'
ожидаемый инициализатор до 'Wire'- что не так?

 

Постоянно наступаю на те же грабли!
То скобок нет, то точки с запятой. Исправил - код работает.

 

Вопрос все равно остается - куда подставить полученные коэф. 0, -1, -4 ?

 

Добрый вечер!
Давно не был на форуме, всё возился с компасом и акселерометром, но вроде бы добил и автопилот по компасу с компенсацией наклона и скетч для движения катера по GPS точкам. Надо еще соединить с картридером, чтобы сразу записывать GPS точки на SD карту, а затем двигаться по ним же или в прямом, или в обратном направлении.
Координаты Канады меняются на свои:

Код (C++):

//на основании   http://mozgochiny.ru/electronics-2/kak-sdelat-rc-kater-s-optsiey-avtopilota-chast-2/
//и http://mozgochiny.ru/electronics-2/kak-sdelat-rc-kater-s-optsiey-avtopilota/
// выход от NEO 6M на pin2 в GPS плату Arduino Uno.
//с платы Arduino Uno GPS выход ТХ pin1 на вход в Main pin7(link),
//компас А4, А5 на плате Main. Монитор порта для компаса 115200 bod

#include <Wire.h>
#include <HMC5883L.h>
#include <ADXL345.h>
#include <SoftwareSerial.h>
#include <SoftEasyTransfer.h>
//#include <Average.h>
//#include "LowPower.h"
//#include <avr/interrupt.h>
//#include <avr/power.h>
//#include <avr/sleep.h>
//#include <avr/io.h>

//Motor Control
int maxspeed;
//maxspeed - motgain * 30 > 0
//MOTGAIN 4
int motgain = 4;
int anglerange = 30;
int rightpin = 10;
int leftpin = 9;
int led = 13;
int overavg = 10;
int steerarray[10];
int throttlearray[10];

//GPS
static int r = 6371;
const float Pi = 3.14159;
int accuracy = 5;
unsigned long lasttime = 0;

SoftEasyTransfer rx;

SoftwareSerial link(7, 8);

struct RECEIVE_DATA_STRUCTURE {
  float latitude;
  float longitude;
};
HMC5883L compass;
ADXL345 accelerometer;
int ADXL345 = 0x53;
// переменные для калмана
float varVolt =  3.5;  // 0.53;// среднее отклонение (ищем в excel)
float varProcess = 0.05; // скорость реакции на изменение (подбирается вручную)
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;
// переменные для калмана

float heading1;
float heading2;
int heading3;
int bearing;

RECEIVE_DATA_STRUCTURE gpsin;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  pinMode(rightpin, OUTPUT);
  pinMode(leftpin, OUTPUT);
  pinMode(led, OUTPUT);
  link.begin(38400);
  rx.begin(details(gpsin), &link);
  digitalWrite(led, HIGH);

  // Initialize Initialize HMC5883L
  Serial.println("Initialize HMC5883L");
  while (!compass.begin())
  {  }

  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(-480, 4);

  // Initialize ADXL345

  if (!accelerometer.begin())
  {
    // delay(500);
  }

  accelerometer.setRange(ADXL345_RANGE_2G);

  // Смещение калибровки
  //X-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1E);  // X-axis offset register
  Wire.write(-0);
  Wire.endTransmission();
  delay(10);
  //Y-axis
  Wire.beginTransmission(ADXL345);
  Wire.write(0x1F); // Y-axis offset register
  Wire.write(-0);
  Wire.endTransmission();
  delay(10);

  //Z-axis
  Wire.beginTransmission (ADXL345);
  Wire.write(0x20); // Z-axis offset register
  Wire.write(1);
  Wire.endTransmission();
  delay(10);

}

// No tilt compensation

float noTiltCompensate(Vector mag)
{
  float heading = atan2(mag.YAxis, mag.XAxis);

  return heading;
}

// Tilt compensation

float tiltCompensate(Vector mag, Vector normAccel)
{
  // Pitch & Roll mpu.setXAccelOffset(youroffset)

  float roll;
  float pitch;

  roll = asin(normAccel.YAxis);
  pitch = asin(-normAccel.XAxis);

  if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78)
  {
    return -1000;
  }

  // Some of these are used twice, so rather than computing
  //  them twice in the algorithem we precompute them before hand.

  float cosRoll = cos(roll);
  float sinRoll = sin(roll);
  float cosPitch = cos(pitch);
  float sinPitch = sin(pitch);

  // Tilt compensation

  float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
  float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

  float heading = atan2(Yh, Xh);

  return heading;
}

// Correct angle

float correctAngle(float heading)
{
  if (heading < 0) {
    heading += 2 * PI;
  }
  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }
  return heading;
}

void loop()
 

код полностью не поместился, продолжен ниже

 

Код (C++):

void loop()
{
  Navigate(49.231623, -122.695656, 80);
  Navigate(49.231600, -122.695745, 80);
  Navigate(49.231545, -122.695796, 80);
  Navigate(49.231474, -122.695756, 80);
  Navigate(49.231450, -122.695659, 80);
  Navigate(49.231472, -122.695568, 80);
  Navigate(49.231535, -122.695523, 80);
  Navigate(49.231600, -122.695572, 80);
}

void Control() {
  while (pulseIn(5, HIGH, 100000) > 0) {
    int throttle = pulseIn(4, HIGH, 20000);
    int steer = pulseIn(5, HIGH, 20000);
    if (throttle > 1000 && steer > 1000) {
      Override(throttle, steer);
    }
    delay(50);
  }
}

void Navigate(float lat2, float long2, int velocity) {
  maxspeed = velocity;
  while (GetDistanceInM(gpsin.latitude, gpsin.longitude, lat2, long2) > accuracy) {
    if (rx.receiveData()) {
      lasttime = millis();
    }
    while (pulseIn(5, HIGH, 100000) > 0) {    //считываем с 5 в схеме вход от пульта
      int throttle = pulseIn(4, HIGH, 20000); //с 4 в схеме вход от пульта
      int steer = pulseIn(5, HIGH, 20000);    //с 5 в схеме вход от пульта
      if (throttle > 1000 && steer > 1000) {
        Override(throttle, steer);
      }
      delay(50);
    }
    if ((millis() - lasttime) < 2000) {
      int bearing = GetBearing();
      int heading3 = GetHeading(gpsin.latitude, gpsin.longitude, lat2, long2);
      int leftmot = MotorSpeed(bearing, heading3, 1);
      int rightmot = MotorSpeed(bearing, heading3, 0);
      if (leftmot < 0) {
        leftmot = 0;
      }
      if (rightmot < 0) {
        rightmot = 0;
      }
      RunMotor(leftmot, 1);
      RunMotor(rightmot, 0);
      Serial.print(bearing);
      Serial.print("  ");
      Serial.print(heading3);
      Serial.print("  ");
      Serial.print(GetDistanceInM(gpsin.latitude, gpsin.longitude, lat2, long2));
      Serial.print("  ");
      Serial.print(leftmot);
      Serial.print("  ");
      Serial.println(rightmot);
      //delay(100);
    }
    else {
      RunMotor(0, 1);
      RunMotor(0, 0);
      Serial.println("No GPS Signal");
    }
  }
  Serial.println("NavLoop Return");
  RunMotor(0, 1);
  RunMotor(0, 0);
}

void Override(int throttle, int steering) {   //дроссель и управление
  int left;
  int right;
  if (throttle > 1600)
  {
    throttle = (int)(0.51 * (throttle - 1500));
    if (steering > 1500) {
      right = (int)(throttle - 0.51 * (steering - 1500));
      if (right < 0)
      {
        right = 0;
      }
      left = throttle;
    }
    else {
      left = (int)(throttle + 0.51 * (steering - 1500));
      if (left < 0)
      {
        left = 0;
      }
      right = throttle;
    }
  }
  else {
    left = 0;
    right = 0;
  }
  RunMotor(right, 0);
  RunMotor(left, 1);
  Serial.print(left);
  Serial.print("  ");
  Serial.println(right);
}

void RunMotor(int val, int option) {
  if (option == 0) {
    //Right Motor
    analogWrite(rightpin, val);
  }
  else {
    //Left Motor
    analogWrite(leftpin, val);
  }
}

int GetBearing() {

  // Read vectors

  Vector mag =  compass.readNormalize();
  Vector acc = accelerometer.readScaled();

  // Calculate headings

  heading1 = noTiltCompensate(mag);
  heading2 = tiltCompensate(mag, acc);

  if (heading2 == -1000)
  {
    heading2 = heading1;
  }


  // Formula: (deg + (min / 60.0)) / (180 / M_PI);

  float declinationAngle = (15 + (26.0 / 60.0)) / (180 / M_PI);
  heading1 += declinationAngle;
  heading2 += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg

  heading1 = correctAngle(heading1);
  heading2 = correctAngle(heading2);

  // Convert to degrees-----------------------------------
  heading1 = heading1 * 180 / M_PI;   // noTiltCompensate (mag)
  heading2 = heading2 * 180 / M_PI;   // tiltCompensate (mag, acc)

  if (heading2 < 0)
  {
    heading2 += 2 * PI;
  }

  if (heading2 > 2 * PI)
  {
    heading2 -= 2 * PI;
  }

  // Convert to degrees

  int val = heading2;

  int filter(val);

  {
    //функция фильтрации
    Pc = P + varProcess;
    G = Pc / (Pc + varVolt);
    P = (1 - G) * Pc;
    Xp = Xe;
    Zp = Xp;
    Xe = (G * (val - Zp)) + Xp; // "фильтрованное" значение
    //return(Xe);
  }
  // int heading3 = ((0,8*heading3) + (0,2*Xe))/2;

  int heading3 = Xe;
  bearing = (int)((heading3) + 270) % 360;

  return bearing;
}

int MotorSpeed(int bearing, int heading3, int option) {
  heading3 = heading3 % 360;
  bearing = bearing % 360;
  if (heading3 == bearing)
  {
    return (maxspeed);
  }
  int o1 = (int)heading3 - (int)bearing;
  int o2 = (int)bearing - (int)heading3;
  int direction = ((360 - ((int)heading3 - (int)bearing)) % 360);
  if (direction < 180) {
    if (direction > anglerange)
    {
      //Right Motor
      if (option == 0) {
        return maxspeed;
      }
      //Left Motor
      else {
        return 0;
      }
    }
    else
    {
      //Right Motor
      if (option == 0) {
        return maxspeed;
      }
      //Left Motor
      else {
        return maxspeed - (direction * motgain);
      }
    }
  }
  else if (direction >= 180)
  {
    if ((360 - direction) > anglerange)
    {
      //Right Motor
      if (option == 0) {
        return 0;
      }
      //Left Motor
      else {
        return maxspeed;
      }
    }
    else
    {
      //Right Motor
      if (option == 0) {
        return maxspeed - ((360 - direction) * motgain);
      }
      //Left Motor
      else {
        return maxspeed;
      }
    }
  }
  return 0;
}

static float GetHeading(float lat1, float long1, float lat2, float long2)
{
  //Current Latitude, Current Longitude
  //Projected Latitude, Projected Longitude
  //To get reverse heading, add 180
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLon = long2 - long1;
  float y = sin(dLon) * cos(lat2);
  float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
  return ConvertToStaticDegrees(atan2(y, x));
}
static float GetDistanceInM(float lat1, float long1, float lat2, float long2)
{
  //Current Latitude, Current Longitude
  //Projected Latitude, Projected Longitude
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLat = lat2 - lat1;
  float dLon = long2 - long1;
  float a = haversin(dLat) + cos(lat1) * cos(lat2) * haversin(dLon);
  float c = 2 * atan2(sqrt(a), sqrt(1 - a));
  return r * c * 1000; // Distance in M
}
static float ConvertToDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= -180) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToStaticDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= 0) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToRadians(float angle)
{
  return (Pi / 180) * angle;
}
static float haversin(float input)
{
  return (sin(input / 2)) * (sin(input / 2));
}

Вторая плата тоже Uno для модуля GPS NEO 6M V2:
Принимается любая помощь по данному проекту, в том числе и по оптимизации кода.