Copter: Is there any problem in function euler_accel_limit()?

[luweiagi]

ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

Line 530 in ed0063d

Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)

The function euler_accel_limit(...) is to translate the angle accel limitations from body frame to euler frame.

In this function, rot_accel(in euler frame) is calculated below:

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

.But I can not figure it out. 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 indicated below

Namely, the code may be written below:

// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
    // body frame angular acceleration max limitation must be positive
    euler_accel.x = fabsf(euler_accel.x);
    euler_accel.y = fabsf(euler_accel.y);
    euler_accel.z = fabsf(euler_accel.z);
	
    float sin_phi = sinf(euler_rad.x);
    float cos_phi = cosf(euler_rad.x);
    float cos_theta = constrain_float((cosf(euler_rad.y)), 0.1f, 1.0f);// prevent 1/cos_theta being infinity
    float tan_theta = constrain_float(tanf(euler_rad.y), -10.0f, 10.0f);// prevent tan_theta  being infinity

    Vector3f rot_accel;

    rot_accel.x =  fabsf(euler_accel.x + tan_theta * (euler_accel.y * sin_phi + euler_accel.z * cos_phi));
    rot_accel.y =  fabsf(euler_accel.y * cos_phi - euler_accel.z * sin_phi);
    rot_accel.z =  fabsf((1 / cos_theta) * (euler_accel.y * sin_phi + euler_accel.z * cos_phi));

    return rot_accel;
}

But my solutions still seems having some problems, because it means that the rot_accel is calculated based on this situations that body accel at three body axises is reaching the max euler_rad at the same time. It definitely does not fit the reality.

[lthall]

Hi @luweikxy,

Thanks again for going over this so thoroughly!

So the problem here is we need to calculate the acceleration limits that will not let us increase past any one individual axis acceleration limit. I think you are calculating the maximum acceleration limits possible, not the minimum that we need.

So for example, if we are banked over by 45 degrees in roll. If we ask apply full yaw then the acceleration will be shared by both the body pitch and body yaw. However, if we apply full yaw and push the pitch stick forward then that euler yaw acceleration is applied only to the body frame yaw and is increased in magnitude because the aircraft is banked by 45 degrees. So we need to limit the yaw acceleration based on this limit rather than the vector addition of both body pitch and body yaw.

I realise this is poorly worded but does it make sense?

Thanks again!

[lthall]

I have been thinking about this. Perhaps what we need to do here is look at the maximum possible acceleration in the direction of that acceleration rather than the maximum safe acceleration in each axis with no knowledge of what is happening in any other axis.

The function would need a little more information though.

[IamPete1]

@lthall Is there any action we should take due to this? It sounds like the there is not a problem so we should close, but maybe there is a enhancement that we could open a new issue for?

[luweiagi]

final solution: Copter: Is there any problem in function euler_accel_limit() ? version-2

[lixiaowei123]

@lthall He wanted to ask how the formula was derived, but you didn't answer his question

[lthall]

@lixiaowei123 I don't agree.