Квадрокоптер Arduino

Об этом проекте

Это не только квадрокоптер ... это машина с открытым исходным кодом!

Теперь возникают вопросы, где и как взять код на квадрокоптер? Итак, ответ - Multiwii.

MultiWii - очень популярное программное обеспечение для управления полетом для мультироторных самоделок с большим сообществом. Он поддерживает различные мультикоптеры с расширенными функциями, такими как управление по Bluetooth с вашего смартфона, OLED-дисплей, барометр, магнитометр, удержание положения GPS и возврат домой, светодиодные ленты и многое другое. Итак, давайте создадим наш полетный контроллер на Arduino!

Шаг 1. Проектирование полетного контроллера

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

Подключения ESC

• D3 <<Сигнальный контакт ESC 1

• D9 <<Сигнальный контакт ESC 3

• D10 <<Сигнальный контакт ESC 2

• D11 <<Сигнальный контакт ESC 4

Подключения модуля Bluetooth

• TX <

• RX <

Соединения MPU-6050

• A4 <

• A5 <

Светодиодный индикатор Indiacator

• D8 <<Анодная ножка светодиода

Подключения приемника

• D2 <<Дроссельная заслонка

• D4 <<Элероны

• D5 <<Элероны

• D6 <<Руль направления

• D7 <

Шаг 2. Создание каркаса

Я купил раму DJI 450 и прикрепил к ней моторы и все остальное. Вы можете посмотреть видео, как я это сделал.

Шаг 3. Присоединение контроллера полета к раме

Затем, наконец, прикрепите esc и приемник к плате, как показано на схемах, и все готово!

Код

• MultiWii.ino

MultiWii.ino C / C ++

 #include "Arduino.h" #include "config.h" #include "def.h" #include "types.h" #include "GPS.h" #include "Serial.h" #include "Датчики. h "#include" MultiWii.h "#include" EEPROM.h "#include  #if GPS // Прототипы функций для других функций GPS // Возможно, они могут быть помещены в файл gps.h, однако это local в gps.cpp static void GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * Bearing); static void GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * latt * lon2, int32_t * lat2, int32 , uint32_t * dist); static void GPS_calc_velocity (void); static void GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); static void GPS_calc_poshold_t * gps_lng); ); static void GPS_calc_nav_rate (uint16_t max_speed); int32_t wrap_18000 (int32_t ang); static bool check_missed_wp (void); void GPS_calc_longitude_scaling (int32_t lat); static void GPS_update_crosstrack (void) p_36000 (int32_t ang); // Leadig filter - TODO:переписать на нормальный C вместо C ++ // Установить задержку gps # если определено (UBLOX) || defined (MTK_BINARY19) #define GPS_LAG 0.5f // UBLOX GPS имеет меньшую задержку, чем MTK и другие # else # define GPS_LAG 1.0f // Мы предполагаем, что MTK GPS имеет задержку в 1 секунду # endif static int32_t GPS_coord_lead [2]; // Lead отфильтрованные gps-координатыclass LeadFilter {public:LeadFilter ():_last_velocity (0) {} // установка минимальных и максимальных значений радио в интерфейсе командной строки int32_t get_position (int32_t pos, int16_t vel, float lag_in_seconds =1.0); void clear () {_last_velocity =0; } private:int16_t _last_velocity;}; int32_t LeadFilter ::get_position (int32_t pos, int16_t vel, float lag_in_seconds) {int16_t Accel_contribution =(vel - _last_velocity) * lag_in_seconds * lag_in_seconds; int16_t vel_contribution =vel * lag_in_seconds; // сохраняем скорость для следующей итерации _last_velocity =vel; return pos + vel_contribution + Accel_contribution;} LeadFilter xLeadFilter; // Фильтр долгой задержки GPS LeadFilter yLeadFilter; // Широта фильтра задержки GPS typedef struct PID_PARAM_ {float kP; float kI; float kD; float Imax; } PID_PARAM; PID_PARAM posholdPID_PARAM; PID_PARAM poshold_ratePID_PARAM; PID_PARAM navPID_PARAM; typedef struct PID_ {интегратор с плавающей запятой; // значение интегратора int32_t last_input; // последний вход для производной float lastderivative; // последняя производная для фильтра нижних частот float output; производная с плавающей точкой;} PID; PID posholdPID [2]; PID poshold_ratePID [2]; PID navPID [2]; int32_t get_P (int32_t error, struct PID_PARAM_ * pid) {return (float) error * pid-> kP;} int32_t get_I (int32_t error, float * dt, struct PID_ * pid, struct PID_PARAM_ * pid_param) {pid-> integrationtor + =((float) error * pid_param-> kI) * * dt; pid-> Integrator =constrain (pid-> integtor, -pid_param-> Imax, pid_param-> Imax); return pid-> integrator;} int32_t get_D (int32_t input, float * dt, struct PID_ * pid, struct PID_PARAM_ * pid_param) {// dt в миллисекундах pid-> производная =(input - pid-> last_input) / * dt; /// Частота среза фильтра нижних частот для вычисления производной. float filter =7.9577e-3; // Устанавливаем на "1 / (2 * PI * f_cut)"; // Примеры для _filter:// f_cut =10 Hz -> _filter =15.9155e-3 // f_cut =15 Hz -> _filter =10.6103e-3 // f_cut =20 Hz -> _filter =7.9577e-3 // f_cut =25 Hz -> _filter =6.3662e-3 // f_cut =30 Hz -> _filter =5.3052e-3 // дискретный фильтр нижних частот, отсекающий // высокочастотный шум, который может свести контроллер с ума pid-> производная =pid-> lastderivative + (* dt / (filter + * dt)) * (pid-> производная - pid-> lastderivative); // обновить состояние pid-> last_input =input; pid-> lastderivative =pid-> производная; // добавляем производный компонент return pid_param-> kD * pid-> производная;} void reset_PID (struct PID_ * pid) {pid-> integtor =0; pid-> last_input =0; pid-> lastderivative =0;} # определить _X 1 # определить _Y 0 # определить RADX100 0.000174532925 uint8_t land_detect; // Обнаружение земли (внешней) static uint32_t land_settle_timer; uint8_t GPS_Frame; // Обнаружен допустимый кадр GPS_Frame, и данные готовы для вычисления навигацииstatic float dTnav; // Дельта-время в миллисекундах для вычислений навигации, обновляется при каждом удачном считывании данных GPS int16_t actual_speed [2] ={0,0}; static float GPS_scaleLonDown; // это используется для компенсации сокращения долготы по мере того, как мы приближаемся к полюсам // Разница между желаемой скоростью и фактической скоростью // обновляется после чтения GPS - 5-10hzstatic int16_t rate_error [2]; static int32_t ошибка [2]; статический int32_t GPS_WP [2]; // В настоящее время используется WPstatic int32_t GPS_FROM [2]; // предыдущая путевая точка для точного трека followint32_t target_bearing; // Это угол от коптера до точки "next_WP" в градусах * 100static int32_t original_target_bearing; // deg * 100, исходный угол до next_WP, когда был установлен next_WP, Также используется для проверки, когда мы передаем WPstatic int16_t crossstrack_error; // Величина угловой коррекции, применяемая к target_bearing, чтобы вернуть коптер на оптимальную траекторию uint32_t wp_distance; // расстояние между самолетом и next_WP в cmstatic uint16_t waypoint_speed_gov; // используется для медленного накручивания при запуске навигации; ////////////////////////////////////// //////////////////////////////////////////////// Скользящее среднее переменные фильтра // # определить GPS_FILTER_VECTOR_LENGTH 5static uint8_t GPS_filter_index =0; статический int32_t GPS_filter [2] [GPS_FILTER_VECTOR_LENGTH]; статический int32_t GPS_filter_sum [2]; статический int32_t_GPS_filter_sum [2]; статический int32_t_ static_read [2];; // градус широты и долготы без десятичных знаков (лат / 10 000 000) static uint16_t Fraction3 [2]; static int16_t nav_takeoff_bearing; // сохраняет азимут при взлете (1deg =1), используемый для поворота в направлении взлета по прибытии домой // Главный навигационный процессор и механизм состояний // TODO:добавить текущие состояния, чтобы облегчить нагрузку на обработку uint8_t GPS_Compute (void) {unsigned char axis; uint32_t dist; // временная переменная для сохранения расстояния в коптер int32_t dir; // временная переменная для хранения каталога коптера static uint32_t nav_loopTimer; // проверяем, что у нас есть допустимый фрейм, если нет, то немедленно возвращаем if (GPS_Frame ==0) return 0; иначе GPS_Frame =0; // проверяем исходное положение и устанавливаем его, если оно не было установлено if (f.GPS_FIX &&GPS_numSat> =5) {#if! defined (DONT_RESET_HOME_AT_ARM) if (! f.ARMED) {f.GPS_FIX_HOME =0;} #endif if (! f.GPS_FIX_HOME &&f.ARMED) {GPS_reset_home_position (); } // Применяем фильтр скользящего среднего к данным GPS if (GPS_conf.filtering) {GPS_filter_index =(GPS_filter_index + 1)% GPS_FILTER_VECTOR_LENGTH; для (ось =0; ось <2; ось ++) {GPS_read [ось] =GPS_coord [ось]; // последние нефильтрованные данные находятся в GPS_latitude и GPS_longitude GPS_degree [axis] =GPS_read [axis] / 10000000; // получить градус, чтобы убедиться, что сумма соответствует int32_t // Насколько близко мы находимся к линии градуса? это первые три цифры из долей градуса // позже мы используем его, чтобы проверить, приближаемся ли мы к линии градуса, если да, отключим усреднение, дробь 3 [ось] =(GPS_read [ось] - GPS_degree [ось] * 10000000 ) / 10000; GPS_filter_sum [ось] - =GPS_filter [ось] [GPS_filter_index]; GPS_filter [ось] [GPS_filter_index] =GPS_read [ось] - (GPS_degree [ось] * 10000000); GPS_filter_sum [ось] + =GPS_filter [ось] [GPS_filter_index]; GPS_filtered [ось] =GPS_filter_sum [ось] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree [ось] * 10000000); if (NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) {// мы используем усреднение GPS только в режиме ожидания ... if (дробь 3 [ось]> 1 &&дробь 3 [ось] <999) GPS_coord [ось] =GPS_filtered [ ось]; }}} // Расчет dTnav // Время для расчета скорости x, y и навигационных pid dTnav =(float) (millis () - nav_loopTimer) / 1000.0; nav_loopTimer =миллис (); // предотвращаем запуск из-за плохого GPS dTnav =min (dTnav, 1.0); // постоянное вычисление расстояния и пеленгов для графического интерфейса и прочего - От дома до коптера GPS_bearing (&GPS_coord [LAT], &GPS_coord [LON], &GPS_home [LAT], &GPS_home [LON], &dir); GPS_distance_cm (&GPS_coord [LAT], &GPS_coord [LON], &GPS_home [LAT], &GPS_home [LON], &dist); GPS_distanceToHome =dist / 100; GPS_directionToHome =dir / 100; if (! f.GPS_FIX_HOME) {// Если у нас нет домашнего набора, ничего не отображать GPS_distanceToHome =0; GPS_directionToHome =0; } // Проверяем настройку ограждения и при необходимости выполняем RTH // TODO:автоматическая высадка if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; ломать; case NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold (); // Приземляемся в удержание позиции f.THROTTLE_IGNORED =1; // Игнорировать стик Throtte input f.GPS_BARO_MODE =1; // Возьмите под контроль режим BARO land_detect =0; // Сброс детектора земли f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Отметить наземный процесс NAV_state =NAV_STATE_LAND_IN_PROGRESS; ломать; case NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold (); check_land (); // Вызов детектора земли if (f.LAND_COMPLETED) {nav_timer_stop =millis () + 5000; NAV_state =NAV_STATE_LANDED; } ломать; case NAV_STATE_LANDED:// Снять с охраны, если ручка THROTTLE находится минимум или через 5 секунд после обнаружения земли if (rcData [THROTTLE]  0) {// если ноль не достигнут, выполнить прыжок next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } ломать; case NAV_STATE_PROCESS_NEXT:// Обработка следующего шага миссии NAV_error =NAV_ERROR_NONE; если (! вспомнитьWP (следующий_шаг)) {abort_mission (NAV_ERROR_WP_CRC); } else {switch (mission_step.action) {// Команды Waypoiny и hold все начинаются со статусом enroute, он также включает команду LAND case MISSION_WAYPOINT:case MISSION_HOLD_TIME:case MISSION_HOLD_UNLIM:case MISSION_LAND:set_new_altitude (mission_step); GPS_set_next_wp (&mission_step.pos [LAT], &mission_step.pos [LON], &GPS_prev [LAT], &GPS_prev [LON]); если ((wp_distance / 100)> =GPS_conf.safe_wp_distance) abort_mission (NAV_ERROR_TOOFAR); иначе NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev [LAT] =mission_step.pos [LAT]; // Сохраняем координаты wp для точного маршрута calc GPS_prev [LON] =mission_step.pos [LON]; ломать; case MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) // если config и mission_step alt равны нулю set_new_altitude (alt.EstAlt); // RTH возвращается на фактической высоте else {uint32_t rth_alt; если (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; // высота на шаге миссии имеет приоритет else rth_alt =mission_step.altitude; если (alt.EstAlt  0 &&mission_step.parameter1  GPS_conf.nav_max_altitude * 100) _new_alt =GPS_conf.nav_max_altitude * 100; если (_new_alt ==alt.EstAlt) {force_new_altitude (_new_alt); возвращение; } // Мы начинаем с высоты текущего местоположения и постепенно меняем alt_to_hold =alt.EstAlt; // для расчета дельта-времени alt_change_timer =millis (); // сохраняем заданную высоту target_altitude =_new_alt; // сбрасываем наш интегратор высоты alt_change =0; // сохраняем исходную высоту original_altitude =alt.EstAlt; // чтобы решить, достигли ли мы заданной высоты if (target_altitude> original_altitude) {// мы ниже, поднимаемся вверх alt_change_flag =ASCENDING; } else if (target_altitude  =target_altitude) alt_change_flag =REACHED_ALT; // мы не должны командовать мимо нашей цели if (alt_to_hold> =target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) {// мы наверху, идем вниз if (alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // мы не должны командовать мимо нашей цели if (alt_to_hold <=target_altitude) return target_altitude; } // если мы достигли нашей целевой высоты, возвращаем целевую высоту if (alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs (alt_to_hold - target_altitude); // масштаб - это то, как мы генерируем желаемую скорость из прошедшего времени // меньший масштаб означает более высокие скорости int8_t _scale =4; if (alt_to_hold > 4 =снижение 64 см / с по умолчанию int32_t change =(millis () - alt_change_timer)>> _scale; если (alt_change_flag ==по возрастанию) {alt_change + =изменить; } else {alt_change - =изменить; } // для генерации дельты времени alt_change_timer =millis (); return original_altitude + alt_change;} /////////////////////////////////////////////////// ////////////////////////////////////////// Функции GPS-навигации на основе PID // Автор :EOSBandi // На основе кода и идей команды Arducopter:Джейсон Шорт, Рэнди Маккей, Пэт Хики, Хосе Хулио, Яни Хирвинен // Эндрю Триджелл, Джастин Бич, Адам Ривера, Жан-Луи Ноден, Роберто Навони // исходное ограничение не работает с переменными int16_t constrain_int16 (int16_t amt, int16_t low, int16_t high) {return ((amt) <(low)? (low):((amt)> (high)? (high) :( amt))); } ///////////////////////////////////////////////// ///////////////////////////////////// это используется для компенсации сокращения долготы, когда мы идем к полюса // Это нормально рассчитать один раз для каждой настройки путевой точки, так как она немного меняется в пределах досягаемости мультикоптера // void GPS_calc_longitude_scaling (int32_t lat) {GPS_scaleLonDown =cos (lat * 1.0e-7f * 0.01745329251f);} / ////////////////////////////////////////////////// ///////////////////////// ////////// Устанавливает путевую точку для навигации, сбрасывает необходимые переменные и вычисляет начальные значения // void GPS_set_next_wp (int32_t * lat_to, int32_t * lon_to, int32_t * lat_from, int32_t * lon_from) {GPS_WP [LAT] =* lat_to; GPS_WP [LON] =* lon_to; GPS_FROM [LAT] =* lat_from; GPS_FROM [LON] =* lon_from; GPS_calc_longitude_scaling (* широта до); GPS_bearing (&GPS_FROM [LAT], &GPS_FROM [LON], &GPS_WP [LAT], &GPS_WP [LON], &target_bearing); GPS_distance_cm (&GPS_FROM [LAT], &GPS_FROM [LON], &GPS_WP [LAT], &GPS_WP [LON], &wp_distance); GPS_calc_location_error (&GPS_WP [LAT], &GPS_WP [LON], &GPS_FROM [LAT], &GPS_FROM [LON]); waypoint_speed_gov =GPS_conf.nav_speed_min; original_target_bearing =target_bearing;} ///////////////////////////////////////////////////////////// /////////////////////////////////////// Проверьте, не пропустили ли мы как-то пункт назначения // static bool check_missed_wp (void) {int32_t temp; temp =target_bearing - исходный_целевой_брел; temp =wrap_18000 (темп); return (abs (temp)> 10000); // мы прошли маршрутную точку на 100 градусов} //////////////////////////////////////// ////////////////////////////////////////////// Определение расстояния между двумя точки в см // Получение пеленга от pos1 до pos2, возвращает точность 1deg =100 без GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * Bearing) {int32_t off_x =* lon2 - * lon1; int32_t off_y =(* широта2 - * широта1) / GPS_scaleLonDown; * подшипник =9000 + atan2 (-off_y, off_x) * 5729.57795f; // Преобразование выходных редианов в 100xdeg if (* Bearing <0) * Bearing + =36000;} void GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist) {float dLat =( float) (* lat2 - * lat1); // разница широты в 1/10 000 000 градусов float dLon =(float) (* lon2 - * lon1) * GPS_scaleLonDown; // x * dist =sqrt (sq (dLat) + sq (dLon)) * 1.11318845f;} // ************************ ************************************************* *************************** // calc_velocity_and_filtered_position - скорость в долготном и широтном направлениях, вычисленная из GPS-положения // и данных акселерометра // lon_speed выражается в см / с. положительные числа означают движение на восток // lat_speed, выраженное в см / с. положительные числа при движении на север // Примечание:мы используем GPS-координаты напрямую для расчета скорости вместо того, чтобы запрашивать скорость у GPS, потому что // это более точно ниже 1,5 м / с // Примечание:даже если положения проецируются с использованием свинцового фильтра, // скорости вычисляются из неизмененных местоположений GPS. Мы не хотим, чтобы шум от нашего свинцового фильтра влиял на скорость // ************************************ ************************************************* **************** static void GPS_calc_velocity (void) {static int16_t speed_old [2] ={0,0}; статический int32_t last [2] ={0,0}; статический uint8_t init =0; если (инициализация) {float tmp =1.0 / dTnav; actual_speed [_X] =(float) (GPS_coord [LON] - последний [LON]) * GPS_scaleLonDown * tmp; actual_speed [_Y] =(float) (GPS_coord [LAT] - последний [LAT]) * tmp; // TODO:проверять нереалистичные изменения скорости и сигнализировать о возможном ухудшении сигнала GPS if (! GPS_conf.lead_filter) {actual_speed [_X] =(actual_speed [_X] + speed_old [_X]) / 2; фактическая_ скорость [_Y] =(фактическая_ скорость [_Y] + скорость_стар [_Y]) / 2; speed_old [_X] =фактическая_ скорость [_X]; speed_old [_Y] =фактическая_ скорость [_Y]; }} init =1; последний [LON] =GPS_coord [LON]; последний [LAT] =GPS_coord [LAT]; if (GPS_conf.lead_filter) {GPS_coord_lead [LON] =xLeadFilter.get_position (GPS_coord [LON], actual_speed [_X], GPS_LAG); GPS_coord_lead [LAT] =yLeadFilter.get_position (GPS_coord [LAT], фактическая_скорость [_Y], GPS_LAG); }} //////////////////////////////////////////////// ////////////////////////////////////// Вычислить ошибку местоположения между двумя координатами GPS // Потому что мы мы используем широту и долготу для определения наших ошибок расстояния вот краткая диаграмма:// 100 =1 м // 1000 =11 м =36 футов // 1800 =19,80 м =60 футов // 3000 =33 м // 10000 =111 м // статика void GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng) {error [LON] =(float) (* target_lng - * gps_lng) * GPS_scaleLonDown; // Ошибка X error [LAT] =* target_lat - * gps_lat; // Ошибка Y} ///////////////////////////////////////////// ///////////////////////////////////////// Вычислить nav_lat и nav_lon по осям x и y ошибка и скорость //// ЗАДАЧИ:проверьте, что ограничение целевой скорости poshold может быть увеличено для более быстрого poshold lockstatic void GPS_calc_poshold (void) {int32_t d; int32_t target_speed; ось uint8_t; для (ось =0; ось <2; ось ++) {target_speed =get_P (ошибка [ось], &posholdPID_PARAM); // вычисляем желаемую скорость на основе ошибки широты / долготы target_speed =constrain (target_speed, -100,100); // Ограничиваем целевую скорость в режиме poshold до 1 м / с, это помогает избежать разбега .. rate_error [axis] =target_speed - actual_speed [axis]; // вычисление ошибки скорости nav [ось] =get_P (rate_error [ось], &poshold_ratePID_PARAM) + get_I (rate_error [axis] + error [axis], &dTnav, &poshold_ratePID [axis], &poshold_ratePID_PARAM); d =get_D (ошибка [ось], &dTnav, &poshold_ratePID [ось], &poshold_ratePID_PARAM); d =ограничение (d, -2000, 2000); // избавляемся от шума if (abs (actual_speed [axis]) <50) d =0; nav [ось] + =d; // навигация [ось] =ограничение (навигация [ось], -NAV_BANK_MAX, NAV_BANK_MAX); nav [ось] =constrain_int16 (nav [ось], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID [ось] .integrator =poshold_ratePID [ось] .integrator; }} //////////////////////////////////////////////// ////////////////////////////////////// Вычислите желаемые значения nav_lat и nav_lon для дальних полетов, таких как RTH и WP // static void GPS_calc_nav_rate (uint16_t max_speed) {float trig [2]; int32_t target_speed [2]; int32_t tilt; ось uint8_t; GPS_update_crosstrack (); int16_t cross_speed =crossstrack_error * (GPS_conf.crosstrack_gain / 100.0); // проверяем все в порядке? cross_speed =ограничение (cross_speed, -200,200); cross_speed =-cross_speed; температура поплавка =(9000л - target_bearing) * RADX100; trig [_X] =cos (темп); триггерный [_Y] =грех (темп); target_speed [_X] =max_speed * trig [_X] - cross_speed * trig [_Y]; target_speed [_Y] =cross_speed * trig [_X] + max_speed * trig [_Y]; для (ось =0; ось <2; ось ++) {rate_error [ось] =target_speed [ось] - фактическая_скорость [ось]; rate_error [ось] =ограничение (rate_error [ось], - 1000,1000); nav [ось] =get_P (rate_error [ось], &navPID_PARAM) + get_I (rate_error [ось], &dTnav, &navPID [ось], &navPID_PARAM) + get_D (rate_error [ось], &dTnav, &navPID [ось], &navPID; // навигация [ось] =ограничение (навигация [ось], - NAV_BANK_MAX, NAV_BANK_MAX); nav [ось] =constrain_int16 (nav [ось], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID [ось] .integrator =navPID [ось] .integrator; }} static void GPS_update_crosstrack (void) {// Ошибка Crosstrack // ---------------- // Если мы слишком далеко или слишком близко, мы не отслеживаем следующее за поплавком temp =(target_bearing - исходный_target_bearing) * RADX100; crossstrack_error =грех (темп) * расстояние wp_distance; // Счетчики, мы находимся вне линии} ///////////////////////////////////////// ///////////////////////////////////////////// Определение желаемой скорости при навигации по направлению к путевой точке, также реализуйте медленное // увеличение скорости при запуске навигации //// |  waypoint_speed_gov){ waypoint_speed_gov +=(int)(100.0 * dTnav); // increase at .5/ms max_speed =waypoint_speed_gov; } return max_speed;}////////////////////////////////////////////////////////////////////////////////////// Utilities//int32_t wrap_36000(int32_t ang) { if (ang> 36000) ang -=36000; if (ang <0) ang +=36000; return ang;}/* * EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution * with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased * resolution also increased precision of nav calculations*/#define DIGIT_TO_VAL(_x) (_x - '0')uint32_t GPS_coord_to_degrees(char* s) { char *p, *q; uint8_t deg =0, min =0; unsigned int frac_min =0; uint8_t i; // scan for decimal point or end of field for (p =s; isdigit(*p); p++); q =s; // convert degrees while ((p - q)> 2) { if (deg) deg *=10; deg +=DIGIT_TO_VAL(*q++); } // convert minutes while (p>
 q) { if (min) min *=10; min +=DIGIT_TO_VAL(*q++); } // convert fractional minutes // expect up to four digits, result is in // ten-thousandths of a minute if (*p =='.') { q =p + 1; for (i =0; i <4; i++) { frac_min *=10; if (isdigit(*q)) frac_min +=*q++ - '0'; } } return deg * 10000000UL + (min * 1000000UL + frac_min*100UL) / 6;}// helper functions uint16_t grab_fields(char* src, uint8_t mult) { // convert string to uint16 uint8_t i; uint16_t tmp =0; for(i=0; src[i]!=0; i++) { if(src[i] =='.') { i++; if(mult==0) break; else src[i+mult] =0; } tmp *=10; if(src[i]>='0' &&src[i] <='9') tmp +=src[i]-'0'; } return tmp;}uint8_t hex_c(uint8_t n) { // convert '0'..'9','A'..'F' to 0..15 n -='0'; if(n>9) n -=7; n &=0x0F; return n;} //************************************************************************// Common GPS functions //void init_RTH() { f.GPS_mode =GPS_MODE_RTH; // Set GPS_mode to RTH f.GPS_BARO_MODE =true; GPS_hold[LAT] =GPS_coord[LAT]; //All RTH starts with a poshold GPS_hold[LON] =GPS_coord[LON]; //This allows to raise to rth altitude GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON], &GPS_hold[LAT], &GPS_hold[LON]); NAV_paused_at =0; if (GPS_conf.rth_altitude ==0) set_new_altitude(alt.EstAlt); //Return at actual altitude else { // RTH altitude is defined, but we use it only if we are below it if (alt.EstAlt =5) { GPS_home[LAT] =GPS_coord[LAT]; GPS_home[LON] =GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); //need an initial value for distance and bearing calc nav_takeoff_bearing =att.heading; //save takeoff heading //TODO:Set ground altitude f.GPS_FIX_HOME =1; }}//reset navigation (stop the navigation processor, and clear nav)void GPS_reset_nav(void) { uint8_t i; for(i=0;i<2;i++) { nav[i] =0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); reset_PID(&navPID[i]); NAV_state =NAV_STATE_NONE; //invalidate JUMP counter jump_times =-10; //reset next step counter next_step =1; //Clear poi GPS_poi[LAT] =0; GPS_poi[LON] =0; f.GPS_head_set =0; }}//Get the relevant P I D values and set the PID controllers void GPS_set_pids(void) { posholdPID_PARAM.kP =(float)conf.pid[PIDPOS].P8/100.0; posholdPID_PARAM.kI =(float)conf.pid[PIDPOS].I8/100.0; posholdPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.kP =(float)conf.pid[PIDPOSR].P8/10.0; poshold_ratePID_PARAM.kI =(float)conf.pid[PIDPOSR].I8/100.0; poshold_ratePID_PARAM.kD =(float)conf.pid[PIDPOSR].D8/1000.0; poshold_ratePID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; navPID_PARAM.kP =(float)conf.pid[PIDNAVR].P8/10.0; navPID_PARAM.kI =(float)conf.pid[PIDNAVR].I8/100.0; navPID_PARAM.kD =(float)conf.pid[PIDNAVR].D8/1000.0; navPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; }//It was moved here since even i2cgps code needs itint32_t wrap_18000(int32_t ang) { if (ang> 18000) ang -=36000; if (ang <-18000) ang +=36000; return ang;}/**************************************************************************************//**************************************************************************************/...This file has been truncated, please download it to see its full contents.

Схема