Arduino Quadcopter

关于这个项目

它不仅仅是一架四轴飞行器……它还是一台开源机器！

现在问题来了，我在哪里以及如何获得四轴飞行器的代码？所以答案是Multiwii。

MultiWii 是一款非常流行的飞行控制器软件，用于 DIY 多旋翼，拥有庞大的社区。它支持各种具有高级功能的多旋翼飞行器，例如智能手机的蓝牙控制、OLED 显示屏、气压计、磁力计、GPS 位置保持和返回家园、LED 灯条等等。那么让我们使用 Arduino 构建我们的飞行控制器吧！

第一步：飞控设计

这是飞行控制器板的原理图。您可以在通用 PCB 上制作一个，也可以像我一样从制造商那里订购 PCB。

电调连接

• D3 <

• D9 <

• D10 <

• D11 <

蓝牙模块连接

• 发送 <<接收

• 接收<<发送

MPU-6050 连接

• A4 <

• A5 <

LED指示灯

• D8 <

接收器连接

• D2 <<油门

• D4 <

• D5 <<副翼

• D6 <<方向舵

• D7 <

第 2 步：构建框架

我买了一个 DJI 450 框架并将我的电机和所有东西都装在上面。你可以看看我是怎么做的视频。

第 3 步：将飞行控制器连接到框架上

然后最后将电调和接收器连接到电路板上，如原理图所示，一切都完成了！

代码

• MultiWii.ino

MultiWii.inoC/C++

#include "Arduino.h"#include "config.h"#include "def.h"#include "types.h"#include "GPS.h"#include "Serial.h"#include "Sensors.h" h"#include "MultiWii.h"#include "EEPROM.h"#include #if GPS//其他GPS功能的函数原型//这些也许可以去gps.h文件，但是这些是gps.cpp 本地静态 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*t2_t3 ,uint32_t* dist);静态无效 GPS_calc_velocity(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(void), int32_t* gps_lng ); );静态无效 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_crosstrack2(void); p_36000(int32_t ang);// Leadig 过滤器 - TODO:重写为普通 C 而不是 C++// 设置 gps 延迟#if defined(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]; // 铅过滤 gps 坐标类 LeadFilter {public:LeadFilter() :_last_velocity(0) { } // 在 CLI 中设置最小和最大无线电值 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;浮动 kI;浮动 kD;浮动 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 的最后一个输入； // 低通滤波器浮点输出的最后导数；浮点导数;} 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->integrator +=((float)error * pid_param->kI) * *dt; pid->integrator =constrain(pid->integrator,-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->derivative =(input - pid->last_input) / *dt; /// 用于导数计算的低通滤波器截止频率。浮动过滤器 =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->derivative - pid->lastderivative); // 更新状态 pid->last_input =input; pid->lastderivative =pid->derivative; // 添加导数组件 return pid_param->kD * pid->derivative;}void reset_PID(struct PID_* pid) { pid->integrator =0; pid->last_input =0; pid->lastderivative =0;}#define _X 1#define _Y 0#define RADX100 0.000174532925 uint8_t land_detect; //检测陆地（外部）静态 uint32_t land_settle_timer;uint8_t GPS_Frame; // 检测到有效的 GPS_Frame，数据已准备好进行导航计算static float dTnav; // 用于导航计算的 Delta 时间（以毫秒为单位），随每个好的 GPS 更新 readstatic 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]; //精确跟踪的前一个航点跟随int32_t target_bearing; // 这是从直升机到“next_WP”位置的角度，单位为度 * 100static int32_t original_target_bearing; // deg * 100，设置next_WP时到next_WP的原始角度，也用于检查我们何时传递WPstatic int16_t crosstrack_error; // 应用于 target_bearing 的角度校正量，以使直升机回到最佳路径上 32_t wp_distance; // 飞机和 next_WP 之间的距离在 cmstatic uint16_t waypoint_speed_gov; // 用于开始导航时慢速结束；//////////////////////////////////// ////////////////////////////////////////////// 移动平均线过滤变量//#define GPS_FILTER_VECTOR_LENGTH 5static uint8_t GPS_filter_index =0;static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];static int32_t GPS_filter_sum[2];static int32_t GPS_read[2];static int32_t GPS_read[2];static int32_t GPS_filtered32_t GPS_static3_t; //没有任何小数的纬度度数 (lat/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; for (axis =0;axis<2;axis++) { GPS_read[axis] =GPS_coord[axis]; //最新的未过滤数据在 GPS_latitude 和 GPS_longitude GPS_degree[axis] =GPS_read[axis] / 10000000; // 获取度数以确保总和适合 int32_t // 我们离度数线有多近？它是度数分数的前三位数字 // 稍后我们用它来检查我们是否接近度数线，如果是，则禁用平均，分数3[axis] =(GPS_read[axis]- GPS_degree[axis]*10000000 ) / 10000; GPS_filter_sum[axis] -=GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] =GPS_read[axis] - (GPS_degree[axis]*10000000); GPS_filter_sum[axis] +=GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] =GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000); if ( NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) { //我们只在poshold模式下使用gps平均... if (fraction3[axis]>1 &&fraction3[axis]<999) GPS_coord[axis] =GPS_filtered[轴]; } } } //dTnav计算 //计算x,y速度和导航pid的时间 dTnav =(float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer =毫秒（）； // 防止因 GPS 故障造成的加速 dTnav =min(dTnav, 1.0); //连续计算gui和其他东西的距离和方位 - 从家到直升机 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) { //如果我们没有设置home，不显示任何东西 GPS_distanceToHome =0; GPS_directionToHome =0; } //检查围栏设置并在必要时执行RTH //TODO:autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT;休息;案例 NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold(); //落地保持 f.THROTTLE_IGNORED =1; //忽略油门杆输入 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;休息;案例 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;跳转时间--; } 休息; case NAV_STATE_PROCESS_NEXT://处理下一个任务步骤 NAV_error =NAV_ERROR_NONE; if (!recallWP(next_step)) { abort_mission(NAV_ERROR_WP_CRC); } else { switch(mission_step.action) { //Waypoiny 和hold 命令都以航路状态开始，它也包括LAND 命令 case MISSION_WAYPOINT:case MISSION_HOLD_TIME:case MISSION_HOLD_UNLIM:case MISSION_LAND:set_new_altitude(mission_step.altitude); GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_prev[LAT], &GPS_prev[LON]); if ((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坐标以进行精确路线计算 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) //如果配置和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; //任务步骤中的海拔高度优先，否则 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; if(_new_alt ==alt.EstAlt){ force_new_altitude(_new_alt);返回; } // 我们从当前位置高度开始，逐渐改变 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; } // 如果我们达到了目标高度，则返回目标 alt 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 =64cm/s 默认下降 int32_t change =(millis() - alt_change_timer)>> _scale; if(alt_change_flag ==ASCENDING){ alt_change +=change; } else { alt_change -=改变； } // 用于生成增量时间 alt_change_timer =millis(); return original_altitude + alt_change;}////////////////////////////////////////// ////////////////////////////////////////基于PID的GPS导航功能//作者：EOSBandi//基于Arducopter团队的代码和想法：Jason Short、Randy Mackay、Pat Hickey、Jose Julio、Jani Hirvinen//Andrew Tridgell、Justin Beech、Adam Rivera、Jean-Louis Naudin、Roberto Navoni//原始约束不适用于变量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(*lat_to); 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 - original_target_bearing; temp =wrap_18000(temp);返回（绝对（温度）> 10000）； // 我们通过了航点 100 度}////////////////////////////////////// //////////////////////////////////////////// 获取两个之间的距离以厘米为单位的点// 获取从 pos1 到 pos2 的方位，返回一个 1deg =100 precisionvoid GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t*earth) { int32_t off_x =*lon2 - *lon1; int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *轴承 =9000 + atan2(-off_y, off_x) * 5729.57795f; //将输出redians转换为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 Lat浮动)(*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.5m/s 以下更准确// 注意：即使位置是使用超前滤波器投影的，速度是根据未改变的 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; if (init) { float tmp =1.0/dTnav; actual_speed[_X] =(float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp; actual_speed[_Y] =(float)(GPS_coord[LAT] - last[LAT]) * tmp; //TODO：如果 (!GPS_conf.lead_filter) { actual_speed[_X] =(actual_speed[_X] + speed_old[_X]) / 2; 检查不切实际的速度变化和信号导航关于可能的 gps 信号衰减actual_speed[_Y] =(actual_speed[_Y] + speed_old[_Y]) / 2; speed_old[_X] =actual_speed[_X]; speed_old[_Y] =actual_speed[_Y]; } } 初始化=1;最后[LON] =GPS_coord[LON]; last[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], actual_speed[_Y], GPS_LAG); }}//////////////////////////////////////////////// //////////////////////////////////// 计算两个 GPS 坐标之间的位置误差// 因为我们使用纬度和经度来计算我们的距离误差，这是一个快速图表：// 100 =1m// 1000 =11m =36 feet// 1800 =19.80m =60 feet// 3000 =33m// 10000 =111m//static 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 错误}///////////////////////////////////////////// /////////////////////////////////////// 根据 x 和 y 计算 nav_lat 和 nav_lon错误和速度//// TODO：检查是否可以为 snappier poshold lockstatic void GPS_calc_poshold(void) { int32_t d; 增加 poshold 目标速度约束。 int32_t target_speed; uint8_t 轴； for (axis=0;axis<2;axis++) { target_speed =get_P(error[axis], &posholdPID_PARAM); // 根据纬度/经度误差计算所需速度 target_speed =constrain(target_speed,-100,100); // 将 poshold 模式下的目标速度限制为 1m/s，这有助于避免失控.. rate_error[axis] =target_speed - actual_speed[axis]; // 计算速度误差 nav[axis] =get_P(rate_error[axis], &poshold_ratePID_PARAM) +get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d =get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d =约束(d, -2000, 2000); // 消除噪音 if(abs(actual_speed[axis]) <50) d =0;导航[轴] +=d; // nav[axis] =constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); nav[axis] =constrain_int16(nav[axis], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[axis].integrator =poshold_ratePID[axis].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 倾斜； uint8_t 轴； GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); //检查是否正常？ cross_speed =约束（cross_speed，-200,200）； cross_speed =-cross_speed;浮动温度 =(9000l - target_bearing) * RADX100;触发[_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]; for (axis=0;axis<2;axis++) { rate_error[axis] =target_speed[axis] - actual_speed[axis]; rate_error[axis] =constrain(rate_error[axis],-1000,1000); nav[axis] =get_P(rate_error[axis], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM); // nav[axis] =constrain(nav[axis],-NAV_BANK_MAX,NAV_BANK_MAX); nav[axis] =constrain_int16(nav[axis], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[axis].integrator =navPID[axis].integrator; }}static void GPS_update_crosstrack(void) { // Crosstrack Error // ---------------- // 如果我们离得太远或太近，我们不会跟踪浮动temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin(temp) * 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.

示意图