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

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #135 -: Февруари 28, 2020, 03:47:14 pm »
juliang, написах ти кода малко по-четимо, че с тия дълги редове на променливите се бърках.
Направих нещо подобно, но има проблем, все още не работи. Ще го направя, но целта ми е да стане по-най-краткия и сбит код, затова реших да пробвам и твоята идея, макар че това което направих е почти същото.

Там където е в червено, в първата функция е първата точка от маршрута с адрес 0. Следващата червена е променливата от брояча на цикъла. Правилен ли е този запис? Т.е. първата функция distance ( Point, Route_1 [ 0 ] ) е да намери число d_min за  d_cur < d_min, за да тръгне да работи, след това започва да сравнява.
Записа Point_min = i обаче ще показва винаги докъде е стигнал брояча, а не номера на най-малката точка.

Хвърли един поглед и ако може коригирай.


byte Route_1 [ 60 ];     // маршрут от 60 запаметени точки
byte Point = 0;            // текуща точката от маршрута
byte Point_min = 0;    // точката от маршрута с най-кратко разстояние
word d_min = 0;        // най-краткото разстояние
word d_cur = 0;         // текущо разстояние

d_min = distance ( Point, Route_1 [ 0 ] );    // функция за дистанцията от първата               
                                                                                   точка в маршрута
for ( int i = 0; i < 60; i++) {                      // цикъл
           
d_cur = distance ( Point, Route_1 [ i ] );     // функция за текущата дистанция сменя
                                                                                  се от цикъла
if ( d_cur  < d_min ) {                               // условие, ако текущата дистанция е по-
                                                                               малка от най-малката досега
d_min = d_cur;                                         // минималната е равна на текущата дист.

Point_min = i;                                           // точката на минималната дистанция
     
     }
}
« Последна редакция: Февруари 28, 2020, 04:02:27 pm от EDM electronics »

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 254
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #136 -: Февруари 28, 2020, 07:19:53 pm »
EDM electronics
Помисли как ще въвеждаш точките. И като въвеждаш няма винаги да са 60!
От там и ограничението на проверката в FOR.
Ползвай while до края на въведения масив.
Записвай в еепром колко са броя на въведените точки.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #137 -: Февруари 28, 2020, 08:08:23 pm »
EDM electronics
Помисли как ще въвеждаш точките. И като въвеждаш няма винаги да са 60!
От там и ограничението на проверката в FOR.
Ползвай while до края на въведения масив.
Записвай в еепром колко са броя на въведените точки.
valex да, ще ползвам while - прекъснат цикъл, защото променливата му е външна а точно нея смятам да ползвам при запускане на цикъла. for е непрекъснат цикъл и ползвам просто за пробата.
Маршрутите ще са с 3 отделни променливи и с въвеждане на точките в паметта ще се увеличава стойността на съответния маршрут ++. После тази стойност също отива в епрома. Същата стойност ще се ползва и за условието на цикъла. Ще се ползва и за началото на адресите на паметта за следващия маршрут. Така ще мога да ползвам оптимално цялата памет, като всеки маршрут ще започва веднага след предходния без да губя клетки от паметта. Всичко това съм направил. Остана функцията за определяне на най-късото разстояние от точките на избрания маршрут и текущата позиция.

Мога да я направя по няколко начина, но търся най-краткия запис.
« Последна редакция: Февруари 28, 2020, 08:42:36 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #138 -: Февруари 28, 2020, 09:21:18 pm »
Ето как работи скеча за намиране на най-късото разстояние на точка от маршрута до текущата позиция:
Нулевата точка всъщност е един такт за запис на първата точка от маршрута, която после влиза в условието за сравнение със следващата, така, че тя не влиза в сметката. Броенето на точките започва от 1-ца. Засега определям най-кратката точка, но мисля по въпроса как да намеря и точката. Точките в 5-те примера са в метри. Това е запис от порт-монитора.

dist.  424.86  d_min  424.86  point 0
dist.  424.86  d_min  424.86  point 1
dist.  396.27  d_min  396.27  point 2
dist.  809.99  d_min  396.27  point 3
dist.  1085.41  d_min  396.27  point 4
dist.  2133.37  d_min  396.27  point 5

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #139 -: Февруари 28, 2020, 09:47:20 pm »
Алгоритъма е готов изцяло. Вече намира най-малката точка и адреса й - точката от маршрута. Допуснал съм очевидна грешка, чак сега я видях, затова не се получаваха нещата. Втората точка от маршрута от 5 точки е най-късото разстояние.

dist.  424.86  d_min  424.86  point 0  smal_point 0
dist.  424.86  d_min  424.86  point 1  smal_point 0
dist.  396.27  d_min  396.27  point 2  smal_point 2
dist.  809.99  d_min  396.27  point 3  smal_point 2
dist.  1085.41  d_min  396.27  point 4  smal_point 2
dist.  2133.37  d_min  396.27  point 5  smal_point 2

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 2 774
Re: GPS-автопилот - алгоритъм
« Отговор #140 -: Март 01, 2020, 11:58:20 am »
Да, това ми е идеята, само можеш да почнеш цикъла от втора точка - първа вече си я сметнал извън него :). То се вижда и как повтаря резултата ..

for ( int i = 1; i < 60; i++) { 

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #141 -: Март 01, 2020, 03:10:11 pm »
Да, това ми е идеята, само можеш да почнеш цикъла от втора точка - първа вече си я сметнал извън него :). То се вижда и как повтаря резултата ..

for ( int i = 1; i < 60; i++) {
juliang , вече го направих, работи идеално.
Изглежда така:
int j = 0;
while ( j < Route_1) {
    addressW2 = j * 8;               
    addressL2 = addressW2 + 4;
    if (d_min != 0) j++;
    EEPROM.get(addressW2, W2);
    EEPROM.get(addressL2, L2);
   
    // point 2
    W2 = W2 * pi / 180;
    L2 = L2 * pi / 180;
   
    d = 2 * r * sqrt(pow((W2 - W1) / 2, 2) + pow(cos((W2 + W1)/2), 2) * pow((L2 - L1) / 2, 2));
    if ( j == 0) d_min = d;
    if ( d < d_min ) {
    d_min = d;
    cPoint = j;
   }
}

Записът  if (d_min != 0) j++; кара цикъла да не брои, докато не се зареди първото уравнение d_min = d;. След това започва отново броене от нула и до края на цикъла. В предния пост е видно, нулата на j се повтаря два пъти заради първоначалното зареждане на стойност за сравнение на if ( d < d_min ) .

Продължавам нататък.
« Последна редакция: Март 01, 2020, 04:47:30 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #142 -: Март 15, 2020, 05:08:06 pm »
Доста напреднах с автопилота, почти е готов скеча. Остана ми само алгоритъма за котвата, но не съм го мислил още. Чакам и да ми пристигне компаса - датчика за абсолютна ориентация. Всичко останало - магнитометри не става. Само магнитометър за нищо не става, защото комбинирания датчик има компенсация от жироскопа при наклон, не се мени ъгъла на компаса и няма никакъв дрейф, кове.

За GPS използвах една стара библиотека и заема много малко ресурс, защото на мен ми трябват само 3-4 параметъра от приемника. Така мисля, че ще успея да кача абсолютно целия проект в едно Ардуино Нано. Но ще го дублирам и в STM32. Проблема с този процесор е, че няма апаратно ЕЕПРОМ, а трябва да се емулира от флаша. Не съм правил още такава гимнастика, но е голяма боза и после по-трудно се работи с ардуинската библиотека, става сложно.

Скеча досега е от 850 реда без библиотеките и заема около 70-80% от паметта. Ако не се събере, защото още не съм включвал компаса, може двата датчика да отидат на отделен контролер, т.е. да стане с два. Ако е само автопилота ще се събере на един, но с котвата е под въпрос.

Направих около 15 менюта, които се избират и програмират с многофункционален енкодер. Но той може да се замени и с 4 бутона.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #143 -: Март 15, 2020, 05:10:06 pm »
,

Неактивен VITAN

  • Подготвен
  • **
  • Публикации: 184
Re: GPS-автопилот - алгоритъм
« Отговор #144 -: Март 15, 2020, 08:13:23 pm »
Браво, браво  8)
Е, с 2 ардуино-та ще стане  :)

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #145 -: Март 21, 2020, 02:22:24 pm »
Сблъсках се със следния проблем:
Счетох, че вътрешната памет на Ардуино НАНО е твърде малка за запис на маршрути и поставих външна 24CL128. Това е огромна памет за целта.
Проблема обаче се състои в това, че езикът IDE /компилатора най-вече/ възприема най-голямо число с променлива запетая тип float. То е с големина 4 байта. Има и double, но то е същото float.
При смятане с такива числа или запис в паметта точността се ограничава до 7 разряда, независимо от запетаята. Т.е. точността на 4235672.3 е същата като 42.356723. Последната цифра /осмия разряд/ варира с 2-3 стойности.

Някой от програмистите, имате ли идея как да го направя с абсолютна точност до 8 разряд?

Пробвах при запис с умножение по сто, а при четене с деление на 100, но не става, запазва се точността до 7 разряд, осмия вече варира.

Записа правя, като записвам 4 отделни байта с разбиване на числото float на 4, а при четене се четат отделните байтове и се сумира в тип float . Ползвам две отделни функции за запис и четене.

#include <Eeprom24C128_256.h>
#define EEPROM_ADDRESS  0x50
static Eeprom24C128_256 eeprom(EEPROM_ADDRESS);

void setup()
{
  float  val_1 = 42.899751;
  float  val_2 = 0;
  Serial.begin(9600);
  eeprom.initialize();
 
  EEPROM_float_write(0, val_1);
  for (int ii = 0; ii<4; ii=ii+4)
  {
    val_2 = EEPROM_float_read(ii);

  // запис в серниния порт
 
  Serial.println(val_2,6);
  }
 
}

void loop(){}


void EEPROM_float_write (int addr, float  val) // запис в ЕЕПРОМ

  byte *x = (byte *)&val;
  for(byte i = 0; i < 4; i++)
  {
    eeprom.writeByte(i+addr, x);
    delay(10);
  }
}

float  EEPROM_float_read (int addr) // четене от ЕЕПРОМ
{   
  byte x[4];
  for(byte i = 0; i < 4; i++) x = eeprom.readByte(i+addr);
  float  *y = (float *)&x;
  delay(10);
  return y[0];
}

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 2 774
Re: GPS-автопилот - алгоритъм
« Отговор #146 -: Март 21, 2020, 06:12:08 pm »
Ако работиш с числа с плаваща запетая нещата вероятно ще са малко по-добри, но трябва да внимаваш изключително много при преминаването от абсолютни географски координати към разстояния. Не е изключено катастрофално да загубваш точност, защото ардуиното си работи с числа със 7-8 значещи цифри.

Аз като говоря не приказвам...
Решението е да вземеш една сравнително близка точка за "нула" и да работиш с цели числа спрямо нея. 4 байта са +/- 2 милиарда, което ако работиш в сантиметри са +/- 20 000 километра. Така че ако смяташ в цели числа дори и ако приемеш нулата за нула градуса северна и нула градуса източна... пак България ще влезе в обхвата. А ако хванеш една точка в България, или примерно 45 градуса северна и 45 градуса източна, то ще можеш да обхванеш цяла България дори и в милиметри (което е абсолютно безмислено).

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

НЕ МОЖЕШ да работиш с 8-я разряд, защото сметките ти ще са грешни. Дробните числа се представят като а*1/2 + b*1/4 + c*1/8 + d*1/16 + .... (а, b, c, d.... са битове и са 0 или 1) и просто няма как да запишеш точно 1/3 или 1/7. Затова вътрешно се смята с 8 знака точност, но ти се дава само до 7-я, който със сигурност е точен, а 8-я се спестява защото не винаги отговаря на истината.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #147 -: Март 21, 2020, 07:33:28 pm »
Ползвам формулата с тангенси, котангенси и т.н. предимно защото само с нея мога да реализирам котвата. Не че твоята идея не е добра, напротив. Дори съм решил, скоро като завърша проекта, да направя и твоя алгоритъм, винаги мога да направя отделен скеч, като сменя само алгоритъма на навигацията, друго се запазва.

Седем разряда са напълно достатъчни за точността, но аз малко прекалявам и се престаравам. Осмия разряд от координатите от 0-9 дава толеранс 1 м. Точността ми в този разряд бяга с до 3 единици или 30 см. Това говоря при запис на координатите в паметта. Такава точност при следване на маршрут не ми е нужна и бих казал практически невъзможна, защото има много други фактори, които ще пречат на такава точност, главния от които е GPS-приемника.

При него 8-ми разряд варира с до 10-15 единици и ако няма хубав сигнал дори повече - играе си там постоянно се мени. Мислих да му слагам медиантен филтър и може би ще му сложа, защото този филтър елиминира отскоците и не дава средна средна аритметична стойност, като сбор от някщолко, а отделя средното показание от няколкото, колкото задам - примерно 5-6, ако настроя GPS-a на 5-6 Hz опресняване.

Та с тая любителска техника не мога да постигна кой знае какво. Има много добри приемници, но са скъпи и у нас трудно се намират, а и не виждам чак такъв смисъл, да гоня сантиметри точност. Дори в тази връзка съм предвидил умишлено намаляване на точността с около 2 м. Т.е. при наближаване на отправната точка, ако разчитам на навигатора да мина през нея, елеминирайки вълни, вятър и т.н., няма да се случи често и ще подмина точката, тогава навигатора ще дава команда назад, кръгове и т.н., което е недопустимо. Зетова в скеча съм задал условие, ако текущото разстояние е по-малко или равно на 2 метра, се зарежда следващата запеметена точка от маршрута за следване. Два метра това е радиус, т.е. трябва да улуча кръг с размер 4 м, изпусна ли го почва кръжене.  ;)

Иначе има софтуерно решение на проблема с точността на 8-мия разряд, но все още не съм го осмислил. Функция, която свежда грешката на 8-мия разряд до 9.5, т.е. остава погрешност само 0,5 или това е да го наречем 5 см. Ако имаш интерес погледни и коментирай:

http://arduino.ru/forum/programmirovanie/etyudy-dlya-nachinayushchikh-tip-float-kak-ne-utonut#comment-341969

//   Возведение числа f в целую, неотрицательную степень n
//
float iPow(const float f, const int n) {
   if (n <= 0) return 1;
   if (n == 1) return f;
   float f2 = iPow(f, n / 2);
   f2 *= f2;
   return (n & 1) ? f2 * f : f2; // если n - нечётно, надо ещё раз домножить на f
}

Неактивен nizo

  • Стабилен
  • ****
  • Публикации: 734
  • Thoughts become things!
Re: GPS-автопилот - алгоритъм
« Отговор #148 -: Март 25, 2020, 09:53:36 pm »
EDM, ако отвориш документацията:

https://www.arduino.cc/reference/en/language/variables/data-types/double/

пише, че double е обикновенно 4 байта, но Arduino Due има 8 байтов double с такава конструкция.


Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #149 -: Март 25, 2020, 11:15:06 pm »
Аз мисля, че това зависи от компилатора. Т.е. така се компилира, че едно 4-байтово число се чете на 4 такта по 1 байт. Double трябва да го чете на 8 такта на генератора, защото процесора е 8-битов. Ардуино Дуо или сега съм поръчал Мега са също 8-битови. Ама щом казват, може и да е така, не съм задълбавал.

Като правя пробите с GPS-а, последните две цифри от координатите на числото float с точност до 8 знака или 6 след запетаята, та тия 5-ти и 6-ти ми дават сантиметрите 0-99.
Направих друга функция за запис във външна EEPROM на число float на "С" и ми дава голяма точност, закръгля в рамките на 1-ца. Това ще рече 1 см точност - напълно ненужно е да търся повече. А загубих доста време, докато направя коректен запис и четене в паметта. Сега е ОК.
GPS-а и той играе леко, точността има дрейф от порядъка на 20-30 см. Ако някой прояви интерес, ще публикувам функциите - запис и четене.

Неточността обаче ще се появи не от електрониката, защото при мен тя е без компромис, а от това че GPS-навигацията за граждански цели се прави умишлено с код-дрейф на времената на приемане, за да варира постоянно точността, за де не се ползва за военни цели или терористични актове в рамките на 2-10 м. Т.е. запомням днес една точа, а утре тя е на 10 м вляво, вдругиден е на 7 м северозапад. Затова и има специални точни геодезични GPS-приемници, но те се водят на отчет, там няма дрейф.

Та малко дълго стана, но float е пре-пре-пре достатъчен за моя проект. Наното го запълних на 80% и не знам дали ще ми стигне памет. Имам още работа по компаса. Най-много памет ми отне дисплея, имам 25 екран-менюта.