diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 634473723e..3f07c7b0ac 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -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 =