Автопилот Arduplane - открытое ПО
Ахренеть… 16 градусов ошибка!
а ведь я сравинвал этот код с предыдущей версией 2.7 и неверное тоже самое было в прошлых версиях…
и как оно летало???
огородаме, огородаме)))
а вообще дело не в версиях а в том что там в исходниках нада править коэффициэнты под свою месность.
теперь вы в курсе где косяк и как его решить - с вас патч в мейнстрим))))
вот переделал новый расчет курса.
как говорится почувствуйте разницу!!!
четко по точкам аж квадратные углы))
теперь посмотрите как железка считает растояние - с ним таже проблема
/180*pi == 0.01745277777777777777
так какой вариант правильный?
я летал с float lon11=long2*0.00000000174527777777;
вроде нормуль
широта долгота в моей формуле с проекуцией меркатора должна быть в радианах.
коэффициент 0.00000000174527777777 для градусов в виде целого числа DDDddddddd что в переводе во флоат далобы DDD.ddddddd если у вас широта\долгота сразу в градусах то коэффициент нужен /180*pi == 0.01745277777777777777
если как у меня то ещё 7мь ноликов
судя по коду ардупайлота координаты там тоже хранятся как у меня в виде целого с постоянной запятой, я брал точность деситичной части максимальной , там скорее всего тоже потому и коэффициэнт должен быть 0.00000000174527777777 но на всякий случай стоит перепроверить
правильно будет когда loc2->lng * коэф == долгота в радианах
точнее сказать не могу потому как от ардупайлота вижу только код)))) самоё железки у меня нету потому рабираться более тонко чтото лениво)))
вот облетнанные в xplane функции навигации AP_2_8, в 2.7 вроде тоже самое ,
для тех кто захочет попробовать - чтобы модель летала по прямой а не отклонялась влево и не крутила круги над вейпоинтами а четко проходила
в модуле navigation.pde заменить функции
long get_distance(struct Location *loc1, struct Location *loc2)
{
float pRad = 0.00000000174527777777;
float lat2=loc2->lat* pRad;
float lon2=loc2->lng* pRad;
float lat= loc1->lat* pRad;
float lon= loc1->lng* pRad;float dlx=6371000*(lon-lon2);
float dly=6371000*log(tan(lat/2+M_PI_4))-6371000*log(tan(lat2/2+M_PI_4));
return sqrt(dlx*dlx+dly*dly)*cos(lat);//nav_info->dist=sqrt(dlx*dlx+dly*dly)*cos(lat);
/* old code with bug
//if(loc1->lat == 0 || loc1->lng == 0)
// return -1;
//if(loc2->lat == 0 || loc2->lng == 0)
// return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;*/
}long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}long get_bearing(struct Location *loc1, struct Location *loc2)
{
float pRad = 0.00000000174527777777;
float lat2=loc2->lat* pRad;
float lon2=loc2->lng* pRad;
float lat= loc1->lat* pRad;
float lon= loc1->lng* pRad;float dlx=6371000*(lon-lon2);
float dly=6371000*log(tan(lat/2+M_PI_4))-6371000*log(tan(lat2/2+M_PI_4));long bearing = (atan2(dlx,dly)*180/M_PI+180) * 100.00;
//nav_info->azimuth=atan2(dlx,dly)*180/M_PI+180;
// old code with bug
// long off_x = loc2->lng - loc1->lng;
// long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
// long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
// if (bearing < 0) bearing += 36000;return bearing;
}
и в модуле
defines.h
добавить константу
#define M_PI_4 0.7853981634
еще погоняю в симе, после чего выложу прошивку целиком
тут:hobby.msdatabase.ru/project-updates/…/a
будет называться AP_2_8_Beta4.rar
хочу еще доделать нормально режим LANDING
ато чтото врет с курсом, садится куда попало (
Что-то как-то у вас всё сложно.
Вот как у меня:
extern ap_state_type apstate;
#define PI 3.1415926538f
#define PIm2 6.2831853071f
#define PId2 1.5707963267f
#define PId180 0.0174532925f
…
if(apstate.gps_initialized==1)
{
apstate.gps_dx=(apstate.gps_curlon-apstate.gps_startlon)*cos(apstate.gps_startlat*PId180/60.0f)*1852.0f;
apstate.gps_dy=(apstate.gps_curlat-apstate.gps_startlat)*1852.0f;
apstate.gps_dz=(apstate.gps_curalt-apstate.gps_startalt);
apstate.gps_distance=sqrt((apstate.gps_dx*apstate.gps_dx)+(apstate.gps_dy*apstate.gps_dy));
apstate.gps_azimuth_to_base=atan2(-apstate.gps_dy,-apstate.gps_dx);
}
…
apstate.gps_***lat/lon - в угловых минутах.
apstate.gps_***alt - в метрах
1852.0f - это столько метров по земле весит одна угловая минута на экваторе. Или это же - одна морская миля.
Вычисления всё float. Double - пустая трата времени и ресурсов. Впрочем, в gcc, по-моему, double = float.
Что-то как-то у вас всё сложно.
ну вот у вас как раз как в ардупайлоте
только коэфициенты не жеско задаются а пересчитываются
а не объеясните откуда взяли cos(apstate.gps_startlat*PId180/60.0f)
а всё понял, минуты)))
scaleLongDown == cos(apstate.gps_startlat*PId180/60.0f)
будет нехватать мегагерцев надо будет переделать у себя в коде ))) хотя с меркатором интереснее, там можно сделать учет скукоженности земли на полюсах )))
в контексте ардупайлота
scaleLongDown = cos(loc2->lat* pRad);
scaleLongUp = 1/cos(loc2->lat* pRad);
эффект будет тотже
а ещё пользоваться меркатором это тру олдскульная навигация)))
ее для навигации с 1569 года используют))))
Можно один раз при старте получить свои текущие координаты и для них посчитать, какому количеству метров на земной поверхности равен 1 градус (минута, радиан, тысячная, что там внутри кода используется) по широте и 1 градус по долготе. А в цикле просто умножать на этот коэффициент, ловко избегая тригонометрии. С учетом того, что наши модели улетают не очень далеко от точки старта, погрешность будет невелика.
в принципе понятно зачем разработчики ардупилота выразили несколько действий в константу
с точки зрения оптимизации быстродействия одно действие меньше сожрет тактов процессора,
сначала хотел сделать навигацию как в меге в одной функции и расчет дистанции и азимута, но потом посмотрел вызовы которые ссылаются на эти функции, они из нескольких модулей
и побоялся накосячить, поэтому выбрал простой путь. оптимально навигацию считать конечнож по событию поступления данных GPS они приходят на самых лучших модулях 5-10 раз в секунду
а результат азимута и дистанции сохранять в переменную.
что касается коррекции на приплюснутость, а сколько дельта между минимумом и максимумом? если сотые доли градуса то смысл весьма сомнителен…
но если делать то либо при старте считать либо в какой нить совсем “редкий” цикл запихнуть.
и вот еще вопрос, в этой тригонометрии исключений быть не может? скажем на нулевых значениях, или когда координаты совпадают?
а результат азимута и дистанции сохранять в переменную.
тут палка о двух концах - экономете мегагерцы, неэкономите память, у меги ее всего 8 килобайт
что касается коррекции на приплюснутость
это я пошутил так)))) фактически приплюснутость дает на широте 60градусов около 15 км смещения, но в локальной навигации это неважно там не абсолютные а относительные базы координаты
в этой тригонометрии исключений быть не может
подозреваю может быть а может не быть, посмотрите в исходники avr-libc функцию atan2 в теории там может быть деление на ноль на практике - посмотрите
что касается коррекции на приплюснутость, а сколько дельта между минимумом и максимумом?
На это можно покласть при полетах в радиусе до 100-200км.
Испытал связку Ardupilot 2.7.1 + OSD Remzibi. К ардупилоту подключено 3 канала управления: тангаж, крен, газ. Установлено это добро было на самодельную модель-верхнеплан размахом 1,4м. Конечно, погода, мягко говоря, не совсем подходящая для испытаний, ветер дул 6 м/с, но выбора нет, на неделе нужно на работу ходить.
Сразу о грустном. Видео полетов записывалось на старенький ноутбук, а я так увлекся полетами, что прозевал момент, когда холодный ноябрьский ветер выдул из батареи всю энергию. Ноут отключился в момент записи, и видео-файл получил фатальные повреждения. Поэтому видео пока не будет. Попробую повторить запись в следующие выходные.
Теперь о главном. Первым был опробован режим стабилизации. Да, чуть не забыл, все настройки ардупилота оставлены по умолчанию, т.е. пиды и т.д. стоковые. Тем не менее, стабилизация работала на отлично, бросаю ручки, и модель тут же выходит в горизонтальный полёт. Собственно, используя данный режим, можно передавать управление моделью маленьким детям, старикам и беременным женщинам. Зарулился, отпускаешь ручки и модель в горизонте. Не получается лететь в нужную сторону или модель далеко и не можешь определить её положение? Щелчок тумблера в положение RTL и через десяток секунд модель над головой. О режиме RTL и добавить, в общем-то, больше нечего. Стабильно возвращает модель в точку HOME и удерживает её там. В безветренную погоду летает по кругу (это я раньше проверял), в ветер как получиться, замысловатыми овалами и восьмёрками, но над головой. Разумеется, должен заметить, что, в общем-то, модель далеко не пилотажка, а создавалась изначально для FPV полётов, т.е. обладает врожденной тягой к ровному полету, и, наверное, выводить её в горизонт не великий подвиг. Тем не менее, с непривычки впечатляет.
Самый интересный, на мой взгляд, режим, полет по точкам. Тут тоже без сюрпризов, всё работает. Модель летает от точки к точке, попутно борясь с ветром, затем возвращается «домой» и всё сначала. К сожалению, оценить точность руления во время сильного ветра было довольно сложно, но вроде бы всё ок.
Пора уже и об OSD вспомнить, а вот тут не всё так гладко как хотелось бы, возникло несколько вопросов. Итак, ну во-первых, вся информация передаваемая ардупилотом отображается на экране. Искусственный горизонт и режимы работы без проблем. Проблемы возникли в следующем. Индикатор внизу экрана в виде спутниковой антенны показывает количество видимых спутников? Почему-то он показывал исключительно цифру 4-ре. Хотя когда я раньше использовал OSD без автопилота, то цифра была 8 или 9. Далее, скорость и удаление от точки старта показывает отлично, а вот высоту и вариометр… У земли как и положено всё по нулям, но после запуска цифры скачут как бешенные в сотнях метров. Хотя при всём при этом автопилот водит модель по высоте ровно, примерно на 150-ти метрах, как и планировалось в миссии. Ещё есть замечание по отображению режима полета по точкам. При включении на экране появляется номер точки к которой летим и расстояние до неё в метрах, к примеру WP1 147. (эх, жалко видео нет) Так вот, цифры после 2-го знака, в примере это 7, попадают в поле отображения горизонта и постоянно мерцают, т.е. надпись о режимах нужно сдвигать влево на два знакоместа. Где, кстати это делается, в прошивке ardupilota?
Очень длинный текст получается, пока умолкаю. Буду очень признателен, если кто поможет разобраться с возникшими вопросами.
P.S. в программировании Arduino я, надеюсь временно, практически полный 0. Убедительная просьба, как нибудь по доступнее…😃
нашел в коде нестыковку изза которой домашние координаты сохраняются не точно, а это может стать причиной того что модель скрылась в неизвестном направлении.
Суть в следующем некоторые ЖПС модули с батарейкой горячего старта некоторое время после старта сначала выдают позицию своего прошлого положения но разумется с указанием низкого качества сигнала в HDOP, потом идут нулевые широта и долгота, затем ловятся спутники и растет качество выражаемое в типе fix нет/2d/3d/Dgps
суть проблемы в том что иму должна передавать значение качества сигнала ардупилоту и только тогда когда качество 3D fix должна сохраняться домашняя локация. Передачу Gps.quality я уже реализовал в иму 1.8 когда делал прошивку ардупилота под ремзиби осд, теперь надо поправить код ардупилот 2.8
суть дилемы:
если я оставлю в цикле ожидание сохранения home пока не появится Gps.quality соответствующее 3d fix то те товарищи которые случайно его подключат к иму со старой прошивкой (котрая не передает Gps.quality ) и не смотрят в окно терминала, что им пишет пилот получат непонятный эффект зависания, опять же если использовать ArdupilotSim для симуляции в XPLANE не заточенный под выдачу этого сигнала то тоже будет зависон.
с другой стороны запускать на авторежимах с ложной точкой home тоже не айс, а сделать повторный ресет ардупилоту перед запуском можно и забыть… будет человеческий фактор там где виновата логика реализованная в коде
2 Роман Петров
в этой прошивке количество спутников заменено на качество жпс сигнала (пожелания были в этом обсуждении)
баллы 0-2 лучше не летать
3-4 - нормуль
алгоритм примерно такой:
if (fix < 1)
quality = 0; // No FIX
else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5)
else if(HDOP > 30)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
хотя я бф не назвал HDOP = 25 (тут *10 т.е 2.5) хорошим, реально хорошо когда меньше 2.0 а под открытым небом хороший навигатор дает < 1.0
по поводу расстояния до маршрутной точки, была найдена ошибка в расчете курса и расстояния которая наследовалась во всех версиях ардупилота включая 2.7, патченные функции я выкладывал,
что касается с отсутствием округления в расстоянии до точки - спасибо за баг-репорт, обязательно исправлю
Ага, про спутники всё понятно. Не много не привычна 4-х бальная шкала, но это мелочи. А вот интересно почему так высота в OSD скакала?
Ага, про спутники всё понятно. Не много не привычна 4-х бальная шкала, но это мелочи. А вот интересно почему так высота в OSD скакала?
хорошоб видео, тут разные причины могут быть, навскидку если не сделал повторный сброс ардупилоту после того как жпс модуль хорошо захватил спутники то вполне возможно была сохранена ошибочно точка home и она же записалась в осд
четырехбальная шкала повзаимстована из билиотек ардупилот мега,
думаю надо сделать пятый бал на идеальный HDOP, где нить <1
на счет предполетной подготовки и сброса Хом позишин
а почему бы не сделать махание к примеру рудером на земле до появления валидных данных
пилот видит что самаль виляет хвостом - значит не все так … ждем
успокоился - жмем ресет иначе просто не даем работать газом !
типа некая защита от дурака 😃
а для сима - таже версия прошивок только заточенной под сим 😃
как сделано в мегапилоте 😃
Где, кстати это делается, в прошивке ardupilota?
в 2.7 это модуль CGS Remzibi
void print_control_mode(void)
…
Serial.print(“$M,”);
Serial.print(“130,”); // Колонка для начала текста
Serial.print(“11,”); // Ряд для начала текста
Serial.print(“169,”); // dec address of First Character - это я не понял к чему, взаимстовано из ориг. версии
Serial.print(“00,”); // dec address of Last Character
2 lio
мой мотив против этого - что будет как со мной и другими форумянами которые начали пробовать пилот до того как полностью перевели и заучили английский мануал…
“почему когда я включаю “авто” самолет загибает элероны и руль высоты” ведь если я это сделаю в полете он упадет. (если я не ошибаюсь такое гдето было если при включении/ресете ардупилота переключатель режима не мануал и газ не убран - типа к взлету неготов)
пока думаю надо как то определять версию иму (или сима) и в случае если старая выдавать ругательное сообщение в консоль, инициировать дом как есть а вместо постоянного синего светодиода на пилоте мигать им, что не все совсем в порядке.
В этом случае если прошивка иму старая то ну неточно будет дом, но полетать на стабилизации можно и в симе тоже.
осталось дело за малым как понять в коде что старая версия иму…
Про тонкости сброса точки Home не знал, но проблем с этим не возникало, т.е при включении с пульта RTL самолёт четко летел домой. На сброс OSD перед вылетом нажимал, после чего возникала надпись HOME SAVED и возникали нули в расстоянии, скорости и высоте, только потом, во время полета, высота скакала как бешеная. Запишу видео, поговорим на эту тему поконкретнее, ок?
На счет виляния хвостом. Думаю, элегантнее было бы повесить на свободный выход ardupilota какую-нибудь простенькую пищалку.
Что-то как-то у вас всё сложно.
Вот как у меня:
…
Вычисления всё float. Double - пустая трата времени и ресурсов. Впрочем, в gcc, по-моему, double = float.
Почему то был уверен что у вас целочисленная математика.
ведь используя только 8-битные константы уже можно курс с точностью ~0.5град вычислять.