AC_AttitudeControl_Heli.cpp的void AC_AttitudeControl_Heli::rate_bf_to_motor_roll

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

void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads)

函数作用：。

机体俯仰、倾斜角速率控制的执行，由速率控制器计算电机输出，以达到以弧度/秒为单位的目标速率

main函数全局调用情况如下：五个地方出现

函数声明情况如下：

函数声明文件如下：

函数流程图如下：

函数逻辑顺序图如下：

函数原始代码如下：

// body-frame rate controller机体角速率控制器

// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second

void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads)

{

float roll_pd, roll_i, roll_ff; // used to capture pid values滚转的PID数值

float pitch_pd, pitch_i, pitch_ff; // used to capture pid values俯仰的PID数值

float rate_roll_error_rads, rate_pitch_error_rads; // simply target_rate - current_rate当前俯仰和滚转的误差

float roll_out, pitch_out;

const Vector3f& gyro = _ahrs.get_gyro(); // get current rates得到角速率

// calculate error计算误差

rate_roll_error_rads = rate_roll_target_rads - gyro.x;

rate_pitch_error_rads = rate_pitch_target_rads - gyro.y;

// pass error to PID controller传递误差给PID控制器

_pid_rate_roll.set_input_filter_all(rate_roll_error_rads);

_pid_rate_roll.set_desired_rate(rate_roll_target_rads);

_pid_rate_pitch.set_input_filter_all(rate_pitch_error_rads);

_pid_rate_pitch.set_desired_rate(rate_pitch_target_rads);

// call p and d controllers调用P和D控制器

roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d();

pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d();

// get roll i term获得滚转的积分

roll_i = _pid_rate_roll.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_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){

if (_flags_heli.leaky_i){

roll_i = _pid_rate_roll.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);

}else{

roll_i = _pid_rate_roll.get_i();

}

// get pitch i term获得俯仰的积分

pitch_i = _pid_rate_pitch.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_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){

if (_flags_heli.leaky_i) {

pitch_i = _pid_rate_pitch.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);

}else{

pitch_i = _pid_rate_pitch.get_i();

}

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

roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_vff(rate_roll_target_rads), _dt);

pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_vff(rate_pitch_target_rads), _dt);

// add feed forward and final output加反馈并输出

roll_out = roll_pd + roll_i + roll_ff;

pitch_out = pitch_pd + pitch_i + pitch_ff;

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

if (fabsf(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {

roll_out = constrain_float(roll_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);

_flags_heli.limit_roll = true;

}else{

_flags_heli.limit_roll = false;

}

if (fabsf(pitch_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) {

pitch_out = constrain_float(pitch_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);

_flags_heli.limit_pitch = true;

}else{

_flags_heli.limit_pitch = false;

}

// output to motors输出电机控制

_motors.set_roll(roll_out);

_motors.set_pitch(pitch_out);

// Piro-Comp, or Pirouette Compensation is a pre-compensation calculation, which basically rotates the Roll and Pitch Rate I-terms as the

// helicopter rotates in yaw. Much of the built-up I-term is needed to tip the disk into the incoming wind. Fast yawing can create an instability

// as the built-up I-term in one axis must be reduced, while the other increases. This helps solve that by rotating the I-terms before the error occurs.

// It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption.

Piro Comp或Pirouette补偿是一种预补偿计算，基本上随着直升机在偏航中旋转，旋转横滚和俯仰率I项。大部分累积的I项需要将圆盘倾斜到迎面而来的风中。快速偏航会造成不稳定性，因为一个轴上的累积I项必须减少，而另一个轴上的累积I项则增加。这有助于在错误发生之前旋转I项来解决这个问题。它确实假设转子的空气动力学和力学基本上与主轴对称，这是一个普遍有效的假设。

if (_piro_comp_enabled){

int32_t piro_roll_i, piro_pitch_i; // used to hold I-terms while doing piro comp

piro_roll_i = roll_i;

piro_pitch_i = pitch_i;

Vector2f yawratevector;

yawratevector.x = cosf(-_ahrs.get_gyro().z * _dt);

yawratevector.y = sinf(-_ahrs.get_gyro().z * _dt);

yawratevector.normalize();

roll_i = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y;

pitch_i = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y;

_pid_rate_pitch.set_integrator(pitch_i);

_pid_rate_roll.set_integrator(roll_i);

}

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

caffe中loss函数代码分析--caffe学习（16）
接上篇:caffe中样本的label一定要从序号0开始标注吗?–caffe学习(15) A: 1:数学上来说,损失函数loss值和label从0开始还是从1或者100开始是没有直接联系的,以欧式距离损 ...
AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. 函数作用:传统直升机(单旋翼带尾桨)带平衡杠 ...
planner_wisdom()，fftw_wisdom_lookup()，fftw_measure_runtime()，init_test_array()函数代码分析
代码分析以fftw2.15为例,原代码在fftw/planner.c中 planner_wisdom()函数是fftw为了运行效率提出的wisdom机制,主要思想是通过查找之前相似数据(结构.大小等相 ...
AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. float AC_AttitudeContr ...
AC_AttitudeControl_Heli.cpp的AC_PosControl::set_dt函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. void AC_PosControl::se ...
WinCE 开始菜单StartMenu_Create()函数代码分析
//================================================================================================== ...
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 ...
【FFmpeg编程进阶】（15）FLV 编码器Codec初始化 ff_mpv_encode_init() 代码分析 -- 待更新
[FFmpeg编程进阶](15)FLV 编码器Codec初始化 ff_mpv_encode_init 代码分析一.ff_mpv_encode_init() 在前面分析<[FFmpeg编程进阶] ...

AC_AttitudeControl_Heli.cpp的void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch函数代码分析

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

最新文章

热门文章