Автопилот Arduplane - открытое ПО
дело в том что симулятор дает проге ArdupilotSim истинный, готовый курс, который в свою очередь передается плате ардупилота в байте YAW (тут имеет смысл конечно поверить, вдруг опять бага именно в ArdupilotSim )
но почемуто мне кажется что если была ошибка в магнитном склонении то дуги былибы направлены в одну сторону, например к северу , а дефакто как будто самоль всегда тянет влево, в результате из дуг прямого и обратного курса получается бочка “()” как будто его отталкивает бесполетная зона над ВПП
который в свою очередь передается плате ардупилота в байте YAW
вот про это я и говорю, там эта штука судя по коду когда приходят данные снаружи просто засовывает их в компас, а компас когда отдает обратно вносит поправку на магнитное склонение. те компас видит вместо магнитного севера реальный север, и поправка на магнитное склонение заставляет его лететь левее. по крайней мере так это выглядит в коде. попробуйте плате сказать что магнптное склонение равно нулю и посмотреть что будет. разумеется в реальнмо полёте магнитное склонение нужно указать правильное. опятьже это всё догадкана основе беглово анализа кода
а, мысль понял, вечерком попробую!, спасибо
хотя забавно будет если компас в конфиге выключен а магнитное склонение влияет…
надыбал такой пост:
diydrones.com/…/special-request-for-a-new?id=70584…
там речь о функции:
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;
}
судя по скриншотам, как раз таже хрень
хехе фокус, видать кусок старого кода в новую версию закоммитили гдето
вот как у них:
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км.