diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
index f4ec48252e..c6a280c7e1 100644
--- a/conf/airframes/CDW/debug_i2c.xml
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -173,12 +173,12 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 190eddab91..1d94fee657 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -209,8 +209,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index caf5a57f66..8d97ace3df 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -172,8 +172,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/g1_vision.xml b/conf/airframes/ENAC/quadrotor/g1_vision.xml
index 74483993be..0892e2eaf0 100644
--- a/conf/airframes/ENAC/quadrotor/g1_vision.xml
+++ b/conf/airframes/ENAC/quadrotor/g1_vision.xml
@@ -153,8 +153,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
index f8308819d0..a9095dd873 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
@@ -187,8 +187,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml
index cf39a0e511..4536233f45 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1.xml
@@ -207,8 +207,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml
index c7fa6181e2..95552ba9b7 100644
--- a/conf/airframes/ENAC/quadrotor/navgo.xml
+++ b/conf/airframes/ENAC/quadrotor/navgo.xml
@@ -220,8 +220,8 @@
-
-
+
+
diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/ENAC/quadrotor/nova1.xml
index c291651477..dafdff39a8 100644
--- a/conf/airframes/ENAC/quadrotor/nova1.xml
+++ b/conf/airframes/ENAC/quadrotor/nova1.xml
@@ -178,8 +178,8 @@
-
-
+
+
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml
index 25d70feb2c..dd5169359a 100644
--- a/conf/airframes/NoVa_L.xml
+++ b/conf/airframes/NoVa_L.xml
@@ -201,12 +201,12 @@
-
-
+
+
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
index 7c21062f52..ad12551489 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
@@ -160,9 +160,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
index 42697269f7..9b805c686d 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
@@ -178,9 +178,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml
index d26068af31..9389fc0821 100644
--- a/conf/airframes/Poine/booz2_a1.xml
+++ b/conf/airframes/Poine/booz2_a1.xml
@@ -151,9 +151,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml
index 3115132ede..6dc5afda02 100644
--- a/conf/airframes/Poine/booz2_a1p.xml
+++ b/conf/airframes/Poine/booz2_a1p.xml
@@ -155,9 +155,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 93b6d91981..e5729293e7 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -173,12 +173,12 @@
-
-
+
+
diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml
index 49517cfd04..f747e97c59 100644
--- a/conf/airframes/Poine/h_hex.xml
+++ b/conf/airframes/Poine/h_hex.xml
@@ -146,9 +146,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
index 36c3e670aa..ee83c8370a 100644
--- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml
+++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
@@ -225,9 +225,9 @@ second attempt
-
-
-
+
+
+
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml
index 98ec793295..ee9f6b9dba 100644
--- a/conf/airframes/UofAdelaide/A1000_LISA.xml
+++ b/conf/airframes/UofAdelaide/A1000_LISA.xml
@@ -162,9 +162,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml
index af835bc8eb..448b1e2923 100644
--- a/conf/airframes/UofAdelaide/A1000_NOVA.xml
+++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml
@@ -185,8 +185,8 @@
-
-
+
+
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
index 1fe5eac6b0..b4eb06b94e 100644
--- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
@@ -185,8 +185,8 @@
-
-
+
+
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml
index fe9c3f92ea..b31411f3d0 100755
--- a/conf/airframes/UofAdelaide/booz2_a1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_a1000.xml
@@ -226,9 +226,9 @@ second attempt
-
-
-
+
+
+
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml
index e8facb8506..a8c78a3bce 100644
--- a/conf/airframes/UofAdelaide/lisa_a1000.xml
+++ b/conf/airframes/UofAdelaide/lisa_a1000.xml
@@ -141,9 +141,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 45ff45af07..82f1aaea4a 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -195,9 +195,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml
index 018953fec6..22d1e126a7 100644
--- a/conf/airframes/booz2_ppzuav.xml
+++ b/conf/airframes/booz2_ppzuav.xml
@@ -160,8 +160,8 @@
-
-
+
+
diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml
index 479a4580e6..7376de7e3c 100644
--- a/conf/airframes/esden/jt_lisam.xml
+++ b/conf/airframes/esden/jt_lisam.xml
@@ -137,9 +137,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml
index ea7f9d1803..f29c34b3a0 100644
--- a/conf/airframes/esden/lisa_asctec.xml
+++ b/conf/airframes/esden/lisa_asctec.xml
@@ -151,9 +151,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/esden/lisa_asctec_aspirin.xml b/conf/airframes/esden/lisa_asctec_aspirin.xml
index da175c827d..01e6493d1e 100644
--- a/conf/airframes/esden/lisa_asctec_aspirin.xml
+++ b/conf/airframes/esden/lisa_asctec_aspirin.xml
@@ -151,9 +151,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index b8148c503f..004a3f0490 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -165,9 +165,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 61363372e9..0cd3a409f0 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -146,9 +146,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
index af1e2ec468..c92116b483 100644
--- a/conf/airframes/esden/synerani_4B.xml
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -149,9 +149,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/mm/rotor/qmk1.xml b/conf/airframes/mm/rotor/qmk1.xml
index 073a777144..ba57d38764 100644
--- a/conf/airframes/mm/rotor/qmk1.xml
+++ b/conf/airframes/mm/rotor/qmk1.xml
@@ -152,8 +152,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml
index 66a4250869..879ebcb016 100644
--- a/conf/airframes/obsolete/booz2_Aron.xml
+++ b/conf/airframes/obsolete/booz2_Aron.xml
@@ -145,9 +145,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml
index d2f95992a2..07af854a2e 100644
--- a/conf/airframes/obsolete/booz2_NoVa.xml
+++ b/conf/airframes/obsolete/booz2_NoVa.xml
@@ -185,8 +185,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml
index e28af665cb..350eca0093 100644
--- a/conf/airframes/obsolete/booz2_NoVa_001.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_001.xml
@@ -185,8 +185,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml
index 958f2dabcc..0dca405b1e 100644
--- a/conf/airframes/obsolete/booz2_NoVa_002.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_002.xml
@@ -185,8 +185,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml
index 6e32dc7af9..b34c272223 100644
--- a/conf/airframes/obsolete/booz2_a2.xml
+++ b/conf/airframes/obsolete/booz2_a2.xml
@@ -145,9 +145,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml
index b4bf3d6b9b..f546148b32 100644
--- a/conf/airframes/obsolete/booz2_a3.xml
+++ b/conf/airframes/obsolete/booz2_a3.xml
@@ -131,8 +131,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml
index 5e122b613e..7fa08869a7 100644
--- a/conf/airframes/obsolete/booz2_a4.xml
+++ b/conf/airframes/obsolete/booz2_a4.xml
@@ -109,8 +109,8 @@
-
-
+
+
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml
index 0e42003e7f..fc2c37f70a 100644
--- a/conf/airframes/obsolete/booz2_a5.xml
+++ b/conf/airframes/obsolete/booz2_a5.xml
@@ -147,9 +147,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml
index 764f9d4a59..a349ec4aa2 100644
--- a/conf/airframes/obsolete/booz2_s1.xml
+++ b/conf/airframes/obsolete/booz2_s1.xml
@@ -138,9 +138,9 @@
-
-
-
+
+
+
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml
index f5b0edb23a..899ff28868 100644
--- a/conf/airframes/obsolete/booz2_x1.xml
+++ b/conf/airframes/obsolete/booz2_x1.xml
@@ -150,9 +150,9 @@
-
-
-
+
+
+
diff --git a/conf/settings/settings_UofAdelaide.xml b/conf/settings/settings_UofAdelaide.xml
index 01a4ae6480..0ad179f67e 100644
--- a/conf/settings/settings_UofAdelaide.xml
+++ b/conf/settings/settings_UofAdelaide.xml
@@ -55,9 +55,9 @@
-
-
-
+
+
+
diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml
index a87a031849..20a04ce36b 100644
--- a/conf/settings/settings_booz2.xml
+++ b/conf/settings/settings_booz2.xml
@@ -44,9 +44,9 @@
-
-
-
+
+
+
diff --git a/conf/settings/settings_booz2_jtm.xml b/conf/settings/settings_booz2_jtm.xml
index 441c31d1e6..22b005f7e6 100644
--- a/conf/settings/settings_booz2_jtm.xml
+++ b/conf/settings/settings_booz2_jtm.xml
@@ -44,9 +44,9 @@
-
-
-
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 5e81cb5b5e..61613bd440 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -97,9 +97,9 @@ void guidance_v_init(void) {
guidance_v_mode = GUIDANCE_V_MODE_KILL;
- guidance_v_kp = GUIDANCE_V_HOVER_KP;
- guidance_v_kd = GUIDANCE_V_HOVER_KD;
- guidance_v_ki = GUIDANCE_V_HOVER_KI;
+ guidance_v_kp = ABS(GUIDANCE_V_HOVER_KP);
+ guidance_v_kd = ABS(GUIDANCE_V_HOVER_KD);
+ guidance_v_ki = ABS(GUIDANCE_V_HOVER_KI);
guidance_v_z_sum_err = 0;
@@ -261,9 +261,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
/* compute the error to our reference */
- int32_t err_z = ins_ltp_pos.z - guidance_v_z_ref;
+ int32_t err_z = guidance_v_z_ref - ins_ltp_pos.z;
Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
- int32_t err_zd = ins_ltp_speed.z - guidance_v_zd_ref;
+ int32_t err_zd = guidance_v_zd_ref - ins_ltp_speed.z;
Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
@@ -297,12 +297,13 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
#endif
/* our error command */
- guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
- ((-guidance_v_kd * err_zd) >> 21) +
- ((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
+ guidance_v_fb_cmd = ((guidance_v_kp * err_z) >> 12) +
+ ((guidance_v_kd * err_zd) >> 21) +
+ ((guidance_v_ki * guidance_v_z_sum_err) >> 21);
- guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
- // guidance_v_delta_t = guidance_v_fb_cmd;
+ // z-axis pointing down -> positive error means we need less thrust
+ guidance_v_delta_t = - (guidance_v_ff_cmd + guidance_v_fb_cmd);
+ // guidance_v_delta_t = -guidance_v_fb_cmd;
}