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 28 Packages Projects Releases Wiki Activity

Merge pull request #447 from TobiasMue/energy_ctrl

Browse Source 
fix v_ctl_climb_loop and v_ctl_altitude_loop are called by different fre...
This commit is contained in:
Gautier Hattenberger
2013-05-28 07:11:26 -07:00
parent e5d03719e1 542148ec02
commit 11ce75394d
1 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+10 -9
View File
@@ -257,7 +257,8 @@ void v_ctl_init( void ) {
v_ctl_throttle_setpoint = 0;
}
const float dt = 1.0 / ((float)CONTROL_FREQUENCY);
const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
/**
* outer loop
@@ -275,7 +276,7 @@ void v_ctl_altitude_loop( void )
BoundAbs(sp, v_ctl_max_climb);
float incr = sp - v_ctl_climb_setpoint;
BoundAbs(incr, 10 * dt);
BoundAbs(incr, 2 * dt_navigation);
v_ctl_climb_setpoint += incr;
}
@@ -304,7 +305,7 @@ void v_ctl_climb_loop( void )
// airspeed_setpoint ratelimiter:
// AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt);
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
@@ -356,8 +357,8 @@ void v_ctl_climb_loop( void )
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
{
v_ctl_auto_throttle_nominal_cruise_throttle +=
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt
+ en_tot_err * v_ctl_energy_total_igain * dt;
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
+ en_tot_err * v_ctl_energy_total_igain * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_throttle,0.0f,1.0f);
}
@@ -373,16 +374,16 @@ void v_ctl_climb_loop( void )
en_dis_err = -vdot_err;
// adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
if(v_ctl_climb_setpoint>0) v_ctl_climb_setpoint += - 10.5 * dt;
if(v_ctl_climb_setpoint<0) v_ctl_climb_setpoint += 10.5 * dt;
if(v_ctl_climb_setpoint>0) v_ctl_climb_setpoint += - 30. * dt_attidude;
if(v_ctl_climb_setpoint<0) v_ctl_climb_setpoint += 30. * dt_attidude;
}
/* pitch pre-command */
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
{
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt
+ v_ctl_energy_diff_igain * en_dis_err * dt;
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude
+ v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
}
float v_ctl_pitch_of_vz =