Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

rotorcraft gains: do not use ABS anymore, warn instead if gains are negative

Browse Source
This commit is contained in:
Felix Ruess
2012-02-20 21:40:02 +01:00
parent f59cd0fcdd
commit df76dc8fbe
8 changed files with 182 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/fraser_lisa_m_rotorcraft.xml
+24 -24
View File
@@ -127,13 +127,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -166,22 +166,22 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
@@ -190,9 +190,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KI" value="-20"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -201,9 +201,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+20 -5
View File
@@ -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;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+10 -3
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+26 -12
View File
@@ -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 );
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+25 -12
View File
@@ -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 );
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+22 -9
View File
@@ -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 );
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+26 -13
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+29 -9
View File
@@ -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);