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

add more PIDs for the vertical control on fixedwing

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-05-16 16:27:31 +02:00
parent 9ca8f6c3e6
commit 531d3cbae5
3 changed files with 96 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/tuning_ctl_new.xml
+7 -3
View File
@@ -16,7 +16,7 @@
<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"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
<dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg" alt_unit_coef="0.0174533"/>
<dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
<dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
</dl_settings>
@@ -56,8 +56,12 @@
<dl_settings name="airspeed">
<dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_airspeed_setpoint" shortname="as_sp"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pgain" shortname="as_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_igain" shortname="as_igain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_pgain" shortname="as_t_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_dgain" shortname="as_t_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_igain" shortname="as_t_igain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_pgain" shortname="as_p_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_dgain" shortname="as_p_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_igain" shortname="as_p_igain"/>
<dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_groundspeed_setpoint" shortname="gs_sp"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_pgain" shortname="gs_pgain"/>
<dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_igain" shortname="gs_igain"/>
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+81 -18
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 0.3
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.4
float v_ctl_auto_throttle_pitch_of_vz_pgain;
float v_ctl_auto_throttle_pitch_of_vz_dgain;
@@ -91,14 +91,20 @@ uint8_t v_ctl_speed_mode;
#ifdef USE_AIRSPEED
float v_ctl_auto_airspeed_setpoint;
float v_ctl_auto_airspeed_controlled;
float v_ctl_auto_airspeed_pgain;
float v_ctl_auto_airspeed_igain;
float v_ctl_auto_airspeed_sum_err;
float v_ctl_auto_airspeed_throttle_pgain;
float v_ctl_auto_airspeed_throttle_dgain;
float v_ctl_auto_airspeed_throttle_igain;
float v_ctl_auto_airspeed_throttle_sum_err;
float v_ctl_auto_airspeed_pitch_pgain;
float v_ctl_auto_airspeed_pitch_dgain;
float v_ctl_auto_airspeed_pitch_igain;
float v_ctl_auto_airspeed_pitch_sum_err;
float v_ctl_auto_groundspeed_setpoint;
float v_ctl_auto_groundspeed_pgain;
float v_ctl_auto_groundspeed_igain;
float v_ctl_auto_groundspeed_sum_err;
#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
#define V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR (RadOfDeg(15.))
#define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 0.2
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
#endif
@@ -139,9 +145,14 @@ void v_ctl_init( void ) {
#ifdef USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
v_ctl_auto_airspeed_sum_err = 0.;
v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
v_ctl_auto_airspeed_throttle_dgain = V_CTL_AUTO_AIRSPEED_THROTTLE_DGAIN;
v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN;
v_ctl_auto_airspeed_throttle_sum_err = 0.;
v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN;
v_ctl_auto_airspeed_pitch_dgain = V_CTL_AUTO_AIRSPEED_PITCH_DGAIN;
v_ctl_auto_airspeed_pitch_igain = V_CTL_AUTO_AIRSPEED_PITCH_IGAIN;
v_ctl_auto_airspeed_pitch_sum_err = 0.;
v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
@@ -236,17 +247,69 @@ static inline void v_ctl_set_throttle( void ) {
}
#ifdef USE_AIRSPEED
#define AIRSPEED_LOOP_PERIOD (1./60.)
// Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint])
static inline void v_ctl_set_airspeed( void ) {
static float last_err_vz = 0.;
static float last_err_as = 0.;
// Bound airspeed setpoint
Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
// Airspeed control loop (input: airspeed controlled, output: throttle controlled)
// Compute errors
float err_vz = estimator_z_dot - v_ctl_climb_setpoint;
float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD;
last_err_vz = err_vz;
if (v_ctl_auto_throttle_igain < 0.) {
v_ctl_auto_throttle_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
}
if (v_ctl_auto_pitch_igain < 0.) {
v_ctl_auto_pitch_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
}
float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
v_ctl_auto_airspeed_sum_err += err_airspeed;
BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD;
last_err_as = err_airspeed;
if (v_ctl_auto_airspeed_throttle_igain > 0.) { // ! sign
v_ctl_auto_airspeed_throttle_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;
BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain);
}
if (v_ctl_auto_airspeed_pitch_igain > 0.) { // ! sign
v_ctl_auto_airspeed_pitch_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;
BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain);
}
// Reset integrators in manual or before flight
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
v_ctl_auto_throttle_sum_err = 0.;
v_ctl_auto_pitch_sum_err = 0.;
v_ctl_auto_airspeed_throttle_sum_err = 0.;
v_ctl_auto_airspeed_pitch_sum_err = 0.;
}
// Pitch loop
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_vz
+ v_ctl_auto_pitch_dgain * d_err_vz
+ v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err
- v_ctl_auto_airspeed_pitch_pgain * err_airspeed
- v_ctl_auto_airspeed_pitch_dgain * d_err_airspeed
- v_ctl_auto_airspeed_pitch_igain * v_ctl_auto_airspeed_pitch_sum_err;
// Throttle loop
controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ v_ctl_auto_airspeed_pgain * err_airspeed
+ v_ctl_auto_airspeed_igain * v_ctl_auto_airspeed_sum_err;
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_pgain * err_vz
+ v_ctl_auto_throttle_dgain * d_err_vz
+ v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_airspeed_throttle_pgain * err_airspeed
+ v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed
+ v_ctl_auto_airspeed_throttle_igain * v_ctl_auto_airspeed_throttle_sum_err;
}
@@ -262,13 +325,11 @@ static inline void v_ctl_set_groundspeed( void ) {
void v_ctl_climb_loop ( void ) {
// Set pitch
v_ctl_set_pitch();
Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
// Set throttle
switch (v_ctl_speed_mode) {
case V_CTL_SPEED_THROTTLE:
// Set pitch
v_ctl_set_pitch();
// Set throttle
v_ctl_set_throttle();
break;
#ifdef USE_AIRSPEED
@@ -285,6 +346,8 @@ void v_ctl_climb_loop ( void ) {
break;
}
// Set Pitch output
Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
// Set Throttle output
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
+8
View File
@@ -36,6 +36,14 @@
#define V_CTL_SPEED_GROUNDSPEED 2
extern float v_ctl_auto_pitch_dgain;
extern float v_ctl_auto_airspeed_throttle_pgain;
extern float v_ctl_auto_airspeed_throttle_dgain;
extern float v_ctl_auto_airspeed_throttle_igain;
extern float v_ctl_auto_airspeed_throttle_sum_err;
extern float v_ctl_auto_airspeed_pitch_pgain;
extern float v_ctl_auto_airspeed_pitch_dgain;
extern float v_ctl_auto_airspeed_pitch_igain;
extern float v_ctl_auto_airspeed_pitch_sum_err;
extern uint8_t v_ctl_speed_mode;