Магнитный указатель курса (с компенсацией наклона) для картплоттера
- Войдите на сайт для отправки комментариев
Сб, 03/06/2017 - 07:22
На форуме http://www.rusfishing.ru/forum/showthread.php?t=247235 тема про самоделки для картплоттеров.
#include <Wire.h>
#include <HMC5883L.h>
#include <MPU6050.h>
#include <String.h>
#include <SoftwareSerial.h>
SoftwareSerial GarminSerial(2, 3, 1); // RX, TX ,Logic invretor
HMC5883L compass;
MPU6050 mpu;
float heading1;
float heading2;
#define Filter_read 20 //!!! только четное число сколько читать значений из АЦП для работы фильтра.Выбрано для частоты 10 Гц
#define Filter_get 1 //!!! только четное число или единица сколько взять значений для расчета среднего фильтром
long filter_heading2; //для получения множества значений raw из регистра преобразования АЦП после чтения по значениям Filter_read и Filter_get
String str_hdt;
long last_nmea_send;
//____________________________________________________________________________________________________________________________________
void setup()
{
Serial.begin(4800);
GarminSerial.begin(4800);
//GarminSerial.begin(38400);
// Initialize MPU6050
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
delay(500);
}
/*
// Enable bypass mode
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true) ;
mpu.setSleepEnabled(false);
*/
// Initialize Initialize HMC5883L
while (!compass.begin())
{
delay(500);
}
// 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(-4,-125);
}
//_________________________________________________________________________________________________________________________________
// 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()
{
double StartMillis = millis();
filter_heading_calc ();
// Output
/*
Serial.print(millis()- StartMillis,0 );
Serial.print(" mS: ");
Serial.print(heading1,0); //noTiltCompensate(mag)
Serial.print(" : ");
Serial.println(heading2,0); //tiltCompensate(mag, acc)
*/
//if (millis() - last_nmea_send >= 10){
hdtSentenceMake();
Serial.println(str_hdt);
GarminSerial.print(str_hdt);
// last_nmea_send = millis();
//}
}
//_____________________________________________________________________________________________________________________________
void get_heading(){
// Read vectors
Vector mag = compass.readNormalize();
Vector acc = mpu.readScaledAccel();
// Calculate headings
heading1 = noTiltCompensate(mag);
heading2 = tiltCompensate(mag, acc);
if (heading2 == -1000)
{
heading2 = heading1;
}
// Set declination angle on your location and fix heading
// You can find your declination on: http://magnetic-declination.com/
// (+) Positive or (-) for negative
// For Bytom / Poland declination angle is 4'26E (positive)
// Formula: (deg + (min / 60.0)) / (180 / M_PI);
//float declinationAngle = 0;//(4.0 + (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)
}
//______________________________________________________________________________________________________________________________
void filter_heading_calc () { // подпрограмма считывания и фильтрации значений
float sortedValues[Filter_read]; //назначили массив с sortedValues[Filter_read]
for (int i = 0; i < Filter_read; i++) { //цикл от 0 до Filter_read
again: get_heading(); // идем на подпрограмму получения данных из АЦП в переменную ltw
if ((heading2 / heading2) != 1 ) {
//Serial.println("*************fuck***************");
goto again;
}
// Serial.print(" adc[");
// Serial.print(i);
// Serial.print("] = ");
// Serial.println(ltw);
float value = heading2 ; //value получает значение одного измерения raw ads
int j; //переменная "j" куда вставлять значение при сортировке
if (value < sortedValues[0] || i == 0) { //если полученое значение valve < sortedValues[0] или это самое начало "i",то
j = 0; //"j"= 0 это первая позиция
} //конец if
else { //иначе
for (j = 1; j < i; j++) { //цикл j от 1 до j < i искать "j"
if (sortedValues[j - 1] <= value && sortedValues[j] >= value) { //ищем место"j" в sortedValues[]куда вставить value
break; //"j" найдено,остановить выполнение цикла
} //конец if
} //конец цикла j
} //конец else
for (int k = i; k > j; k--) { //все значения от "i" до "j" (вниз к--) нужно поднять на одну позицию вверх
sortedValues[k] = sortedValues[k - 1]; //чтобы освободить место "j" для valve
} //конец цикла
sortedValues[j] = value; //вставить считаное valve в sortedValues[] в позицию "j"
} //конец цикла от 0 до Filter_read
/*
for (int i = 0; i < Filter_read; i++) { //цикл от 0 до Filter_read
Serial.print(" sortedValues[");
Serial.print(i);
Serial.print("] = ");
Serial.println(sortedValues[i]);
}
*/
//расчет среднего знач из Filter_get. Эти значения взяты после отброса лишних значений
float filter_heading = 0;
if (Filter_get%2 == 0) //если Filter_get четное, то считаем так
// четное
{
for (int i = Filter_read / 2 - Filter_get/2; i < (Filter_read / 2 + Filter_get/2); i++) {
filter_heading += sortedValues[i];
}
heading2 = filter_heading / Filter_get;
}
if (Filter_get == 1) ////если Filter_get =1, то не считаем а берем из середины одно значение
{
byte nomb = Filter_read / 2;
heading2 = sortedValues[nomb];
}
}
//__________________________________________________________________________________________________
void hdtSentenceMake(){
char Sentence [20];
byte XOR;
str_hdt = "$HCHDM,";
str_hdt += String(heading2,1);
str_hdt += ",M*";
str_hdt.toCharArray(Sentence, str_hdt.length() + 1);
//считать XOR все символы кроме $ , и стоп на *
int i;
for (XOR = 0, i = 0; i < sizeof(Sentence); i++)
{
if (Sentence[i] == '*') break;
if (Sentence[i] != '$' ) XOR ^= Sentence[i];
}
if (XOR < 0x10) str_hdt +="0";
str_hdt += String(XOR, HEX);
}
//__________________________________________________________________________________________________
Появилось время, я сделал макет своего магнитного указателя, и поженил его с Garmin Echomap chirp 73SV.
Макет сделан на Arduino UNO , будет работать и на других платах. На куске полистирола приклеил магнитометр HMC5983 и MPU 6050 для измерения гиро и ускорения. Магнитометр на точном поворотном устройстве отцентровал (офсет X ,Y). Получилось достаточно точно. На 360 градусов поворота поворотного устройства, разница показаний магнитометра и поворотки была не более 1 градуса, при том, что на моей широте магнитные линии падают под углом около 60 градусов. Затем в софте добавил MPU6050 для компенсации наклона, и это очень хорошо работает до наклона 45 градусов. Что я увидел? Все работает. Есть особенности.
1.Пробовал разные скорости (4800 и 38400) и разный интервал передачи данных ( от 1 до 10 ГЦ) в картплоттер. При любых комбинациях часть NMEA предложений на странице диагностика связи прибор пропускает часть сообщений как сообщения с неверной контрольной суммой, хотя с этим все в порядке. Пока не понял причину. Возможно это нормально, т.е. так устроен софт прибора.
2. Пробовал разные фильтры использовать (и калмана тоже библиотечного ардуиновского). Не нравиться, и не проходит тест на стресс. Начинаю свой указатель быстро крутить/вертеть/стучать/кидать и происходит глюк или переполнение фильтра. Поэтому использовал программный фильтр, который использую при аналоговых измерениях. Он меня устраивает.
3. У Garmin для подключения NMEA 0183 нет диф.пары. У него RX/TX(-) соединяется с минусом питания. Синий провод передает данные из картплоттера, коричневый - принимать данные. Но сигнал логики на RX нужно инвертировать. Это легко делается использованием библиотеки <SoftwareSerial.h> и указать при объявлении единичку после номеров пинов. Это приводит к инвертированию логики на данном сериал порте. Вот примерно так SoftwareSerial GarminSerial(2, 3, 1); // RX, TX ,Logic_Inv
4. При таком подключении прибор нормально принимает сигнал от ардуины с логикой 3.3В. А значит можно использовать платы типа PRO MICRO 3.3V.
5. Разные экраны/режимы работы картплоттера с разной скоростью отображают изменения показаний от магнитного указателя курса. Экран «окружающая среда» очень быстро реагирует на поворот датчика, т.е. практически не отстает от него. Экран «Данные» и «Карта» отображают изменения медленно, ступенчато. Причем вращение карты иногда идет быстрее , чем изменение показаний направления в цифрах. Вращение карты без рывков, скорость поворота приемлемая.
Калибровка,библиотеки и методы взял у поляка видео https://www.youtube.com/watch?v=UQntG3JQvOA
Выкладываю ссылку на архив со своим черновиком софта для ардуины и использованные библиотеки https://yadi.sk/d/eaYfuaEZ3JmSZF
Видео https://youtu.be/KFqytMOUzq8 и https://youtu.be/911eIVlXlhM
Удачи.
Поляк, конечно же молодец, я тоже его библиотеками пользовался для создания наклонно компенсированного компаса. Однако он не калибрует смещение (оффсет) по оси Z. После того, как я изменил скетч и стал калибровать ось Z (переворачивая магнитометр "вверх ногами"), компенсация стала равномерно работать при любых (0-360) по азимуту направлениях наклона (даже больше 45 градусов) (была однобокость, это многие пользователи заметили). Ограничения 45 градусов - программное, я его убрал, т.к. прибор компенсирует и большие углы. Очень полезно для уменьшения флуктуацй включить внутренний НЧ фильтр в самом MPU6050 с самой низкой полосой пропускания..
Измеренная погрешность определения азимута не превышает 3 градуса.
(фильтр Калмана не стал использовать, когда казалось бы все настроил он самовозбудился через час работы. Использую программную фильтрацию - программный НЧ фильтр. Для лодки вполне годное решение.)
Если не трудно, скиньте те данные, которые передаёт компас в навигатор, хочу на своём Humminbird 998 попробовать.
Без компаса, просто с ардино через серийный порт посылать данные
Исправлен баг в подсчете контрольной суммы. Не забываем менять свои коэф. калибровки магнитометра в
compass.setOffset(-4,-125);
#include <Wire.h> #include <HMC5883L.h> #include <MPU6050.h> #include <String.h> #include <SoftwareSerial.h> SoftwareSerial GarminSerial(2, 3, 1); // RX, TX ,Logic invretor HMC5883L compass; MPU6050 mpu; float heading1; float heading2; #define Filter_read 200 //!!! только четное число сколько читать значений из АЦП для работы фильтра.Выбрано 20 для частоты 10 Гц или 200 для 1Гц #define Filter_get 1 //!!! только четное число или единица сколько взять значений для расчета среднего фильтром long filter_heading2; //для получения множества значений raw из регистра преобразования АЦП после чтения по значениям Filter_read и Filter_get String str_hdm; String STR_CRC_XOR =""; //____________________________________________________________________________________________________________________________________ void setup() { Serial.begin(9600); Serial.println("HMC5883L_compensation_MPU6050_next_7_1000ms.ino");//******************************************************************************* GarminSerial.begin(4800); //GarminSerial.begin(38400); // Initialize MPU6050 while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) { delay(500); } // Initialize Initialize HMC5883L while (!compass.begin()) { delay(500); } // 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(-4,-125); } //_________________________________________________________________________________________________________________________________ // 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() { double StartMillis = millis(); filter_heading_calc (); hdmSentenceMake(); Serial.println(str_hdm); GarminSerial.print(str_hdm); } //_____________________________________________________________________________________________________________________________ void get_heading(){ // Read vectors Vector mag = compass.readNormalize(); Vector acc = mpu.readScaledAccel(); // Calculate headings heading1 = noTiltCompensate(mag); heading2 = tiltCompensate(mag, acc); if (heading2 == -1000) { heading2 = heading1; } // 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) } //______________________________________________________________________________________________________________________________ void filter_heading_calc () { // подпрограмма считывания и фильтрации значений float sortedValues[Filter_read]; //назначили массив с sortedValues[Filter_read] for (int i = 0; i < Filter_read; i++) { //цикл от 0 до Filter_read again: get_heading(); // идем на подпрограмму получения данных из АЦП в переменную ltw if ((heading2 / heading2) != 1 ) { //Serial.println("*************fuck***************"); goto again; } float value = heading2 ; //value получает значение одного измерения raw ads int j; //переменная "j" куда вставлять значение при сортировке if (value < sortedValues[0] || i == 0) { //если полученое значение valve < sortedValues[0] или это самое начало "i",то j = 0; //"j"= 0 это первая позиция } //конец if else { //иначе for (j = 1; j < i; j++) { //цикл j от 1 до j < i искать "j" if (sortedValues[j - 1] <= value && sortedValues[j] >= value) { //ищем место"j" в sortedValues[]куда вставить value break; //"j" найдено,остановить выполнение цикла } //конец if } //конец цикла j } //конец else for (int k = i; k > j; k--) { //все значения от "i" до "j" (вниз к--) нужно поднять на одну позицию вверх sortedValues[k] = sortedValues[k - 1]; //чтобы освободить место "j" для valve } //конец цикла sortedValues[j] = value; //вставить считаное valve в sortedValues[] в позицию "j" } //конец цикла от 0 до Filter_read //расчет среднего знач из Filter_get. Эти значения взяты после отброса лишних значений float filter_heading = 0; if (Filter_get%2 == 0) //если Filter_get четное, то считаем так // четное { for (int i = Filter_read / 2 - Filter_get/2; i < (Filter_read / 2 + Filter_get/2); i++) { filter_heading += sortedValues[i]; } heading2 = filter_heading / Filter_get; } if (Filter_get == 1) ////если Filter_get =1, то не считаем а берем из середины одно значение { byte nomb = Filter_read / 2; heading2 = sortedValues[nomb]; } } //__________________________________________________________________________________________________ void hdmSentenceMake(){ char Sentence [20]; byte XOR; STR_CRC_XOR =""; str_hdm = "$HCHDM,"; str_hdm += String(heading2,1); str_hdm += ",M*"; str_hdm.toCharArray(Sentence, str_hdm.length() + 1); //считать XOR все символы кроме $ , и стоп на * int i; for (XOR = 0, i = 0; i < sizeof(Sentence); i++) { if (Sentence[i] == '*') break; if (Sentence[i] != '$' ) XOR ^= Sentence[i]; } if (XOR < 0x10) str_hdm +="0"; STR_CRC_XOR = String(XOR, HEX); STR_CRC_XOR.toUpperCase(); str_hdm += STR_CRC_XOR; } //__________________________________________________________________________________________________mikelari
А как вы подаёте данные с компаса на навигатор? На нём есть отдельный вход nmea0183 ? У меня humminbird 998, там нет такого, только интернет. Есть выход 0183, там такие данные-
С гпс приёмника идё такое-
Пробовал на этот вход подавать данные с эмулятора-
в таком виде показывает курс.
Какие есть варианты? Разбивать сообщения с гпс и подсовывать туда данные с магнитного компаса?
Да у моего эхолота есть отдельные вход/выход для nmea0183 и для nmea2000.
Если у Вас нет входа nmea0183, можно использовать конвертор уровней для получения нормальной дифф. пары и библиотеку nmea2000 . Этот вход/выход надеюсь имеется у humminbird 998.
$GPRMC это "рекомендуемый минимум специальных данных GNSS" в котором нет данных магнитного указателя курса
Что значит разбивать ? Формируется и передается в эхолот отдельное предложение.
Удачи.
Добрый день. Вы не могли бы подсказать точную распиновку подключения Ардуино к порту NMEA2000 в устройстве Garmin Echomap chirp 73SV?
Используется подключение к NMEA0183. Для подключения к NMEA2000 нужно серьезно переработать всё.