Arduino Quadcopter

このプロジェクトについて

クワッドコプターだけではありません...オープンソースマシンです！

今、質問が来ます、クワッドコプターのコードをどこでどのように入手しますか？答えはMultiwiiです。

MultiWiiは、大規模なコミュニティを持つDIYマルチローター用の非常に人気のあるフライトコントローラーソフトウェアです。スマートフォンによるBluetooth制御、OLEDディスプレイ、気圧計、磁力計、GPS位置保持と帰宅、LEDストリップなど、高度な機能を備えたさまざまなマルチコプターをサポートしています。それでは、Arduinoを使用してフライトコントローラーを構築しましょう！

ステップ1：フライトコントローラーの設計

フライトコントローラーボードの回路図は次のとおりです。汎用PCBで作成することも、私が行ったようにメーカーにPCBを注文することもできます。

ESC接続

• D3 <

• D9 <

• D10 <

• D11 <

Bluetoothモジュールの接続

• TX <

• RX <

MPU-6050接続

• A4 <

• A5 <

LEDインディアケーター

• 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ファイルに移動する可能性がありますが、これらはgps.cppにローカルstaticvoid 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 * lat2、int 、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（void）; static uint16_t GPS_calc ）; 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）; int32_t wra p_36000（int32_t ang）; // Leadigフィルター-TODO：C ++ではなく通常のCに書き換えます// gpslag＃if defined（UBLOX）を設定します|| defined（MTK_BINARY19）#define GPS_LAG 0.5f // UBLOX GPSのラグはMTKおよびother＃else＃define GPS_LAG 1.0fよりも小さい// MTK GPSのラグは1秒であると想定しています＃endifstatic int32_t GPS_coord_lead [2]; //リードフィルタリングされたgpscoordinatesclassLeadFilter {public：LeadFilter（）：_ last_velocity（0）{} // CLIで最小および最大無線値を設定int32_tget_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ラグフィルターLeadFilteryLeadFilter; //緯度GPSラグフィルターtypedefstruct 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_ {float integrator; //インテグレータ値int32_tlast_input; //微分floatlastderivativeの最後の入力; //ローパスフィルターフロート出力の最後の導関数; float派生;} 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エラー、float * dt、struct PID_ * pid、struct PID_PARAM_ * pid_param）{pid-> integrator + =（（float）error * pid_param-> kI）* * dt; pid->積分器=constraint（pid->積分器、-pid_param-> Imax、pid_param-> Imax）; return pid-> integer;} int32_t get_D（int32_t input、float * dt、struct PID_ * pid、struct PID_PARAM_ * pid_param）{// dt（ミリ秒）pid->微分=（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; //派生コンポーネントを追加returnpid_param-> kD * pid-> derivative;} void reset_PID（struct PID_ * pid）{pid-> integer =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が検出され、データはnav計算の準備ができていますstatic float dTnav; //ナビゲーション計算のミリ秒単位のデルタ時間。GPSが適切であるたびに更新されます。readstaticint16_tactual_speed[2] ={0,0}; static float GPS_scaleLonDown; //これは、極に向かって縮小する経度を相殺するために使用されます//目的の移動速度と実際の移動速度の差// GPS読み取り後に更新-5-10hzstaticint16_t rate_error [2]; static int32_t error [2]; static int32_t GPS_WP [2]; //現在使用されているWPstaticint32_t GPS_FROM [2]; //正確なトラックを追跡するための以前のウェイポイントint32_ttarget_bearing; //これは、ヘリコプターから「next_WP」の位置までの角度（度単位）です* 100static int32_t original_target_bearing; // deg * 100、next_WPが設定されたときのnext_WPへの元の角度。WPstaticint16_tcrosstrack_errorを渡すときのチェックにも使用されます。 //ヘリコプターを最適なpathuint32_twp_distanceに戻すためにtarget_bearingに適用される角度補正の量。 //平面とnext_WP間の距離（cmstatic uint16_t waypoint_speed_gov）; //ナビゲーション開始時の低速巻き上げに使用; ////////////////////////////////////// //////////////////////////////////////////////// 移動平均フィルタ変数//＃defineGPS_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; //小数点なしの緯度経度（lat / 10 000 000）static uint16_tfraction3 [2]; static int16_t nav_takeoff_bearing; //自宅に到着したときに離陸方向に回転するために使用される離陸時の方位を保存します//メインナビゲーションプロセッサと状態エンジン// TODO：処理の負担を軽減するために進行中の状態を追加しますuint8_t GPS_Compute（void）{unsigned char axis; uint32_t dist; // distをcopterint32_tdirに格納する一時変数; // dirをヘリコプターに格納する一時変数staticuint32_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桁//後で、次数の線に近いかどうかを確認するために使用します。近い場合は、平均化を無効にします。fraction3[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 =millis（）; // 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）{//ホームが設定されていない場合は、何も表示しないGPS_distanceToHome =0; GPS_directionToHome =0; } //フェンスの設定を確認し、必要に応じてRTHを実行します// TODO：自動着陸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：//スロットルスティックが最小または土地が検出されてから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; if（！recallWP（next_step））{abort_mission（NAV_ERROR_WP_CRC）; } else {switch（mission_step.action）{// Waypoinyおよびholdコマンドはすべて、途中のステータスで開始します。これには、LANDコマンドも含まれます。caseMISSION_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];壊す;ケースMISSION_RTH：f.GPS_head_set =0; if（GPS_conf.rth_altitude ==0 &&Mission_step.altitude ==0）// configおよびmission_stepaltがゼロの場合set_new_altitude（alt.EstAlt）; // RTHは実際の高度で戻りますelse {uint32_t rth_alt; if（mission_step.altitude ==0）rth_alt =GPS_conf.rth_altitude * 100; //ミッションステップの高度が優先されますelserth_alt =Mission_step.altitude; if（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_tchange =（millis（）-alt_change_timer）>> _scale; if（alt_change_flag ==ASCENDING）{alt_change + =変更; } else {alt_change- =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_tconstraint_int16（int16_t amt、int16_t low、int16_t high）{return（（amt）<（low）？（low）：（（amt）>（high）？（high）:( amt）））; } ///////////////////////////////////////////////// /////////////////////////////////////これは、経度の縮小を相殺するために使用されます。極//これはウェイポイント設定ごとに1回計算しても問題ありません。これは、マルチコプターの到達範囲内で少し変化するためです// 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）; return（abs（temp）> 10000）; //ウェイポイントを100度通過しました} //////////////////////////////////////// ////////////////////////////////////////////// 2つの間の距離を取得するポイント（cm //）pos1からpos2への方位を取得し、1deg =100の精度を返しますvoidGPS_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 =（* lat2- * lat1）/ 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から計算されたlonおよびlat方向の速度cm / sで表されます。正の数は、東に移動することを意味します// lat_speedはcm / sで表されます。北に移動するときの正の数//注：gpsに速度を要求する代わりに、gpsの位置を直接使用して速度を計算します。//これは1.5m / s未満の方が正確です//注：位置はリードフィルターを使用して投影されますが、速度は、変更されていないgps位置から計算されます//。リードフィルターからのノイズが速度に影響を与えないようにします// *************************************** ************************************************** **************** static void GPS_calc_velocity（void）{static int16_t speed_old [2] ={0,0}; static int32_t last [2] ={0,0}; static 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：非現実的な速度の変化と信号のナビゲーションをチェックしますif（！GPS_conf.lead_filter）{actual_speed [_X] =（actual_speed [_X] + speed_old [_X]）/ 2; actual_speed [_Y] =（actual_speed [_Y] + speed_old [_Y]）/ 2; speed_old [_X] =actual_speed [_X]; speed_old [_Y] =actual_speed [_Y]; }} init =1; last [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）; }} //////////////////////////////////////////////// ////////////////////////////////////// 2つのGPS座標間の位置誤差を計算する//緯度と経度を使用して距離エラーを実行しています：// 100 =1m // 1000 =11m =36フィート// 1800 =19.80m =60フィート// 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エラーエラー[LAT] =* target_lat- * gps_lat; // Yエラー} ///////////////////////////////////////////// //////////////////////////////////////// xとyからnav_latとnav_lonを計算しますエラーと速度//// TODO：より迅速な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）; // lat / lonエラーから目的の速度を計算しますtarget_speed =constant（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; nav [axis] + =d; // nav [axis] =Constraint（nav [axis]、-NAV_BANK_MAX、NAV_BANK_MAX）; nav [axis] =contramp_int16（nav [axis]、-GPS_conf.nav_bank_max、GPS_conf.nav_bank_max）; navPID [axis] .integrator =poshold_ratePID [axis] .integrator; }} //////////////////////////////////////////////// ////////////////////////////////////// RTHなどの距離飛行に必要なnav_latとnav_lonを計算しますおよび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 =strike（cross_speed、-200,200）; cross_speed =-cross_speed; float temp =（9000l --target_bearing）* RADX100; trig [_X] =cos（temp）; trig [_Y] =sin（temp）; 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] =Constraint（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_RAM）; // nav [axis] =contramp（nav [axis]、-NAV_BANK_MAX、NAV_BANK_MAX）; nav [axis] =contramp_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）{//クロストラックエラー// ---------------- //遠すぎたり近すぎたりすると、floatに続く追跡を行いません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.

回路図