Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

update adaptive control loops

Browse Source 
traj ref is not used by default
airspeed ratio is used whith USE_AIRSPEED

Conflicts:

	conf/settings/fw_ctl_n.xml
This commit is contained in:
Gautier Hattenberger
2011-03-16 11:10:13 +01:00
parent 418745863e
commit dab24945b8
5 changed files with 120 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/fw_ctl_n.xml
+40
View File
@@ -4,7 +4,47 @@
<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"/>
conf/telemetry/fw_h_ctl_a.xml
+1 -1
View File
@@ -38,7 +38,7 @@
<mode name="debug">
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="COMMANDS" period="0.5"/>
<message name="COMMANDS" period="0.1"/>
<message name="FBW_STATUS" period="1"/>
<message name="ACTUATORS" period="5"/> <!-- For trimming -->
</mode>
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
-2
View File
@@ -39,9 +39,7 @@ extern float v_ctl_auto_pitch_dgain;
extern uint8_t v_ctl_speed_mode;
#ifdef PITCH_TRIM
extern float v_ctl_pitch_loiter_trim;
extern float v_ctl_pitch_dash_trim;
#endif
#endif /* FW_V_CTL_N_H */
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+77 -34
View File
@@ -60,11 +60,11 @@ float h_ctl_ref_roll_accel;
float h_ctl_ref_pitch_angle;
float h_ctl_ref_pitch_rate;
float h_ctl_ref_pitch_accel;
#define H_CTL_REF_W 2.5
#define H_CTL_REF_W 5.
#define H_CTL_REF_XI 0.85
#define H_CTL_REF_MAX_P RadOfDeg(100.)
#define H_CTL_REF_MAX_P RadOfDeg(150.)
#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
#define H_CTL_REF_MAX_Q RadOfDeg(100.)
#define H_CTL_REF_MAX_Q RadOfDeg(150.)
#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
/* inner roll loop parameters */
@@ -96,6 +96,13 @@ float h_ctl_aileron_of_throttle;
float h_ctl_elevator_of_roll;
float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
bool_t use_airspeed_ratio;
float airspeed_ratio2;
#define AIRSPEED_RATIO_MIN 0.5
#define AIRSPEED_RATIO_MAX 2.
float v_ctl_pitch_loiter_trim;
float v_ctl_pitch_dash_trim;
inline static void h_ctl_roll_loop( void );
inline static void h_ctl_pitch_loop( void );
@@ -148,6 +155,17 @@ void h_ctl_init( void ) {
h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
#endif
use_airspeed_ratio = FALSE;
airspeed_ratio2 = 1.;
#ifdef PITCH_TRIM
v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
#else
v_ctl_pitch_loiter_trim = 0.;
v_ctl_pitch_dash_trim = 0.;
#endif
}
/**
@@ -175,24 +193,27 @@ void h_ctl_course_loop ( void ) {
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
static float airspeed_ratio2;
#ifdef USE_AIRSPEED
static inline void compute_airspeed_ratio( void ) {
float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */
if (throttle_diff > 0)
airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED - NOMINAL_AIRSPEED);
else
airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED - MINIMUM_AIRSPEED);
if (use_airspeed_ratio) {
// low pass airspeed
static float airspeed = 0.;
airspeed = ( 15*airspeed + estimator_airspeed ) / 16;
// compute ratio
float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
Bound(airspeed_ratio, 0.5, 2.);
Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
} else {
airspeed_ratio2 = 1.;
}
}
#endif
void h_ctl_attitude_loop ( void ) {
if (!h_ctl_disabled) {
// compute_airspeed_ratio();
#ifdef USE_AIRSPEED
compute_airspeed_ratio();
#endif
h_ctl_roll_loop();
h_ctl_pitch_loop();
}
@@ -209,6 +230,7 @@ inline static void h_ctl_roll_loop( void ) {
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;
h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
@@ -223,6 +245,11 @@ inline static void h_ctl_roll_loop( void ) {
h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;
if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;
}
#else
h_ctl_ref_roll_angle = h_ctl_roll_setpoint;
h_ctl_ref_roll_rate = 0.;
h_ctl_ref_roll_accel = 0.;
#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
@@ -242,26 +269,28 @@ inline static void h_ctl_roll_loop( void ) {
estimator_p = (err - last_err)/(1/60.);
last_err = err;
#endif
float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT;
float d_err = estimator_p - h_ctl_ref_roll_rate;
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
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);
}
cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr;
cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err;
float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
+ h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
//float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel
// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
// - h_ctl_roll_attitude_gain * err
// - h_ctl_roll_rate_gain * derr
// - h_ctl_roll_igain * h_ctl_roll_sum_err
// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
//x cmd /= airspeed_ratio2;
// float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
// - h_ctl_roll_attitude_gain * err
// - h_ctl_roll_rate_gain * d_err
// - h_ctl_roll_igain * h_ctl_roll_sum_err
// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
//
cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -275,12 +304,16 @@ float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
#endif
#ifdef PITCH_TRIM
float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
inline static void loiter(void) {
float pitch_trim;
#ifdef USE_AIRSPEED
if (estimator_airspeed > NOMINAL_AIRSPEED) {
pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
} else {
pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
}
#else
float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
if (throttle_diff > 0) {
float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
@@ -289,6 +322,7 @@ inline static void loiter(void) {
float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
}
#endif
h_ctl_pitch_loop_setpoint += pitch_trim;
}
@@ -309,10 +343,11 @@ inline static void h_ctl_pitch_loop( void ) {
loiter();
#endif
#ifdef USE_ANGLE_REF
// Update reference setpoints for pitch
h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT);
if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) {
@@ -323,14 +358,21 @@ inline static void h_ctl_pitch_loop( void ) {
h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q;
if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.;
}
#else
h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint;
h_ctl_ref_pitch_rate = 0.;
h_ctl_ref_pitch_accel = 0.;
#endif
// Compute errors
float err = estimator_theta - h_ctl_ref_pitch_angle;
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
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);
}
float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
+ h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate
@@ -338,7 +380,8 @@ inline static void h_ctl_pitch_loop( void ) {
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
// cmd /= airspeed_ratio2;
cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
+2
View File
@@ -69,4 +69,6 @@ extern float h_ctl_ref_pitch_angle;
extern float h_ctl_ref_pitch_rate;
extern float h_ctl_ref_pitch_accel;
extern bool_t use_airspeed_ratio;
#endif /* FW_H_CTL_A_H */