From e8dbf9b03661a3ae86d49de0c106188fb450d49b Mon Sep 17 00:00:00 2001 From: Tobias Muench Date: Sat, 8 Jun 2013 17:58:37 +0200 Subject: [PATCH] [fixedwing] additonals for energy ctrl better handling for engine out and max_acc can be set in airframe closes #497 --- .../firmwares/fixedwing/guidance/energy_ctrl.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 3f07c7b0aca..27095c67eba 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -95,7 +95,7 @@ float v_ctl_airspeed_pgain; float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low float v_ctl_max_climb; -float v_ctl_max_acceleration = 0.5; +float v_ctl_max_acceleration; /* inner loop */ float v_ctl_climb_setpoint; @@ -145,10 +145,16 @@ float v_ctl_pitch_setpoint; #warning "No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED" #define STALL_AIRSPEED NOMINAL_AIRSPEED #endif +#ifndef V_CTL_GLIDE_RATIO +#define V_CTL_GLIDE_RATIO 8. +#warning "V_CTL_GLIDE_RATIO not defined - default is 8." +#endif #ifndef AIRSPEED_SETPOINT_SLEW #define AIRSPEED_SETPOINT_SLEW 1 #endif - +#ifndef V_CTL_MAX_ACCELERATION +#define V_CTL_MAX_ACCELERATION 0.5 +#endif ///////////////////////////////////////////////// // Automatically found airplane characteristics @@ -212,6 +218,8 @@ void v_ctl_init( void ) { v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint; v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN; + v_ctl_max_acceleration = V_CTL_MAX_ACCELERATION; + /* inner loops */ v_ctl_climb_setpoint = 0.; @@ -345,7 +353,7 @@ void v_ctl_climb_loop( void ) float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot ); // Flight Path Outerloop: positive means needs to climb more: needs extra energy - float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_setpoint; + float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled; // Total Energy Error: positive means energy should be added float en_tot_err = gamma_err + vdot_err; @@ -392,6 +400,7 @@ void v_ctl_climb_loop( void ) + v_ctl_auto_pitch_of_airspeed_dgain * vdot + v_ctl_energy_diff_pgain * en_dis_err + v_ctl_auto_throttle_nominal_cruise_pitch; +if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO; v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch; Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)