PX4源碼分析以及思路隨筆1.docx

PX4源碼分析，以及思路隨筆。目錄：1.0 環(huán)境安裝1.1 roll pitch yaw2.1 loop()3.1 fastloop() 3.1 .1 read_AHRS() ins.update() 3.1.2 rate_controller_run() _motors.set_roll() (嵌套了rate_bf_to_motor_roll) 3.1.3 motors_output() update_throttle_filter() update_battery_resistance() update_lift_max_from_batt_voltage() output_logic() output_armed_stabilizing()1.0環(huán)境安裝1. 首先安裝px4_toolchain_installer_v14_win.exe，并配置好java環(huán)境（安裝jdk，32位）。2. 安裝GitHub網(wǎng)站：/dev/docs/building-px4-with-make.html若提示失敗，在IE瀏覽器中打開(kāi)網(wǎng)頁(yè)，http變?yōu)閔ttps，不斷嘗試。3. 克隆程序（需要翻墻），可能多次失敗。4. 從C:px4toolchainmsys1.0內(nèi)的eclipse批處理文件打開(kāi)eclipse。5. 按照/dev/docs/editing-the-code-with-eclipse.html從第二張圖開(kāi)始。注：第二張圖位置為ardupilot的位置。返回目錄1.1 ROLL YAW PITCH/yuzhongchun/article/details/22749521在航空中，pitch, yaw, roll如圖2所示。pitch是圍繞X軸旋轉(zhuǎn)，也叫做俯仰角，如圖3所示。yaw是圍繞Y軸旋轉(zhuǎn)，也叫偏航角，如圖4所示。roll是圍繞Z軸旋轉(zhuǎn)，也叫翻滾角，如圖5所示。返回目錄2.1 loop函數(shù)：1. Setup各種初始化，先忽略。2.初始定義第一個(gè)是函數(shù)名，第二個(gè)單位為赫茲為過(guò)多少時(shí)間調(diào)用一次，第三個(gè)單位為微秒，即為最大花費(fèi)時(shí)間。const AP_Scheduler:Task Copter:scheduler_tasks = SCHED_TASK(rc_loop, 100, 130), /* 控制角 */ SCHED_TASK(throttle_loop, 50, 75), /*油門(mén)控制*/ SCHED_TASK(update_GPS, 50, 200), /* GPS更新*/3.從Loop()開(kāi)始。 (1)等待數(shù)據(jù)ins.wait_for_sample();我認(rèn)為ins是 Inertial Sensor，也就是指慣性傳感器。void AP_InertialSensor:wait_for_sample(void)uint32_t timer = micros(); （2）計(jì)算最大循環(huán)時(shí)間 。perf_info_check_loop_time(timer - fast_loopTimer); 檢查循環(huán)時(shí)間被PI Loops使用？G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;fast_loopTimer = timer; 記錄當(dāng)前循環(huán)時(shí)間mainLoop_count+; 主要循環(huán)的故障檢測(cè) (3)快速循環(huán)，主要的循環(huán)，重要的一步！因此首選需主要研究此函數(shù)！fast_loop(); （4）任務(wù)調(diào)度scheduler.tick(); 告訴調(diào)度程序，一個(gè)流程已經(jīng)走完？uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();任務(wù)周期scheduler.run(time_available);任務(wù)調(diào)度 返回目錄3.1 fastloop()函數(shù) Fastloop，最關(guān)鍵的循環(huán)(400hz)，下面對(duì)每個(gè)函數(shù)進(jìn)行工作的分析。1.更新傳感器并更新姿態(tài)的函數(shù)read_AHRS();2.進(jìn)行角速度PID運(yùn)算的函數(shù)attitude_control.rate_controller_run();3.直升機(jī)框架判斷 （HELI_FRAME）#if FRAME_CONFIG = HELI_FRAME update_heli_control_dynamics();#endif /HELI_FRAME4.輸出電機(jī)PWM值的函數(shù)motors_output();5.慣性導(dǎo)航/ Inertial Navread_inertia();6.擴(kuò)展卡爾曼濾波器 / check if ekf has reset target heading check_ekf_yaw_reset();7. 更新控制模式,并計(jì)算本級(jí)控制輸出下級(jí)控制輸入. / run the attitude controllers update_flight_mode();8. 擴(kuò)展卡爾曼濾波器/ update home from EKF if necessaryupdate_home_from_EKF();9. 檢查是否落地or追回 / check if weve landed or crashedupdate_land_and_crash_detectors();10. 攝像機(jī)#if MOUNT = ENABLED / camera mounts fast update camera_mount.update_fast();#endif11. 記錄傳感器健康狀態(tài)？ / log sensor health if (should_log(MASK_LOG_ANY) Log_Sensor_Health();返回目錄3.1.1 Read_AHRS()函數(shù)1.read_AHRSvoid Copter:read_AHRS(void) / Perform IMU calculations and get attitude info /-#if HIL_MODE != HIL_MODE_DISABLED /Mavlink協(xié)議支持 ，全傳感器仿真？模擬？（full sensor simulation.） / update hil before ahrs update gcs_check_input();#endif ahrs.update();/重要！2. ahrs.update()其中主要函數(shù)為ahrs.update()，函數(shù)為：/DCM：Direction CosineMatrix，方向余弦矩陣AP_AHRS_DCM:update(void) float delta_t; if (_last_startup_ms = 0) _last_startup_ms = AP_HAL:millis(); (1)更新加速度計(jì)和陀螺儀 / tell the IMU to grab some data _ins.update(); / ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); / if the update call took more than 0.2 seconds then discard it, / otherwise we may move too far. This happens when arming motors / in ArduCopter if (delta_t 0.2f) memset(&_ra_sum0, 0, sizeof(_ra_sum); _ra_deltat = 0; return; (2)使用陀螺儀更新四元素矩陣（做余弦矩陣的歸一化） / Integrate the DCM matrix using gyro inputs matrix_update(delta_t); (3)標(biāo)準(zhǔn)化 / Normalize the DCM matrix normalize();(4)執(zhí)行漂移修正使用加速度計(jì)和羅盤(pán)與該次計(jì)算的四元素矩陣做差，修正出下一次陀螺儀的輸出 / Perform drift correction drift_correction(delta_t); (5)檢查錯(cuò)誤值 / paranoid check for bad values in the DCM matrix check_matrix(); (6)計(jì)算俯仰角，偏航角，翻轉(zhuǎn)角 / Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); (7)更新三角函數(shù)的值，包括俯仰角的余弦和翻滾角的余弦 / update trig values including _cos_roll, cos_pitch update_trig();返回目錄 _ins.update()更新加速度計(jì)和陀螺儀 / tell the IMU to grab some data_ins.update(); 函數(shù)定義：void AP_InertialSensor:update(void)：在函數(shù)中：_backendsi-update(); 為主要操作步驟。_backendsi的定義為：(是一個(gè)指針數(shù)組) AP_InertialSensor_Backend *_backendsINS_MAX_BACKENDS; 查找_backends所指向的數(shù)據(jù)，有兩個(gè)函數(shù)值得注意：void AP_InertialSensor:_add_backend(AP_InertialSensor_Backend *backend) Void AP_InertialSensor:detect_backends(void)考慮在detect_backends()中進(jìn)行了調(diào)用：#elif HAL_INS_DEFAULT = HAL_INS_PX4 | HAL_INS_DEFAULT = HAL_INS_VRBRAIN _add_backend(AP_InertialSensor_PX4:detect(*this);因此得出_backends指向的是類(lèi) AP_InertialSensor_PX4。找到函數(shù)如下：bool AP_InertialSensor_PX4:update(void) / get the latest sample from the sensor drivers _get_sample(); / uint8_t _num_accel_instances; / uint8_t _num_gyro_instances;for (uint8_t k=0; k_num_accel_instances; k+) update_accel(_accel_instancek); /加速度計(jì) for (uint8_t k=0; ksuspend_timer_procs();/任務(wù)調(diào)度，推遲進(jìn)程 /_imu._new_accel_datainstance為布爾型變量，推測(cè)為觀察是否可以更新 /其中_publish_accel()應(yīng)該就是if (_imu._new_accel_datainstance) /兩個(gè)參數(shù)，其中一個(gè)是instance，另一個(gè)參考為過(guò)濾后的加速度計(jì)數(shù)據(jù)？ _publish_accel(instance, _imu._accel_filteredinstance); _imu._new_accel_datainstance = false; / possibly update filter frequency 設(shè)定頻率？ if (_last_accel_filter_hzinstance != _accel_filter_cutoff() _imu._accel_filterinstance.set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff(); _last_accel_filter_hzinstance = _accel_filter_cutoff(); /任務(wù)調(diào)度，推測(cè)為經(jīng)過(guò)一個(gè)進(jìn)程后重新設(shè)置 hal.scheduler-resume_timer_procs();下面對(duì)于陀螺儀的控制基本沒(méi)有區(qū)別：void AP_InertialSensor_Backend:update_gyro(uint8_t instance) hal.scheduler-suspend_timer_procs(); if (_imu._new_gyro_datainstance) _publish_gyro(instance, _imu._gyro_filteredinstance); _imu._new_gyro_datainstance = false; / possibly update filter frequency if (_last_gyro_filter_hzinstance != _gyro_filter_cutoff() _imu._gyro_filterinstance.set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff(); _last_gyro_filter_hzinstance = _gyro_filter_cutoff(); hal.scheduler-resume_timer_procs();然后需要看一下_publish_accel()這個(gè)函數(shù):void AP_InertialSensor_Backend:_publish_accel(uint8_t instance, const Vector3f &accel) _imu._accelinstance = accel; _imu._accel_healthyinstance = true; if (_imu._accel_raw_sample_ratesinstance 0 & rate_error_rads 0) | (integrator 0) integrator = get_rate_roll_pid().get_i(); / Compute output in range -1 +1進(jìn)行PID計(jì)算，得到控制量output。/其中g(shù)et_d()和getp中有參數(shù)_kp和_kd，沒(méi)有找到是如何進(jìn)行計(jì)算的！/這里是個(gè)大問(wèn)題。 float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d();/ Constrain output， 限制output的值，如果是非法的則變?yōu)?，如果比0小則變/為0,大于1則為1 return constrain_float(output, -1.0f, 1.0f);剩余的rate_bf_to_motor_pitch(_ang_vel_target_rads.y)和rate_bf_to_motor_yaw(_ang_vel_target_rads.z)與第一個(gè)沒(méi)有差別在這三個(gè)函數(shù)中，我們分別獲得了三個(gè)參數(shù)，_roll_in，_pitch_in和_yaw_in，取值范圍在-1到1，應(yīng)該會(huì)應(yīng)用在fastloop()中的下個(gè)函數(shù)motors_output()。返回目錄3.1.3 motors_output()函數(shù)解釋為使用_roll_control_input、_pitch_control_input、_yaw_control_input再進(jìn)行換算，得出各個(gè)電機(jī)的PWM值。首先查看定義：void Copter:motors_output() / check if we are performing the motor test if (ap.motor_test) motor_test_output(); else if (!ap.using_interlock) / if not using interlock switch, set according to Emergency Stop status / where Emergency Stop is forced false during arming if Emergency Stop switch / is not used. Interlock enabled means motors run, so we must / invert motor_emergency_stop status for motors to run. motors.set_interlock(!ap.motor_emergency_stop); motors.output(); 明顯其中調(diào)用motors.output()為關(guān)鍵一步。首先查看定義：void AP_MotorsMulticopter:output() / update throttle filter 更新高度過(guò)濾器？?jī)?nèi)部還需仔細(xì)看 update_throttle_filter(); / update battery resistance 看電池內(nèi)阻。 update_battery_resistance();/ calc filtered battery voltage and lift_max 過(guò)濾后的電池電壓/ 以及從此電壓獲得的最大升力比 update_lift_max_from_batt_voltage();/ run spool logic 運(yùn)行飛行(盤(pán)旋)邏輯/進(jìn)行各種判定，比如提升發(fā)動(dòng)機(jī)推力，降低發(fā)動(dòng)機(jī)推力，盤(pán)旋等等。 output_logic(); / calculate thrust 計(jì)算推力 output_armed_stabilizing(); / convert rpy_thrust values to pwm 將rpy_thrust 轉(zhuǎn)化為pwm信號(hào) output_to_motors();返回目錄 update_throttle_filter()還是先查看定義：void AP_MotorsMulticopter:update_throttle_filter()if (armed() /首先查看電機(jī)是否已經(jīng)裝備 /其中_loop_rate是update被調(diào)用的頻率，通常為400hz /_throttle_in 被看做油門(mén)的輸入 _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); / constrain filtered throttle 將其數(shù)值限制在0到1之間。 if (_throttle_filter.get() 1.0f) _throttle_filter.reset(1.0f); else _throttle_filter.reset(0.0f); 其中apply函數(shù)定義：(apply函數(shù)還未吃透)apply(T sample, float dt) return _filter.apply(sample, _cutoff_freq, dt);T DigitalLPF:apply(const T &sample, float cutoff_freq, float dt) if (cutoff_freq = 0.0f | dt = 0.0f) _output = sample; return _output; 感覺(jué)函數(shù)就是將_throttle_in傳給了但是函數(shù)外部并沒(méi)有傳值，不知道是什么意思。get()和reset()意思很簡(jiǎn)單，字面理解即可。（內(nèi)部函數(shù)沒(méi)有找到）返回目錄 update_battery_resistance意義為更新電池的內(nèi)阻。先看定義：void AP_MotorsMulticopter:update_battery_resistance()/ if disarmed reset resting voltage and current/ 如果未裝備則復(fù)位電壓和電流 if (!_flags.armed) _batt_voltage_resting = _batt_voltage; _batt_current_resting = _batt_current; _batt_timer = 0; else / update battery resistance when throttle is over hover throttle / 更新電池內(nèi)阻，在盤(pán)旋油門(mén)的狀態(tài)時(shí)？ if (_batt_timer 400) & (_batt_current_resting*2.0f) = _hover_out) / filter reaches 90% in 1/4 the test time _batt_resistance += 0.05f*( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance); _batt_timer += 1; else / initialize battery resistance to prevent change in resting voltage estimate _batt_resistance = (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting); 返回目錄 update_lift_max_from_batt_voltage定義：（這個(gè)函數(shù)由于感覺(jué)用處不大，暫時(shí)先不考慮）void AP_MotorsMulticopter:update_lift_max_from_batt_voltage() / sanity check battery_voltage_min is not too small / if disabled or misconfigured exit immediately if(_batt_voltage_max = _batt_voltage_max) | (_batt_voltage 1float _throttle_rpy_mix_desired; / desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 12 seconds） Break;（3）盤(pán)旋模式？ case SPIN_WHEN_ARMED: / Motors should be stationary or at spin when armed. /