Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

fixedwing energy_ctrl

Browse Source 
rate limiter with GS protect
rate limiter now in m/s/s (AIRSPEED_SETPOINT_SLEW)
fix in init
This commit is contained in:
Tobias Muench
2013-05-06 18:18:17 +02:00
parent 841dfe842c
commit 61512c26a4
2 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/Twinstar_energyadaptive.xml
+2 -1
View File
@@ -156,7 +156,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<define name="GLIDE_AIRSPEED" value="12." unit="m/s"/>
<define name="RACE_AIRSPEED" value="25." unit="m/s"/>
<define name="STALL_AIRSPEED" value="8." unit="m/s"/>
<define name="AIRSPEED_SETPOINT_SLEW" value="1" unit="s"/> <!--default is 1-->
<define name="AIRSPEED_SETPOINT_SLEW" value="0.2" unit="m/s/s"/> <!--default is 1-->
<define name="CARROT" value="4." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
@@ -183,6 +183,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<define name="ALTITUDE_MAX_CLIMB" value="5."/> <!--default 2-->
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.3"/>
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_PITCH" value="0."/> <!-- default 0 -->
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.15"/>
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+17 -18
View File
@@ -199,39 +199,32 @@ void v_ctl_init( void ) {
/* outer loop */
v_ctl_altitude_setpoint = 0.;
#ifdef V_CTL_ALTITUDE_PGAIN
v_ctl_altitude_pre_climb = 0.;
v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
#endif
#ifdef V_CTL_AIRSPEED_PGAIN
v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
#ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
#else
v_ctl_auto_throttle_nominal_cruise_pitch = 0.;
#endif
v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint;
v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
/* inner loops */
v_ctl_climb_setpoint = 0.;
/* "auto throttle" inner loop parameters */
v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
#ifdef V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT
v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
#endif
#ifdef V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN
v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
#endif
#ifdef V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN
v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
#endif
#ifdef V_CTL_ENERGY_TOT_PGAIN
@@ -239,6 +232,12 @@ void v_ctl_init( void ) {
v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
#else
v_ctl_energy_total_pgain = 0.;
v_ctl_energy_total_igain = 0.;
v_ctl_energy_diff_pgain = 0.;
v_ctl_energy_diff_igain = 0.;
#warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
#endif
#ifdef V_CTL_ALTITUDE_MAX_CLIMB
@@ -302,8 +301,8 @@ static float low_pass_vdot(float v)
void v_ctl_climb_loop( void )
{
// airspeed_setpoint ratelimiter:
float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew; // FIXME
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * NOMINAL_AIRSPEED);
float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt);
v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
@@ -314,8 +313,8 @@ void v_ctl_climb_loop( void )
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
// Do not allow controlled airspeed below the setpoint
if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
// reset integrator of ground speed loop
v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain);
}