diff --git a/conf/airframes/ENAC/fixed-wing/funjet2_new.xml b/conf/airframes/ENAC/fixed-wing/funjet2_new.xml index f576a4f104..7e0f08d981 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet2_new.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet2_new.xml @@ -161,7 +161,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/funjet3.xml b/conf/airframes/ENAC/fixed-wing/funjet3.xml index 75e3056a77..0c1a15c896 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet3.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet3.xml @@ -135,7 +135,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml index 560cc636bd..1b7640023f 100644 --- a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml +++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml @@ -141,7 +141,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-3.xml b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml index a219362b81..1f0f798f14 100644 --- a/conf/airframes/ENAC/fixed-wing/spocIII-3.xml +++ b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml @@ -144,7 +144,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/twinjet2.xml b/conf/airframes/ENAC/fixed-wing/twinjet2.xml index ed2685154e..34cf09d6c4 100644 --- a/conf/airframes/ENAC/fixed-wing/twinjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/twinjet2.xml @@ -143,7 +143,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index 670795e0f0..1cb3cb6056 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -159,7 +159,7 @@ - + diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml index ca557a2fe9..110ef0809f 100644 --- a/conf/airframes/flixr_discovery.xml +++ b/conf/airframes/flixr_discovery.xml @@ -258,7 +258,7 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation - + diff --git a/conf/settings/tuning_ctl_adaptive.xml b/conf/settings/tuning_ctl_adaptive.xml index 4c0e110ba4..59638e5d05 100644 --- a/conf/settings/tuning_ctl_adaptive.xml +++ b/conf/settings/tuning_ctl_adaptive.xml @@ -39,7 +39,7 @@ - + diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml index d71424e425..c5b77c2e7d 100644 --- a/conf/settings/tuning_ctl_new.xml +++ b/conf/settings/tuning_ctl_new.xml @@ -44,7 +44,7 @@ - + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index 1056044be4..ec3afeeade 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -123,10 +123,9 @@ void v_ctl_init( void ) { /* "auto throttle" inner loop parameters */ v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; v_ctl_auto_throttle_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle; - 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_CLIMB_THROTTLE_INCREMENT; v_ctl_auto_throttle_pgain = ABS(V_CTL_AUTO_THROTTLE_PGAIN); - v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN; + v_ctl_auto_throttle_igain = ABS(V_CTL_AUTO_THROTTLE_IGAIN); v_ctl_auto_throttle_dgain = 0.; v_ctl_auto_throttle_sum_err = 0.; v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN; diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 67bf6806d1..81d3520369 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -130,7 +130,7 @@ void v_ctl_init( void ) { v_ctl_auto_throttle_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle; v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT; v_ctl_auto_throttle_pgain = ABS(V_CTL_AUTO_THROTTLE_PGAIN); - v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN; + v_ctl_auto_throttle_igain = ABS(V_CTL_AUTO_THROTTLE_IGAIN); v_ctl_auto_throttle_dgain = 0.; v_ctl_auto_throttle_sum_err = 0.; v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN; @@ -232,9 +232,9 @@ static inline void v_ctl_set_throttle( void ) { float d_err = err - last_err; last_err = err; - if (v_ctl_auto_throttle_igain < 0.) { + if (v_ctl_auto_throttle_igain > 0.) { v_ctl_auto_throttle_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain)); + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (v_ctl_auto_throttle_igain)); } // PID loop + feedforward ctl @@ -242,7 +242,7 @@ static inline void v_ctl_set_throttle( void ) { + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - v_ctl_auto_throttle_pgain * err + v_ctl_auto_throttle_dgain * d_err - + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err; + - v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err; } @@ -261,9 +261,9 @@ static inline void v_ctl_set_airspeed( void ) { 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.) { + 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)); + 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; @@ -306,7 +306,7 @@ static inline void v_ctl_set_airspeed( void ) { + 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_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;