PX4源码分析以及思路随笔1.docx

PX4源码分析，以及思路随笔。目录：1.0 环境安装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环境安装1. 首先安装px4_toolchain_installer_v14_win.exe，并配置好java环境（安装jdk，32位）。2. 安装GitHub网站：/dev/docs/building-px4-with-make.html若提示失败，在IE浏览器中打开网页，http变为https，不断尝试。3. 克隆程序（需要翻墙），可能多次失败。4. 从C:px4toolchainmsys1.0内的eclipse批处理文件打开eclipse。5. 按照/dev/docs/editing-the-code-with-eclipse.html从第二张图开始。注：第二张图位置为ardupilot的位置。返回目录1.1 ROLL YAW PITCH/yuzhongchun/article/details/22749521在航空中，pitch, yaw, roll如图2所示。pitch是围绕X轴旋转，也叫做俯仰角，如图3所示。yaw是围绕Y轴旋转，也叫偏航角，如图4所示。roll是围绕Z轴旋转，也叫翻滚角，如图5所示。返回目录2.1 loop函数：1. Setup各种初始化，先忽略。2.初始定义第一个是函数名，第二个单位为赫兹为过多少时间调用一次，第三个单位为微秒，即为最大花费时间。const AP_Scheduler:Task Copter:scheduler_tasks = SCHED_TASK(rc_loop, 100, 130), /* 控制角 */ SCHED_TASK(throttle_loop, 50, 75), /*油门控制*/ SCHED_TASK(update_GPS, 50, 200), /* GPS更新*/3.从Loop()开始。 (1)等待数据ins.wait_for_sample();我认为ins是 Inertial Sensor，也就是指惯性传感器。void AP_InertialSensor:wait_for_sample(void)uint32_t timer = micros(); （2）计算最大循环时间 。perf_info_check_loop_time(timer - fast_loopTimer); 检查循环时间被PI Loops使用？G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;fast_loopTimer = timer; 记录当前循环时间mainLoop_count+; 主要循环的故障检测 (3)快速循环，主要的循环，重要的一步！因此首选需主要研究此函数！fast_loop(); （4）任务调度scheduler.tick(); 告诉调度程序，一个流程已经走完？uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();任务周期scheduler.run(time_available);任务调度 返回目录3.1 fastloop()函数 Fastloop，最关键的循环(400hz)，下面对每个函数进行工作的分析。1.更新传感器并更新姿态的函数read_AHRS();2.进行角速度PID运算的函数attitude_control.rate_controller_run();3.直升机框架判断 （HELI_FRAME）#if FRAME_CONFIG = HELI_FRAME update_heli_control_dynamics();#endif /HELI_FRAME4.输出电机PWM值的函数motors_output();5.惯性导航/ Inertial Navread_inertia();6.扩展卡尔曼滤波器 / check if ekf has reset target heading check_ekf_yaw_reset();7. 更新控制模式,并计算本级控制输出下级控制输入. / run the attitude controllers update_flight_mode();8. 扩展卡尔曼滤波器/ update home from EKF if necessaryupdate_home_from_EKF();9. 检查是否落地or追回 / check if weve landed or crashedupdate_land_and_crash_detectors();10. 摄像机#if MOUNT = ENABLED / camera mounts fast update camera_mount.update_fast();#endif11. 记录传感器健康状态？ / log sensor health if (should_log(MASK_LOG_ANY) Log_Sensor_Health();返回目录3.1.1 Read_AHRS()函数1.read_AHRSvoid Copter:read_AHRS(void) / Perform IMU calculations and get attitude info /-#if HIL_MODE != HIL_MODE_DISABLED /Mavlink协议支持 ，全传感器仿真？模拟？（full sensor simulation.） / update hil before ahrs update gcs_check_input();#endif ahrs.update();/重要！2. ahrs.update()其中主要函数为ahrs.update()，函数为：/DCM：Direction CosineMatrix，方向余弦矩阵AP_AHRS_DCM:update(void) float delta_t; if (_last_startup_ms = 0) _last_startup_ms = AP_HAL:millis(); (1)更新加速度计和陀螺仪 / 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)标准化 / Normalize the DCM matrix normalize();(4)执行漂移修正使用加速度计和罗盘与该次计算的四元素矩阵做差，修正出下一次陀螺仪的输出 / Perform drift correction drift_correction(delta_t); (5)检查错误值 / paranoid check for bad values in the DCM matrix check_matrix(); (6)计算俯仰角，偏航角，翻转角 / Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); (7)更新三角函数的值，包括俯仰角的余弦和翻滚角的余弦 / update trig values including _cos_roll, cos_pitch update_trig();返回目录 _ins.update()更新加速度计和陀螺仪 / tell the IMU to grab some data_ins.update(); 函数定义：void AP_InertialSensor:update(void)：在函数中：_backendsi-update(); 为主要操作步骤。_backendsi的定义为：(是一个指针数组) AP_InertialSensor_Backend *_backendsINS_MAX_BACKENDS; 查找_backends所指向的数据，有两个函数值得注意：void AP_InertialSensor:_add_backend(AP_InertialSensor_Backend *backend) Void AP_InertialSensor:detect_backends(void)考虑在detect_backends()中进行了调用：#elif HAL_INS_DEFAULT = HAL_INS_PX4 | HAL_INS_DEFAULT = HAL_INS_VRBRAIN _add_backend(AP_InertialSensor_PX4:detect(*this);因此得出_backends指向的是类 AP_InertialSensor_PX4。找到函数如下：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); /加速度计 for (uint8_t k=0; ksuspend_timer_procs();/任务调度，推迟进程 /_imu._new_accel_datainstance为布尔型变量，推测为观察是否可以更新 /其中_publish_accel()应该就是if (_imu._new_accel_datainstance) /两个参数，其中一个是instance，另一个参考为过滤后的加速度计数据？ _publish_accel(instance, _imu._accel_filteredinstance); _imu._new_accel_datainstance = false; / possibly update filter frequency 设定频率？ 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(); /任务调度，推测为经过一个进程后重新设置 hal.scheduler-resume_timer_procs();下面对于陀螺仪的控制基本没有区别：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()这个函数: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进行PID计算，得到控制量output。/其中get_d()和getp中有参数_kp和_kd，没有找到是如何进行计算的！/这里是个大问题。 float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d();/ Constrain output， 限制output的值，如果是非法的则变为0，如果比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)与第一个没有差别在这三个函数中，我们分别获得了三个参数，_roll_in，_pitch_in和_yaw_in，取值范围在-1到1，应该会应用在fastloop()中的下个函数motors_output()。返回目录3.1.3 motors_output()函数解释为使用_roll_control_input、_pitch_control_input、_yaw_control_input再进行换算，得出各个电机的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(); 明显其中调用motors.output()为关键一步。首先查看定义：void AP_MotorsMulticopter:output() / update throttle filter 更新高度过滤器？内部还需仔细看 update_throttle_filter(); / update battery resistance 看电池内阻。 update_battery_resistance();/ calc filtered battery voltage and lift_max 过滤后的电池电压/ 以及从此电压获得的最大升力比 update_lift_max_from_batt_voltage();/ run spool logic 运行飞行(盘旋)逻辑/进行各种判定，比如提升发动机推力，降低发动机推力，盘旋等等。 output_logic(); / calculate thrust 计算推力 output_armed_stabilizing(); / convert rpy_thrust values to pwm 将rpy_thrust 转化为pwm信号 output_to_motors();返回目录 update_throttle_filter()还是先查看定义：void AP_MotorsMulticopter:update_throttle_filter()if (armed() /首先查看电机是否已经装备 /其中_loop_rate是update被调用的频率，通常为400hz /_throttle_in 被看做油门的输入 _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); / constrain filtered throttle 将其数值限制在0到1之间。 if (_throttle_filter.get() 1.0f) _throttle_filter.reset(1.0f); else _throttle_filter.reset(0.0f); 其中apply函数定义：(apply函数还未吃透)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; 感觉函数就是将_throttle_in传给了但是函数外部并没有传值，不知道是什么意思。get()和reset()意思很简单，字面理解即可。（内部函数没有找到）返回目录 update_battery_resistance意义为更新电池的内阻。先看定义：void AP_MotorsMulticopter:update_battery_resistance()/ if disarmed reset resting voltage and current/ 如果未装备则复位电压和电流 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 / 更新电池内阻，在盘旋油门的状态时？ 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定义：（这个函数由于感觉用处不大，暂时先不考虑）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）盘旋模式？ case SPIN_WHEN_ARMED: / Motors should be stationary or at spin when armed. /