Программный код

Заметка посвящена изучению кода Ardupilot 2.8 (и унаследованных огрехов из предыдущих)

Цели:

Изучение кода с тем чтобы описать особенности использования устройства
Выявление и устранение ошибок
Очистка от устаревшего функционала для освобождения пространства в памяти

Для проверки сделанных выводов использованы

XPLANE 9 desktop с моделью PT60RC
ArdupilotSim - для связи симулятора и usb-serial
arduino duemilanove - в качестве usb-serial адаптера
плата ArduPilot-v15 с дополнительными соединениями по каналам газа и руля направления
Прошивка Ardupilot 2.8 beta

Выявленные недочеты:

Существенная ошибка в навигации приводящая к движению "по дуге" по маршрутным точкам (исправлено в beta 4)
Резкий подброс модели в режиме landing при достижении координат точки указанной в качестве точки приземления (устранено в beta 5 посредством плавного увеличения угла)
Ошибка отключения газа от канала ручного управления в заключительной стадии автоматической посадки (устранено в beta 5)
Ошибка навигации по высоте порядка -20м (устранено в beta 5 введен коэф PID в зависимости удаления от точки)
Ошибка навигации по курсу порядка +/- 5 градусов (устранено в beta 5 введен коэф PID в зависимости удаления от точки)
Резкие маневры вблизи маршрутных точек (устранено в beta 5 введен коэф PID в зависимости удаления от точки)
Неточное прохождение маршрутных точек (не устранено)
Определение пропадания сигнала от передатчика для аналогового приемника, не снабженного Failsafe (не устранено)
Расчет курса относительно положения фюзеляжа самолета (гироскоп), не берется в расчет масса модели, скорость, т.е. момент инерции (это особенно заметно если задать большое усиление в канале руля направления без сглаживания) (не устранено)
Отсутствие учета сдвига домашней точки. (не устранено)

Алгоритмы кода

Общие принципы.

Редактирование кода может осуществляться в редакторе Arduino ide, но существуют другие редакторы (MS Visual Studio c++, atmel avr studio ). Родная среда ардуино всем хороша для работы с одним pde файлом, однако когда используется множество файлов и библиотек как в ардупилоте, становится неудобным поиск, т.к. нет поддержки поиска во всех открытых страницах, этого недостатка лишена MS Visual Studio, она умеет искать на всех открытых вкладках, есть даже плагин для нее для работы с ардуино (мне пока попробовать не удалось) собственно я открываю код в двух редакторах в студии ищу и изучаю код, а в ардуино-редакторе вношу правки и заливаю в плату, после каждого сохранения в среде разработки ардуино Visual Studio определяет что данные изменены и запрашивает обновить.

Особенности аппаратной платформы ардуино в том что процессор обладает одним ядром, ни среда разработки ни аппаратная платформа не поддерживает многопоточность, вместо этого используются прерывания и два основных цикла setup и loop. Язык программирования с++ при этом в случае если вам непонятна языковая конструкция - ответ следует искать в пособиях и форумах по этому языку

Основным модулем считается pde файл имеющий название совпадающее с названием папки в которую он вложен, в начале этого файла идет декларация библиотек и глобальных переменных в соответствии с требованиями с++ затем две процедуры setup и loop, после каждого сброса сначала один раз выполняется

программный код записанный в процедуру setup а затем бесконечное количество раз (или пока не зависнет изза ошибки в коде) код процедуры loop.

Код ардупилота (а также платы сенсоров) сделан по принципу нескольких циклов выполняющихся последовательно, один цикл (fast loop) выполняется 50 раз в секунду, второй (medium loop) 10, в ветки этих циклов расставлены функции которые должны выполняться с требуемой периодичностью

Ручной режим

канал руля высоты подсоединен к порту ардуино № ...

Считывание канала автоматически по прерываниям и доступно в переменной ... типа ...

туду: дописать алгоритмы

Устранение недочетов.

1. Ошибка заключалась в неверных функциях, что приводило к искажениям при расчете курса и дистанции в завсисимости от долготы и направления до десятка градусов. Новые функции адаптированы из Ardupilot-mega и выглядят так:

ong 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);

}

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;

return bearing;

}

2. Резкий подброс модели в режиме landing при достижении координат точки указанной в качестве точки приземления.

В алгоритме ардупилота предусмотрена величина угла кабрирования при посадке задаваемая константой

#define LAND_PITCH

В заключительной стадии посадки происходило единовременное изменение угла от 0 дл 10-15 градусов что приводило к резкому "подбросу" модели, исправлено заменой резкого увеличения угла кабрирования на плавный посредством использования функции

void incr_land_pith() // todo: по значению верт. скорости при наличии сонара

{

if (nav_pitch < LAND_PITCH * 100) { //increased by 1/20 deg each call

nav_pitch += 5;

}

3. Ошибка отключения газа от канала ручного управления заключалась в том что канал газа задавался и при чтении каналов приемника в модуле radio, процедура read_radio()

rc_3.servo_out = rc_3.control_in;

и соответственно, попытки установить иные значения в навигации не обеспечивали требуемых значений на всех стадиях посадки. В качестве решения из процедуры read_radio() была убрана эта строка и помещена в соответствующие секции update_current_flight_mode() в те режимы где ручная коррекция газа востребована.

4. Ошибка навигации по высоте - причина ошибки пока не локализована, и вероятно находится в калькуляции PID но она нивелирована функцией в которой речь в пункте 6. Вероятно одной из причин является то что пидя дают отклонение на коррекцию существующей ошибки но реакция модели запаздывает и к следующему моменту времени хотя существующаяя ошибка и устранена, но появляется новая. Усиление же значений пид посредством задания встроенных параметров приводит к колебаниям модели при резких маневрах и в маршрутных точках

5. Ошибка навигации по курсу - причина ошибки пока не локализована, и вероятно находится в калькуляции PID но она нивелирована функцией в которой речь в пункте 6

6. Резкие маневры вблизи маршрутных точек. во избежание этого в зависимости от близости к маршрутной точке введены весовые коэффициенты допускающие сильное маневрирование при большом удалении от маршрутных точек, и сдерживающие резкие маневры при близости к точке

функция вычисления коэф. коррекции ошибки высоты

float K_alt_by_wp_distance()

{

if (wp_distance > 100)

{ return 4.0f;}

else if (wp_distance > 60)

{ return 2.0f;}

else if (wp_distance > 40)

{ return 1.0f;}

else

{ return 0.1f;}

}

и использована при вычислении вектора курса

nav_pitch = pidNavPitchAltitude.get_pid(altitude_error * K_alt_by_wp_distance() , dTnav);

функция вычисления коэф. коррекции ошибки курса:

float K_nav_by_wp_distance()

{

if (wp_distance > 100)

{

if abs(bearing_error > 60 )

{ return 1.0f; } else {return 2.0f;}

}

else if (wp_distance > 22)

{ return 1.0f;}

else

{ return 0.1f;} // disable naw near waypoint

}

7. В ардупилоте маршрутная точка считается достигнутой если расстояние до нее составляет wp_radius - радиус заданный из утилиты configtoolglobal вместе с координатами точек, т.е достигнув радиуса точки автопилот начинает навигацию к следующей, не достигая по возможности ближайшей дистанции к точке, ошибка существенна если задать радиус 20м и использовать его для посадки, логично изменить код таким образом что считать, что точка достигнута в тот момент, после проходжения радиуса когда дистанция от точки начинает возрастать.

8. по этому пункту, пока сказать нечего, не реализовано.

9. Нет учета момента инерции модели.

На следующем скриншоте изображена траектория взлета модели до точки взлета, затем должно быть снижение до высоты первой маршрутной точки, но модель снижается ниже необходимой траектории и затем пытается вновь набрать высоту

Google Sites

Report abuse