ardupilot疑难代码分析《1》如果转换机体加速度限制到欧拉轴加速度

目录

文章目录

摘要

记录ardupilot疑难代码分析《1》如果转换机体加速度限制到欧拉轴,欢迎一起探讨。

1.程序源码

cpp 复制代码
        //将横滚-俯仰和偏航加速度极限转换为欧拉轴----- translate the roll pitch and yaw acceleration limits to the euler axis
        const Vector3f euler_accel = euler_accel_limit(_euler_angle_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
cpp 复制代码
Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const Vector3f &euler_accel)
{
	//sin_phi:表示横滚
    float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
    float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
    float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
    //旋转加速度
    Vector3f rot_accel;
    //只要有0就设置0
    if (is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x)
    		|| is_negative(euler_accel.y) || is_negative(euler_accel.z))
    {
        rot_accel.x = euler_accel.x;
        rot_accel.y = euler_accel.y;
        rot_accel.z = euler_accel.z;
    } else
    {
    	//正常运行进去
        rot_accel.x = euler_accel.x;
        rot_accel.y = MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi);
        rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / sin_phi), euler_accel.z / cos_phi);
    }
    //得到旋转加速度
    return rot_accel;
}

暂未解决 网址官网提问

相关推荐
yi.Ist28 分钟前
数据结构 —— 键值对 map
数据结构·算法
s1533531 分钟前
数据结构-顺序表-猜数字
数据结构·算法·leetcode
Coding小公仔34 分钟前
LeetCode 8. 字符串转换整数 (atoi)
算法·leetcode·职场和发展
GEEK零零七40 分钟前
Leetcode 393. UTF-8 编码验证
算法·leetcode·职场和发展·二进制运算
DoraBigHead2 小时前
小哆啦解题记——异位词界的社交网络
算法
木头左3 小时前
逻辑回归的Python实现与优化
python·算法·逻辑回归
lifallen7 小时前
Paimon LSM Tree Compaction 策略
java·大数据·数据结构·数据库·算法·lsm-tree
web_Hsir9 小时前
vue3.2 前端动态分页算法
前端·算法
地平线开发者11 小时前
征程 6M 部署 Omnidet 感知模型
算法·自动驾驶
秋说12 小时前
【PTA数据结构 | C语言版】线性表循环右移
c语言·数据结构·算法