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;
}

暂未解决 网址官网提问

相关推荐
凌肖战26 分钟前
力扣网C语言编程题:在数组中查找目标值位置之二分查找法
c语言·算法·leetcode
weixin_478689761 小时前
十大排序算法汇总
java·算法·排序算法
luofeiju1 小时前
使用LU分解求解线性方程组
线性代数·算法
SKYDROID云卓小助手2 小时前
无人设备遥控器之自动调整编码技术篇
人工智能·嵌入式硬件·算法·自动化·信号处理
ysa0510302 小时前
数论基础知识和模板
数据结构·c++·笔记·算法
GEEK零零七2 小时前
Leetcode 1103. 分糖果 II
数学·算法·leetcode·等差数列
今天背单词了吗9803 小时前
算法学习笔记:7.Dijkstra 算法——从原理到实战,涵盖 LeetCode 与考研 408 例题
java·开发语言·数据结构·笔记·算法
重庆小透明4 小时前
力扣刷题记录【1】146.LRU缓存
java·后端·学习·算法·leetcode·缓存
desssq4 小时前
力扣:70. 爬楼梯
算法·leetcode·职场和发展
clock的时钟5 小时前
暑期数据结构第一天
数据结构·算法