PX4源码分析，以及思路随笔。
目录：
1.0 环境安装
1.1 roll pitch yaw
2.1 loop()
3.1 fastloop()
3.1 .1 read_AHRS()
3.1.1.1 ins.update()
3.1.2 rate_controller_run()
3.1.2.1 _motors.set_roll()
(嵌套了rate_bf_to_motor_roll)
3.1.3 motors_output()
3.1.3.1 update_throttle_filter()
3.1.3.2 update_battery_resistance()
3.1.3.3 update_lift_max_from_batt_voltage()
3.1.3.4 output_logic()
3.1.3.5 output_armed_stabilizing()
1.0环境安装
1.首先安装px4_toolchain_installer_，并配置好java环境（安装jdk，32位）。
2.安装GitHub
网站：/dev/docs/building-px4-with-make.html
若提示失败，在IE浏览器中打开网页，http变为https，不断尝试。
3.克隆程序（需要），可能多次失败。
4.从C:\px4\toolchain\msys\1.0内的eclipse批处理文件打开eclipse。
5.按照/dev/docs/editing-the-code-with-eclipse.html从第二张图开始。
注：第二张图位置为ardupilot的位置。
返回目录
1.1 ROLL YAW PITCH
blog.csdn/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_FRAME
4.输出电机PWM值的函数
motors_output();
5.惯性导航
// Inertial Nav
read_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 necessary
update_home_from_EKF();
9.检查是否落地or追回
// check if we've landed or crashed
update_land_and_crash_detectors();
10.摄像机
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
11.记录传感器健康状态？？
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
返回目录
3.1.1 Read_AHRS()函数angular安装
1.read_AHRS
void 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 Cosine Matrix，方向余弦矩阵
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_sum[0], 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();
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论