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

integrator on pitch loop

Browse Source
This commit is contained in:
Pascal Brisset
2009-08-11 09:20:13 +00:00
parent c951685d5f
commit 91c5a1a678
3 changed files with 22 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/fw_h_ctl_a_settings.xml
+1
View File
@@ -7,6 +7,7 @@
<dl_settings NAME="fw_h_ctl_a">
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kff" module="fw_h_ctl_a"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" module="fw_h_ctl_a" handler="SetRollIGain"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" module="fw_h_ctl_a" handler="SetPitchIGain"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/fw_h_ctl_a.c
+14 -1
View File
@@ -68,7 +68,9 @@ float h_ctl_roll_setpoint;
float h_ctl_roll_attitude_gain;
float h_ctl_roll_rate_gain;
float h_ctl_roll_igain;
float h_ctl_pitch_igain;
float h_ctl_roll_sum_err;
float h_ctl_pitch_sum_err;
float h_ctl_roll_Kff;
pprz_t h_ctl_aileron_setpoint;
float h_ctl_roll_slew;
@@ -210,6 +212,10 @@ void h_ctl_attitude_loop ( void ) {
#define H_CTL_REF_DT (1./60.)
inline static void h_ctl_roll_loop( void ) {
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_roll_sum_err = 0;
h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint - h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;
h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
@@ -277,9 +283,16 @@ inline static void h_ctl_pitch_loop( void ) {
#endif
float err = estimator_theta - h_ctl_ref_pitch_angle;
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_pitch_sum_err = 0;
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err;
float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
cmd /= airspeed_ratio2;
sw/airborne/fw_h_ctl_a.h
+7
View File
@@ -37,7 +37,9 @@
#include "airframe.h"
extern float h_ctl_roll_sum_err;
extern float h_ctl_pitch_sum_err;
extern float h_ctl_roll_igain;
extern float h_ctl_pitch_igain;
extern float h_ctl_roll_Kff;
#define H_CTL_ROLL_SUM_ERR_MAX 100.
@@ -46,6 +48,11 @@ extern float h_ctl_roll_Kff;
h_ctl_roll_igain = _gain; \
}
#define fw_h_ctl_a_SetPitchIGain(_gain) { \
h_ctl_pitch_sum_err = 0; \
h_ctl_pitch_igain = _gain; \
}
/* inner roll loop parameters */
extern float h_ctl_ref_roll_angle;
extern float h_ctl_ref_roll_rate;