Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

update the adaptive control for fixedwing and split some of the settings

Browse Source 
files
This commit is contained in:
Gautier Hattenberger
2011-03-22 10:30:49 +01:00
parent 2c3e0a3892
commit 8dcd9c7f40
9 changed files with 164 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/fixedwing/control_new.makefile
+1 -1
View File
@@ -1,4 +1,4 @@
# new fixed wing control loops with merged auto pitch and auto throttle, adaptive horizontal control
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c
$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl_a.c $(SRC_FIXEDWING)/fw_v_ctl_n.c
conf/settings/flight_params.xml
+34
View File
@@ -0,0 +1,34 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<!-- General flight parameters for Fixedwing A/C -->
<settings>
<dl_settings>
<dl_settings NAME="flight params">
<dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
</dl_settings>
<dl_settings NAME="mode">
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
<dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time" shortname="flight time"/>
<dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
<dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
<strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
<strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
<key_press key="greater" value="1"/>
<key_press key="less" value="-1"/>
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
conf/settings/fw_ctl_n.xml
+6 -42
View File
@@ -4,53 +4,19 @@
<settings>
<dl_settings>
<dl_settings NAME="flight params">
<dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
</dl_settings>
<dl_settings NAME="mode">
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
<dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time" shortname="flight time"/>
<dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
<dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="nav" handler="SetNavRadius">
<strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
<strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
<key_press key="greater" value="1"/>
<key_press key="less" value="-1"/>
</dl_setting>
</dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="ir">
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_lateral_correction" shortname="360_lat_corr" module="infrared" param="IR_LATERAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
<dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL" module="stabilization/stabilization_adaptive"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
@@ -94,8 +60,6 @@
<dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
<dl_setting MAX="0." MIN="-0.2" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
<dl_setting MAX="0.3" MIN="0" STEP="0.01" VAR="cte_igain"/>
<dl_setting MAX="20" MIN="0" STEP="1" VAR="cte_max"/>
</dl_settings>
</dl_settings>
</dl_settings>
conf/settings/infrared.xml
+20
View File
@@ -0,0 +1,20 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="ir">
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
</dl_settings>
</dl_settings>
</settings>
conf/settings/ins_basic.xml
+10
View File
@@ -0,0 +1,10 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="ins">
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
</dl_settings>
</dl_settings>
</settings>
conf/settings/ir_i2c.xml
+11
View File
@@ -3,6 +3,17 @@
<settings>
<dl_settings>
<dl_settings NAME="IR I2C">
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
<dl_setting MAX="3" MIN="0" STEP="1" VAR="ir_i2c_conf_word" module="sensors/infrared_i2c" values="1|2|4|8" handler="SetConfWord"/>
</dl_settings>
</dl_settings>
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+27 -11
View File
@@ -57,7 +57,7 @@ float v_ctl_auto_throttle_pgain;
float v_ctl_auto_throttle_igain;
float v_ctl_auto_throttle_dgain;
float v_ctl_auto_throttle_sum_err;
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.3
float v_ctl_auto_throttle_pitch_of_vz_pgain;
float v_ctl_auto_throttle_pitch_of_vz_dgain;
@@ -70,14 +70,21 @@ float v_ctl_auto_pitch_pgain;
float v_ctl_auto_pitch_dgain;
float v_ctl_auto_pitch_igain;
float v_ctl_auto_pitch_sum_err;
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
#ifndef V_CTL_AUTO_PITCH_DGAIN
#define V_CTL_AUTO_PITCH_DGAIN 0.
#endif
#ifndef V_CTL_AUTO_PITCH_IGAIN
#define V_CTL_AUTO_PITCH_IGAIN 0.
#endif
float controlled_throttle;
pprz_t v_ctl_throttle_setpoint;
pprz_t v_ctl_throttle_slewed;
// Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s
uint8_t v_ctl_speed_mode;
@@ -181,16 +188,21 @@ void v_ctl_altitude_loop( void ) {
static inline void v_ctl_set_pitch ( void ) {
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
v_ctl_auto_pitch_sum_err = 0;
// Compute errors
float err = estimator_z_dot - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
v_ctl_auto_pitch_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
if (v_ctl_auto_pitch_igain < 0.) {
v_ctl_auto_pitch_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
}
// PI loop + feedforward ctl
nav_pitch = nav_pitch
nav_pitch = 0. //nav_pitch FIXME it really sucks !
+ v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint
+ v_ctl_auto_pitch_pgain * err
+ v_ctl_auto_pitch_dgain * d_err
@@ -201,13 +213,18 @@ static inline void v_ctl_set_pitch ( void ) {
static inline void v_ctl_set_throttle( void ) {
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
v_ctl_auto_throttle_sum_err = 0;
// Compute errors
float err = estimator_z_dot - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
v_ctl_auto_throttle_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
if (v_ctl_auto_throttle_igain < 0.) {
v_ctl_auto_throttle_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
}
// PID loop + feedforward ctl
controlled_throttle = v_ctl_auto_throttle_cruise_throttle
@@ -269,8 +286,7 @@ void v_ctl_climb_loop ( void ) {
}
// Set Throttle output
float f_throttle = controlled_throttle;
v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
@@ -279,7 +295,7 @@ void v_ctl_climb_loop ( void ) {
#endif
#ifndef V_CTL_THROTTLE_SLEW
#define V_CTL_THROTTLE_SLEW 1.
#define V_CTL_THROTTLE_SLEW (1.)
#endif
/** \brief Computes slewed throttle from throttle setpoint
called at 20Hz
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+51 -16
View File
@@ -107,17 +107,44 @@ float v_ctl_pitch_dash_trim;
inline static void h_ctl_roll_loop( void );
inline static void h_ctl_pitch_loop( void );
// Some default course gains
// H_CTL_COURSE_PGAIN needs to be define in airframe
#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
#endif
#ifndef H_CTL_COURSE_DGAIN
#define H_CTL_COURSE_DGAIN 0.
#endif
// Some default roll gains
// H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
#ifndef H_CTL_ROLL_RATE_GAIN
#define H_CTL_ROLL_RATE_GAIN 0.
#endif
#ifndef H_CTL_ROLL_IGAIN
#define H_CTL_ROLL_IGAIN 0.
#endif
#ifndef H_CTL_ROLL_KFFA
#define H_CTL_ROLL_KFFA 0.
#endif
#ifndef H_CTL_ROLL_KFFD
#define H_CTL_ROLL_KFFD 0.
#endif
// Some default pitch gains
// H_CTL_PITCH_PGAIN needs to be define in airframe
#ifndef H_CTL_PITCH_DGAIN
#define H_CTL_PITCH_DGAIN 0.
#endif
#ifndef H_CTL_PITCH_IGAIN
#define H_CTL_PITCH_IGAIN 0.
#endif
#ifndef H_CTL_PITCH_KFFA
#define H_CTL_PITCH_KFFA 0.
#endif
#ifndef H_CTL_PITCH_KFFD
#define H_CTL_PITCH_KFFD 0.
#endif
void h_ctl_init( void ) {
h_ctl_course_setpoint = 0.;
@@ -158,7 +185,7 @@ void h_ctl_init( void ) {
use_airspeed_ratio = FALSE;
airspeed_ratio2 = 1.;
#ifdef PITCH_TRIM
#ifdef USE_PITCH_TRIM
v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
#else
@@ -227,9 +254,6 @@ inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_roll_sum_err = 0;
#ifdef USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
@@ -271,9 +295,16 @@ inline static void h_ctl_roll_loop( void ) {
#endif
float d_err = estimator_p - h_ctl_ref_roll_rate;
if (h_ctl_roll_igain < 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
}
else {
if (h_ctl_roll_igain < 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain));
} else {
h_ctl_roll_sum_err = 0.;
}
}
cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err;
@@ -303,7 +334,7 @@ float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
#endif
#ifdef PITCH_TRIM
#ifdef USE_PITCH_TRIM
inline static void loiter(void) {
float pitch_trim;
@@ -335,11 +366,8 @@ inline static void h_ctl_pitch_loop( void ) {
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_pitch_sum_err = 0;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
#ifdef PITCH_TRIM
#ifdef USE_PITCH_TRIM
loiter();
#endif
@@ -369,9 +397,16 @@ inline static void h_ctl_pitch_loop( void ) {
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
if (h_ctl_roll_igain < 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_pitch_sum_err = 0.;
}
else {
if (h_ctl_pitch_igain < 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain));
} else {
h_ctl_pitch_sum_err = 0.;
}
}
float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
+4 -4
View File
@@ -46,16 +46,16 @@ extern float h_ctl_pitch_Kffa;
extern float h_ctl_pitch_Kffd;
extern float h_ctl_pitch_of_roll;
#define H_CTL_ROLL_SUM_ERR_MAX 100.
#define H_CTL_PITCH_SUM_ERR_MAX 100.
#define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.)
#define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.)
#define fw_h_ctl_a_SetRollIGain(_gain) { \
h_ctl_roll_sum_err = 0; \
h_ctl_roll_sum_err = 0.; \
h_ctl_roll_igain = _gain; \
}
#define fw_h_ctl_a_SetPitchIGain(_gain) { \
h_ctl_pitch_sum_err = 0; \
h_ctl_pitch_sum_err = 0.; \
h_ctl_pitch_igain = _gain; \
}