AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate

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

函数作用：传统直升机（单旋翼带尾桨）带平衡杠的特技模式，通过飞手的滚转和俯仰输入直接驱动倾斜盘（自动倾斜器）。

函数调用情况如下（项目中在哪里出现）：五个地方

函数声明情况如下：

函数声明文件如下：

函数流程图如下：

函数逻辑顺序图如下：

函数原始代码如下：

// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode通过飞手的滚转和俯仰输入直接驱动倾斜盘（自动倾斜器），带平衡杠的特技模式

void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds)

{

// convert from centidegrees on public interface to radians从度转换为弧度

float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);

// store roll, pitch and passthroughs保存滚转、俯仰、偏航

// NOTE: this abuses yaw_rate_bf_rads

_passthrough_roll = roll_passthrough;

_passthrough_pitch = pitch_passthrough;

_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;

// set rate controller to use pass through设置速率控制

_flags_heli.flybar_passthrough = true;

// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)

_attitude_target_ang_vel.x = _ahrs.get_gyro().x;

_attitude_target_ang_vel.y = _ahrs.get_gyro().y;

// accel limit desired yaw rate加速度限制

if (get_accel_yaw_max_radss() > 0.0f) {

float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;

float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;

rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);

_attitude_target_ang_vel.z += rate_change_rads;

} else {

_attitude_target_ang_vel.z = yaw_rate_bf_rads;

}

integrate_bf_rate_error_to_angle_errors();

_att_error_rot_vec_rad.x = 0;

_att_error_rot_vec_rad.y = 0;

// update our earth-frame angle targets更新角速率

Vector3f att_error_euler_rad;

// convert angle error rotation vector into 321-intrinsic euler angle difference

// NOTE: this results an an approximation linearized about the vehicle's attitude

if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {

_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);

_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);

_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);

}

// handle flipping over pitch axis处理俯仰的跳动

if (_attitude_target_euler_angle.y > M_PI/2.0f) {

_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);

_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);

_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);

}

if (_attitude_target_euler_angle.y < -M_PI/2.0f) {

_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);

_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);

_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);

}

// convert body-frame angle errors to body-frame rate targets

_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);

// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates期望的俯仰、倾斜

_rate_target_ang_vel.x = _attitude_target_ang_vel.x;

_rate_target_ang_vel.y = _attitude_target_ang_vel.y;

// add desired target to yaw期望的偏航

_rate_target_ang_vel.z += _attitude_target_ang_vel.z;

_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);

}

AC_AttitudeControl_Heli.cpp的AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_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的AC_AttitudeControl_Heli::rate_target_to_motor_yaw函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. float AC_AttitudeContr ...
AC_AttitudeControl_Heli.cpp的void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch函数代码分析
代码基于ardupilot3.4.2RC2,仅分析ArduCopter(多旋翼.直升机)构型中代码逻辑.使用关系,其他构型(飞机.云台.车)后续考虑分析. void AC_AttitudeContro ...
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::passthrough_bf_roll_pitch_rate_yaw函数代码分析

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

最新文章

热门文章