Справочници, схемотехника, теория > Програмиране на микроконтролери, програматори, цифрови проекти
GPS-автопилот - алгоритъм
dmitarp:
"The output of the three sensors measuring x, y and z components of the magnetic field, together with the pitch and roll are used to calculate the bearing, each of these components are also made available in there raw form."
Това подчертаното е това, което търсиш и което ти наричаш азимут. Bearing е ъгълът между твоето направление (определено от GPS точките) и севера.
EDM electronics:
Проверих по предложението - съкратената формула на juliang, няма изменение на бързодействието по третата съкратена формула на dmitarp.
Днес цял ден правих опити да съкратя корен квадратен с 2-ра степен на уравнението, но няма как да се получи с цел опростяване.
Долу следват: време в микросекунди и метри. Всички примери работят еднакво.
352.00 424.86
260.00 424.86
260.00 jul424.86
604.00 424.86
348.00 424.86
264.00 424.86
264.00 jul424.86
612.00 424.86
348.00 424.86
260.00 424.86
256.00 jul424.86
612.00 424.86
348.00 424.86
256.00 424.86
256.00 jul424.86
EDM electronics:
Пробвах и формулата за азимута, но нещо не се получава. Не съм сигурен дали ардуиното приема функцията atan2(); Дава някаква грешка.
--- Код: ---// β - азимут в градуси
// β = atan2(X,Y)
// X = cos θb * ∆L
// Y = cos θa * sin θb – sin θa * cos θb.
// β = atan2(cos θb * ∆L,cos θa * sin θb – sin θa * cos θb)
time1 = micros();
double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) – sin (phi1) * cos (phi2)) * pi / 180;
time1 = micros() - time1;
Serial.print(time1);
Serial.print (" ");
Serial.print(B);
Serial.println();
--- Край на кода ---
dmitarp:
Имаш проблем във минуса пред синуса, – sin (phi1) * cos (phi2)) * pi / 180; нещо много дълъг ми се вижда този минус не знам как си го написал, при мен също даваше грешка при компилирането, след като смених минуса стана всичко добре.
double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2)) * 180 / pi;
Освен това си объркал превръщането. Тази формула ще ти я смятана най бавно защото има много тригонометрични функции.
EDM electronics:
--- Цитат на: dmitarp в Февруари 18, 2020, 10:36:34 pm ---Имаш проблем във минуса пред синуса, – sin (phi1) * cos (phi2)) * pi / 180; нещо много дълъг ми се вижда този минус не знам как си го написал, при мен също даваше грешка при компилирането, след като смених минуса стана всичко добре.
double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2)) * 180 / pi;
Освен това си объркал превръщането. Тази формула ще ти я смятана най бавно защото има много тригонометрични функции.
--- Край на цитат ---
Да, да, току що и аз го открих. Раздробих формулата и го открих. Този минус съм го копирал от поста ти и той е всъщност тире. Сега нещата работят коректно, оправил съм и градусите, давам скеча:
--- Код: --- // β - азимут в градуси
// β = atan2(X,Y)
// X = cos θb * ∆L
// Y = cos θa * sin θb – sin θa * cos θb.
// β = atan2(cos θb * ∆L,cos θa * sin θb – sin θa * cos θb)
time1 = micros();
// double X = cos (phi2) * (l2 - l1);
// double Y = cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2);
double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2));
B = B * 180 / pi;
time1 = micros() - time1;
Serial.print(time1);
Serial.print (" ");
Serial.print(B);
Serial.println();
--- Край на кода ---
Ето и данните от сериал-порта, показва 64,39 гр.:
608.00 424.86
352.00 424.86
256.00 424.86
240.00 jul424.86
704.00 64.39
608.00 424.86
352.00 424.86
256.00 424.86
256.00 jul424.86
688.00 64.39
624.00 424.86
352.00 424.86
256.00 424.86
256.00 jul424.86
704.00 64.39
Навигация
[0] Списък на темите
Премини на пълна версия