09-07-2025, 19:26
Mam już mnóstwo kodu i już pływa, ale mam problem pływaniem w lini prostej.
Używam starego APM jako bazy ardurower średnio się sprawdza do tego celu.
Na razie używam samego GPS i kompasu, bez żyroskopu bo go wcale nie rozumiem..
Chciał bym się dowiedzieć czy kod wygląda dobrze dla łódki...
i
Może by ktoś chętny co nie co poprawił lub podpowiedział....Sprawdził i utwierdził by mnie czy to działa prawidłowo. Bo niby wskazania są ale nie wiem czy wystarczająco dokładne.
Używam starego APM jako bazy ardurower średnio się sprawdza do tego celu.
Na razie używam samego GPS i kompasu, bez żyroskopu bo go wcale nie rozumiem..
Chciał bym się dowiedzieć czy kod wygląda dobrze dla łódki...
Kod:
#include "HMC5883L.h"
#include "ReadEprom.h"
#include <Wire.h>
void HMC5883L_Init() {
Wire.begin();
Wire.beginTransmission(HMC5883L_ADDRESS);
Wire.write(0x00); // Rejestr konfiguracji A
Wire.write(0x70); // 8 pomiarów na średnią, 15Hz(0x70) lub 30Hz(0x78)
Wire.endTransmission();
Wire.beginTransmission(HMC5883L_ADDRESS);
Wire.write(0x01); // Rejestr konfiguracji B
Wire.write(0x20); // Ustawienie wzmocnienia 40
Wire.endTransmission();
Wire.beginTransmission(HMC5883L_ADDRESS);
Wire.write(0x02); // Rejestr trybu
Wire.write(0x00); // Tryb ciągłego pomiaru
Wire.endTransmission();
Wire.requestFrom(HMC5883L_ADDRESS, 6);
}
// Zmienne globalne
float offsetX = 56.50, offsetY = -215.00, offsetZ = 0.0;
float declinationAngle = 5.0;
float rotationAngle = 90.0;
float heading = 0;
bool isUpsideDown = false; // Domyślnie czujnik jest w normalnej orientacji
float x;
float y;
unsigned long os_czas_kom = 0;
bool AwariaKom = false;
void readRawData(int16_t &x, int16_t &y, int16_t &z) {
// Ustaw wskaźnik na pierwszy rejestr danych (0x03)
Wire.beginTransmission(HMC5883L_ADDRESS);
Wire.write(0x03);
Wire.endTransmission();
Wire.requestFrom(HMC5883L_ADDRESS, 6);
if (Wire.available() == 6) {
x = (Wire.read() << 8) | Wire.read();
z = (Wire.read() << 8) | Wire.read();
y = (Wire.read() << 8) | Wire.read();
os_czas_kom = millis();
AwariaKom = false;
}
}
unsigned long lastReadTime = 0; // Zmienna globalna
void Read_HMC5883L() {
if (millis() - lastReadTime > 70) { // Odczyt co 70 ms
int16_t rawX, rawY, rawZ;
readRawData(rawX, rawY, rawZ);
if (isUpsideDown) {
rawY = -rawY;
x = rawX - offsetX;
y = rawY + offsetY;
}else{
x = rawX - offsetX;
y = rawY - offsetY;
}
heading = atan2(y, x) * 180.0 / M_PI;
heading += declinationAngle;
heading += rotationAngle;
if (heading < 0) heading += 360.0;
if (heading >= 360) heading -= 360.0;
if (millis() - os_czas_kom > 3000){
heading = 0;
AwariaKom = true;
}
lastReadTime = millis(); // Aktualizacja czasu odczytu
}
}
void calibrateXY() {
int16_t rawX, rawY, rawZ;
int16_t minX = 32767, minY = 32767, maxX = -32768, maxY = -32768;
char userInput;
// Zapytaj użytkownika o potwierdzenie rozpoczęcia kalibracji
Serial.println(F("Czy chcesz rozpoczac kalibracje kompasu(T/N)"));
Serial.println(F("Naciskajac 'T' masz 5s do obracania lodka"));
// Czekaj na odpowiedź użytkownika
while (true) {
if (Serial.available() > 0) {
userInput = Serial.read(); // Odczytanie odpowiedzi użytkownika
if (userInput == 'T') { // Jeżeli odpowiedź to 't' (tak)
Serial.println(F("Rozpoczynam kalibracje..."));
break; // Kontynuuj kalibrację
} else if (userInput == 'N') { // Jeżeli odpowiedź to 'n' (nie)
Serial.println(F("Kalibracja anulowana."));
return; // Zakończ funkcję, kalibracja nie zostanie wykonana
}
}
}
delay(5000);
Serial.println(F("Obracaj lodka przez 10s"));
// Zbieranie danych przez 10 sekund
for (int i = 0; i < 5000; i++) { // 5000 próbek w 10 sekund
readRawData(rawX, rawY, rawZ);
// Szukaj minimum i maksimum dla obu osi X i Y
if (rawX < minX) minX = rawX;
if (rawX > maxX) maxX = rawX;
if (rawY < minY) minY = rawY;
if (rawY > maxY) maxY = rawY;
delay(2); // opóźnienia dla większej liczby próbek
}
// Obliczanie ofsetów na podstawie minimalnych i maksymalnych wartości
offsetX = (maxX + minX) / 2.0;
offsetY = (maxY + minY) / 2.0;
// Zapisz ofsety do pamięci eprom i wyswietl
SaveOfsetCompas();
delay(10);
Serial.print(F("Ofset X: "));
Serial.println(offsetX);
Serial.print(F("Ofset Y: "));
Serial.println(offsetY);
// Kalibracja zakończona
Serial.println(F("Kalibracja zakonczona"));
}
// Funkcja do zmiany deklinacji przez UART z potwierdzeniem
void changeDeclinationAngle() {
// Wyświetlanie obecnej wartości deklinacji
Serial.print(F("Aktualna deklinacja: "));
Serial.println(declinationAngle);
Serial.println(F("Czy chcesz zmienic wartosc deklinacji? (T/N)"));
Serial.println(F("www.magnetic-declination.com"));
char userInput;
while (true) {
if (Serial.available() > 0) {
userInput = Serial.read(); // Odczytanie odpowiedzi użytkownika
if (userInput == 'T') { // Jeżeli odpowiedź to 't' (tak)
Serial.println(F("Podaj nowa wartosc deklinacji:"));
break; // Kontynuuj zmianę deklinacji
} else if (userInput == 'N') { // Jeżeli odpowiedź to 'n' (nie)
Serial.println(F("Zmiana deklinacji anulowana."));
return; // Zakończ funkcję, nie zmieniaj deklinacji
}
}
}
// Oczekiwanie na wprowadzenie nowej wartości deklinacji
while (true) {
if (Serial.available() > 0) {
String input = Serial.readStringUntil('\n'); // Odczytanie wejścia użytkownika
input.trim(); // Usuń zbędne białe znaki
// Sprawdź, czy wprowadzenie jest liczbą
float newDeclination = input.toFloat();
if (newDeclination != 0.0 || input == "0") {
declinationAngle = newDeclination; // Ustaw nową deklinację
SaveDeclinationCompas();
delay(10);
Serial.print(F("Nowa deklinacja: "));
Serial.println(declinationAngle);
break;
} else {
Serial.println(F(""));
}
}
}
}
void changeRotationAngle() {
// Wyświetlanie obecnej wartości kąta obrotu
Serial.println(F(""));
Serial.print(F("Aktualny kat obrotu: "));
Serial.print(rotationAngle);
Serial.println(F("°"));
Serial.println(F(""));
Serial.println(F("Wybierz opcje zmiany kata obrotu:"));
Serial.println(F("1.Ustaw kat 0°"));
Serial.println(F("2.Ustaw kat 90° (wew APM)"));
Serial.println(F("3.Ustaw kat 180°"));
Serial.println(F("4.Ustaw kat 270°"));
Serial.println(F("5.Wprowadz dowolny kat (1-359°)"));
Serial.println(F("0.Powrot"));
while (true) {
if (Serial.available() > 0) {
char userInput = Serial.read(); // Odczytanie wyboru użytkownika
// Usuń znaki końca linii z bufora
if (userInput == '\n' || userInput == '\r') {
continue;
}
switch (userInput) {
case '1':
rotationAngle = 0;
SaveRotationCompas();
Serial.println(F("Kat obrotu ustawiony na 0°."));
return; // Wyjście z funkcji
case '2':
rotationAngle = 90;
SaveRotationCompas();
Serial.println(F("Kat obrotu ustawiony na 90°."));
return; // Wyjście z funkcji
case '3':
rotationAngle = 180;
SaveRotationCompas();
Serial.println(F("Kat obrotu ustawiony na 180°."));
return; // Wyjście z funkcji
case '4':
rotationAngle = 270;
SaveRotationCompas();
Serial.println(F("Kat obrotu ustawiony na 270°."));
return; // Wyjście z funkcji
case '5':
Serial.println(F(""));
Serial.println(F("Podaj nową wartosc kata obrotu (1-359°)"));
while (true) {
if (Serial.available() > 0) {
String input = Serial.readStringUntil('\n'); // Odczytanie wejścia użytkownika
input.trim(); // Usuń zbędne białe znaki
// Sprawdź, czy wprowadzenie jest liczbą
float newRotationAngle = input.toFloat();
// Sprawdzamy, czy kąt mieści się w zakresie 1-359
if (newRotationAngle >= 1.0 && newRotationAngle <= 359.0) {
rotationAngle = newRotationAngle; // Ustaw nowy kąt obrotu
SaveRotationCompas();
delay(10);
Serial.print(F("Nowy kat obrotu: "));
Serial.print(rotationAngle);
Serial.println(F("°"));
return; // Zakończ funkcję
}
}
}
return;
case '0':
return; // Wyjście z funkcji
}
}
}
}
void changeOrientation() {
Serial.print(F("Aktualna pozycja: "));
if (isUpsideDown) {
Serial.println(F("do gory nogami"));
}else{
Serial.println(F("normalna"));
}
Serial.println(F("Wybierz orientacje kompasu:"));
Serial.println(F("1.Normalna orientacja(wew APM)"));
Serial.println(F("2.Czujnik wlutowany do gory nogami"));
Serial.println(F("0.Powrot"));
while (true) {
if (Serial.available() > 0) {
char userInput = Serial.read(); // Odczytanie wyboru użytkownika
// Usuń znaki końca linii z bufora
if (userInput == '\n' || userInput == '\r') {
continue;
}
switch (userInput) {
case '1':
isUpsideDown = false; // Normalna orientacja
SaveUpSideCompas();
Serial.println(F("Czujnik ustawiony w normalnej orientacji."));
return;
case '2':
isUpsideDown = true; // Odwrócona orientacja
SaveUpSideCompas();
Serial.println(F("Czujnik ustawiony do gory nogami."));
return;
case '0':
return; // Wyjście z funkcji
}
}
}
}
i
Kod:
#ifndef HMC5883L_H
#define HMC5883L_H
#include <Arduino.h>
// Adres czujnika HMC5883L
#define HMC5883L_ADDRESS 0x1E
void HMC5883L_Init(); //INICJALIZACJA
extern float heading; //DANE KOMPAU
extern bool AwariaKom; //AWARIA KOMPASU
extern float offsetX;
extern float offsetY;
extern float offsetZ;
extern float declinationAngle;
extern float rotationAngle;
extern bool isUpsideDown;
// FUNKCJE
void calibrateXY();
void Read_HMC5883L();
void changeDeclinationAngle();
void changeRotationAngle();
void changeOrientation();
#endifMoże by ktoś chętny co nie co poprawił lub podpowiedział....Sprawdził i utwierdził by mnie czy to działa prawidłowo. Bo niby wskazania są ale nie wiem czy wystarczająco dokładne.