Ardupilot飞控姿态角与姿态角速度控制过程分析

1.自稳模式初始化
2.自稳模式更新函数
3.姿态角速度代码控制过程分析
4.电机PWM控制运算
摘要
本节主要记录自己学习ardupilot飞控代码的姿态角速度的控制过程
分析过程：主要从ardupilot自稳模式开始，然后到角速度控制，最后到输出PWM控制电机实现无人机的基本运动模式的过程。思考：无人机工作在自稳模式，需要输入的是什么，自稳模式工作后输出了什么，输出的值怎么传到姿态角速度控制，姿态角速度控制最后输出了的PID怎么控制无人机的?

1.自稳模式初始化
从bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)函数开始
寻找自稳模式初始化：

[cpp]success = stabilize_init(ignore_checks); //姿态自我稳定模式[/cpp]

[cpp]

bool Copter::stabilize_init(bool ignore_checks)

{

    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high

	//如果着陆，我们切换的模式不能是手动控制模式并且油门值不能太高，否则返回。

    if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&

            (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle()))

    {

        return false;

    }

    //将目标高度设置为零以用于报告--------set target altitude to zero for reporting

    pos_control->set_alt_target(0);



    return true;

}

看下其中一个函数：

   void set_alt_target(float alt_cm) //设置目标高度

   { 

      _pos_target.z = alt_cm; 

   }

   这个函数就是设置目标高度的函数，我们是自稳模式，定高不需要设置，自稳模式设置不是特别复杂。

   下面看下自稳模式运行函数

[/cpp]
2.自稳模式更新函数
（1）从update_flight_mode()函数开始;
（2） stabilize_run(); //自稳控制
自稳模式作用完一定会把运算的PID数据作为姿态角速度环的目标输入量，我们看下这个函数的实现过程：

[cpp]
void Copter::stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
float pilot_throttle_scaled;

//如果没有解锁，油门死区没有设置，获取电机安全锁没有开，立即返回---------if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) //要想解锁，这些条件全部是假的，才可以；_flags.interlock=0,不转
{
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}

//清空着落标志------------------------clear landing flag
set_land_complete(false); //ap.land_complete=0没有着落

motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); //设置油门限制不使能

//申请简单的模式到飞行器-----------------------------------------apply SIMPLE mode transform to pilot inputs
update_simple_mode();

//将遥控器输入转换成倾斜角度----------------------------------------convert pilot input to lean angles
//要做的：转换成无人机的倾斜角度作为期望的角度（单位是浮点数据）-----------To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);

//获取目标偏航角速度-----------------------------------------------get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

//获取飞行目标所需要的油门值-----------------------------------------get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());

//调用姿态控制器---------------------------------------------------call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

//机体坐标角速率控制器运行周期100hz-----------------------------------body-frame rate controller is run directly from 100hz loop

//输出飞行油门值--------------------------------------------------output pilot's throttle
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}
[/cpp]

Ardupilot飞控姿态角与姿态角速度控制过程分析

评论 (0)