Fail-Safe - Автопилот для полетов по камере
А на входе в Си функцию как я уже говорил можно две пары координат иметь - стартовую и текущую (например прямо в тексте NMEA), а на выходе быть может - байт курса на цель (360 градусов= 0 градусов = 256 = 0) и пара байт расстояния до цели в метрах.
Хотя с параметрами функции быть может Вовик подскажет/уточнит, на что там в RCAP упор делается - на какие параметры бэйсик расчитывает и в какой они форме.
Я, собственно, так и предполагал. В текст существующей программы добавить несколько строк, которые ожидают после включения автопилота когда появяться достоверные координаты точки старта, после этого эти координаты запоминаются как точка возврата на все время полета.
Функция вычисления азимута по координатам, хорошо если бы на входе имела бы четыре символьних последовательности формата ГГ.ММ,МММ - т.е. градусы, минуты с точностью до тысячных долей долготы и широты точки возврата и текущие значения координат. По умолчанию все координаты отнесены к северной широте и восточной долготе. На выходе иметь двухбайтное слово, численно равное азимуту на точку возврата в градусах. Тогда такую отлаженную функцию можно было бы вставить в имеющийся текст программы.
Там будет еще нюанс по использованию оперативки подпрограммой расчета данной функции. Оперативка там регистровая и маленькая. Какие то регистры уже задействованы компилятором исходной программы. Впрочем - в этом я уже слабовато разбираюсь. 😊
Пробую приобрести ЕМ-406 в геопарке, обещают 2 неделю, что должны появиться, пока ждемс
Сегодня взял у них ЕМ-406
Гы… сервис у них, однако, позвонил, заказик мой уже посеяли 😦, правда обещали завтра подвезти 😃 .
Оперативка вроде в сей микрухе не только регистровая, да еще 256 байт ЕЕПРОМ, таблицу с тригонометрией туды можно запихнуть, хотя ПИКи для меня темный лес.
В текст существующей программы добавить несколько строк
Ну немного и я за себя взялся - хоть поставил ентот ПикБасицПро, подвязал его к МпЛабу и скомпилил RCAP.
Прикольно - высокий уровень языка - я уже и забыл что енто такое.
Когда для смены проца нужно лишь тип проца и поменять.
А хекс уже другой построится.
Оперативка там регистровая и маленькая.
Ну вот для ентого и прикола ради перекомпилил под пик18 - у него ресурсов гораздо поболее.
Ну немного конечно подправить пришлось асм обработчик прерывания в Servo.Inc
Но скомпилил для 18Ф4431 без ошибок сейчас - сказалась обратная совместимость асма по мнемокодам.
Каталог проекта для пик16 и 18 прилагаю - вдруг кому интересно.
Использование ПЗУ -
Для пик18:
Program Memory Bytes Used: 7702
Program Memory Bytes Free: 8682
Для Пик16:
Program Memory Words Used: 4015
Program Memory Words Free: 4177
А вот про ОЗУ не знаю как узнать - сколько использовано/свободно.
Хотя не известно, заработает ли ? Надо прикола ради ентот хекс зашить в макетку.
Так а там для отладки ЛЦД на 44780 используется насколько я понял ?
Тем более есть повод в свою макетку залить енто дело.
Ну немного конечно подправить пришлось асм обработчик прерывания в Servo.Inc
В исходном обработчике есть ошибка - там после записи новых коэффициентов деления надо в управляющем регистре таймера установить младший бит. Автор же загоняет в регистр единицу - в результате предделитель таймера паузы сбрасывается (старшие биты регистра). Практически в работе аппарата это вызывает дрожание-дрейф сервомашинки. Я этот трабл две недели выискивал в прошлом году - потом устранил. 😎
Я мечтаю, чтоб хоть и на 0.5гц, но чтобы эта сволочь не тормозила по 6-7 а иногда и 10с в поворотах на скоростях 130-150км/ч, а то блин такие вилюшки получаются… в окружность 70м с трудом наводится, в 50м уже промахивается с первого раза., это ежели до точки меньше 350м и надо повернуть на 110 градусов и больше…
Серж - ты уже достаточно взрослый, пора уже и мечтать по взрослому.
Ну то есть насколько я своим дилетантским взглядом понимаю положение вещей - пора уже браться за внутренности ГПС вычислений, без ентого любые скоростные (с высоким центростремительным ускорением) эволюции сопровождаются потерей позиции из-за фильтрации ВНУТРИ ГПС - он то думает, что НА ЗЕМЛЕ возможны ускорения не более 0,5 Г с резиной по асфальту. Вот и фильтрует базар как следует. А если фильтрацию перенастраивать в зависимости от показаний бортового датчика ускорений ну и быть могет барометрического альтиметра при 3D навигации - могет получится совсем другое дело.
Хотя енто как я понял ну совершенно другая история - многосторонняя увязка инерциалки с низко уровневыми ГПС данными. Простора для мозга хоть отбавляй.
p.s. Опять жру пиво - стиль изложения соответственный.
2 Вовик - чего-то мне с первого взгляда ентот Бэйсик проект не особо понравился - своей реализацией ППМ ввода/вывода как минимум. Ну и тем, что рулит одним каналом. Хотя как всегда - лучшее - враг хорошего.
Однако Если привлекать аппаратуру 18ф4431, вполне можно 3-5 каналов весьма точно вводить и пару или десять весьма точно выводить - в зависимости от того, способен ли Power Control модуль полноценно PPM формировать - я его структуры в деталях так и не понял, хотя неоднократно пытался.
С отладочным ЛЦД я насколько понимаю ошибся - там один отладочный выход, на который инфа льётся во внешний терминал в асинхронном виде.
В макетку пока не залил.
Цель замышляемого дела - модифицировать софт так, чтобы он смог работать с любой GPS-головкой.
Я заткнулся на том, что не смог написать расчет азимута на цель по известным координатам на Пикбейсике без библиотек функций. Ну, не программист я. 😊
Может кто возьмется помоч ?
Написал код (пока для проверки на MatLab’e) без библиотечных функций
Вход:
широта и долгота точки цели в формате NMEA GGA , т.е. GGMM.MMMM (LAT) и GGGMM.MMMM (LON)
широта и долгота текущей точки (в том же формате)
Выход:
азимут на цель
расстояние до цели
Ошибка порядка 0.1%
Вот только от плавучки не смог отказаться и числа в формате double
To: vovic
Володь, проверь пожалуйста, может влезет библиотека для плавающей точки.
Вот только от плавучки не смог отказаться и числа в формате double
Все легко сделать в целочисленной арифметике. Если кому-то нужно, могу рассказать, как. Но кодировать в Пикбейсике не хочу.
Лучше всего использовать fixed point. Нужно только быстрое умножение и деление больших целых чисел.
Все легко сделать в целочисленной арифметике. Если кому-то нужно, могу рассказать, как.
Мне нужно. Кодировать буду на ассемблере.
Володь, проверь пожалуйста, может влезет библиотека для плавающей точки.
На днях попробую. Если до НГ не успею (конец года - как всегда) то на каникулах рождественских точно выясню.
Мне нужно. Кодировать буду на ассемблере.
Начнем с конца, т.е. с арктангенса. Пусть есть разность широт dY и долгот dX. Они целые, в каких-то одинаковых единицах. Делаем таблицу арктангенса для аргументов 0-0.5 включительно (т.е углы от 0 до 45). Количество элементов в таблице зависит от требуемой точности. Лучше, если это количество равно степени 2 плюс 1. Тогда в будущем умножение можно заменить на сдвиг. Вот к примеру таблица из 33 значений дает точность не хуже 2х градусов
const char atnTab[33] =
{0, 2, 4, 5, 7, 9, 11, 12, 14, 16, 17, 19, 21, 22, 24, 25,
27, 28, 29, 31, 32, 33, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45};
short atn; // результат
Дальше обрабатываем единственную неопределенность
if(dX == 0 && dy == 0){
atn = 0; // Но это неопределенность!!
}
Если неопределенности нет, то начинаем бить на октанты и в каждом октанте вычисляем арктангенс по-своему
if(dX >= 0 && dY >= 0){
if(dX <= dY)
atn = atnTab[dX*32/dY];
else
atn = 90 - atnTab[dY*32/dX];
}
else if(dX >= 0 && dY <= 0){
if (dx > -dY)
atn = 90 + atnTab[-dY*32/dX];
else
atn = 180 - atnTab[dX*32/(-dY)];
}
else if(…
и так еще 4 октанта
Видно, что при таком подходе:
-только одна операция деления
-деления на 0 быть не может
-результат деления всегда от 0 до 32
-результат арктангенса получается однозначным от 0 до 359
Важно, чтобы не было переполнения при умножении на 32. Т.е. либо для результата умножения использовать бОльшую разрядность, либо быть уверенным, что в dX и dY пять старших разрядов свободны.
Вроде можно и без ассемблера обойтись.
Посмотрите набросал куски кода в ПИКБейсике, там считывание полей с координатами и засовывание их в две двухбайтных переменных и вычитание двухбайтных переменных цели и текущей позиции. Код только в виде примера, не уверен что правильно, но мож пригодится.
Посмотрите набросал куски кода в ПИКБейсике, там считывание полей с координатами и засовывание их в две двухбайтных переменных и
Нельзя координату в двухбайтную переменную засовывать. Это неприемлемая потеря точности. Двухбайтное представление годится только для разности текущей позиции и цели.
Дык там так и сделано… педполагается, что далеко улетать никто не будет 😃
но на мой взгляд эта кактавасия не совсем корректна.
У меня 3 байта для представления координат. и штатный сишный арктангенс. на 16 мгц считается за 40 мкс.
но пользуюсь арктангенсом не часто, 301 раз в секунду, поэтому времени на другие вычисления- вагон.
У меня 3 байта для представления координат.
serj, если GPS выдает координаты в десятичном формате ddmm.mmmm и dddmm.mmmm, то 3х байтов вроде как мало, если не отбрасывать младшую цифру. Напрашиваются 32 бита. Или делать вычитание с координатами в десятичном виде.
Библиотеку Си vovic-у пристегнуть может быть проблематично. Проще действительно в ассемблере.
Пардон, мож муторно написал, одна координата запихивается в 2 слова, в одно целая часть в минутах(естественно градусы не откидываются), во второе дробная часть минут, разница укладывается в двубайтное. Последняя цифирь действительно откидывается, если ее GPS вообще выдает. Предпологается, что с данным агрегатом никто не будет улетать далее расстояния в 32 минуты от точки возврата. Насчет ассемблера еще не известно как он там с асемблером общается, этот куцый бейсик, наверное самый простой вариант все таки подцепить библиотеку с плавающей точкой на том же бейсике, благо она существует.
Дык там так и сделано… педполагается, что далеко улетать никто не будет 😃
но на мой взгляд эта кактавасия не совсем корректна.У меня 3 байта для представления координат. и штатный сишный арктангенс. на 16 мгц считается за 40 мкс.
но пользуюсь арктангенсом не часто, 301 раз в секунду, поэтому времени на другие вычисления- вагон.
Если вычитать поотдельности градусы, минуты и дробную часть минут,
то 2х байт для самой разницы вполне хватит.
0xFFFF это в десятичном виде 65535. Знак можно хранить и учитывать отдельно.
Т.е., если принять первую цифру - минуты, то остаётся 4 знака на дробную часть.
Разница в одну тысячную минуты - это два метра. Величина сравнимая с шумом позиции.
А 6.5535 минут - это порядка 10000 метров.
Никто не мешает при большей удалённости брать первые две цифры за минуты.
Надо только это учесть при пересчёте.
На самом деле просил vovic’a пристегнуть библиотеку с плавающей точкой
для разложения синуса и арктангенса в ряд Тейлора.
Если же использовать таблицу вместо ряда, можно попробовать сравнить точность для целых чисел.
Я прошу прощения, что не попробовал пока с библиотекой. Запарка в конце года - смогу только на каникулах.
По поводу вычитания. Тысячные доли минут отбрасывать нежелательно. Посмотрите на файлы записей треков моих полетов.Файл открывается в Exel. Каждая строка - это точка. Третий и второй с конца столбцы данных - это координаты точек. Даже беглого взгляда на координаты достаточно, чтобы убедиться, что младший разряд совсем не тонет в шумах GPS. Последний столбец - это высота точки над уровнем геоида в дециметрах. Тоже, кстати, в шумах не тонет младший разряд.
Разрядность до вычитания должна быть полной. Кто-то может летать точно на пятидесятом градусе северной широты - это как раз Сторожевое, где у нас превосходный склон для полетов в динаме. Из-за регулярного пересечения границы между 50 и 49 градусами, отбрасывание даже самого старшего разряда градусов до вычитания - нарушит работу автопилота. Т.е. полный формат координат нужен до вычитания.
Как после вычитания? Для планерных применений можно смело брать максимальное удаление - до 3 км от точки старта. В пересчете на географические координаты - это примерно до двух угловых минут по долготе и до полутора минут по широте ( это применительно к Воронежу. В Мурманске если кто летает - то по долготе будет раза в два больше) . Поэтому, если разность будет в двоичной форме - то двухбайтного слова вполне хватает. А если в двоично-десятичной - то нет.
Разность должна быть со знаком - для определения квадрантов азимута.
И еще про точность. Очень не хочется отбрасывать тысячные доли минуты еще и потому, что в случае удачной разработки, есть желание подумать над написанием программы автоматической посадки планера на ВПП - примерно так, как сажали Буран в конце его единственного орбитального полета. Задачка посложнее будет - но ооочень интересная. 😉
Я откидывал только десятитысячные до вычитания, хотя можно и после, градусы переводил в минуты. Если хотите развивать прогу от бейсика надо отказываться и прогу переписывать с нуля.
Мжно пару вопросов? Какой автопилот(co-pilot по схеме на приведенной Вами ссылке) Вы используете, где бы взглянуть на это дело поподробнее и как взглянуть на файлы записей треков Ваших полетов?
Файлы треков выложены вот в этой теме:
rcopen.com/forum/f37/topic42591
Автопилот я использую вот этот:
rcpilot.sourceforge.net/modules/rcap/index.php
Он работает от GPS-навигатора eTrex. Перед стартом в навигатор заносится координата точки старта и навигатор взводится командой GO TO вручную. После этого колпак планера закрывается и он стартует в режиме ручного управления.
Других co-pilot я не использую.
От Бейсика, возможно, уходить надо. Но я не уверен, что на проект хватит сил и ума.
А не подскажете, что в начальных двух колонках?