diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
index e628d55fc5..f4ec48252e 100644
--- a/conf/airframes/CDW/debug_i2c.xml
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -190,10 +190,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 574f3a3920..190eddab91 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -219,10 +219,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index be8fe770a1..caf5a57f66 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -184,10 +184,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/g1_vision.xml b/conf/airframes/ENAC/quadrotor/g1_vision.xml
index 67a9a2e18d..74483993be 100644
--- a/conf/airframes/ENAC/quadrotor/g1_vision.xml
+++ b/conf/airframes/ENAC/quadrotor/g1_vision.xml
@@ -165,10 +165,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
index 9342f142f6..f8308819d0 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
@@ -198,10 +198,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml
index bfe3915ba7..cf39a0e511 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1.xml
@@ -218,10 +218,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml
index 4a4dc96070..c7fa6181e2 100644
--- a/conf/airframes/ENAC/quadrotor/navgo.xml
+++ b/conf/airframes/ENAC/quadrotor/navgo.xml
@@ -230,10 +230,10 @@
diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/ENAC/quadrotor/nova1.xml
index 7d20fcdd35..c291651477 100644
--- a/conf/airframes/ENAC/quadrotor/nova1.xml
+++ b/conf/airframes/ENAC/quadrotor/nova1.xml
@@ -189,10 +189,10 @@
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml
index f19d555dea..25d70feb2c 100644
--- a/conf/airframes/NoVa_L.xml
+++ b/conf/airframes/NoVa_L.xml
@@ -218,10 +218,10 @@
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
index f7369f61f7..7c21062f52 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
@@ -171,9 +171,9 @@
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
index a1984b313c..42697269f7 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
@@ -189,9 +189,9 @@
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml
index f4cc8fa05b..d26068af31 100644
--- a/conf/airframes/Poine/booz2_a1.xml
+++ b/conf/airframes/Poine/booz2_a1.xml
@@ -163,9 +163,9 @@
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml
index 6db693b02c..3115132ede 100644
--- a/conf/airframes/Poine/booz2_a1p.xml
+++ b/conf/airframes/Poine/booz2_a1p.xml
@@ -167,9 +167,9 @@
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 619eaa63f2..93b6d91981 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -190,10 +190,10 @@
diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml
index 5ee4808535..49517cfd04 100644
--- a/conf/airframes/Poine/h_hex.xml
+++ b/conf/airframes/Poine/h_hex.xml
@@ -158,9 +158,9 @@
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
index 3acb7cddb5..36c3e670aa 100644
--- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml
+++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
@@ -236,9 +236,9 @@ second attempt
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml
index 96d3b550b0..98ec793295 100644
--- a/conf/airframes/UofAdelaide/A1000_LISA.xml
+++ b/conf/airframes/UofAdelaide/A1000_LISA.xml
@@ -174,9 +174,9 @@
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml
index bf1b5475da..af835bc8eb 100644
--- a/conf/airframes/UofAdelaide/A1000_NOVA.xml
+++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml
@@ -196,11 +196,11 @@
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
index 69ae72cc79..1fe5eac6b0 100644
--- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
@@ -196,11 +196,11 @@
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml
index 3f92456cd1..fe9c3f92ea 100755
--- a/conf/airframes/UofAdelaide/booz2_a1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_a1000.xml
@@ -238,9 +238,9 @@ second attempt
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml
index 602360c0ed..e8facb8506 100644
--- a/conf/airframes/UofAdelaide/lisa_a1000.xml
+++ b/conf/airframes/UofAdelaide/lisa_a1000.xml
@@ -153,9 +153,9 @@
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 754a4218a8..45ff45af07 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -206,9 +206,9 @@
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml
index 1adc5b0091..018953fec6 100644
--- a/conf/airframes/booz2_ppzuav.xml
+++ b/conf/airframes/booz2_ppzuav.xml
@@ -171,11 +171,11 @@
diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml
index 4cedb04709..479a4580e6 100644
--- a/conf/airframes/esden/jt_lisam.xml
+++ b/conf/airframes/esden/jt_lisam.xml
@@ -157,9 +157,9 @@
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml
index d3fce04266..ea7f9d1803 100644
--- a/conf/airframes/esden/lisa_asctec.xml
+++ b/conf/airframes/esden/lisa_asctec.xml
@@ -163,9 +163,9 @@
diff --git a/conf/airframes/esden/lisa_asctec_aspirin.xml b/conf/airframes/esden/lisa_asctec_aspirin.xml
index 870c42f4c5..da175c827d 100644
--- a/conf/airframes/esden/lisa_asctec_aspirin.xml
+++ b/conf/airframes/esden/lisa_asctec_aspirin.xml
@@ -163,9 +163,9 @@
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index f0d65c187d..b8148c503f 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -183,9 +183,9 @@
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index aa6b40beec..61363372e9 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -158,9 +158,9 @@
diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
index 2e77614988..af1e2ec468 100644
--- a/conf/airframes/esden/synerani_4B.xml
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -161,9 +161,9 @@
diff --git a/conf/airframes/mm/rotor/qmk1.xml b/conf/airframes/mm/rotor/qmk1.xml
index 974b3171ce..073a777144 100644
--- a/conf/airframes/mm/rotor/qmk1.xml
+++ b/conf/airframes/mm/rotor/qmk1.xml
@@ -164,9 +164,9 @@
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml
index e69958cd20..66a4250869 100644
--- a/conf/airframes/obsolete/booz2_Aron.xml
+++ b/conf/airframes/obsolete/booz2_Aron.xml
@@ -155,9 +155,9 @@
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml
index 06977da101..d2f95992a2 100644
--- a/conf/airframes/obsolete/booz2_NoVa.xml
+++ b/conf/airframes/obsolete/booz2_NoVa.xml
@@ -196,11 +196,11 @@
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml
index e6c6e887db..e28af665cb 100644
--- a/conf/airframes/obsolete/booz2_NoVa_001.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_001.xml
@@ -196,11 +196,11 @@
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml
index 9b9e1acc23..958f2dabcc 100644
--- a/conf/airframes/obsolete/booz2_NoVa_002.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_002.xml
@@ -196,11 +196,11 @@
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml
index 12109d9671..6e32dc7af9 100644
--- a/conf/airframes/obsolete/booz2_a2.xml
+++ b/conf/airframes/obsolete/booz2_a2.xml
@@ -155,9 +155,9 @@
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml
index f886dfb90b..b4bf3d6b9b 100644
--- a/conf/airframes/obsolete/booz2_a3.xml
+++ b/conf/airframes/obsolete/booz2_a3.xml
@@ -140,9 +140,9 @@
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml
index d01320b38f..5e122b613e 100644
--- a/conf/airframes/obsolete/booz2_a4.xml
+++ b/conf/airframes/obsolete/booz2_a4.xml
@@ -118,9 +118,9 @@
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml
index ed1d48b032..0e42003e7f 100644
--- a/conf/airframes/obsolete/booz2_a5.xml
+++ b/conf/airframes/obsolete/booz2_a5.xml
@@ -159,9 +159,9 @@
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml
index ea5499d7c6..764f9d4a59 100644
--- a/conf/airframes/obsolete/booz2_s1.xml
+++ b/conf/airframes/obsolete/booz2_s1.xml
@@ -151,9 +151,9 @@
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml
index 3e0d629c4e..f5b0edb23a 100644
--- a/conf/airframes/obsolete/booz2_x1.xml
+++ b/conf/airframes/obsolete/booz2_x1.xml
@@ -161,9 +161,9 @@
diff --git a/conf/settings/settings_UofAdelaide.xml b/conf/settings/settings_UofAdelaide.xml
index 1986ef4a07..01a4ae6480 100644
--- a/conf/settings/settings_UofAdelaide.xml
+++ b/conf/settings/settings_UofAdelaide.xml
@@ -66,11 +66,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml
index ba6291e052..a87a031849 100644
--- a/conf/settings/settings_booz2.xml
+++ b/conf/settings/settings_booz2.xml
@@ -55,11 +55,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/conf/settings/settings_booz2_jtm.xml b/conf/settings/settings_booz2_jtm.xml
index 89b15eae65..441c31d1e6 100644
--- a/conf/settings/settings_booz2_jtm.xml
+++ b/conf/settings/settings_booz2_jtm.xml
@@ -55,11 +55,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 2471d0f3a9..b9f7b9d439 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -86,11 +86,11 @@ void guidance_h_init(void) {
INT_VECT2_ZERO(guidance_h_pos_err_sum);
INT_EULERS_ZERO(guidance_h_rc_sp);
INT_EULERS_ZERO(guidance_h_command_body);
- guidance_h_pgain = GUIDANCE_H_PGAIN;
- guidance_h_igain = GUIDANCE_H_IGAIN;
- guidance_h_dgain = GUIDANCE_H_DGAIN;
- guidance_h_ngain = GUIDANCE_H_NGAIN;
- guidance_h_again = GUIDANCE_H_AGAIN;
+ guidance_h_pgain = ABS(GUIDANCE_H_PGAIN);
+ guidance_h_igain = ABS(GUIDANCE_H_IGAIN);
+ guidance_h_dgain = ABS(GUIDANCE_H_DGAIN);
+ guidance_h_ngain = ABS(GUIDANCE_H_NGAIN);
+ guidance_h_again = ABS(GUIDANCE_H_AGAIN);
}
@@ -224,12 +224,12 @@ void guidance_h_run(bool_t in_flight) {
__attribute__ ((always_inline)) static inline void guidance_h_hover_run(void) {
/* compute position error */
- VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_sp);
+ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_sp, ins_ltp_pos);
/* saturate it */
VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
+ VECT2_COPY(guidance_h_speed_err, -ins_ltp_speed);
/* saturate it */
VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -293,13 +293,13 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
#endif
/* compute position error */
- VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_ref);
+ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, ins_ltp_pos);
/* saturate it */
VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
//VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
- VECT2_DIFF(guidance_h_speed_err, ins_ltp_speed, guidance_h_speed_ref);
+ VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, ins_ltp_speed);
/* saturate it */
VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -321,8 +321,8 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
}
// multiply by vector orthogonal to speed
VECT2_ASSIGN(guidance_h_nav_err,
- vect_prod * (-ins_ltp_speed.y),
- vect_prod * ins_ltp_speed.x);
+ vect_prod * ins_ltp_speed.y,
+ vect_prod * (-ins_ltp_speed.x));
// divide by 2 times dist ( >> 16 )
VECT2_SDIV(guidance_h_nav_err, guidance_h_nav_err, dist*dist);
// *2 ??