Телеметрия (часть 1)
Может быть, я уже в который раз открываю Америку, но я таки нашел компактный и быстрый алгоритм извлечения корня из 32-битного целого. Аналог “деления столбиком” в двоичной системе:
unsigned short lsqrt(unsigned long a){
unsigned long rem = 0;
unsigned long root = 0;
for(int i=0; i<16; i++){
root <<= 1;
rem = ((rem<< 2) + (a >> 30));
a <<= 2;
root ++;
if(root<= rem){
rem -= root;
root++;
}
else
root--;
}
return (unsigned short)(root >> 1);
}
С предыдущим алгоритмом (раза в три компактнее, но медленным аж жуть) нахождение корня занимало аж до 800000+ циклов процессора. Ни о каком вывод на экран в течение этого процесса и речи не шло. А с этим алгоритмом - сами можете примерно подсчитать, раз этак в тысячу быстрее 😃
Если еще интересно , почитай здесь : forum.sources.ru/index.php?showtopic=9933.
Как я понял, при желании алгоритм можно еще ускорить, но наверно ценой места?
Скажите, минута - достаточно для старта любого GPS модуля?
Добавление: ну, для душевного спокойствия можно за стартовую позицию взять не единичный сэмпл с приемника, а усреднение по 8 сэмплам. Ст о ит?
Нет, недостаточно. Некторые стартуют 3 минуты, некоторые 10 секунд (при горячем старте). Почему не делать простой монитор параметра А? По сути, это зеркало 2Д/3Д режима, но там тупо показывают - релевантны координаты или нет.
Усреднение безсмысленно, ИМХО. Точност не возрастёт.
Чтобы повысить точность, нужно записать 8-16 семплов, проанализировать их (разница показаний по прямой соседних семплов) и если всё в пределах допуска, то брать один семпл, который больше нравится.
Например, если патчевая антена направлена вбок, к стене здания, где стоит микроволновка, показания могут плясать на ±100 км.
Ждать, пока не придут гарантированно адекватные 3D fix данные, нецелесообразно, потому что парсинг усложняется в разы - порядок следования NMEA строк у каждого GPS модуля свой собственный. Полагаться на то, что вот в этот вот момент в GPGSA показан 3D fix, а значит в последующих строках идут данные с 3D Fix’ом, нельзя. Кто знает, вдруг между GPGSA и GPRMC модуль свалился в 2D fix, а в пачке строк GPGSA идет после GPRMC, и информация о том, что это 2D Fix, а не 3D, придет уже после того, как 2D-данные ошибочно приняты за 3D. Как это обходить, я вообще не знаю.
Обходить надо слева 😃 Строки, а именно пачка строк, получаются из одного определения. Если в GGA параметр такой, то во всех остаьных он будет таким же. Следующий пакет будет иметь другие параметры. Максимум, что может меняться в течение одного пакета, это время.
Про парсинг говорили уже. На Си надо было писать 😃 Всего три вложеных свитча…
Типа вот. Больше не позволяет форум вставить:
interrupt [USART1_RXC] void RXC_isr(void)
{
tempGPS = UDR1;
switch (iGPS) {
case ‘F’: {
switch (Scheduler_task) {
//-------------------------------------------- классифицируем заголовок
case 1:
if (tempGPS == ‘$’) { Scheduler_task++; }
break;
case 2:
if (tempGPS == ‘G’) { Scheduler_task++; }
else { Emergency_exit(); }
break;
case 3:
if (tempGPS == ‘P’) { Scheduler_task++; }
else { Emergency_exit(); }
break;
case 4:
switch (tempGPS) {
case ‘G’: Scheduler_task = 5;
break;
case ‘R’: Scheduler_task = 55;
break;
case ‘V’: Scheduler_task = 105;
break;
default: Mistake_exit();
break;
}
break;
//---------------------------------------------- классифицируем второй символ заголовка строки
case 5:
switch (tempGPS) {
case ‘G’: Scheduler_task++;
break;
case ‘S’: Scheduler_task = 156;
break;
default: Mistake_exit();
break;
}
break;
case 55:
if (tempGPS == ‘M’) { Scheduler_task++; }
else { Mistake_exit(); }
break;
case 105:
if (tempGPS == ‘T’) { Scheduler_task++; }
else { Mistake_exit(); }
break;
//---------------------------------------------- классифицируем третий символ заголовка строки
case 6:
if (tempGPS == ‘A’) { Scheduler_task++; iGPS = ‘G’; }
else { Mistake_exit(); }
break;
case 56:
if (tempGPS == ‘C’) { Scheduler_task++; iGPS = ‘R’; }
else { Mistake_exit(); }
break;
case 106:
if (tempGPS == ‘G’) { Scheduler_task++; iGPS = ‘V’; }
else { Mistake_exit(); }
break;
case 156:
if (tempGPS == ‘A’) { Scheduler_task++; iGPS = ‘S’; }
else { Mistake_exit(); }
break;
} // end of switch (Scheduler_task)
break;
}
case ‘G’: {
switch (Scheduler_task) {
//----------------------------------------------- а вот теперь пошёл разбор строки GGA
case 7:
if (tempGPS == ‘,’) { chislo_propuskov++; }
if (chislo_propuskov == 6) {
Scheduler_task++;
chislo_propuskov = 0;
}
break;
case 8:
if ( tempGPS != ‘,’ ) { rejim_opredelenia = tempGPS; }
else { Scheduler_task++; }
break;
…
>Если в GGA параметр такой, то во всех остаьных он будет таким же. Следующий пакет будет иметь другие параметры.
А можно получить в студию номер поля с этим мараметром в строке NMEA? Ну, или любой критерий, позволяющий с уверенностью говорить, что именно во эта вот строка - из пакета номер Х.
>Почему не делать простой монитор параметра А?
Лехко. Только я в GPS-делах не гуру, потому и делаю глупости.
А код - да, у меня на асме почти такой же код.
LDI ZL, LOW (NMEA_COMPLETE_STRING); loading NMEA sentence header
LDI ZH, HIGH (NMEA_COMPLETE_STRING)
LD R16, Z+; $ symbol
LD R17, Z+; G symbol
LD R18, Z+; P symbol
LD R19, Z+; ? symbol
LD R20, Z+; ? symbol
LD R21, Z+; ? symbol
STS NMEA_HEADER_C1, R19
STS NMEA_HEADER_C2, R20
STS NMEA_HEADER_C3, R21
LDS R19, NMEA_HEADER_C1; processing RMC sentence
LDS R20, NMEA_HEADER_C2
LDS R21, NMEA_HEADER_C3
CPI R19, 'R'
BRNE _not_RMC_sentence
CPI R20, 'M'
BRNE _not_RMC_sentence
CPI R21, 'C'
BRNE _not_RMC_sentence
LDI R16, 3
RCALL NMEA_parse_latitude
LDI R16, 5
RCALL NMEA_parse_longtitude
LDI R16, 8
RCALL NMEA_parse_heading
_not_RMC_sentence:
LDS R19, NMEA_HEADER_C1; processing VTG sentence
LDS R20, NMEA_HEADER_C2
LDS R21, NMEA_HEADER_C3
CPI R19, 'V'
BRNE _not_VTG_sentence
CPI R20, 'T'
BRNE _not_VTG_sentence
CPI R21, 'G'
BRNE _not_VTG_sentence
LDI R16, 1
RCALL NMEA_parse_heading
LDI R16, 7
RCALL NMEA_parse_speed
_not_VTG_sentence:
LDS R19, NMEA_HEADER_C1; processing GGA sentence
LDS R20, NMEA_HEADER_C2
LDS R21, NMEA_HEADER_C3
CPI R19, 'G'
BRNE _not_GGA_sentence
CPI R20, 'G'
BRNE _not_GGA_sentence
CPI R21, 'A'
BRNE _not_GGA_sentence
LDI R16, 2
RCALL NMEA_parse_latitude
LDI R16, 4
RCALL NMEA_parse_longtitude
RCALL NMEA_clear_parse_error
LDI R16, 7
RCALL NMEA_parse_integer_number
LDS R20, NMEA_PARSE_ERROR
SBRS R20, 0
STS NUM_SATELLITES, R16
_not_GGA_sentence:
LDS R19, NMEA_HEADER_C1; processing GSA sentence
LDS R20, NMEA_HEADER_C2
LDS R21, NMEA_HEADER_C3
CPI R19, 'G'
BRNE _not_GSA_sentence
CPI R20, 'S'
BRNE _not_GSA_sentence
CPI R21, 'A'
BRNE _not_GSA_sentence
RCALL NMEA_clear_parse_error
LDI R16, 2
RCALL NMEA_parse_integer_number
LDS R20, NMEA_PARSE_ERROR
SBRS R20, 0
STS TMP_FIXMODE, R17
_not_GSA_sentence:
LDS R19, NMEA_HEADER_C1; processing GLL sentence
LDS R20, NMEA_HEADER_C2
LDS R21, NMEA_HEADER_C3
CPI R19, 'G'
BRNE _not_GLL_sentence
CPI R20, 'L'
BRNE _not_GLL_sentence
CPI R21, 'L'
BRNE _not_GLL_sentence
LDI R16, 1
RCALL NMEA_parse_latitude
LDI R16, 3
RCALL NMEA_parse_longtitude
_not_GLL_sentence:
“Какой кошмар !” [Фрекен Бок] 😲
Я в таких случаях (когда набор возможных тэгов ограничен и заранее известен) пользуюсь следующим совершенно ненаучным, но вполне эффективным способом:
uint8_t nmea_hash (char *p)
{
uint8_t i, h=0;
for (i=0; i<5; i++)
{
h<<= 1;
if (h & 0x80)
{
h&= 0x7F;
h++;
}
h^= *p++;
}
return h;
}
...
switch (nmea_hash (NmeaBuffer))
{
case hash_GPGGA:
...
case hash_GPVTG:
...
“Формула” такой хэш-функции подбирается практически от балды; достаточно чтобы на ожидаемом массиве тэгов результат был уникальным. На ассемблере оно получается еще лучше - буквально в две-три команды и без проверок, поскольку можно оперировать флагом переноса…
Данная конкретная конструкция “настроена” на набор сентенсов от Garmin’а.
PS: Да, я заполняю буфер, начиная с ‘$’, и заканчиваю по ‘*’, сразу же их отбрасывая. Чексам проверять ленюсь. 😃
Мож у кого-нибудь есть какая-нибудь предыдущая версия вашего творения, которая вам не нужна, я б купил недорого.
Нужна только высота и таймер.
Полетал по камере тока что, после захода солнца, красотища! Выключаешь двигатель, бросаешь все ручки и планер как-будто застывает в небе, только еле заметно земля где-то внизу плывет.
С помехами запутался окончательно - недалеко от себя бывает управление дергает так что 90 градусов разом самолет разварачивает и видео тоже штырит сильно, а далеко-далеко улетаешь - все четко и без помех. Эхх, телеметрию надо конечно…
Полетал по камере тока что, после захода солнца, красотища! Выключаешь двигатель, бросаешь все ручки и планер как-будто застывает в небе, только еле заметно земля где-то внизу плывет.
С помехами запутался окончательно - недалеко от себя бывает управление дергает так что 90 градусов разом самолет разварачивает и видео тоже штырит сильно, а далеко-далеко улетаешь - все четко и без помех. Эхх, телеметрию надо конечно…
Не в тему конечно но насколько далеко разнесены у Вас передатчик видео и rc приёмник, должно быть не менее 25 см. Я щас разнес и летаю до 2 км, не одной помехи.
Не в тему конечно но насколько далеко разнесены у Вас передатчик видео и rc приёмник, должно быть не менее 25 см. Я щас разнес и летаю до 2 км, не одной помехи.
Приемник RC (hitec electron 35 Mhz) практически разделен только стенкой фюзеляжа от видео-передатчика (2.4 Ghz), расстояние сантиметра три.
Причем раньше я таких проблем почему-то не замечал, казалось все идеально работало (даже отписывался где-то тут об этом).
Попробую сейчас отнести видео-передатчик чуть подальше к хвосту, а антенну приемника по крылу запустить.
P.S. В Питере кто-нибудь успешно пользует телеметрию smalltim’а? Может дадите на 5 минут, попробовать к моему помехо-неустойчивому самолету подключить? 😃
“Какой кошмар !” [Фрекен Бок] 😲
Я не стал делать буфер. Слишком много оперативы.
А по вычислительной возможности то же самое получается - в прерывании заталкивается буфер, а потом разгребается. Я сразу вынимаю нужные параметры и момещаю в строки. Далее строки перегоняю в числа (в основном цикле) и всё.
>Если в GGA параметр такой, то во всех остаьных он будет таким же. Следующий пакет будет иметь другие параметры.
А можно получить в студию номер поля с этим мараметром в строке NMEA? Ну, или любой критерий, позволяющий с уверенностью говорить, что именно во эта вот строка - из пакета номер Х.
Например, блохи дёргают лапкой перед стартом нового пакета.
Во-вторых можно использовать таймаут - пакеты идёт раз в секунду обычно и передаются максимум 1/3 секунды. Несложно отслеживать по таймеру время между приходом байта (любого). Если оно больше Х, значит стартует новый пакет.
В-третьих можно не изобретать мега-универсальный код, а посмотреть, с чего начинается пакет у Вашего приёмника 😉 Обычно с GGA.
>Лехко. Только я в GPS-делах не гуру, потому и делаю глупости.
Ну зачем так сразу категорично… А-параметр отображает не всё. Релевантность не гарантирует точность. Я бы таки 3-d режима дождался… 😃
Про поле.
Например, в GLL:
6. Статус A = данные верны
V = данные не верны
RMC:
2. Состояние: А = действительный, V = предупреждение навигационного приёмника
Ну и далее, в других строках он тоже встречается.
Я не стал делать буфер. Слишком много оперативы.
А по вычислительной возможности то же самое получается - в прерывании заталкивается буфер, а потом разгребается. Я сразу вынимаю нужные параметры и момещаю в строки. Далее строки перегоняю в числа (в основном цикле) и всё.
Если не секрет, на каком кристалле строите, что памяти нехватает ?
(А то когда-то давно, на x51, где памяти было мало, а числогрызности - еще меньше, я тоже сразу раскладывал поля по мере приема. Только делал это не проверками, а разбором по дереву…)
Однако, и в Вашем случае хэш-функция будет удобнее (и быстрее, и компактнее) грозди сравнений:
По доллару - сбрасываем и счетчик и хэш. Каждый принятый байт добавляем к хэшу, после пятого - определяем какой сентенс мы в данный момент принимаем, и дальше считаем только запятые, раскладывая принятые параметры “по порядку номеров”. - Никакой стейт-машины, всего одна табличка с указателями на “переменные” (или оффсетами) для сохранения каждого параметра, или нулем, если это поле пропускаем…
ЗЫ: Единственный потенциальный минус у такого подхода - это если мы допускаем возможность подключения совершенно произвольного приемника, из которого может выпасть “неожиданный” пакет, хэш для которого совпадет с одним из известных.
Однако, и в Вашем случае хэш-функция будет удобнее (и быстрее, и компактнее) грозди сравнений:
По доллару - сбрасываем и счетчик и хэш. Каждый принятый байт добавляем к хэшу, после пятого - определяем какой сентенс мы в данный момент принимаем, и дальше считаем только запятые, раскладывая принятые параметры “по порядку номеров”. - Никакой стейт-машины, всего одна табличка с указателями на “переменные” (или оффсетами) для сохранения каждого параметра, или нулем, если это поле пропускаем…
Чип мега 128, но унеё в оперативе ещё FAT для флешки сидит.
Так у меня так же сделано! 😃 Определяю строку, считаю запятые, распихиваю символы по буферу. Код только выглядит громоздким, а так там всего 3 вложеных свитча, которые довольно шустро выполняются. Зато по окончанию приёма пакета у меня в буфере готовые данные, там, где надо. Только флажок монирорь и всё 😉
А на 51-х я сейчас вынужден работать 😵 Програмлю всякое древнее барахло, которое попьяне закупили дяденьки-начальники…
>Единственный потенциальный минус у такого подхода - это если мы допускаем возможность подключения совершенно произвольного приемника, из которого может выпасть “неожиданный” пакет, хэш для которого совпадет с одним из известных.
Еще один минус - если пакет по дороге побъется и хэш мусора совпадет с нужным хэшем. Так что я пока по старинке. 3 чтения из памяти и 3 сравнения - знаете ли, не так уж много.
smalltim, только что закончил собирать телеметрию с вашей прошивкой и схемой (только разводку платы немного изменил под детали, что были). Заработало сразу, еще раз благодарю за проделанную работу!
Датчики не цеплял никакие, в среду подключу к своему самолету и проверю на помехи. На этих выходных уже разнес приемник rc от передатчика видео и камеру поставил под крыло, прямо рядом с передатчиком.
Если после всех этих манипуляций помехи от телеметрии не исчезнут - не знаю что и делать тогда… просто мистика получится
smalltim, только что закончил собирать телеметрию с вашей прошивкой и схемой (только разводку платы немного изменил под детали, что были). Заработало сразу, еще раз благодарю за проделанную работу!
Датчики не цеплял никакие, в среду подключу к своему самолету и проверю на помехи. На этих выходных уже разнес приемник rc от передатчика видео и камеру поставил под крыло, прямо рядом с передатчиком.
Если после всех этих манипуляций помехи от телеметрии не исчезнут - не знаю что и делать тогда… просто мистика получится
Да, выглядит оно сурово 😃 Пожалуйста, наставьте блокировочных конденсаторов по 0.1 … 1 мкф поближе к микросхемам между питанием и землей. У меня в схеме этого нет 😦
А я основательно перетряхнул код на предмет оптимальности и компактности и полностью доделал всё, что касается GPS. Ура! Сегодня, как детеныша спать уложу, первое подключение GPS модуля! 😃
А с завтрашнего вечера четыре дня халявы, буду отлаживать GPS и обновлять блог 😃
Ещё совет разместить телеметрию подальше от приёмника, как писал не ближе 25 см. У меня осталась ещё одна платка от smalltim, но без датчиков давления, думою потестить её. Просто долго игрался в поисках устранения помех и наконец то победил.
Ещё раз обращаю внимание.
Провода жестко скрутить спиралью.
приемник rc от передатчика не ближе 25 см от видео передатчика.
В таком варианте даже кольца не нужны. Притом сперва полетал со скрученными проводами но приёмник лежал в носу прям под передатчиком, резко увеличилась дальность, чем без скрученных. А как разнес приемник и передатчик, на 2 км отлетал и небыло помех, дальше не решаюсь пока 😃
Окей, в этот раз буду пробовать с разнесенным как можно дальше приемником.
Какие провода скручивать, от серв или питание телеметрии, камеры, передатчика?
Окей, в этот раз буду пробовать с разнесенным как можно дальше приемником.
Какие провода скручивать, от серв или питание телеметрии, камеры, передатчика?
Провода серв, вобщем те провода которые входят в приёмник. Кстати и антену приёмника тоже подальше.
Еще один минус - если пакет по дороге побъется и хэш мусора совпадет с нужным хэшем. Так что я пока по старинке. 3 чтения из памяти и 3 сравнения - знаете ли, не так уж много.
Не-а, не побъется. Негде ему биться. 😁
Я даже чексам не проверяю, бо на единицах (а даже и на десятках) сантиметров длины и единицах килобод скорости для возникновения битовой ошибки нужна ТАКАЯ помеха, от которой будет уже не до точности ОМП; - модель бы спасти…
С другой стороны, если не контролировать валидность извлеченных из строки данных, то куда бы ни попал этот гипотетический сбой - в заголовок, или в поле данных, - результат будет строго одинаковым.
Впрочем, я ведь никого не уговариваю… Просто поделился технологией.
Не-а, не побъется. Негде ему биться. 😁
Я даже чексам не проверяю,
+1
Тем более, что чексам там хреновая и нормальной защиты всё равно не обеспечивает…