Автор Тема: GPS-автопилот - алгоритъм  (Прочетена 6860 пъти)

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #75 -: Февруари 16, 2020, 11:26:34 pm »
Разбира се, ако гоним точност, а не някакво демо за уау!

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #76 -: Февруари 17, 2020, 09:40:28 am »
Много песимистично, колеги, много песимистично?
Като ви слуша човек, направо да се откаже от кахър, колко ресурс дърпал един синус от Ардуино и една такава библиотека.
Не съм имал поставена задача в живота си, да не е била изпълнена, а тази е поставена много отдавна, просто си чакаше реда! Гема си ям, нерешена не я оставям! - ммммм...ммммъъъ ;)
На кой му пука, колко щяло да се натовари това Ардуино и какъв ресур щяло да глътне, за какво ми е ресурс в ресто, нека му използвам целия ресурс - да го ева...

Все още не съм правил опити с GPS-а, защото довършвам вчерашната работа, която не оставям за утре! - всичко по реда си.
Има стотици любителски разработки на автопилоти с Ардуино, видно не им е свършил ресурса. А ако се появи такъв проблем, първо има STM32, второ може да се добави още едно Ардуино или да се ползва дуе. Дуото е пре-пре-пре достатъчно не за един, а за няколко такива проекта в паралел.

Гледах в някакъв клип, че библиотеката TinyGPS++ заема около 70 % от паметта на Ардуино. Има обаче и други библиотеки с по-малко плавя, както казва juliang и да речем там процента ще е 35 %. От цялата тая плява на мен ще ми трябват само един параметър - азимут на отсечка. Другите параметри: разстояние, скорост, мога да добавя, като глезотия-гъзария в проекта, да изписва от време на време на монитора.   ;)
Имам да управлявам само един серво-рул за маршрутизатора. Той няма да ми отнеме повече от 10%, дори 5%. За котвата нещата ще са малко по-сложни и там ще ми трябва малко повече памет, но да речем примерно не повече от 30% от капацитета на Ардуино.
Нещата се правят последователно и накрая се сглобяват. Ще се види, ако не стига, ще пробвам на STM32, макар и да не съм работил с тоя контролер, защото първо е 3,3V логика, мисля само 4-5 пина имаше на 5V, после съм чувал, не работят с него всички ардуински библиотеки.

До 10-тина дни ще ви покажа нагледно, колко ресурс дърпа TinyGPS++ .

Иначе, ако някой иска да помогне, в предния ми пост съм дал линк с макроси на алгоритъма - формулата на Хаверсин, ако някой ги разбира, няма работа, чуди се какво да прави, човърка му се мозъка, може да преобразува формулата на ардуински език. В него няма функции аркосинус, аркокосинус, аркотангенс и аркокотангенс, то и аз не ги знаех тия понятия до оня ден, но мисля ,че е реципрочната стойност на функциите и могат лесно да се представят с деление на 1-ца.
Примерно:
tg= a/b става arqtg=1/tg

« Последна редакция: Февруари 17, 2020, 12:04:58 pm от EDM electronics »

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #77 -: Февруари 17, 2020, 10:07:18 am »
 8) Няма лошо, само практиката ще каже това да, това - не.
Аз обаче бих изпървом потърсил нещо миниатюрно неардуинско.
Имаше едни мини РС-та на една платка, май даже орязан ОС можеше да поеме.
Например това: https://itsfoss.com/linux-based-mini-pc/ ( 11 Mini PCs That Come With Linux Pre-installed )

И не мислиш за ограничения.... Програмираш и на високо ниво, вероятността за грешки в кода нямалява.  8)

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #78 -: Февруари 17, 2020, 11:12:16 am »
VITAN, караш ме да ходя за врабчета не с прашка, а с балистична ракета "Сармат" с 16 бойни глави.
Не съм виждал някой, да ползва микрокомпютър вместо микроконтролер.
Това да вкараш цял декстоп компютър за толкова елементарно решение.....не е сериозно, не е оправдано от всички гледни точки.

Моля Ви, давайте смислени и адекватни постове, не просто да има чесане не езика.

Колегата dimitar с два съвсем кратки поста допринесе най-много за темата. Даде и алгоритъма на автопилота, и формулата на GPS-азимута. Кратко, точно, ясно.

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #79 -: Февруари 17, 2020, 12:23:18 pm »
Това е сериозен проект, който или го почвам като помощ или не.
И иска много време. Не случайно програмисти са отказали.
(Нямам толкова ресурс като едно време - тогава имаше с дни да се чудя какво да го правя, но тия ми години минаха....)

GPS в телефоните се появи, когато те станаха достатъчно мощни.
Едно ардуино е като https://en.wikipedia.org/wiki/Nokia_3310 Nokia 3310.
Вярно ще излезе по-слъпо, но със сигурност до вярното решение ще се стигне по-бързо! Нещата ще са доста по "plug and play".

А, ако тръгне всички и има свободен изчислителен ресурс, тогава се мисли за поевтиняване - съответно за някаква малка серия.
Не го намирам за чесане на клавиатура. Това е, другото е конкретика, за което нямам време, не че не ми е интересно. Само опресняването на математиката
{да не стигаме до Линейна Алгебра и Аналитична геометрия (сааакън!) ще ми отнеме седмица}.

Друго нямам, успех  8)

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #80 -: Февруари 17, 2020, 12:46:17 pm »
Извинявай, последно.
А защо не вземеш да програмираш тая пущина със смартфон?
Имаш OS, GPS, USB.... ресурс.
Смартфон на старо, нова батерия, писане на апликация за Андроид.
Вярно не е каквото искаш, но погледни и от този ъгъл.

With MIT App Inventor,
anyone can build apps with global impact

https://appinventor.mit.edu/

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #81 -: Февруари 17, 2020, 01:46:15 pm »
Извинявай, последно.
А защо не вземеш да програмираш тая пущина със смартфон?
Имаш OS, GPS, USB.... ресурс.
Смартфон на старо, нова батерия, писане на апликация за Андроид.
Вярно не е каквото искаш, но погледни и от този ъгъл.

Ами основната причина за такова решение, което дори на СЪН не би ми дошло наум е, че смартфона няма множество входове и изходи, необходими за свързването на всички датчици и управление на  изходите. И защо да правя тези абсурдизми, като едно Ардуино би ми свършило работа?

Отделно ценността на Ардуино е, че има голям набор от библиотеки и скечове почти по всякаква любителска тема. Спестява се куп време. Това му е ценното на Ардуино. Без тези библиотеки задачата се усложнява значително много. Да го кажа на по-прост език:
Примерно ми трябва някаква програма за компютъра, има я в Интернет готова и безплатна, но аз да съм толкова твърдоглав, да искам да вървя по трънливия път, като патологичен мъченик, да не търся на хляба мекото и работата лекото, да седна сега да уча декстоп рограмиране, за да си направя моя програма, дето никак няма да работи по-добре от тая утвърдената - безплатната, щото ще е пълна с недостатъци, които ще искат отстраняване с времето - ето това ме караш да правя.

Друг пример:
Представи си, купиш си телевизор от Технополис и от него стърчат, като тестикули няколко стари GSM-а външно или с допълнителна издатина на кутията, като багажник на мотоциклет, че "по-практично" тях да ползват корейците и японците, вместо контролери. Представяш ли си какъв ташак ще е? Подобно нещо никъде не съм виждал, дори чувал!

Или представи си вместо контролер, както ти предлагаш, да се ползва компютър, който е с размер 100 пъти по-голяма от проектираното устройство и 1300 пъти от микроконтролера, но пък има нужда от 40 входа и 10 изхода. От де ще ги извадиш тия пинове от компютъра и кой ще го носи това желязо и с какво ще го захранва? Не ми се мисли и колко километра кабели ще стърчат!

Дано вече е станало ясно!

« Последна редакция: Февруари 17, 2020, 02:17:57 pm от EDM electronics »

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #82 -: Февруари 17, 2020, 02:21:04 pm »
Ти си пишеш всичко от нула, вземаш нещо на готово, само ако го разбираш на 100% и му вярваш.
Никога не си правя илюзии от готовите решения.
Смартфона не е голям, с него до работещо решение ще се стигне много по-бързо, особено при дебъгването.

Никак не настоявам на решението си, нито да съм прав. От опита си с програмиране >> проекти с Microchip и Arduino -
интуицията ми подсказва, че ще е зор. Ще иска сметки с числа с двойна точност (double). Не, че не е най-елегантно с Ардуино, но е най-бавно и трудоемко.

Хайде, забравяме. Няма какво повече. Ако искаш, тук има големи спецове: http://mcu-bg.com/mcu_site/ { даже гурута  8) }
Сигурно ги знаеш, но все пак.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #83 -: Февруари 17, 2020, 02:30:10 pm »
Ти си пишеш всичко от нула, вземаш нещо на готово, само ако го разбираш на 100% и му вярваш.
Никога не си правя илюзии от готовите решения.
Смартфона не е голям, с него до работещо решение ще се стигне много по-бързо, особено при дебъгването.

Никак не настоявам на решението си, нито да съм прав. От опита си с програмиране >> проекти с Microchip и Arduino -
интуицията ми подсказва, че ще е зор. Не, че не е най-елегантно с Ардуино, но е най-бавно и трудоемко.

Хайде, забравяме. Няма какво повече. Ако искаш, тук има по-големи спецове: http://mcu-bg.com/mcu_site/ { даже гурута  8) }
Сигурно ги знаеш, но все пак.

Колега, не съм търсил помощ нито за програмиране, нито за избор на контролер, нито за електрониката, нито "знаещи" програмисти, защото съм си САМОДОСТАТЪЧЕН!
Моля ви се, не ми предлагайте помощ, която не съм ви искал!
Ме ми трябва да ме впечатлявате с "гурута", те не ме интересуват.
Моля ви, контактувайте си там с вашите гурута и не ме занимавайте с тях, много моля?


Исках само да обсъдим АЛГОРИТЪМ. Темата се изчерпа с поста на dimitarp.
Оттук насетне, ако някой иска с нещо да допринесе, по-горе бях писал. Има готов код, който чака да се преобразува в ардуински.
Но понеже съм сигурен, че такъв многознайко няма да се намери, просто мисля темата е изчерпана.
« Последна редакция: Февруари 17, 2020, 02:51:14 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #84 -: Февруари 17, 2020, 02:37:01 pm »
И понеже се ядосах от много дразнещи постове, отдавна исках да ви кажа една българска, по-точно сливенска поговорка, дето не я знаете, която много пасва на форумното общуване и е време да стане публично достояние:

PUTKA на площад не се EBE!
Да ви кажа ли защо?
Защото се натрупва един народ, заобгражда интересната обстановка тоя народ и от всички страни и посоки започват да се дават акъли:
- Не така бе, не така, не се прави така, махни се аз да ти покажа как стават тия работи, и то от хора дето не са си намерили PUTKA, ами я карат предимно на коляно-мотовилков механизъм.  :D

Направо съжалявам, че въобще пуснах тая тема, защото и без това сам щях да се справя.
Достатъчно беше да попитам на руски и английски в Гугал и Ю-Туб и щяха да ми излязат цял куп предложения.

Неактивен van

  • Стабилен
  • ****
  • Публикации: 536
Re: GPS-автопилот - алгоритъм
« Отговор #85 -: Февруари 17, 2020, 07:17:27 pm »
Тъй като не съм барал П*тkа опс ГПС, явно знам най-много, да си кажа: най ми хареса алгоритма на Юли, където всеки градус се умножава по една константа ако вземем радиуса на земното кълбо Rземя=6378.137km и разделим на 360 градуса за ширината Х и 360 за дължината У и получаваме двете константи умножаваме геолокацията си като минутите са градуси делено на 60 а секундите са минути делени на 60 и така стигаме до две числа ъгли за Х и Y в градуси с двойна точност, които умножаваме по двете константи една за Х и друга за У и получаваме абсолютни координати в правоъгълна координатна система Х, У и остава да вземем на новата точка следваща да ги преобразуваме пак като ги превърнем от градуси минути секунди и стотни от секунди пак в градуси и умножим по съответните константи и получим новите X,Y координати после е въпрос на проста тригонометрия да се изчислява в правоъгълна координатна система положението ни в един правоъгълник и да се насочваме към желаната точка като координатите от GPS -а ъглови ги претаковаме в абсолютни Х,У..все едно се движим по X,Y - плоскост /равнина/ (Плоската Земя). /може да се направи проста програмка (скриптче), да се изкарват от GPS Конвертираните данни, да се види има ли смисъл така да се смята земята за плоска./
Пък ако не ви кефи плоската формула за плоската земя, винаги можете да си ползвате познатата формула обяснена в https://en.wikipedia.org/wiki/Haversine_formula накрая има 91 + + нейни имплементации във външните линкове.

Легенда: *-е
« Последна редакция: Февруари 17, 2020, 07:44:17 pm от van »

Активен dmitarp

  • Стабилен
  • ****
  • Публикации: 783
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #86 -: Февруари 17, 2020, 07:40:16 pm »
Така или иначе си я пуснал темата, да добавя малко и аз, защото  на мен ми е интересна. Първо малко относно точността на изчисленията. Единственият линеен размер в сферичните координати е радиуса на земята r = 6371300 m, някъде съм го срещал и като r = 6378100 m. Която и цифра да се вземи грешката е в рамките на сантиметри за разстояния до 50 метра. Другите координати са ъглови, които се получават от GPS, т.е. северната ширина и дължината. Най-краткото разстояние между две точки, винаги минава през голямата окръжност на сферата. Дължината на дъгата по голямата окръжност (т.е. разстоянието между двете точки) е равна на d = r*phi, и сега за да имаме точност от порядъка на един метър ъгълът трябва да бъде отчетен с точност около 1/r=1x10-5 в градуси или 1.6x10-7 в радиани. Тези числа са на границата на точност на 32 битово число с плаваща запетая (float), според мен е добре да се работи с 64 битово число (double). Всякакъв опит да се направи библиотека, която може да използва 16 битово число, с цел пестене на време е безсмислена, защото това ще доведе до сериозни грешки.
Сега малко за функцията haversine, която се използва за намиране на разстоянието между две точки. Haversine не е име на човек, а име на тригонометрична функция и затова е неправилно да се казва формула на haversine. Тази формула може да се опрости като се знае, че при малки разстояния разликата в ъгловите координати е малка и, че sin (θ) = θ, за малки ъгли тогава формулата става:

d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2))

Може да се въведе и още едно опростяване свързано с косинусите от близки ъгли, което пак важи за малки разстояния ( тук под малки разстояние разбирам от порядъка 20, до 30 км), cos(phi1) * cos(phi2) = (cos((phi2 + phi1)/2)^2. Тогава формулата ще изглежда така:

d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2)).

Вижда се, че от 5 тригонометрични функции остава само една + още един квадрат.
За да се разбере предимството на опростяването съм направил няколко опита с различни контролери в Адруино среда

Първата група опити е свързана с разстояние 50.4 м, ширината на един магазин с координати в първата точка с.ш. = 43.418968, и.д. = 24.626770, и втора точка с.ш. = 43.419409, и.д.=24.626914.

Контролер          Време, [μs]          Разстояние, [m]
                        1          2          3          1          2          3
Due                  194      103       73         50.4    50.4      50.4
Teensy 3.6        61        31         22         50.54  50.54    50.54
Uno                  608      356       256       51.08  51.08    51.08
STM32F407ZET 67        34        25         50.4    50.4     50.4

Тук с едно 1, 2 и 3 е означено формулата която се използва, 1 е пълната формула, 2 е след първото опростяване и 3 след второто.
Няма никаква разлика във времената, ако се използва (float) вместо (double),освен при STM32F407ZET, където времената намаляват почти двойно, с изключение на третата формула, където времето остава не променено. Трябва да се има в предвид, че STM32F407ZET и Teensy 3.6 имат вградено хардуерно 32 битово FPU (float point unit).

Втората група опити е свързана с разстояние 15.79 м, ширината на една сграда с координати в първата точка с.ш. = 43.411376, и.д. = 24.627289, и втора точка с.ш. = 43.411325, и.д.=24.627470.

Контролер          Време, [μs]          Разстояние, [m]
                        1          2          3          1          2          3
Due                  192      99        70         15.68  15.68    15.68
Teensy 3.6        58        30         21        15.8    15.8      15.8
Uno                  600      352       256       15.82  15.82    15.82
STM32F407ZET 67        33        24         15.68   15.68    15.68

Координатите, както и разстоянията са взети от google map, нямам GPS модул.
Опростяването на формулата не води до загуба на точност, а времето за изчисление се намалява повече от два пъти.
От опитите се вижда, че Uno се справя много добре със задачата, изчислението отнема половин мили секунда, което означава, че до 10 милисекунди може да направиш още 15, 20 подобни изчисления. А 100 хертца sample rate е съвсем достатъчно за една лодка, дори и 50 хертца биха свършили работата. Мисля, че Uno като параметри и скорост би се справил с един автопилот, какъвто се търси.
Накрая искам да призова тези, които твърдят, че има по-лесна формула нека да я демонстрират, така както и аз, ще ми бъде много полезна и не само на мен.
Ето и кода ако има някой забележки относно измерването на времето, нека допълни, може и да не съм прав:
#define pi 3.14159265359
#define r  6371300  // radius

// coordinates of points in degree
// point 1
double phi1 = 43.411376;
double l1   = 24.627289;

// point 2
double phi2 = 43.411325;
double l2   = 24.627470;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  // coordinates of points in radians:
  // point 1
  phi1 = phi1 * pi / 180;
  l1   = l1 * pi / 180;

  // point 2
  phi2 = phi2 * pi / 180;
  l2   = l2 * pi / 180;
}

void loop() {
  double time1;
  time1 = micros();
  // distance between points full formula
  double  d = 2 * r * asin(sqrt(pow(sin((phi2 - phi1) / 2), 2) + cos(phi1) * cos(phi2) * pow(sin((l2 - l1) / 2), 2)));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();
 
  time1 = micros();
  // distance between points first approximation
  d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();

  time1 = micros();
  //distance between points second approximation
  d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();
 
  delay(5000);
}
« Последна редакция: Февруари 17, 2020, 08:05:47 pm от dmitarp »

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 254
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #87 -: Февруари 17, 2020, 08:12:06 pm »
dmitarp
Поздравления за изследването!
Пълно и точно!
Професионално!

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #88 -: Февруари 17, 2020, 08:51:07 pm »
dmitarp, златен си.  :D
Вери импортант персона в автопилотите.  :D 
Благодаря много!

Много работа си свършил, чак се чувствам задължен.
Ще изпробвам скеча ти, също с примери от Гугал мапс. Щом работи с примери от Гугал, от това по-показателно здраве му кажи.
Да опростяването на формулата не дава забележима грешка. По принцип не е нужна голяма точност. Плюс-минус 1 м в голямото море няма никакво значение.

Обаче в ардуинската среда най-голямото число с плаваща запетая е 32 бита  - float. double е същото число float. Затова и измерванията ти не показват разлика с промяната му.
Щом Ардуиното може да направи изчисления със скорост 100 Хц това е предостатъчно, като се има предвид, че много добър GPS приемник работи на 10 Хц. И 5 Хц са достатъчни.

Интересно ми е как мериш бързодействието и ако се напълни паметта с останалата част от програмата, дали ще изчислява със същата скорост?
От тази формула може ли да се изведе азимута на отсечката?

Активен dmitarp

  • Стабилен
  • ****
  • Публикации: 783
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #89 -: Февруари 17, 2020, 09:00:22 pm »
Формулите за азимута ти ги дадох по напред:
β = atan2(X,Y)

X = cos θb * sin ∆L

Y = cos θa * sin θb – sin θa * cos θb * cos ∆L.

Тук θa и θb са ширините на точките a  и b съответно, а ∆L е разликата между дължините.

Ако две точки имат еднакви ширини, ягълът е 90 градуса, а ако имат еднакви дължини, ъгълът е 0 градуса
Но тук може да се направи опростяване, но малко и тази формула ще отнеме повече време. Ардуиното използва стандартен С, всички тригонометрични функции, ги има.
X = cos θb * ∆L

Y = cos θa * sin θb – sin θa * cos θb.

Но както спомена по-рано на магнитометъра трудно може да се разчита, това е голям проблем, имам няколко и въпреки калибрирането дават голямо отклонение. Исках да изпробвам на Сименс един, но така и не го купих, на него по може да се вярва, но не знам BNO055