diff --git a/conf/settings/control/stabilization_att_int.xml b/conf/settings/control/stabilization_att_int.xml index 4b3966baa3..f57868aba3 100644 --- a/conf/settings/control/stabilization_att_int.xml +++ b/conf/settings/control/stabilization_att_int.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/settings/control/stabilization_att_int_quat.xml b/conf/settings/control/stabilization_att_int_quat.xml index 77c45ae68c..97865309d7 100644 --- a/conf/settings/control/stabilization_att_int_quat.xml +++ b/conf/settings/control/stabilization_att_int_quat.xml @@ -6,7 +6,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h index 0054e5bdfd..7bfbd42b92 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h @@ -46,8 +46,6 @@ struct FloatAttitudeGains { struct FloatVect3 surface_i; }; -extern struct FloatEulers stabilization_att_sum_err; - extern float stabilization_att_fb_cmd[COMMANDS_NB]; extern float stabilization_att_ff_cmd[COMMANDS_NB]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h index b65fbd30d8..751da0a98e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h @@ -40,14 +40,8 @@ struct Int32AttitudeGains { }; extern struct Int32AttitudeGains stabilization_gains; -extern struct Int32Eulers stabilization_att_sum_err; extern int32_t stabilization_att_fb_cmd[COMMANDS_NB]; extern int32_t stabilization_att_ff_cmd[COMMANDS_NB]; -#define stabilization_attitude_common_int_SetKiPhi(_val) { \ - stabilization_gains.i.x = _val; \ - stabilization_att_sum_err.phi = 0; \ - } - #endif /* STABILIZATION_ATTITUDE_COMMON_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h index 9b72570412..08e4dd235c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h @@ -33,5 +33,6 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" extern struct FloatAttitudeGains stabilization_gains; +extern struct FloatEulers stabilization_att_sum_err; #endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h index 9488c542de..eba7a64b82 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h @@ -25,4 +25,6 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" +extern struct Int32Eulers stabilization_att_sum_err; + #endif /* STABILIZATION_ATTITUDE_EULER_INT_H */ 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 310736686e..a881c75197 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -38,7 +38,6 @@ struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]; struct FloatQuat stabilization_att_sum_err_quat; -struct FloatEulers stabilization_att_sum_err; struct FloatRates last_body_rate; struct FloatRates body_rate_d; @@ -105,9 +104,9 @@ static void send_att(void) { &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, - &stabilization_att_sum_err.phi, - &stabilization_att_sum_err.theta, - &stabilization_att_sum_err.psi, + &stabilization_att_sum_err_quat.qx, + &stabilization_att_sum_err_quat.qy, + &stabilization_att_sum_err_quat.qz, &stabilization_att_fb_cmd[COMMAND_ROLL], &stabilization_att_fb_cmd[COMMAND_PITCH], &stabilization_att_fb_cmd[COMMAND_YAW], @@ -156,7 +155,6 @@ void stabilization_attitude_init(void) { } float_quat_identity(&stabilization_att_sum_err_quat); - FLOAT_EULERS_ZERO( stabilization_att_sum_err ); FLOAT_RATES_ZERO( last_body_rate ); FLOAT_RATES_ZERO( body_rate_d ); @@ -184,7 +182,6 @@ void stabilization_attitude_enter(void) { stabilization_attitude_ref_enter(); float_quat_identity(&stabilization_att_sum_err_quat); - FLOAT_EULERS_ZERO( stabilization_att_sum_err ); } void stabilization_attitude_set_failsafe_setpoint(void) { @@ -305,22 +302,18 @@ void stabilization_attitude_run(bool_t enable_integrator) { RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); +#define INTEGRATOR_BOUND 1.0 /* integrated error */ if (enable_integrator) { - struct FloatQuat new_sum_err, scaled_att_err; - /* update accumulator */ - scaled_att_err.qi = att_err.qi; - scaled_att_err.qx = att_err.qx / IERROR_SCALE; - scaled_att_err.qy = att_err.qy / IERROR_SCALE; - scaled_att_err.qz = att_err.qz / IERROR_SCALE; - float_quat_comp(&new_sum_err, &stabilization_att_sum_err_quat, &scaled_att_err); - float_quat_normalize(&new_sum_err); - QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); - float_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat); + stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE; + stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE; + stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE; + Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); } else { /* reset accumulator */ float_quat_identity(&stabilization_att_sum_err_quat); - FLOAT_EULERS_ZERO( stabilization_att_sum_err ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); 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 fb93a8bc85..145d902815 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -56,7 +56,6 @@ struct Int32AttitudeGains stabilization_gains = { #endif struct Int32Quat stabilization_att_sum_err_quat; -struct Int32Eulers stabilization_att_sum_err; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; @@ -79,9 +78,9 @@ static void send_att(void) { //FIXME really use this message here ? &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, - &stabilization_att_sum_err.phi, - &stabilization_att_sum_err.theta, - &stabilization_att_sum_err.psi, + &stabilization_att_sum_err_quat.qx, + &stabilization_att_sum_err_quat.qy, + &stabilization_att_sum_err_quat.qz, &stabilization_att_fb_cmd[COMMAND_ROLL], &stabilization_att_fb_cmd[COMMAND_PITCH], &stabilization_att_fb_cmd[COMMAND_YAW], @@ -128,7 +127,6 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); int32_quat_identity(&stabilization_att_sum_err_quat); - INT_EULERS_ZERO( stabilization_att_sum_err ); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); @@ -145,7 +143,6 @@ void stabilization_attitude_enter(void) { stabilization_attitude_ref_enter(); int32_quat_identity(&stabilization_att_sum_err_quat); - INT_EULERS_ZERO(stabilization_att_sum_err); } @@ -242,22 +239,18 @@ void stabilization_attitude_run(bool_t enable_integrator) { struct Int32Rates* body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate)); +#define INTEGRATOR_BOUND 100000 /* integrated error */ if (enable_integrator) { - struct Int32Quat new_sum_err, scaled_att_err; - /* update accumulator */ - scaled_att_err.qi = att_err.qi; - scaled_att_err.qx = att_err.qx / IERROR_SCALE; - scaled_att_err.qy = att_err.qy / IERROR_SCALE; - scaled_att_err.qz = att_err.qz / IERROR_SCALE; - int32_quat_comp(&new_sum_err, &stabilization_att_sum_err_quat, &scaled_att_err); - int32_quat_normalize(&new_sum_err); - QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); - int32_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat); + stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE; + stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE; + stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE; + Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); } else { /* reset accumulator */ int32_quat_identity(&stabilization_att_sum_err_quat); - INT_EULERS_ZERO( stabilization_att_sum_err ); } /* compute the feed forward command */