I need someone to back me up here on this bug I believe I have found. I was reviewing the L1 controller code in an effort to use the controller for heli high speed waypoint navigation. I think the calculation for nav_roll_cd is incorrect. Here is the current code that calculates this variable
int32_t AP_L1_Control::nav_roll_cd(void) const
{
float ret;
ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81
ret = constrain_float(ret, -9000, 9000);
return ret;
}
My concern is where the multiplication by 100.0f is placed. It is inside the parenthesis for the conversion from radians to degrees. I believe the intent of the multiplication by 100 is to convert the value to centidegrees. in its current form it multiplies the radian value of the roll angle by 100. This line should read
ret = cosf(_ahrs.pitch) * 100.0f * degrees(atanf(_latAccDem * 0.101972f));