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

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

AMENG
2018-12-19 / 0 评论 / 16 阅读 / 正在检测是否收录...
温馨提示:
本文最后更新于2021年08月23日,已超过1054天没有更新,若内容或图片失效,请留言反馈。

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]

0

评论 (0)

取消