Автопилот Arduplane - открытое ПО
хехе фокус, видать кусок старого кода в новую версию закоммитили гдето
вот как у них:
static long get_distance(struct Location *loc1, struct Location *loc2)
{
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;
}
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
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;
}
А вот как у меня, равноугольные, равномасштабные координаты получаются согласно ссылке ru.wikipedia.org/wiki/Проекция_Меркатора%D0%9F%D1%80%D0%BE%D0%B5%D0%BA%D1%86%D0%B8%D1%8F_%D0%9C%D0%B5%D1%80%D0%BA%D0%B0%D1%82%D0%BE%D1%80%D0%B0
void calc_navigation(long src_lat,long src_lon,long dst_lat,long dst_lon,struct navigation* nav_info){
float lat11=dst_lat*0.00000000174527777777;
float lon11=dst_lon*0.00000000174527777777;
float lat=(src_lat*0.00000000174527777777);
float lon=(src_lon*.00000000174527777777);
float dlx=6371000*(lon-lon11);
float dly=6371000*log(tan(lat/2+M_PI_4))-6371000*log(tan(lat11/2+M_PI_4));
nav_info->dist=sqrt(dlx*dlx+dly*dly)*cos(lat);
nav_info->azimuth=atan2(dlx,dly)*180/M_PI+180;
/*debug_log_long((long)(dlx),mybuf,“#”)
debug_log_long((long)(dly),mybuf,“#”)
debug_log_long((long)(az),mybuf,“#”)
debug_log_long((long)(ddist),mybuf,“\n”)*/
}
если догадаетесь как поменять код то вперёд, если нет - попробуйте посмотреть симуляцию на экваторе или около него
растояние до точки они там тоже по томуже принципу определяют неправильно
чтобы определялось правильно у них в прошивке в файлеке ArduPilotMega.pde имеются следующие строчки:
static float scaleLongUp = 1; // used to reverse longitude scaling
static float scaleLongDown = 1; // used to reverse longitude scaling
они используются для того чтобы не считать гиперболических функций а заместо этого просто умножать широту на коэффициент - соотвественно решение 2 - посчитайте чему равны эти коэффициенты на вашей широте и впишите
соотвественно их прошивочка ничинает работать криво если вы переезжает в другую широту и чем сильнее переезжаете тем сильнее заметно
коэффициенты 1 1 это для случая на экваторе
посчитать коэффициэнты можно посчитав размер градуса по широте\долготе и поделив один на другой
один берется широта\долгота второй долгота\широта
scaleLongDown должен быть <1
scaleLongDown=1/scaleLongUp
но я бы попробовал пофиксить код с использованием равноугольной\равновеликой проекции , например, меркатора, и попытаться протолкнуть патчик в мейнстрим, хотя код будет тяжелее с точки зрения мегагерцев которых у аврки нетакуж и много)).
поробовал сравнить в с++,
там проще дебажить чем в ардуино
вроде одинаково возвращает вектор
#include “stdafx.h”
#include <math.h>
#define pi 3.14159265
#define M_PI_4 0.785398163397448309616
double radians(double degree)
{
double radian = 0;
radian = degree * (pi/180.000000);
return radian;
}
double Bearing(double lat1, double long1, double lat2, double long2)
{
//double bearingdegrees = 0.0;
lat1 = radians(lat1);
long1 = radians(long1);
lat2 = radians(lat2);
long2 = radians(long2);
double bearingradians = atan2(asin(long2-long1)*cos(lat2),cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(long2-long1));
double bearingdegrees = radians(bearingradians);
if (bearingdegrees < 0)
{
bearingdegrees = 360 + bearingdegrees;
}
return bearingdegrees;
}
double Bearing2(double lat1, double long1, double lat2, double long2)
{
double off_x = long2 - long1;
double off_y = lat2 - lat1;
double bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}
double Bearing3(double lat1, double long1, double lat2, double long2)
{
float lat11=lat2*0.00000000174527777777;
float lon11=long2*0.00000000174527777777;
float lat=(lat1*0.00000000174527777777);
float lon=(long1*.00000000174527777777);
float dlx=6371000*(lon-lon11);
float dly=6371000*log(tan(lat/2+M_PI_4))-6371000*log(tan(lat11/2+M_PI_4));
//nav_info->dist=sqrt(dlx*dlx+dly*dly)*cos(lat);
//nav_info->azimuth=atan2(dlx,dly)*180/M_PI+180;
double bearingdegrees =atan2(dlx,dly)*180/pi+180;
return bearingdegrees;
}
/* add setup code here */
int _tmain(int argc, _TCHAR* argv[])
{
double b =0;
double b2 =0;
double b3 =0;
double lat1 = 55.940933;
double long1 = 37.671061;
double lat2 = 55.934683;
double long2 =37.725134;
// b = Bearing(51.618017,2.48291,51.041394,2.373047); // это вообще хрень
b2 = Bearing2(lat1, long1, lat2, long2)/100.00;
b3 = Bearing3(lat1, long1, lat2, long2);
return 0;
}
/* result
b2 96.593245770746620 double
b3 96.592420855030213 double
*/
ошибка сильнее заметна когда угол подальше от Pi/2*N
потестируйте с
double lat1 = 55;
double long1 = 37;
double lat2 = 58;
double long2 =38;
стороны треугольника в вашем тесте 5.4 х 0.63 в моем 1х1 те на котором косяк сильнее всего очевиден
вот ))) у меня там градусы целочисленные с постоячнной точкой поправил код для работы с даблами
#include <stdio.h>
#include <math.h>
#define pi 3.14159265
#define M_PI_4 0.785398163397448309616
double radians(double degree)
{
double radian = 0;
radian = degree * (pi/180.000000);
return radian;
}
double Bearing(double lat1, double long1, double lat2, double long2)
{
//double bearingdegrees = 0.0;
lat1 = radians(lat1);
long1 = radians(long1);
lat2 = radians(lat2);
long2 = radians(long2);
double bearingradians = atan2(asin(long2-long1)*cos(lat2),cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(long2-long1));
double bearingdegrees = radians(bearingradians);
if (bearingdegrees < 0)
{
bearingdegrees = 360 + bearingdegrees;
}
return bearingdegrees;
}
double Bearing2(double lat1, double long1, double lat2, double long2)
{
double off_x = long2 - long1;
double off_y = lat2 - lat1;
double bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}
double Bearing3(double lat1, double long1, double lat2, double long2)
{
float lat11=lat2/180*pi;
float lon11=long2/180*pi;
float lat=lat1/180*pi;
float lon=long1/180*pi;
float dlx=6371000*(lon-lon11);
float dly=6371000*log(tan(lat/2+M_PI_4))-6371000*log(tan(lat11/2+M_PI_4));
//nav_info->dist=sqrt(dlx*dlx+dly*dly)*cos(lat);
//nav_info->azimuth=atan2(dlx,dly)*180/M_PI+180;
double bearingdegrees =atan2(dlx,dly)*180/pi+180;
return bearingdegrees;
}
/* add setup code here */
int main(int argc, char* argv[])
{
double b =0;
double b2 =0;
double b3 =0;
double lat1 = 55;
double long1 = 37;
double lat2 = 56;
double long2 =38;
// b = Bearing(51.618017,2.48291,51.041394,2.373047); // это вообще хрень
b2 = Bearing2(lat1, long1, lat2, long2)/100.00;
b3 = Bearing3(lat1, long1, lat2, long2);
printf(“%f %f \n”,b2,b3);
return 0;
}
компилируем
gcc 1.c -lm
1.c:4:1: warning: “M_PI_4” redefined
In file included from 1.c:2:
/usr/include/math.h:364:1: warning: this is the location of the previous definition
запускаем и ужасаемся
./a.out
45.000000 29.525853
а вот поправленый код с вашими координатами:
./a.out
96.593246 101.659954
правка замел в Bearing3
c:
*0.00000000174527777777
на
/180*pi
к стати ещё можно в момент инициализации посчитать scaleLongUp/Down по допустим координатам первого вейпоинта
тогда и математика будет простой и ошибки не будет
/180*pi == 0.01745277777777777777
Ахренеть… 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