Fail-Safe - Автопилот для полетов по камере
Цель замышляемого дела - модифицировать софт так, чтобы он смог работать с любой 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 я не использую.
От Бейсика, возможно, уходить надо. Но я не уверен, что на проект хватит сил и ума.
А не подскажете, что в начальных двух колонках?
В первой колонке данные барометрического высотомера, во второй - барометрического вариометра с нулем возле 500.
Там есть еще записи, где ненулевая четвертая с конца колонка - это температура воздуха на крыле в градусах Цельсия, умноженная на два (т.е. цифра 45 - означает 22,5 градуса Цельсия.)
Используя только целые двубайтовые числа получить азимут между точками можно.
На удалении от 100 м. от базы точность будет не хуже нескольких градусов.
Чем дальше - тем точнее. Используя таблицу на 45 элементов и кучу IF’ов
получается точность 1 градус.
Но имеем только азимут, без расстояния.
Т.е. при пролёте над заданной точкой азимут поменяется на 180 градусов.
Для простых вычислений такой подход наверное приемлим.
Если не получится использовать библиотеку с плавающей точкой,
после НГ оформлю код.
в случае удачной разработки, есть желание подумать над написанием программы автоматической посадки планера на ВПП - примерно так, как сажали Буран в конце его единственного орбитального полета. Задачка посложнее будет - но ооочень интересная. 😉
А вот здесь точность нужна получше.
Плавающая или фиксированная точка.
И числа 32 bit.
Я других самолетов и не держу, только те, что летают с брошенными ручками.
Так что рулить можно и хвостом.
Назовите если можно конкретные модели. Мне нужно.
Есть готовый проект:
rcpilot.sourceforge.net/modules/rcap/index.php
Цель замышляемого дела - модифицировать софт так, чтобы он смог работать с любой GPS-головкой.
Я заткнулся на том, что не смог написать расчет азимута на цель по известным координатам на
Пикбейсике без библиотек функций.
Может кто возьмется помоч ?
Я хочу попробовать, мне тоже автопилот интересен.
Как он работает с навигатором ? Навигатор как то подает сигналы “куда рулить” ? Если да то как они выглядят ?