AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor

代码基于ardupilot3.4.2RC2，仅分析ArduCopter（多旋翼、直升机）构型中代码逻辑、使用关系，其他构型（飞机、云台、车）后续考虑分析。

float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)

函数作用：

速率控制器计算电机输出，控制机体偏航角速率的达到目标速率，以弧度/秒为单位。

函数全局调用情况如下：

函数声明情况如下：

函数声明文件如下：

函数流程图如下：

函数逻辑顺序图如下：

函数原始代码如下：

// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second速率控制器计算电机输出，控制机体偏航角速率的达到目标速率，以弧度/秒为单位。

float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)

{

float pd,i,vff; // used to capture pid values for logging用于捕获用于记录的pid值

float current_rate_rads; // this iteration's rate这个迭代的速度

float rate_error_rads; // simply target_rate - current_rate目标偏航角速率与当前偏航角速率的误差

float yaw_out;

// get current rate获得当前速率

// To-Do: make getting gyro rates more efficient?获得机体当前的偏航角速率

current_rate_rads = _ahrs.get_gyro().z;

// calculate error and call pid controller计算误差，调用PID控制器

rate_error_rads = rate_target_rads - current_rate_rads;

// pass error to PID controller输入误差到PID控制器

_pid_rate_yaw.set_input_filter_all(rate_error_rads);

_pid_rate_yaw.set_desired_rate(rate_target_rads);

// get p and d获得P、D

pd = _pid_rate_yaw.get_p() + _pid_rate_yaw.get_d();

// get i term获得积分项

i = _pid_rate_yaw.get_integrator();

// update i term as long as we haven't breached the limits or the I term will certainly reduce更新I项，要求不超限，或正在减少

if (!_flags_heli.limit_yaw || ((i>0&&rate_error_rads<0)||(i<0&&rate_error_rads>0))) {

if (((AP_MotorsHeli&)_motors).rotor_runup_complete()) {

i = _pid_rate_yaw.get_i();

} else {

i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up如果电机未运行，则使用泄漏的I项，以避免过度积聚

}

// For legacy reasons, we convert to centi-degrees before inputting to the feedforward换为度的单位，并输入到反馈

vff = yaw_velocity_feedforward_filter.apply(_pid_rate_yaw.get_vff(rate_target_rads), _dt);

// add feed forward加反馈并输出

yaw_out = pd + i + vff;

// constrain output and update limit flag限制输出并更新限制标志

if (fabsf(yaw_out) > AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX) {

yaw_out = constrain_float(yaw_out,-AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);

_flags_heli.limit_yaw = true;

}else{

_flags_heli.limit_yaw = false;

}

// output to motors输出电机控制

return yaw_out;

}

AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析相关推荐

AC_AttitudeControl_Heli.cpp的AC_PosControl::set_dt函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. void AC_PosControl::se ...
caffe中loss函数代码分析--caffe学习（16）
接上篇:caffe中样本的label一定要从序号0开始标注吗?–caffe学习(15) A: 1:数学上来说,损失函数loss值和label从0开始还是从1或者100开始是没有直接联系的,以欧式距离损 ...
planner_wisdom()，fftw_wisdom_lookup()，fftw_measure_runtime()，init_test_array()函数代码分析
代码分析以fftw2.15为例,原代码在fftw/planner.c中 planner_wisdom()函数是fftw为了运行效率提出的wisdom机制,主要思想是通过查找之前相似数据(结构.大小等相 ...
AC_AttitudeControl_Heli.cpp的void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. void AC_AttitudeContro ...
AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. 函数作用:传统直升机(单旋翼带尾桨)带平衡杠 ...
WinCE 开始菜单StartMenu_Create()函数代码分析
//================================================================================================== ...
OPENCV例子\samples\cpp\tutorial_code\ImgProc\changing_contrast_brigh的代码分析
该changing_contrast_brigh示例的主要内容为改变图片的对比度和亮度,主要过程: 通用图像处理运算符是获取一个或多个输入图像并生成输出图像的函数图像变换可以看作是: 点运算符(像素 ...
linux ip rcv,Linux网络层 ip_rcv()函数代码分析（__pskb_pull_tail）
int ip_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt) //几个结构sk_buff套接字缓存,n ...
Linux内核分析2：一个简单的时间片轮转多道程序内核代码分析
Lab2:一个简单的时间片轮转多道程序内核代码席金玉 <Linux内核分析>MOOC课程http://mooc.study.163.com/course/USTC-100002900 ...

AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析

AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析相关推荐

最新文章

热门文章