pixhawk(ArduCopter V3.3)源码分析—导航制导部分

首先要感谢蜗牛大神的讲课,博客里面的流程图是蜗牛大神PPT里面的图,看了这个PPT我才把之前看过的一些代码理出来一点头绪~
以guided模式中guided_pos_control_run()模式为例

  1. 函数的调用
    fast_loop();—update_flight_mode();若检测到遥控器5通道相应信号,执行guided模式的guided_run();函数。
  2. guided_run();函数中以guided_pos_control_run();函数为例分析。

    pixhawk(ArduCopter V3.3)源码分析—导航制导部分_第1张图片
    pixhawk(ArduCopter V3.3)源码分析—导航制导部分_第2张图片
    导航制导部分的控制流程图如下:

    分析如下(电脑画太麻烦了~手画的比较low,大家见谅~):

    图中(1)期望加速度(2)期望速度_vel_desired (3)期望位置_pos_target (4)计算目标速度时的前馈项 (5)目标速度_vel_target (6)姿态融合算得的位置_curr_pos (7)姿态融合算得的速度_vel_curr (8)计算目标加速度时的前馈项_accel_feedforward (9)目标加速度_accel_target (10)目标角度 (11)姿态融合算得的加速度 (12)姿态融合算得的角度 (13)目标角加速度_rate_bf_target (14)姿态融合算得的角加速度

以XY平面的导航计算为例,从左向右,在guided_pos_control_run—wp_nav.update_wpnav();—_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);函数中更新了XY平面的导航信息。
desired_vel_to_pos(dt);函数为由(2)期望速度_vel_desired 积分得到 (3)期望位置_pos_target。

pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);,是图中的函数1,通过(3)期望位置_pos_target计算得到(5)目标速度_vel_target。一路为位置微分后的前馈项,另一路是目标位置和姿态融合算得的位置做差后,经过P计算得到。这里的P计算有线性区和非线性区。

rate_to_accel_xy(dt, ekfNavVelGainScaler);是图中的函数2,通过目标速度算得目标加速度。一路为速度微分后的前馈项,另一路是目标速度和姿态融合算得的速度做差后,经过PI计算得到。

accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);是图中的函数3,通过目标加速度(重力加速度在机体各个轴上的分量)算得目标角度。

之后在attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);

attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(),wp_nav.get_pitch(),get_auto_heading(), true);函数中通过上一步计算的到的角度计算出应有的角加速度_rate_bf_target。

之后的PID计算在fast_loop()函数中的attitude_control.rate_controller_run();函数中实现。电机输出在fast_loop()—motors.output();—output_armed_stabilizing();函数中实现。output_armed_stabilizing();函数的定义在AP_MotorsMatrix.cpp中的AP_MotorsMatrix类里。

你可能感兴趣的:(pixhawk)