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

目录

文章目录

• 目录
• 摘要
• 1.程序源码

摘要

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

1.程序源码

        //将横滚-俯仰和偏航加速度极限转换为欧拉轴----- 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()});

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

暂未解决 网址官网提问