Автопилот Arduplane - открытое ПО
Народ, чем отличается ардупилот мега 2.24 и выше от 2.12? Чего там суперкрутого появилось?
Из крутого:
Fly to here
point camera here
Остальное в основном это исправление багов.
Из крутого:
Fly to here
point camera here
а как это выглядит функционально?
Аха, значит, камерку доработали…
Значит, стоит портировать. Спасибо 😉
а как это выглядит функционально?
- Жмакаешь на карте правой кнопкой -> Fly to here, и он летит в указанную точку( ну еще высоту задаешь)
- Жмакаешь на карте правой кнопкой -> point camera here, и камера все время направлена в указанную точку на земле.
никак не могу понять что у меня с маршрутными точками, вернее с домашней точкой. версии пилота и 2.7 и бэта 2.8
хотел опробовать симуляцию возврата и посадки в xplane скачал configtoolglobal с оффсайта, вношу точки все точки сохраняются а домашняя то там то сям но не там где надо после повторного чтения
при записи configtoolglobal выдает предупреждение something wrong
может в 2.7 и выше подругому как маршрут заливают?
Маршрутные точки в ардупилот заливаю через ArduPilotConfigTool как на видео. Но в ардупилоте домашняя точка не сохраняется, она определяется при инициализации GPS. Как в симе не знаю.
странно она както определялась, сделал вывод что если тестируешь в симуляторе то после того как самоль стоит на полосе и связь установлена надо нажимать ресет чтобы актуальные координаты
были гарантировано занесены в память как точка старта и она же возврата…
Облетал прошивку 2.8 в X-Plane 9
какаято странность - самолет уклоняется от точного курса градусов на 15 влево
а вокруг маршрутных точек нарезает круги, это бага или так задумано?
посмотрите курс не прямой а дугой и круги вокруг точек
во вложении круги над вейпоинтами красными стрелками , дуга вместо прямой - синей
какаято странность - самолет уклоняется от точного курса градусов на 15 влево
вот кусочек кода из компаса ардупайлота -
Compass::Compass(AP_Var::Key key) :
_group(key, PSTR(“COMPASS_”)),
_orientation_matrix (&_group, 0),
_offset (&_group, 1),
_ declination (&_group, 2, 0.0, PSTR(“DEC”)),
_null_init_done(false),
product_id(AP_COMPASS_TYPE_UNKNOWN)
{
// Default the orientation matrix to none - will be overridden at group load time
// if an orientation has previously been saved.
_orientation_matrix.set(ROTATION_NONE);
}
компасу както можно задавать магнитное склонение, параметр зависящий от месности где пускаете самолёт. по сути это угл между магнимтным севером и настоящим севером, если ардупайлот пользуется компасом для установки курса то неправильное магнитное склонение даст неправильный курс
ищите как это магнитное склонение сказать железяке
вот тут веб-тулза чтоб узнать магнитное склонение в вашей месности www.ngdc.noaa.gov/geomagmodels/…/calcDeclination
вводите широту и долготу оно считает
ошибка курса в 15 градусов в общем и иожет дать петли вокруг вейпоинтов
удя по вашему профилю вы в москве, для москвы магнитное склонение 10 градусов с копейкамите магнитный север правее настоящего на 10 с копейками градусов
только вот в вашем случае непонятно почему наоборот отклоняется влево
нашел в коде что оно вроде как умеет рботать с какойто хитрой штукой\моделью которая выдает готовые цифры курса которые суются в компас, подозреваю если эта штука активировано то магнитное скланение нужно говорить 0, там толи по гпс толи как курс определяется в этом режиме, в этом случае наверное правельно указанное склонение дало бы обратный хэффект как у вас.
дело в том что симулятор дает проге 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 и неверное тоже самое было в прошлых версиях…
и как оно летало???
огородаме, огородаме)))
а вообще дело не в версиях а в том что там в исходниках нада править коэффициэнты под свою месность.
теперь вы в курсе где косяк и как его решить - с вас патч в мейнстрим))))
вот переделал новый расчет курса.
как говорится почувствуйте разницу!!!
четко по точкам аж квадратные углы))
теперь посмотрите как железка считает растояние - с ним таже проблема