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 27 Packages Projects Releases Wiki Activity

tuning the control loop on sim

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-24 22:01:51 +00:00
parent 34009c86fa
commit 2331400ed7
7 changed files with 78 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/booz2_simulator_nps.makefile
+2 -2
View File
@@ -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
conf/messages.xml
+6 -3
View File
@@ -768,9 +768,12 @@
<field name="sum_err_phi" type="int32"/>
<field name="sum_err_theta" type="int32"/>
<field name="sum_err_psi" type="int32"/>
<field name="delta_a_err" type="int32"/>
<field name="delta_e_err" type="int32"/>
<field name="delta_r_err" type="int32"/>
<field name="delta_a_fb" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_fb" type="int32"/>
<field name="delta_r_fb" type="int32"/>
<field name="delta_a_ff" type="int32" alt_unit_coef="0.0000153"/>
<field name="delta_e_ff" type="int32"/>
<field name="delta_r_ff" type="int32"/>
<field name="delta_a" type="int32"/>
<field name="delta_e" type="int32"/>
<field name="delta_r" type="int32"/>
conf/settings/settings_booz2.xml
+2 -2
View File
@@ -17,9 +17,9 @@
<dl_settings NAME="Att Loop">
<dl_setting var="booz_stabilization_pgain.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi"/>
<dl_setting var="booz_stabilization_pgain.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi" />
<dl_setting var="booz_stabilization_dgain.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain p"/>
<dl_setting var="booz_stabilization_igain.x" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi"/>
<dl_setting var="booz_stabilization_igain.x" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi" handler="SetKiPhi"/>
<dl_setting var="booz_stabilization_ddgain.x" min="0" step="1" max="1000" module="stabilization/booz_stabilization_attitude" shortname="ddgain p"/>
<dl_setting var="booz_stabilization_pgain.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain tetha"/>
<dl_setting var="booz_stabilization_dgain.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain q"/>
sw/airborne/booz/booz2_telemetry.h
+6 -3
View File
@@ -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]); \
sw/airborne/booz/stabilization/booz_stabilization_attitude.h
+8 -1
View File
@@ -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 */
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c
+23 -16
View File
@@ -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;
sw/airborne/booz/test/test_scaling.c
+31 -4
View File
@@ -25,7 +25,7 @@
#include <math.h>
#include <inttypes.h>
#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); a<=(1<<N_OFFSET); a++) {
int32_t b = (a>>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<<N_OFFSET);
printf("%- d %- d %- d %- d %.1f\n", a, b, c, d, e);
}
}