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

Активен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 319
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #120 -: Февруари 23, 2020, 03:10:39 pm »
За по-мощни неща с Arduino  може да се ползват платки NUCLEO
https://www.digikey.bg/products/en/development-boards-kits-programmers/evaluation-boards-embedded-mcu-dsp/786?k=NUCLEO
Имат пинове съвместими с Arduino и допълнителни. Подържат се от софтуера.
https://www.st.com/resource/en/data_brief/nucleo-f429zi.pdf
Ползвал съм NUCLEO F429ZI за проби.
А има и такива Arduino вършачки малко по-скъпи :(
https://www.udoo.org/udoo-x86/

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 731
Re: GPS-автопилот - алгоритъм
« Отговор #121 -: Февруари 23, 2020, 04:12:21 pm »
НАНО-то удовлетворява на 99% всички любителски проекти, програмирането е супер лесно. Ако говорим за професионална работа, то ардуино няма място - горе видеото показва защо.

Вчера си направих експеримент да ускоря ардуиното с обикновен блинк. Замених функциите на digitalWrite(HIGH)  digitalWrite(LOW) с директен запис на изхода на порта PORTB |= (1<<7);  PORTB &= ~(1<<7); и скоростта нарасна 2 пъти - от 8 uS на функция стана 4 uS, а записа в паметта намаля също 2 пъти.

Има библиотеки с алтернативни основни функции на Ардуино, които записи за идентични на "С", но са лесно четими, като ардуинските. Ускорението с тях е от 2 пъти нагоре и толкова по-малко памет.
Подобни гимнастики обаче не са нужни в 99% от случаите. Но използвайки такава библиотека реално може да се удвои най-малко флаш пространството и бързината на всяко ардуино.

Ако ми затрябва нещо по-мощно от НАНО, ще ползвам МЕГА, защото е компактно и се лепи на платка. С по-мощни проекти не бих са захванал, но и не виждам какво.
Ардуино НАНО търпи над 1200-1500 реда скеч заедно с накачулените библиотеки. Това е много нещо...

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


Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 731
Re: GPS-автопилот - алгоритъм
« Отговор #122 -: Февруари 24, 2020, 02:37:14 pm »
В момента работя над записа и четене в EEPROM.
Паметта е само 1 KB и стига за 2 маршрута и запис на 2ка точки /ширина и дължина/ с дължина на всеки запис по 32 бита или това прави 2 отделни маршрута по 60 точки. Има и опция 3-ти маршрут от 120 точки, като 1-ви и 2-ри се изтриват и не се ползват.

Изтриването направих с цикъл за всеки отделен маршрут по адреси. Записа се прави на всяка двойка точки /ширина и дължина/ с единичен клик на бутона на енкодера. Влизане в меню програмиране със задържане на бутона и излизане отново със задържане. Натиснат и въртене ляво-дясно избирам един от трите маршрута. Това е за изтриването и записа.

Четенето от първата записана точка до последната е лесно. Срещам трудност при четене, когато се намирам някъде по средата на маршрута или не в началото или края. Маршрута може да е права линия или кръг.

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

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 464
Re: GPS-автопилот - алгоритъм
« Отговор #123 -: Февруари 24, 2020, 06:10:11 pm »
Въпросът е как да намеря най-близката записана точка в паметта от текущото местонахождение?
Питагор... към него се обърни... Е, ще трябва да смяташ разстоянието от всички точки до текущата.

За ЕЕПРОМ-а имай две неща пред вид: първо записа и четенето оттам са бавни (записа много, четенето не толкова), и второ - паметта е с определен брой цикли на запис, така че избягвай да записваш в нея неща всяка секунда примерно...

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 731
Re: GPS-автопилот - алгоритъм
« Отговор #124 -: Февруари 24, 2020, 07:19:27 pm »
Питагор... към него се обърни... Е, ще трябва да смяташ разстоянието от всички точки до текущата.

За ЕЕПРОМ-а имай две неща пред вид: първо записа и четенето оттам са бавни (записа много, четенето не толкова), и второ - паметта е с определен брой цикли на запис, така че избягвай да записваш в нея неща всяка секунда примерно...
Тези неща с броя цикли на флаша, ако не се знаят, то накъде...... При мен записа е ръчен с натискане на бутона. Докато се изчерпи лимита ще минат стотици години.

Питагор не е нужен, нали dmitarp направи основното в проекта - формулите за разстоянието между две съседни точки и ъгъла на отсечката им спрямо севера - те си работят идеално и те ще смятат разстоянието.

Що се отнася до записа и четенето в ЕЕПРОМ:
Ардуино има много хубава библиотека за запис, четене и ъпдейт - т.е. запис само когато има разлика между числото в клетката и записваемото число, та се работи много лесно и удобно. Има дори отделни функции, когато става въпрос за числа със и без плаваща запетая.

Ъпдейт-записът може да са направи обаче и с функция запис под условие, ако записа не е равен на записваемото число. По подобен начин смятам да направя нещата с STM32, защото там ардуинската библиотека ЕЕПРОМ не работи, дори няма отделена такава, а трябва да се конфигурира от флаша.

Аз решение за горната задача имам, но исках да чуя и чуждото мнение. Хич не е проста задача - да прочетеш записа на точките в паметта, да ги сравниш спрямо текущата позиция и да избереш най-късата "клечка" - това да е отправната точка. И после трябва да зададеш, дали да тръгнеш наляво или надясно, т.е. да сумира точките или изважда.

В ардуино езикът има една много хубава функцийка:
sensVal = min(sensVal, 100); // проверяем если sensVal больше 100, то senseVal будет присвоено 100

Та ще се пусне един цикъл - четене от паметта на съответния запаметен маршрут, което ще започне от 1-ва точка и стигне до последна по броя записи - максимално 60. Последователно по горната функция ще отхвърля по-дългата "клечка" и остатакъ ще заема мястото на цифрата 100. После цикъла продължава, докато накрая остане най-късата клечка. Но на всеки прочит ще се следи номера на точката, а той отговаря на съответния адрес. Като знаем номера ще се дава команда да започне четенето от паметта от тази точка напред или назад, т.е. опция реверс.

Функцията min() може да бъде заменена с условие и сравнение със знак < или >  и изпълнение на условието - запис в променлива на по-близката точка.

Точният алгоритъм съм го написал на хартия в блоково изпъление, остава да го напиша в скеча, но има доста мислене.

Та ако има други предложения и ако сте разбрали какво се иска, целия съм в "слух"?

Това е скицата за запис на маршрутите, 1-вия започва от 64-и адрес до 543 и 2-рия от 544 до 1023:
// Запис на маршрут--------------------------------------------------------------

if (enc1.isClick() && flag_1 == 0 && flag_2 == 1) {      // условие

flag_1 = 0;

if (Route  == 0)  {                                                // брояч на маршрута
Route_1++;                                                          // брояч на запаметени точки
if (addressW2 >= 535) addressW2 = 535;          // ограничаваме до 543-8=535
addressW2 = 56 + Route_1 * 8;                          // променлива word address = 0;
addressL2 = addressW2 + 4;                               // ограничeно до 543
EEPROM.put (addressW2, W2);
EEPROM.put (addressL2, L2);
delay (10);
}
if (Route  == 1)  {
Route_2++;                                                        // брояч на запаметени точки
addressW2 = 536 + Route_2 * 8;               
addressL2 = addressW2 + 4;
EEPROM.put (addressW2, W2);
EEPROM.put (addressL2, L2);
delay (10);
}
if (Route  == 2)  {
Route_3++;                                                      // брояч на запаметени точки
addressW2 = 56 + Route_3 * 8;                      // променлива word address = 0;
addressL2 = addressW2 + 4;
EEPROM.put (addressW2, W2);
EEPROM.put (addressL2, L2);
delay (10);
}

}
« Последна редакция: Февруари 24, 2020, 08:00:10 pm от EDM electronics »

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 464
Re: GPS-автопилот - алгоритъм
« Отговор #125 -: Февруари 24, 2020, 08:44:11 pm »
...
В ардуино езикът има една много хубава функцийка:
sensVal = min(sensVal, 100); // проверяем если sensVal больше 100, то senseVal будет присвоено 100

Та ще се пусне един цикъл - четене от паметта на съответния запаметен маршрут, което ще започне от 1-ва точка и стигне до последна по броя записи - максимално 60. Последователно по горната функция ще отхвърля по-дългата "клечка" и остатакъ ще заема мястото на цифрата 100. После цикъла продължава, докато накрая остане най-късата клечка. Но на всеки прочит ще се следи номера на точката, а той отговаря на съответния адрес. Като знаем номера ще се дава команда да започне четенето от паметта от тази точка напред или назад, т.е. опция реверс.


пиша на нещо като си-шарп, надявам се да схванеш идеята:

arr_of_points[100]; //точки от маршрута ти
nearest_point_index = 0;
smallest_distance = distance(current_point, arr_of_points[0]); // функция за пресмятане на разстоянието
for ( i = 0, i < 100, i++)
{
     current_distance = distance(current_point, arr_of_points [ i] );
     if (current_distance < smallest_distance) // ако дистанцията е най-малката досега
          {
                smallest_distance = current_distance; / записваме си я като най-малка
                nearest_point_index = i; // и запомняме индекса на точката до която разстоянието е най-малко
          }
}

Така само с едно минаване през масива ще имаш най-малкот оразстояние и номера на точката, която е най-близо.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 731
Re: GPS-автопилот - алгоритъм
« Отговор #126 -: Февруари 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 319
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #127 -: Февруари 28, 2020, 07:19:53 pm »
EDM electronics
Помисли как ще въвеждаш точките. И като въвеждаш няма винаги да са 60!
От там и ограничението на проверката в FOR.
Ползвай while до края на въведения масив.
Записвай в еепром колко са броя на въведените точки.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 731
Re: GPS-автопилот - алгоритъм
« Отговор #128 -: Февруари 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 731
Re: GPS-автопилот - алгоритъм
« Отговор #129 -: Февруари 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 731
Re: GPS-автопилот - алгоритъм
« Отговор #130 -: Февруари 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

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

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

Неактивен EDM electronics

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

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

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

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

Неактивен EDM electronics

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