diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 41bf1d9d19..6f69ddd230 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -127,13 +127,13 @@ - - - + + + - - - + + + @@ -166,22 +166,22 @@ - - - + + + - - - + + + - - - + + + - - - + + +
@@ -190,9 +190,9 @@ - - - + + + @@ -201,9 +201,9 @@
- - - + + +
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 67ff09df8f..788c975955 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -57,12 +57,27 @@ int32_t guidance_h_igain; int32_t guidance_h_ngain; int32_t guidance_h_again; +/* warn if some gains are still negative */ +#if (GUIDANCE_H_PGAIN < 0) || \ + (GUIDANCE_H_DGAIN < 0) || \ + (GUIDANCE_H_IGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif + #ifndef GUIDANCE_H_NGAIN #define GUIDANCE_H_NGAIN 0 +#else +#if (GUIDANCE_H_NGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif #endif #ifndef GUIDANCE_H_AGAIN #define GUIDANCE_H_AGAIN 0 +#else +#if (GUIDANCE_H_AGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif #endif static inline void guidance_h_hover_run(void); @@ -86,11 +101,11 @@ void guidance_h_init(void) { INT_VECT2_ZERO(guidance_h_pos_err_sum); INT_EULERS_ZERO(guidance_h_rc_sp); INT_EULERS_ZERO(guidance_h_command_body); - guidance_h_pgain = ABS(GUIDANCE_H_PGAIN); - guidance_h_igain = ABS(GUIDANCE_H_IGAIN); - guidance_h_dgain = ABS(GUIDANCE_H_DGAIN); - guidance_h_ngain = ABS(GUIDANCE_H_NGAIN); - guidance_h_again = ABS(GUIDANCE_H_AGAIN); + guidance_h_pgain = GUIDANCE_H_PGAIN; + guidance_h_igain = GUIDANCE_H_IGAIN; + guidance_h_dgain = GUIDANCE_H_DGAIN; + guidance_h_ngain = GUIDANCE_H_NGAIN; + guidance_h_again = GUIDANCE_H_AGAIN; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index eba0bee30c..b28cb320c0 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -37,6 +37,13 @@ #include "generated/airframe.h" +/* warn if some gains are still negative */ +#if (GUIDANCE_V_HOVER_KP < 0) || \ + (GUIDANCE_V_HOVER_KD < 0) || \ + (GUIDANCE_V_HOVER_KI < 0) +#warning "ALL control gains are now positive!!!" +#endif + /* In case Asctec controllers are used without supervision */ #ifndef SUPERVISION_MIN_MOTOR #define SUPERVISION_MIN_MOTOR 1 @@ -97,9 +104,9 @@ void guidance_v_init(void) { guidance_v_mode = GUIDANCE_V_MODE_KILL; - guidance_v_kp = ABS(GUIDANCE_V_HOVER_KP); - guidance_v_kd = ABS(GUIDANCE_V_HOVER_KD); - guidance_v_ki = ABS(GUIDANCE_V_HOVER_KI); + guidance_v_kp = GUIDANCE_V_HOVER_KP; + guidance_v_kd = GUIDANCE_V_HOVER_KD; + guidance_v_ki = GUIDANCE_V_HOVER_KI; guidance_v_z_sum_err = 0; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 140cc9f44a..6139806670 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -31,6 +31,20 @@ struct FloatAttitudeGains stabilization_gains; + +/* warn if some gains are still negative */ +#if (STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif + struct FloatEulers stabilization_att_sum_err; float stabilization_att_fb_cmd[COMMANDS_NB]; @@ -42,24 +56,24 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, - ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN)); + STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN, + STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN, + STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, - ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN)); + STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN, + STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN, + STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, - ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN)); + STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN, + STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN, + STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, - ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN), - ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN)); + STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN, + STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN, + STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 1e5a845b81..614143f878 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -31,6 +31,19 @@ struct Int32AttitudeGains stabilization_gains; +/* warn if some gains are still negative */ +#if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_IGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif + struct Int32Eulers stabilization_att_sum_err; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; @@ -42,24 +55,24 @@ void stabilization_attitude_init(void) { VECT3_ASSIGN(stabilization_gains.p, - ABS(STABILIZATION_ATTITUDE_PHI_PGAIN), - ABS(STABILIZATION_ATTITUDE_THETA_PGAIN), - ABS(STABILIZATION_ATTITUDE_PSI_PGAIN)); + STABILIZATION_ATTITUDE_PHI_PGAIN, + STABILIZATION_ATTITUDE_THETA_PGAIN, + STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, - ABS(STABILIZATION_ATTITUDE_PHI_DGAIN), - ABS(STABILIZATION_ATTITUDE_THETA_DGAIN), - ABS(STABILIZATION_ATTITUDE_PSI_DGAIN)); + STABILIZATION_ATTITUDE_PHI_DGAIN, + STABILIZATION_ATTITUDE_THETA_DGAIN, + STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, - ABS(STABILIZATION_ATTITUDE_PHI_IGAIN), - ABS(STABILIZATION_ATTITUDE_THETA_IGAIN), - ABS(STABILIZATION_ATTITUDE_PSI_IGAIN)); + STABILIZATION_ATTITUDE_PHI_IGAIN, + STABILIZATION_ATTITUDE_THETA_IGAIN, + STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, - ABS(STABILIZATION_ATTITUDE_PHI_DDGAIN), - ABS(STABILIZATION_ATTITUDE_THETA_DDGAIN), - ABS(STABILIZATION_ATTITUDE_PSI_DDGAIN)); + STABILIZATION_ATTITUDE_PHI_DDGAIN, + STABILIZATION_ATTITUDE_THETA_DDGAIN, + STABILIZATION_ATTITUDE_PSI_DDGAIN); INT_EULERS_ZERO( stabilization_att_sum_err ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index e84c238123..800189caf8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -35,6 +35,19 @@ struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB]; +/* warn if some gains are still negative */ +#if (STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif + struct FloatQuat stabilization_att_sum_err_quat; struct FloatEulers stabilization_att_sum_err_eulers; @@ -86,15 +99,15 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) { - VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); + VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); + VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); + VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); + VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); } FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 767c99f06f..46074c1d1c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -35,6 +35,19 @@ struct Int32AttitudeGains stabilization_gains; +/* warn if some gains are still negative */ +#if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \ + (STABILIZATION_ATTITUDE_PSI_IGAIN < 0) +#warning "ALL control gains are now positive!!!" +#endif + struct Int32Quat stabilization_att_sum_err_quat; struct Int32Eulers stabilization_att_sum_err; @@ -93,34 +106,34 @@ void stabilization_attitude_init(void) { /* for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { - VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); - VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); + VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); + VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); + VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); + VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); } */ - VECT3_ASSIGN_ABS(stabilization_gains.p, + VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN); - VECT3_ASSIGN_ABS(stabilization_gains.d, + VECT3_ASSIGN(stabilization_gains.d, STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN); - VECT3_ASSIGN_ABS(stabilization_gains.i, + VECT3_ASSIGN(stabilization_gains.i, STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN); - VECT3_ASSIGN_ABS(stabilization_gains.dd, + VECT3_ASSIGN(stabilization_gains.dd, STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 62891feed3..bb8e42d01a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -36,6 +36,12 @@ #define MAX_SUM_ERR 4000000 +#if (STABILIZATION_RATE_GAIN_P < 0) || \ + (STABILIZATION_RATE_GAIN_Q < 0) || \ + (STABILIZATION_RATE_GAIN_R < 0) +#warning "ALL control gains are now positive!!!" +#endif + #ifndef STABILIZATION_RATE_DDGAIN_P #define STABILIZATION_RATE_DDGAIN_P 0 #endif @@ -45,15 +51,29 @@ #ifndef STABILIZATION_RATE_DDGAIN_R #define STABILIZATION_RATE_DDGAIN_R 0 #endif + #ifndef STABILIZATION_RATE_IGAIN_P #define STABILIZATION_RATE_IGAIN_P 0 +#else +#if (STABILIZATION_RATE_IGAIN_P < 0) +#warning "ALL control gains are now positive!!!" +#endif #endif #ifndef STABILIZATION_RATE_IGAIN_Q #define STABILIZATION_RATE_IGAIN_Q 0 +#else +#if (STABILIZATION_RATE_IGAIN_Q < 0) +#warning "ALL control gains are now positive!!!" +#endif #endif #ifndef STABILIZATION_RATE_IGAIN_R #define STABILIZATION_RATE_IGAIN_R 0 +#else +#if (STABILIZATION_RATE_IGAIN_R < 0) +#warning "ALL control gains are now positive!!!" #endif +#endif + #ifndef STABILIZATION_RATE_REF_TAU #define STABILIZATION_RATE_REF_TAU 4 #endif @@ -101,17 +121,17 @@ void stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_sp); RATES_ASSIGN(stabilization_rate_gain, - ABS(STABILIZATION_RATE_GAIN_P), - ABS(STABILIZATION_RATE_GAIN_Q), - ABS(STABILIZATION_RATE_GAIN_R)); + STABILIZATION_RATE_GAIN_P, + STABILIZATION_RATE_GAIN_Q, + STABILIZATION_RATE_GAIN_R); RATES_ASSIGN(stabilization_rate_igain, - ABS(STABILIZATION_RATE_IGAIN_P), - ABS(STABILIZATION_RATE_IGAIN_Q), - ABS(STABILIZATION_RATE_IGAIN_R)); + STABILIZATION_RATE_IGAIN_P, + STABILIZATION_RATE_IGAIN_Q, + STABILIZATION_RATE_IGAIN_R); RATES_ASSIGN(stabilization_rate_ddgain, - ABS(STABILIZATION_RATE_DDGAIN_P), - ABS(STABILIZATION_RATE_DDGAIN_Q), - ABS(STABILIZATION_RATE_DDGAIN_R)); + STABILIZATION_RATE_DDGAIN_P, + STABILIZATION_RATE_DDGAIN_Q, + STABILIZATION_RATE_DDGAIN_R); INT_RATES_ZERO(stabilization_rate_ref); INT_RATES_ZERO(stabilization_rate_refdot);