How to understand function euler_accel_limit(...)?

luweikxy · November 14, 2017, 7:51am

Hi，

ArduPilot/ardupilot/blob/ed0063d10dd690529fadeabdc35ca5cb32815139/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp#L530


    if (accel_max > 0.0f) {
        float delta_ang_vel = accel_max * _dt;
        target_ang_vel += constrain_float(desired_ang_vel - target_ang_vel, -delta_ang_vel, delta_ang_vel);
    } else {
        target_ang_vel = desired_ang_vel;
    }
    return target_ang_vel;
}


// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
    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;
    if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) {
        rot_accel.x = euler_accel.x;
        rot_accel.y = euler_accel.y;
        rot_accel.z = euler_accel.z;

the function euler_accel_limit(…) is to translate the angle accel from body frame to NED frame, but I cann’t undersdand this function.

Whereas I think the right way is like the euler kinematical equation, just use angle accel instead of angle rate.

the euler kinematical equation is as following: