diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile index 423308b76a..a83b8ad740 100644 --- a/conf/autopilot/booz2_simulator_nps.makefile +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -94,8 +94,8 @@ sim.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c sim.srcs += $(SRC_BOOZ)/booz_stabilization.c sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c #sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_H=\"stabilization/booz_stabilization_attitude_euler.h\" -#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler.c -sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c +sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler.c +#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c diff --git a/conf/messages.xml b/conf/messages.xml index 22b75ea980..033c136152 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -768,9 +768,12 @@ - - - + + + + + + diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index a117154971..ffbf7c20fa 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -17,9 +17,9 @@ - + - + diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index f3992f82fe..c0eca581d7 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -172,9 +172,12 @@ &booz_stabilization_att_sum_err.phi, \ &booz_stabilization_att_sum_err.theta, \ &booz_stabilization_att_sum_err.psi, \ - &booz_stabilization_att_err_cmd[COMMAND_ROLL], \ - &booz_stabilization_att_err_cmd[COMMAND_PITCH], \ - &booz_stabilization_att_err_cmd[COMMAND_YAW], \ + &booz_stabilization_att_fb_cmd[COMMAND_ROLL], \ + &booz_stabilization_att_fb_cmd[COMMAND_PITCH], \ + &booz_stabilization_att_fb_cmd[COMMAND_YAW], \ + &booz_stabilization_att_ff_cmd[COMMAND_ROLL], \ + &booz_stabilization_att_ff_cmd[COMMAND_PITCH], \ + &booz_stabilization_att_ff_cmd[COMMAND_YAW], \ &booz_stabilization_cmd[COMMAND_ROLL], \ &booz_stabilization_cmd[COMMAND_PITCH], \ &booz_stabilization_cmd[COMMAND_YAW]); \ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude.h index 5cb3b01318..77e7a129c3 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude.h @@ -41,6 +41,13 @@ extern struct Int32Vect3 booz_stabilization_dgain; extern struct Int32Vect3 booz_stabilization_ddgain; extern struct Int32Eulers booz_stabilization_att_sum_err; -extern int32_t booz_stabilization_att_err_cmd[COMMANDS_NB]; +extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB]; +extern int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB]; + + +#define booz_stabilization_attitude_SetKiPhi(_val) { \ + booz_stabilization_igain.x = _val; \ + booz_stabilization_att_sum_err.phi = 0; \ + } #endif /* BOOZ2_STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c index 2914e3a5e6..ebc332129f 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c @@ -39,7 +39,8 @@ struct Int32Vect3 booz_stabilization_ddgain; struct Int32Vect3 booz_stabilization_igain; struct Int32Eulers booz_stabilization_att_sum_err; -int32_t booz_stabilization_att_err_cmd[COMMANDS_NB]; +int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB]; +int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB]; static inline void booz_stabilization_update_ref(void); @@ -92,6 +93,9 @@ void booz_stabilization_attitude_enter(void) { } +#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) +#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) + #define MAX_SUM_ERR 4000000 void booz_stabilization_attitude_run(bool_t in_flight) { @@ -100,9 +104,9 @@ void booz_stabilization_attitude_run(bool_t in_flight) { /* compute attitude error */ const struct Int32Eulers att_ref_scaled = { - booz_stabilization_att_ref.phi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC), - booz_stabilization_att_ref.theta >> (ANGLE_REF_RES - INT32_ANGLE_FRAC), - booz_stabilization_att_ref.psi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC) }; + OFFSET_AND_ROUND2(booz_stabilization_att_ref.phi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)), + OFFSET_AND_ROUND2(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)), + OFFSET_AND_ROUND2(booz_stabilization_att_ref.psi, (ANGLE_REF_RES - INT32_ANGLE_FRAC)) }; struct Int32Eulers att_err; EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled); INT32_ANGLE_NORMALIZE(att_err.psi); @@ -118,35 +122,38 @@ void booz_stabilization_attitude_run(bool_t in_flight) { /* compute rate error */ const struct Int32Rates rate_ref_scaled = { - booz_stabilization_rate_ref.x >> (RATE_REF_RES - INT32_RATE_FRAC), - booz_stabilization_rate_ref.y >> (RATE_REF_RES - INT32_RATE_FRAC), - booz_stabilization_rate_ref.z >> (RATE_REF_RES - INT32_RATE_FRAC) }; + OFFSET_AND_ROUND2(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)), + OFFSET_AND_ROUND2(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)), + OFFSET_AND_ROUND2(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) }; struct Int32Rates rate_err; RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled); /* compute PID loop */ - booz_stabilization_att_err_cmd[COMMAND_ROLL] = + booz_stabilization_att_fb_cmd[COMMAND_ROLL] = booz_stabilization_pgain.x * att_err.phi + booz_stabilization_dgain.x * rate_err.p + - ((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi) >> 10); - booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_att_err_cmd[COMMAND_ROLL] + - ((booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x) >> 5); - booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_cmd[COMMAND_ROLL] >> 16; + OFFSET_AND_ROUND((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi), 10); - booz_stabilization_att_err_cmd[COMMAND_PITCH] = + booz_stabilization_att_ff_cmd[COMMAND_ROLL] = + OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x, 5); + + booz_stabilization_cmd[COMMAND_ROLL] = + OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]), 16); + + booz_stabilization_att_fb_cmd[COMMAND_PITCH] = booz_stabilization_pgain.y * att_err.theta + booz_stabilization_dgain.y * rate_err.q + ((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta) >> 10); - booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_att_err_cmd[COMMAND_PITCH] + + booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_att_fb_cmd[COMMAND_PITCH] + ((booz_stabilization_ddgain.y * booz_stabilization_accel_ref.y) >> 5); booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_cmd[COMMAND_PITCH] >> 16; - booz_stabilization_att_err_cmd[COMMAND_YAW] = + booz_stabilization_att_fb_cmd[COMMAND_YAW] = booz_stabilization_pgain.z * att_err.psi + booz_stabilization_dgain.z * rate_err.r + ((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi) >> 10); - booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_att_err_cmd[COMMAND_YAW] + + booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_att_fb_cmd[COMMAND_YAW] + ((booz_stabilization_ddgain.z * booz_stabilization_accel_ref.z) >> 5); booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_cmd[COMMAND_YAW] >> 16; diff --git a/sw/airborne/booz/test/test_scaling.c b/sw/airborne/booz/test/test_scaling.c index 2432b3349b..c554a3f4a1 100644 --- a/sw/airborne/booz/test/test_scaling.c +++ b/sw/airborne/booz/test/test_scaling.c @@ -25,7 +25,7 @@ #include #include -#include "booz_geometry_mixed.h" +#include "math/pprz_algebra_int.h" #define IMU_ACCEL_X_NEUTRAL 32081 #define IMU_ACCEL_X_SENS -2.50411474 @@ -34,10 +34,12 @@ void test_1(void); void test_2(void); +void test_3(void); int main(int argc, char** argv){ - test_2(); + // test_2(); + test_3(); return 0; } @@ -50,10 +52,10 @@ void test_1(void) { double neutral_f = (double)IMU_ACCEL_X_NEUTRAL; double sensitivity_f = 1./IMU_ACCEL_X_SENS; - double sensor_raw_f = BOOZ_ACCEL_I_OF_F(value_f) * sensitivity_f + neutral_f; + double sensor_raw_f = ACCEL_BFP_OF_REAL(value_f) * sensitivity_f + neutral_f; int32_t sensor_raw_i = rint(sensor_raw_f); - double scaled_sensor_f = BOOZ_ACCEL_I_OF_F(value_f); + double scaled_sensor_f = ACCEL_BFP_OF_REAL(value_f); #if 1 int32_t scaled_sensor_i = ((sensor_raw_i - IMU_ACCEL_X_NEUTRAL) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; #endif @@ -90,3 +92,28 @@ void test_2(void) { } } + +#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) +#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) +#define N_OFFSET 2 +void test_3(void) { + + int a; + for (a=-(1<>N_OFFSET); + int32_t c; + // if ( a>=0 ) + // c = (a+(1<<(N_OFFSET-1)))>>N_OFFSET; + // else + // c = (a+(1<<(N_OFFSET-1))-1)>>N_OFFSET; + c = OFFSET_AND_ROUND(a, N_OFFSET); + + int32_t d; + d = OFFSET_AND_ROUND2(a, N_OFFSET); + + double e; + e = (double)a/(double)(1<