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 ??