Skip to content

Commit

Permalink
add more PIDs for the vertical control on fixedwing
Browse files Browse the repository at this point in the history
  • Loading branch information
@gautierhattenberger
gautierhattenberger committed May 16, 2011
1 parent 9ca8f6c commit 531d3cb
Show file tree
Hide file tree
Showing 3 changed files with 96 additions and 21 deletions.
10 changes: 7 additions & 3 deletions conf/settings/tuning_ctl_new.xml
View file
Expand Up @@ -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>

Expand Down Expand Up @@ -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"/>
Expand Down
99 changes: 81 additions & 18 deletions sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
View file
Expand Up @@ -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;

Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

}

Expand All @@ -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
Expand All @@ -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);

Expand Down
8 changes: 8 additions & 0 deletions sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
View file
Expand Up @@ -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;

Expand Down

0 comments on commit 531d3cb

Please sign in to comment.