|
|
|
@ -641,8 +641,8 @@ static void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitc
@@ -641,8 +641,8 @@ static void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitc
|
|
|
|
|
poshold.wind_comp_timer = 0; |
|
|
|
|
|
|
|
|
|
// convert earth frame desired accelerations to body frame roll and pitch lean angles |
|
|
|
|
roll_angle = (float)fast_atan((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/(float)M_PI); |
|
|
|
|
pitch_angle = (float)fast_atan(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/(float)M_PI); |
|
|
|
|
roll_angle = (float)fast_atan((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI_F); |
|
|
|
|
pitch_angle = (float)fast_atan(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI_F); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis |
|
|
|
|