diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 1aadf44037..bdb399f3d4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -84,16 +84,16 @@ float stabilization_attitude_get_heading_f(void) { * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { - const float max_phi_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ; - const float max_theta_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ; - const float max_r_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ; - - sp->phi = (int32_t) (radio_control.values[RADIO_ROLL] * max_phi_scale); - sp->theta = (int32_t) (radio_control.values[RADIO_PITCH] * max_theta_scale); + const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); + const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); + const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); + + sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ); + sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += (int32_t) (radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ); + sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ); INT32_ANGLE_NORMALIZE(sp->psi); } if (autopilot_mode == AP_MODE_FORWARD) {