PX4源码分析以及思路随笔1.docx
《PX4源码分析以及思路随笔1.docx》由会员分享，可在线阅读，更多相关《PX4源码分析以及思路随笔1.docx（35页珍藏版）》请在冰点文库上搜索。
PX4源码分析以及思路随笔1
PX4源码分析以及思路随笔1
PX4源码分析，以及思路随笔。
目录：
1.0环境安装
1.1rollpitchyaw
2.1loop（）
3.1fastloop（）
3.1.1read_AHRS（）
3.1.1.1ins.update（）
3.1.2rate_controller_run（）
3.1.2.1_motors.set_roll（）
（嵌套了rate_bf_to_motor_roll）
3.1.3motors_output（）
3.1.3.1update_throttle_filter（）
3.1.3.2update_battery_resistance（）
3.1.3.3update_lift_max_from_batt_voltage（）
3.1.3.4output_logic（）
3.1.3.5output_armed_stabilizing（）
1.0环境安装
1.首先安装px4_toolchain_installer_v14_win.exe，并配置好java环境（安装jdk，32位）。
2.安装GitHub
网站：
http:
//ardupilot.org/dev/docs/building-px4-with-make.html
若提示失败，在IE浏览器中打开网页，http变为https，不断尝试。
3.克隆程序（需要翻墙），可能多次失败。
4.从C:
\px4\toolchain\msys\1.0内的eclipse批处理文件打开eclipse。
5.按照http:
//ardupilot.org/dev/docs/editing-the-code-with-eclipse.html从第二张图开始。
注：
第二张图位置为ardupilot的位置。
返回目录
2.1loop函数：
1.Setup各种初始化，先忽略。
2.初始定义
第一个是函数名，第二个单位为赫兹为过多少时间调用一次，第三个单位为微秒，即为最大花费时间。
constAP_Scheduler:
:
TaskCopter:
:
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"是"InertialSensor"，也就是指惯性传感器。
voidAP_InertialSensor:
:
wait_for_sample（void）
uint32_ttimer=micros（）;
（2）计算最大循环时间。
perf_info_check_loop_time（timer-fast_loopTimer）;检查循环时间
被PILoops使用？
？
G_Dt=（float）（timer-fast_loopTimer）/1000000.0f;
fast_loopTimer=timer;记录当前循环时间
mainLoop_count++;主要循环的故障检测
（3）快速循环，主要的循环，重要的一步！
！
！
！
！
！
！
！
！
！
！
！
因此首选需主要研究此函数！
fast_loop（）;
（4）任务调度
scheduler.tick（）;告诉调度程序，一个流程已经走完？
？
uint32_ttime_available=（timer+MAIN_LOOP_MICROS）-micros（）;
任务周期
scheduler.run（time_available）;任务调度
返回目录
3.1fastloop（）函数
Fastloop，最关键的循环（400hz），下面对每个函数进行工作的分析。
1.更新传感器并更新姿态的函数
read_AHRS（）;
2.进行角速度PID运算的函数
attitude_control.rate_controller_run（）;
3.直升机框架判断（HELI_FRAME）
#ifFRAME_CONFIG==HELI_FRAME
update_heli_control_dynamics（）;
#endif//HELI_FRAME
4.输出电机PWM值的函数
motors_output（）;
5.惯性导航
//InertialNav
read_inertia（）;
6.扩展卡尔曼滤波器
//checkifekfhasresettargetheading
check_ekf_yaw_reset（）;
7.更新控制模式,并计算本级控制输出下级控制输入...
//runtheattitudecontrollers
update_flight_mode（）;
8.扩展卡尔曼滤波器
//updatehomefromEKFifnecessary
update_home_from_EKF（）;
9.检查是否落地or追回
//checkifwe'velandedorcrashed
update_land_and_crash_detectors（）;
10.摄像机
#ifMOUNT==ENABLED
//cameramount'sfastupdate
camera_mount.update_fast（）;
#endif
11.记录传感器健康状态？
？
//logsensorhealth
if（should_log（MASK_LOG_ANY））{
Log_Sensor_Health（）;
返回目录
3.1.1Read_AHRS（）函数
1.read_AHRS
voidCopter:
:
read_AHRS（void）
{
//PerformIMUcalculationsandgetattitudeinfo
//-----------------------------------------------
#ifHIL_MODE!
=HIL_MODE_DISABLED
//Mavlink协议支持，全传感器仿真？
？
模拟？
（fullsensorsimulation.）
//updatehilbeforeahrsupdate
gcs_check_input（）;
#endif
ahrs.update（）;//重要！
}
2.ahrs.update（）
其中主要函数为ahrs.update（），函数为：
//DCM：
DirectionCosine Matrix，方向余弦矩阵
AP_AHRS_DCM:
:
update（void）
{
floatdelta_t;
if（_last_startup_ms==0）{
_last_startup_ms=AP_HAL:
:
millis（）;
}
（1）更新加速度计和陀螺仪
//telltheIMUtograbsomedata
_ins.update（）;
//asktheIMUhowmuchtimethissensorreadingrepresents
delta_t=_ins.get_delta_time（）;
//iftheupdatecalltookmorethan0.2secondsthendiscardit,
//otherwisewemaymovetoofar.Thishappenswhenarmingmotors
//inArduCopter
if（delta_t>0.2f）{
memset（&_ra_sum[0],0,sizeof（_ra_sum））;
_ra_deltat=0;
return;
}
（2）使用陀螺仪更新四元素矩阵（做余弦矩阵的归一化）
//IntegratetheDCMmatrixusinggyroinputs
matrix_update（delta_t）;
（3）标准化
//NormalizetheDCMmatrix
normalize（）;
（4）执行漂移修正
使用加速度计和罗盘与该次计算的四元素矩阵做差，修正出下一次陀螺仪的输出
//Performdriftcorrection
drift_correction（delta_t）;
（5）检查错误值
//paranoidcheckforbadvaluesintheDCMmatrix
check_matrix（）;
（6）计算俯仰角，偏航角，翻转角
//Calculatepitch,roll,yawforstabilizationandnavigation
euler_angles（）;
（7）更新三角函数的值，包括俯仰角的余弦和翻滚角的余弦
//updatetrigvaluesincluding_cos_roll,cos_pitch
update_trig（）;
}
返回目录
3.1.1.1_ins.update（）
更新加速度计和陀螺仪
//telltheIMUtograbsomedata
_ins.update（）;
函数定义：
voidAP_InertialSensor:
:
update（void）：
在函数中：
_backends[i]->update（）;为主要操作步骤。
_backends[i]的定义为：
（是一个指针数组）
AP_InertialSensor_Backend*_backends[INS_MAX_BACKENDS];
查找_backends所指向的数据，有两个函数值得注意：
voidAP_InertialSensor:
:
_add_backend（AP_InertialSensor_Backend*backend）
VoidAP_InertialSensor:
:
detect_backends（void）
考虑在detect_backends（）中进行了调用：
#elifHAL_INS_DEFAULT==HAL_INS_PX4||HAL_INS_DEFAULT==HAL_INS_VRBRAIN
_add_backend（AP_InertialSensor_PX4:
:
detect（*this））;
因此得出_backends指向的是类AP_InertialSensor_PX4。
找到函数如下：
boolAP_InertialSensor_PX4:
:
update（void）
{
//getthelatestsamplefromthesensordrivers
_get_sample（）;
//uint8_t_num_accel_instances;
//uint8_t_num_gyro_instances;
for（uint8_tk=0;k<_num_accel_instances;k++）{
update_accel（_accel_instance[k]）;//加速度计
}
for（uint8_tk=0;k<_num_gyro_instances;k++）{
update_gyro（_gyro_instance[k]）;//陀螺仪
}
returntrue;
}
其中两个重要的函数分别为：
update_accel（_accel_instance[k]）和update_gyro（_gyro_instance[k]）.
首先看update_accel（_accel_instance[k]）：
输入参数为_accel_instance[k]和_gyro_instance[k]，定义为：
#defineINS_MAX_INSTANCES3
uint8_t_accel_instance[INS_MAX_INSTANCES];
uint8_t_gyro_instance[INS_MAX_INSTANCES];
随后看update_accel（）和update_gyro（）的定义：
voidAP_InertialSensor_Backend:
:
update_accel（uint8_tinstance）
{
hal.scheduler->suspend_timer_procs（）;//任务调度，推迟进程
//_imu._new_accel_data[instance]为布尔型变量，推测为观察是否可以更新
//其中_publish_accel（）应该就是
if（_imu._new_accel_data[instance]）{
//两个参数，其中一个是instance，另一个参考为过滤后的加速度计数据？
？
_publish_accel（instance,_imu._accel_filtered[instance]）;
_imu._new_accel_data[instance]=false;
}
//possiblyupdatefilterfrequency设定频率？
if（_last_accel_filter_hz[instance]!
=_accel_filter_cutoff（））{
_imu._accel_filter[instance].set_cutoff_frequency（_accel_raw_sample_rate（instance）,_accel_filter_cutoff（））;
_last_accel_filter_hz[instance]=_accel_filter_cutoff（）;
}
//任务调度，推测为经过一个进程后重新设置
hal.scheduler->resume_timer_procs（）;
}
下面对于陀螺仪的控制基本没有区别：
voidAP_InertialSensor_Backend:
:
update_gyro（uint8_tinstance）
{
hal.scheduler->suspend_timer_procs（）;
if（_imu._new_gyro_data[instance]）{
_publish_gyro（instance,_imu._gyro_filtered[instance]）;
_imu._new_gyro_data[instance]=false;
}
//possiblyupdatefilterfrequency
if（_last_gyro_filter_hz[instance]!
=_gyro_filter_cutoff（））{
_imu._gyro_filter[instance].set_cutoff_frequency（_gyro_raw_sample_rate（instance）,_gyro_filter_cutoff（））;
_last_gyro_filter_hz[instance]=_gyro_filter_cutoff（）;
}
hal.scheduler->resume_timer_procs（）;
}
然后需要看一下_publish_accel（）这个函数:
voidAP_InertialSensor_Backend:
:
_publish_accel（uint8_tinstance,constVector3f&accel）
{
_imu._accel[instance]=accel;
_imu._accel_healthy[instance]=true;
if（_imu._accel_raw_sample_rates[instance]<=0）{
return;
}
//publishdeltavelocity
_imu._delta_velocity[instance]=_imu._delta_velocity_acc[instance];
_imu._delta_velocity_dt[instance]=_imu._delta_velocity_acc_dt[instance];
_imu._delta_velocity_valid[instance]=true;
if（_imu._accel_calibrator!
=NULL&&_imu._accel_calibrator[instance].get_status（）==ACCEL_CAL_COLLECTING_SAMPLE）{
Vector3fcal_sample=_imu._delta_velocity[instance];
//removerotation
cal_sample.rotate_inverse（_imu._board_orientation）;
//removescalefactors
constVector3f&accel_scale=_imu._accel_scale[instance].get（）;
cal_sample.x/=accel_scale.x;
cal_sample.y/=accel_scale.y;
cal_sample.z/=accel_scale.z;
//removeoffsets
cal_sample+=_imu._accel_offset[instance].get（）*_imu._delta_velocity_dt[instance];
_imu._accel_calibrator[instance].new_sample（cal_sample,_imu._delta_velocity_dt[instance]）;
}
}
返回目录
3.1.2attitude_control.rate_controller_run（）
姿态控制函数（PID函数）
attitude_control.rate_controller_run（）;
定义为：
voidAC_AttitudeControl:
:
rate_controller_run（）
{
_motors.set_roll（rate_bf_to_motor_roll（_ang_vel_target_rads.x））;
_motors.set_pitch（rate_bf_to_motor_pitch（_ang_vel_target_rads.y））;
_motors.set_yaw（rate_bf_to_motor_yaw（_ang_vel_target_rads.z））;
}
注：
其中，_ang_vel_target_rads.x
等三个输入参数的取得主要在voidCopter:
:
stabilize_run（）内取得。
推测Angularvelocitytargetrads的含义为：
要求（期望）的角度速度
注：
rate_bf_roll_pitch_yaw（）:
此函数采用“车身骨架坐标系”标示翻滚、俯仰和偏航速率（度/秒）。
例：
此函数的滚动=-1000,间距=-1500,偏航=500会导致飞机向左滚动10度/秒,向前倾斜15度/秒,对于车辆的z轴旋转5度/秒。
返回目录
3.1.2.1_motors.set_roll（）
1.首先分析_motors.set_roll（）
定义：
voidset_roll（floatroll_in）
{_roll_in=roll_in;};
主要就是将输入的参数定义为roll_in，并传值给_roll_in。
注：
_roll_in在fastloop（）内的下个函数motors_output（）函数进行操作，这里我认为是个切入点！
set_roll（）中嵌套的函数为：
rate_bf_to_motor_roll（）
定义为：
floatAC_AttitudeControl:
:
rate_bf_to_motor_roll（floatrate_target_rads）
{
//从陀螺仪获取get_gyro（）.x，也就是当前的飞行器的翻滚角。
floatcurrent_rate_rads=_ahrs.get_gyro（）.x;
//翻滚角的期望与当前数值的差值
floatrate_error_rads=rate_target_rads-current_rate_rads;
//passerrortoPIDcontroller将误差传递给PID控制器,其中有微分过滤
//和将目标数据传入结构体PID。
get_rate_roll_pid（）.set_input_filter_d（rate_error_rads）;
get_rate_roll_pid（）.set_desired_rate（rate_target_rads）;
//进行积分
floatintegrator=get_rate_roll_pid（）.get_integrator（）;
//Ensurethatintegratorcanonlybereducediftheoutputissaturated
//确保积分只在饱和时下降
if（!
_motors.limit.roll_pitch||（（integrator>0&&rate_error_rads<0）||（integrator<0&&rate_error_rads>0）））{
integrator=get_rate_roll_pid（）.get_i（）;
}
//Computeoutputinrange-1~+1进行PID计算，得到控制量output。
//其中get_d（）和getp中有参数_kp和_kd，没有找到是如何进行计算的！
//这里是个大问题。
floatoutput=get_rate_roll_pid（）.get_p（）+integrator+get_rate_roll_pid（）.get_d（）;
//Constrainoutput，限制output的值，如果是非法的则变为0，如果比0小则变
//为0,大于1则为1
returnconstrain_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.
升级会员 