diff --git a/conf/abi.xml b/conf/abi.xml
index 7b8b360d41..223f97b1dc 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -22,17 +22,17 @@
-
+
-
+
-
+
@@ -44,10 +44,6 @@
-
-
-
-
@@ -215,6 +211,35 @@
Pointer to a radio control structure
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
index 43ecdd3ee9..ff2c7a4a85 100644
--- a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
@@ -248,7 +248,6 @@ Aggie Air ARK
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
index e12783d839..c902197b63 100644
--- a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
@@ -200,7 +200,6 @@
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
index cf6841a220..8e6ec4f38b 100644
--- a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
@@ -288,7 +288,6 @@ AggieAir Atomic Tangerine
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_blujay.xml b/conf/airframes/AGGIEAIR/aggieair_blujay.xml
index d96e7d9b0b..840765af96 100644
--- a/conf/airframes/AGGIEAIR/aggieair_blujay.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_blujay.xml
@@ -257,7 +257,6 @@ AggieAir Blujayujay
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml b/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml
index 9f43392647..fcc85f6a82 100644
--- a/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml
@@ -262,7 +262,6 @@ AggieAir Blujayujay
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml b/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml
index a85b1b6206..03c91fb8d6 100644
--- a/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml
@@ -287,7 +287,6 @@ AggieAir El Capitan
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
index d1c7755691..a067bbf247 100644
--- a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
@@ -368,7 +368,6 @@
diff --git a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml
index fa7cea74d2..b714e497bd 100644
--- a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml
@@ -279,7 +279,6 @@ AggieAir RP3 Minion
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml
index a7ee4ea564..89a4e5566a 100644
--- a/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml
@@ -291,7 +291,6 @@ AggieAir Minion Sim
-
diff --git a/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml
index 4cccba89b4..0633ce6fc1 100644
--- a/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml
@@ -287,7 +287,6 @@ AggieAir Minty Fresh
-
diff --git a/conf/airframes/BR/3DplaneLisaS.xml b/conf/airframes/BR/3DplaneLisaS.xml
index 2204562205..5a31b6d759 100644
--- a/conf/airframes/BR/3DplaneLisaS.xml
+++ b/conf/airframes/BR/3DplaneLisaS.xml
@@ -157,7 +157,6 @@ Pin 6 on lisa/s is number 3 in airframefile = ELEVATOR
@@ -197,6 +196,5 @@ Pin 6 on lisa/s is number 3 in airframefile = ELEVATOR
-
diff --git a/conf/airframes/BR/DelFlyDualPWMservo.xml b/conf/airframes/BR/DelFlyDualPWMservo.xml
index 29de2203e2..f46406e60c 100644
--- a/conf/airframes/BR/DelFlyDualPWMservo.xml
+++ b/conf/airframes/BR/DelFlyDualPWMservo.xml
@@ -226,6 +226,5 @@
-
diff --git a/conf/airframes/BR/DreamCacher_bart.xml b/conf/airframes/BR/DreamCacher_bart.xml
index d0d3e353cd..c3ef0eed81 100644
--- a/conf/airframes/BR/DreamCacher_bart.xml
+++ b/conf/airframes/BR/DreamCacher_bart.xml
@@ -171,7 +171,6 @@
@@ -218,6 +217,5 @@
-
diff --git a/conf/airframes/BR/asctec_br.xml b/conf/airframes/BR/asctec_br.xml
index 60f693274b..bd848ec840 100644
--- a/conf/airframes/BR/asctec_br.xml
+++ b/conf/airframes/BR/asctec_br.xml
@@ -162,7 +162,6 @@
diff --git a/conf/airframes/BR/bebop_default.xml b/conf/airframes/BR/bebop_default.xml
index 87ca28fc21..f322b50761 100644
--- a/conf/airframes/BR/bebop_default.xml
+++ b/conf/airframes/BR/bebop_default.xml
@@ -25,7 +25,6 @@
-
@@ -182,7 +181,6 @@
diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml
index 58922bb6c0..b93313a694 100644
--- a/conf/airframes/BR/bebop_indi.xml
+++ b/conf/airframes/BR/bebop_indi.xml
@@ -31,7 +31,6 @@
-
@@ -207,7 +206,6 @@
diff --git a/conf/airframes/BR/bebop_indi_frog.xml b/conf/airframes/BR/bebop_indi_frog.xml
index 3a6a9d0764..787921fac7 100644
--- a/conf/airframes/BR/bebop_indi_frog.xml
+++ b/conf/airframes/BR/bebop_indi_frog.xml
@@ -33,7 +33,6 @@
-
@@ -210,7 +209,6 @@
diff --git a/conf/airframes/BR/ladybird_kit_bart.xml b/conf/airframes/BR/ladybird_kit_bart.xml
index 0db524c82d..fdf6682ece 100644
--- a/conf/airframes/BR/ladybird_kit_bart.xml
+++ b/conf/airframes/BR/ladybird_kit_bart.xml
@@ -176,7 +176,6 @@
@@ -223,6 +222,5 @@
-
diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
index 302eca9d10..366f888f00 100644
--- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
+++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
@@ -184,7 +184,6 @@
@@ -221,7 +220,6 @@
-
diff --git a/conf/airframes/BR/ladybird_kit_indi_bart.xml b/conf/airframes/BR/ladybird_kit_indi_bart.xml
index 3cf9aca4ca..fd3653030b 100644
--- a/conf/airframes/BR/ladybird_kit_indi_bart.xml
+++ b/conf/airframes/BR/ladybird_kit_indi_bart.xml
@@ -180,7 +180,6 @@
@@ -229,6 +228,5 @@
-
diff --git a/conf/airframes/BR/mavtec4_br.xml b/conf/airframes/BR/mavtec4_br.xml
index 9f8354d1a7..e78e38c2f7 100644
--- a/conf/airframes/BR/mavtec4_br.xml
+++ b/conf/airframes/BR/mavtec4_br.xml
@@ -162,7 +162,6 @@
diff --git a/conf/airframes/CDW/cdw_asctec.xml b/conf/airframes/CDW/cdw_asctec.xml
index e2748020b8..a42c485fe5 100644
--- a/conf/airframes/CDW/cdw_asctec.xml
+++ b/conf/airframes/CDW/cdw_asctec.xml
@@ -170,7 +170,6 @@
diff --git a/conf/airframes/CDW/cdw_mavtec.xml b/conf/airframes/CDW/cdw_mavtec.xml
index 2c9f302820..b5596e9e0d 100644
--- a/conf/airframes/CDW/cdw_mavtec.xml
+++ b/conf/airframes/CDW/cdw_mavtec.xml
@@ -34,7 +34,6 @@
-
@@ -194,7 +193,6 @@
diff --git a/conf/airframes/CRIDEA/cridea_quadsuave.xml b/conf/airframes/CRIDEA/cridea_quadsuave.xml
index 63ec89060a..e6b7497d2d 100644
--- a/conf/airframes/CRIDEA/cridea_quadsuave.xml
+++ b/conf/airframes/CRIDEA/cridea_quadsuave.xml
@@ -183,7 +183,6 @@
diff --git a/conf/airframes/ENAC/cyfoam.xml b/conf/airframes/ENAC/cyfoam.xml
index 41e8cb2bf5..f99a3cf422 100644
--- a/conf/airframes/ENAC/cyfoam.xml
+++ b/conf/airframes/ENAC/cyfoam.xml
@@ -62,9 +62,8 @@
-
-
+
@@ -300,7 +299,6 @@
diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml
index 61094cd4e3..c019a7eeef 100644
--- a/conf/airframes/ENAC/fixed-wing/apogee.xml
+++ b/conf/airframes/ENAC/fixed-wing/apogee.xml
@@ -102,6 +102,9 @@
+
+
+
diff --git a/conf/airframes/ENAC/fixed-wing/crrcsim.xml b/conf/airframes/ENAC/fixed-wing/crrcsim.xml
index 1120916a3a..3c3db39a7f 100644
--- a/conf/airframes/ENAC/fixed-wing/crrcsim.xml
+++ b/conf/airframes/ENAC/fixed-wing/crrcsim.xml
@@ -228,7 +228,6 @@
-
diff --git a/conf/airframes/ENAC/fixed-wing/jp.xml b/conf/airframes/ENAC/fixed-wing/jp.xml
index ceef3ed934..c05349021a 100644
--- a/conf/airframes/ENAC/fixed-wing/jp.xml
+++ b/conf/airframes/ENAC/fixed-wing/jp.xml
@@ -280,7 +280,6 @@
diff --git a/conf/airframes/ENAC/fixed-wing/matek_f765_wing.xml b/conf/airframes/ENAC/fixed-wing/matek_f765_wing.xml
index 024d45d932..2918a15917 100644
--- a/conf/airframes/ENAC/fixed-wing/matek_f765_wing.xml
+++ b/conf/airframes/ENAC/fixed-wing/matek_f765_wing.xml
@@ -197,7 +197,6 @@
diff --git a/conf/airframes/ENAC/fixed-wing/tawaki.xml b/conf/airframes/ENAC/fixed-wing/tawaki.xml
index e76a849154..6e51ee5238 100644
--- a/conf/airframes/ENAC/fixed-wing/tawaki.xml
+++ b/conf/airframes/ENAC/fixed-wing/tawaki.xml
@@ -214,7 +214,6 @@
diff --git a/conf/airframes/ENAC/fixed-wing/zagi_mekf_wind.xml b/conf/airframes/ENAC/fixed-wing/zagi_mekf_wind.xml
index ae3d6c9cbe..f88fc8002a 100644
--- a/conf/airframes/ENAC/fixed-wing/zagi_mekf_wind.xml
+++ b/conf/airframes/ENAC/fixed-wing/zagi_mekf_wind.xml
@@ -242,7 +242,6 @@
diff --git a/conf/airframes/ENAC/hoops_gen_ap.xml b/conf/airframes/ENAC/hoops_gen_ap.xml
index 8561a83ec6..e0d3818e40 100644
--- a/conf/airframes/ENAC/hoops_gen_ap.xml
+++ b/conf/airframes/ENAC/hoops_gen_ap.xml
@@ -50,7 +50,6 @@
-
@@ -248,7 +247,6 @@
diff --git a/conf/airframes/ENAC/nucleo144_test.xml b/conf/airframes/ENAC/nucleo144_test.xml
index 158bd594be..f1dbd05b35 100644
--- a/conf/airframes/ENAC/nucleo144_test.xml
+++ b/conf/airframes/ENAC/nucleo144_test.xml
@@ -205,7 +205,6 @@
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_control.xml b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
index 00c00d3f6d..28d356200e 100644
--- a/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
@@ -120,7 +120,6 @@
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
index 60622130f1..5df6415023 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
@@ -209,7 +209,6 @@
-
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
index 823e7adb51..5929c291c8 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
@@ -216,7 +216,6 @@
-
diff --git a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
index cb93b59262..6d44fc38e4 100644
--- a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
+++ b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
@@ -244,7 +244,6 @@
diff --git a/conf/airframes/ENAC/quadrotor/drop1.xml b/conf/airframes/ENAC/quadrotor/drop1.xml
index bc414ec5b0..c1f50247da 100644
--- a/conf/airframes/ENAC/quadrotor/drop1.xml
+++ b/conf/airframes/ENAC/quadrotor/drop1.xml
@@ -70,7 +70,6 @@
-
@@ -281,7 +280,6 @@
diff --git a/conf/airframes/ENAC/quadrotor/kakute_f7.xml b/conf/airframes/ENAC/quadrotor/kakute_f7.xml
index 109563c10b..1507b3b549 100644
--- a/conf/airframes/ENAC/quadrotor/kakute_f7.xml
+++ b/conf/airframes/ENAC/quadrotor/kakute_f7.xml
@@ -247,7 +247,6 @@
diff --git a/conf/airframes/ENAC/quadrotor/niquad_wind.xml b/conf/airframes/ENAC/quadrotor/niquad_wind.xml
index 0db55b6395..dff2f80d56 100644
--- a/conf/airframes/ENAC/quadrotor/niquad_wind.xml
+++ b/conf/airframes/ENAC/quadrotor/niquad_wind.xml
@@ -268,7 +268,6 @@
diff --git a/conf/airframes/ENAC/rover.xml b/conf/airframes/ENAC/rover.xml
index 35917d17ae..39d8671b0b 100644
--- a/conf/airframes/ENAC/rover.xml
+++ b/conf/airframes/ENAC/rover.xml
@@ -87,7 +87,6 @@
diff --git a/conf/airframes/ENAC/rover_ostrich.xml b/conf/airframes/ENAC/rover_ostrich.xml
index 21b28f145e..15478f9895 100644
--- a/conf/airframes/ENAC/rover_ostrich.xml
+++ b/conf/airframes/ENAC/rover_ostrich.xml
@@ -111,7 +111,6 @@
diff --git a/conf/airframes/ESDEN/esden_cocto_lm2a2.xml b/conf/airframes/ESDEN/esden_cocto_lm2a2.xml
index d7dc6fdaa3..1d5a7b696a 100644
--- a/conf/airframes/ESDEN/esden_cocto_lm2a2.xml
+++ b/conf/airframes/ESDEN/esden_cocto_lm2a2.xml
@@ -183,7 +183,6 @@ B2L -> CW
diff --git a/conf/airframes/ESDEN/esden_gain_scheduling_example.xml b/conf/airframes/ESDEN/esden_gain_scheduling_example.xml
index 2c19502564..f99197b93d 100644
--- a/conf/airframes/ESDEN/esden_gain_scheduling_example.xml
+++ b/conf/airframes/ESDEN/esden_gain_scheduling_example.xml
@@ -170,7 +170,6 @@
diff --git a/conf/airframes/ESDEN/esden_hexy_ll11a2pwm.xml b/conf/airframes/ESDEN/esden_hexy_ll11a2pwm.xml
index 1cd5ba2413..af6b4c9aa8 100644
--- a/conf/airframes/ESDEN/esden_hexy_ll11a2pwm.xml
+++ b/conf/airframes/ESDEN/esden_hexy_ll11a2pwm.xml
@@ -180,7 +180,6 @@
diff --git a/conf/airframes/ESDEN/esden_hexy_lm2a2pwm.xml b/conf/airframes/ESDEN/esden_hexy_lm2a2pwm.xml
index f45dbda0cc..7db045c653 100644
--- a/conf/airframes/ESDEN/esden_hexy_lm2a2pwm.xml
+++ b/conf/airframes/ESDEN/esden_hexy_lm2a2pwm.xml
@@ -141,7 +141,6 @@
diff --git a/conf/airframes/ESDEN/esden_lisa2_hex.xml b/conf/airframes/ESDEN/esden_lisa2_hex.xml
index 3f1e0717eb..13e04d85d9 100644
--- a/conf/airframes/ESDEN/esden_lisa2_hex.xml
+++ b/conf/airframes/ESDEN/esden_lisa2_hex.xml
@@ -155,7 +155,6 @@
diff --git a/conf/airframes/ESDEN/esden_qs_asp22.xml b/conf/airframes/ESDEN/esden_qs_asp22.xml
index 48f4fc03fa..75bb8fdb47 100644
--- a/conf/airframes/ESDEN/esden_qs_asp22.xml
+++ b/conf/airframes/ESDEN/esden_qs_asp22.xml
@@ -180,7 +180,6 @@
diff --git a/conf/airframes/ESDEN/esden_quady_ll11a2pwm.xml b/conf/airframes/ESDEN/esden_quady_ll11a2pwm.xml
index 8317959538..6b3dc92ff9 100644
--- a/conf/airframes/ESDEN/esden_quady_ll11a2pwm.xml
+++ b/conf/airframes/ESDEN/esden_quady_ll11a2pwm.xml
@@ -176,7 +176,6 @@
diff --git a/conf/airframes/ESDEN/esden_quady_lm1a1pwm.xml b/conf/airframes/ESDEN/esden_quady_lm1a1pwm.xml
index 108a51a1ca..86a4558a40 100644
--- a/conf/airframes/ESDEN/esden_quady_lm1a1pwm.xml
+++ b/conf/airframes/ESDEN/esden_quady_lm1a1pwm.xml
@@ -137,7 +137,6 @@
diff --git a/conf/airframes/ESDEN/esden_quady_lm2a2pwm.xml b/conf/airframes/ESDEN/esden_quady_lm2a2pwm.xml
index 1b404a89e0..2ca618e807 100644
--- a/conf/airframes/ESDEN/esden_quady_lm2a2pwm.xml
+++ b/conf/airframes/ESDEN/esden_quady_lm2a2pwm.xml
@@ -140,7 +140,6 @@
diff --git a/conf/airframes/ESDEN/esden_quady_lm2a2pwmppm.xml b/conf/airframes/ESDEN/esden_quady_lm2a2pwmppm.xml
index e655ba2281..59e66c2afa 100644
--- a/conf/airframes/ESDEN/esden_quady_lm2a2pwmppm.xml
+++ b/conf/airframes/ESDEN/esden_quady_lm2a2pwmppm.xml
@@ -137,7 +137,6 @@
diff --git a/conf/airframes/ESDEN/esden_quady_ls10pwm.xml b/conf/airframes/ESDEN/esden_quady_ls10pwm.xml
index ac8f50c1c0..5368324c4a 100644
--- a/conf/airframes/ESDEN/esden_quady_ls10pwm.xml
+++ b/conf/airframes/ESDEN/esden_quady_ls10pwm.xml
@@ -140,7 +140,6 @@
diff --git a/conf/airframes/FLIXR/flixr_ladybird_lisa_s.xml b/conf/airframes/FLIXR/flixr_ladybird_lisa_s.xml
index c740e61087..aa85df81ee 100644
--- a/conf/airframes/FLIXR/flixr_ladybird_lisa_s.xml
+++ b/conf/airframes/FLIXR/flixr_ladybird_lisa_s.xml
@@ -81,7 +81,6 @@
-
@@ -220,7 +219,6 @@
diff --git a/conf/airframes/FLIXR/flixr_lisa_mx.xml b/conf/airframes/FLIXR/flixr_lisa_mx.xml
index 3ea2308fd3..586ee0cb94 100644
--- a/conf/airframes/FLIXR/flixr_lisa_mx.xml
+++ b/conf/airframes/FLIXR/flixr_lisa_mx.xml
@@ -218,7 +218,6 @@
diff --git a/conf/airframes/FLIXR/flixr_zmr250_elle0.xml b/conf/airframes/FLIXR/flixr_zmr250_elle0.xml
index 9286beb94f..abd44e9e26 100644
--- a/conf/airframes/FLIXR/flixr_zmr250_elle0.xml
+++ b/conf/airframes/FLIXR/flixr_zmr250_elle0.xml
@@ -161,7 +161,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_enac_hexa_elle0_v1_2.xml b/conf/airframes/HOOPERFLY/hooperfly_enac_hexa_elle0_v1_2.xml
index e1f8dd098a..c19539cdf5 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_enac_hexa_elle0_v1_2.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_enac_hexa_elle0_v1_2.xml
@@ -42,7 +42,6 @@
-
@@ -198,7 +197,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_racerpex_hexa_lisa_mx_20.xml b/conf/airframes/HOOPERFLY/hooperfly_racerpex_hexa_lisa_mx_20.xml
index f83c1495eb..903968c2a1 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_racerpex_hexa_lisa_mx_20.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_racerpex_hexa_lisa_mx_20.xml
@@ -38,7 +38,6 @@
-
@@ -198,7 +197,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_racerpex_octo_lisa_mx_20.xml b/conf/airframes/HOOPERFLY/hooperfly_racerpex_octo_lisa_mx_20.xml
index fe428fb5a8..d727fbca44 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_racerpex_octo_lisa_mx_20.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_racerpex_octo_lisa_mx_20.xml
@@ -37,7 +37,6 @@
-
@@ -192,7 +191,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_elle0_v1_2.xml b/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_elle0_v1_2.xml
index 5b7214764e..34341bcda7 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_elle0_v1_2.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_elle0_v1_2.xml
@@ -42,7 +42,6 @@
-
@@ -194,7 +193,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_lisa_mx_20.xml b/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_lisa_mx_20.xml
index 2ce694da99..fe561bbe5c 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_lisa_mx_20.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_racerpex_quad_lisa_mx_20.xml
@@ -38,7 +38,6 @@
-
@@ -194,7 +193,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_hexa_lisa_mx_20.xml b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_hexa_lisa_mx_20.xml
index 6c987ff838..54c7c321be 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_hexa_lisa_mx_20.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_hexa_lisa_mx_20.xml
@@ -38,7 +38,6 @@
-
@@ -198,7 +197,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0.xml b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0.xml
index bcc10e90a9..613885c7fa 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0.xml
@@ -36,7 +36,6 @@
-
@@ -189,7 +188,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0_v1_2.xml b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0_v1_2.xml
index d6a170339f..94d4e0a858 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0_v1_2.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_elle0_v1_2.xml
@@ -42,7 +42,6 @@
-
@@ -169,7 +168,6 @@
diff --git a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_lisa_mx_20.xml b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_lisa_mx_20.xml
index 73edc0b170..3298a8d353 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_lisa_mx_20.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_teensyfly_quad_lisa_mx_20.xml
@@ -38,7 +38,6 @@
-
@@ -194,7 +193,6 @@
diff --git a/conf/airframes/KS/ks_bebop2_stereo.xml b/conf/airframes/KS/ks_bebop2_stereo.xml
index 0b41749624..1f2dbcae69 100644
--- a/conf/airframes/KS/ks_bebop2_stereo.xml
+++ b/conf/airframes/KS/ks_bebop2_stereo.xml
@@ -193,7 +193,6 @@
-
diff --git a/conf/airframes/MM/bebop.xml b/conf/airframes/MM/bebop.xml
index b9446c9814..bbe6150160 100644
--- a/conf/airframes/MM/bebop.xml
+++ b/conf/airframes/MM/bebop.xml
@@ -32,7 +32,6 @@
-
@@ -193,7 +192,6 @@
diff --git a/conf/airframes/MM/bebop2_lum1_xbee.xml b/conf/airframes/MM/bebop2_lum1_xbee.xml
index 6c1b86e0fd..c66da40d4f 100644
--- a/conf/airframes/MM/bebop2_lum1_xbee.xml
+++ b/conf/airframes/MM/bebop2_lum1_xbee.xml
@@ -39,7 +39,6 @@
-
@@ -219,7 +218,6 @@
diff --git a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
index d372f9894c..c9b59208a4 100644
--- a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
+++ b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
@@ -254,7 +254,6 @@
-
@@ -640,7 +639,6 @@ The most crucial part for the magnetometer calibration:
-
-
@@ -947,8 +946,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_durafly_g2.xml b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
index d6bfcbf886..9800865a97 100644
--- a/conf/airframes/OPENUAS/openuas_durafly_g2.xml
+++ b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
@@ -189,7 +189,6 @@
-
@@ -620,7 +619,6 @@
-
diff --git a/conf/airframes/OPENUAS/openuas_durafly_goblin.xml b/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
index 9351a07307..d48a5facbb 100644
--- a/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
+++ b/conf/airframes/OPENUAS/openuas_durafly_goblin.xml
@@ -135,7 +135,6 @@ NOTES:
-
@@ -486,7 +485,6 @@ NOTES:
-
diff --git a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
index 4cc07586a4..bee2ef0a2a 100644
--- a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
+++ b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
@@ -156,7 +156,6 @@
<-->
-
diff --git a/conf/airframes/OPENUAS/openuas_eflite_t28.xml b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
index a5dab406a7..ee62176617 100644
--- a/conf/airframes/OPENUAS/openuas_eflite_t28.xml
+++ b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
@@ -319,7 +319,6 @@ NOTES:
-
@@ -1106,8 +1105,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml b/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml
index 621585a731..64afd36d01 100644
--- a/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml
+++ b/conf/airframes/OPENUAS/openuas_eflite_umx_sbach_342.xml
@@ -90,7 +90,6 @@
-
-->
@@ -168,7 +167,6 @@
-->
-
diff --git a/conf/airframes/OPENUAS/openuas_itsybitsy.xml b/conf/airframes/OPENUAS/openuas_itsybitsy.xml
index be501e4084..4222f5a241 100644
--- a/conf/airframes/OPENUAS/openuas_itsybitsy.xml
+++ b/conf/airframes/OPENUAS/openuas_itsybitsy.xml
@@ -185,7 +185,6 @@
@@ -241,6 +240,5 @@
-->
-
diff --git a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
index 048404ba78..4253912f90 100644
--- a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
+++ b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
@@ -328,7 +328,6 @@ NOTES:
-
@@ -1113,8 +1112,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml b/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
index e3a3b83ba1..8c75f45395 100644
--- a/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
+++ b/conf/airframes/OPENUAS/openuas_multiplex_twinstar_nd.xml
@@ -255,7 +255,6 @@ NOTES:
-
@@ -889,8 +888,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_own_flexo.xml b/conf/airframes/OPENUAS/openuas_own_flexo.xml
index 10e44d18cc..a6b20df730 100644
--- a/conf/airframes/OPENUAS/openuas_own_flexo.xml
+++ b/conf/airframes/OPENUAS/openuas_own_flexo.xml
@@ -290,7 +290,6 @@ NOTES:
-
@@ -1034,8 +1033,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_own_itsy_bitsy.xml b/conf/airframes/OPENUAS/openuas_own_itsy_bitsy.xml
index ddd088cc0e..57e19b2901 100644
--- a/conf/airframes/OPENUAS/openuas_own_itsy_bitsy.xml
+++ b/conf/airframes/OPENUAS/openuas_own_itsy_bitsy.xml
@@ -79,7 +79,6 @@
-
@@ -233,7 +232,6 @@
diff --git a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
index 70a078e6e4..bc3d8c4603 100644
--- a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
+++ b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
@@ -150,7 +150,6 @@
<-->
-
diff --git a/conf/airframes/OPENUAS/openuas_own_mavtec.xml b/conf/airframes/OPENUAS/openuas_own_mavtec.xml
index 9f9702de73..2a5d4a5cee 100644
--- a/conf/airframes/OPENUAS/openuas_own_mavtec.xml
+++ b/conf/airframes/OPENUAS/openuas_own_mavtec.xml
@@ -32,7 +32,6 @@
-
@@ -191,7 +190,6 @@
diff --git a/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml b/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
index e2fae9a3cd..d8eda59835 100644
--- a/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
+++ b/conf/airframes/OPENUAS/openuas_own_sumo_ii.xml
@@ -292,7 +292,6 @@ NOTES:
-
@@ -1036,8 +1035,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
index cba8e9a79c..85e94e096a 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
@@ -158,7 +158,6 @@
-
@@ -525,7 +524,6 @@ The most crucial part for the magnetometer calibration:
-
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
index 1b6bacbb95..ccbfc09764 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
@@ -160,7 +160,6 @@
-
@@ -578,7 +577,6 @@ second order filter parameters -->
-
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
index aa4b21cefc..bfa4dcf9b1 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
@@ -172,7 +172,6 @@
-
@@ -593,7 +592,6 @@ The most crucial part for the magnetometer calibration:
-
diff --git a/conf/airframes/OPENUAS/openuas_parrot_disco.xml b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
index e1a5739097..a4d0f8271b 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_disco.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_disco.xml
@@ -319,7 +319,6 @@
-
@@ -928,8 +927,6 @@ The most crucial part for the magnetometer calibration:
-
-
diff --git a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
index bf1fb27184..8fba227204 100644
--- a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
+++ b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
@@ -209,7 +209,6 @@
-
@@ -742,7 +741,6 @@ The most crucial part for the magnetometer calibration:
-
-
@@ -631,7 +630,6 @@ NOTES:
-
diff --git a/conf/airframes/UCM/rover_steering.xml b/conf/airframes/UCM/rover_steering.xml
index 8ecb467543..0d121b44d1 100644
--- a/conf/airframes/UCM/rover_steering.xml
+++ b/conf/airframes/UCM/rover_steering.xml
@@ -111,7 +111,6 @@
diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml
index d9338d9329..69f5824f60 100644
--- a/conf/airframes/examples/MentorEnergy.xml
+++ b/conf/airframes/examples/MentorEnergy.xml
@@ -246,7 +246,6 @@
-
diff --git a/conf/airframes/examples/ardrone2.xml b/conf/airframes/examples/ardrone2.xml
index 37d1cc35cf..8a0a894ede 100644
--- a/conf/airframes/examples/ardrone2.xml
+++ b/conf/airframes/examples/ardrone2.xml
@@ -26,7 +26,6 @@
-
@@ -179,7 +178,6 @@
diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml
index 3124edf5b1..67ea321005 100644
--- a/conf/airframes/examples/ardrone2_gazebo.xml
+++ b/conf/airframes/examples/ardrone2_gazebo.xml
@@ -25,7 +25,6 @@
-
@@ -191,7 +190,6 @@
diff --git a/conf/airframes/examples/ardrone2_opticflow_hover.xml b/conf/airframes/examples/ardrone2_opticflow_hover.xml
index 5bb143a961..47be27b55b 100644
--- a/conf/airframes/examples/ardrone2_opticflow_hover.xml
+++ b/conf/airframes/examples/ardrone2_opticflow_hover.xml
@@ -26,7 +26,6 @@
-
@@ -190,7 +189,6 @@
diff --git a/conf/airframes/examples/bebop.xml b/conf/airframes/examples/bebop.xml
index 98b399403d..817e56d174 100644
--- a/conf/airframes/examples/bebop.xml
+++ b/conf/airframes/examples/bebop.xml
@@ -25,7 +25,6 @@
-
@@ -187,7 +186,6 @@
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml
index 2e136b0762..ca2c9d8301 100644
--- a/conf/airframes/examples/bebop2_indi.xml
+++ b/conf/airframes/examples/bebop2_indi.xml
@@ -27,7 +27,6 @@
-
@@ -167,7 +166,6 @@
diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
index bc14eb22d8..3face7cae2 100644
--- a/conf/airframes/examples/bebop2_opticflow.xml
+++ b/conf/airframes/examples/bebop2_opticflow.xml
@@ -194,7 +194,6 @@
diff --git a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
index 89d436999f..5ba710fc2b 100644
--- a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
+++ b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
@@ -33,7 +33,6 @@
-
@@ -199,7 +198,6 @@
diff --git a/conf/airframes/examples/bixler_lisa_m_2.xml b/conf/airframes/examples/bixler_lisa_m_2.xml
index 99503fda0c..a779e5df03 100644
--- a/conf/airframes/examples/bixler_lisa_m_2.xml
+++ b/conf/airframes/examples/bixler_lisa_m_2.xml
@@ -155,7 +155,6 @@
-
diff --git a/conf/airframes/examples/bumblebee_quad.xml b/conf/airframes/examples/bumblebee_quad.xml
index e5ad690435..e154377a9e 100644
--- a/conf/airframes/examples/bumblebee_quad.xml
+++ b/conf/airframes/examples/bumblebee_quad.xml
@@ -62,7 +62,6 @@
current to magnetic field distortion curve.
http://wiki.paparazziuav.org/wiki/ImuCalibration#Calibrating_for_Current
-->
-
@@ -209,7 +208,6 @@
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index 0d7c252ccc..c4aecd8b08 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -49,7 +49,9 @@
-
+
+
+
@@ -79,15 +81,14 @@
-
+
-
+
-
+
@@ -140,7 +141,7 @@
-
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
@@ -203,6 +204,12 @@
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
@@ -342,7 +320,6 @@
diff --git a/conf/airframes/examples/ladybird_lisa_s.xml b/conf/airframes/examples/ladybird_lisa_s.xml
index 80e243cfbb..62eeaa58b2 100644
--- a/conf/airframes/examples/ladybird_lisa_s.xml
+++ b/conf/airframes/examples/ladybird_lisa_s.xml
@@ -181,7 +181,6 @@
@@ -232,6 +231,5 @@
-
diff --git a/conf/airframes/examples/ladybird_lisa_s_bluegiga.xml b/conf/airframes/examples/ladybird_lisa_s_bluegiga.xml
index ec47cbdf64..96b77bd67b 100644
--- a/conf/airframes/examples/ladybird_lisa_s_bluegiga.xml
+++ b/conf/airframes/examples/ladybird_lisa_s_bluegiga.xml
@@ -181,7 +181,6 @@
@@ -231,6 +230,5 @@
-
diff --git a/conf/airframes/examples/microjet_lisa_m.xml b/conf/airframes/examples/microjet_lisa_m.xml
index d2cf37540d..e673ea253b 100644
--- a/conf/airframes/examples/microjet_lisa_m.xml
+++ b/conf/airframes/examples/microjet_lisa_m.xml
@@ -189,7 +189,6 @@
diff --git a/conf/airframes/examples/quadrotor_elle0.xml b/conf/airframes/examples/quadrotor_elle0.xml
index a9ce4aaf3c..f6c1d5948e 100644
--- a/conf/airframes/examples/quadrotor_elle0.xml
+++ b/conf/airframes/examples/quadrotor_elle0.xml
@@ -170,7 +170,6 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index f948a7cbd3..8334c5d0ab 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -170,7 +170,6 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml
index 5079d3c74d..b35f096594 100644
--- a/conf/airframes/examples/quadrotor_lisa_mx.xml
+++ b/conf/airframes/examples/quadrotor_lisa_mx.xml
@@ -187,7 +187,6 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_mx_mavlink.xml b/conf/airframes/examples/quadrotor_lisa_mx_mavlink.xml
index 8bc8632a6d..a67dc1758b 100644
--- a/conf/airframes/examples/quadrotor_lisa_mx_mavlink.xml
+++ b/conf/airframes/examples/quadrotor_lisa_mx_mavlink.xml
@@ -199,7 +199,6 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml
index d078bdfa1f..741ecc4c64 100644
--- a/conf/airframes/examples/quadrotor_lisa_s.xml
+++ b/conf/airframes/examples/quadrotor_lisa_s.xml
@@ -179,7 +179,6 @@
@@ -216,6 +215,5 @@
-
diff --git a/conf/airframes/examples/quadrotor_revo.xml b/conf/airframes/examples/quadrotor_revo.xml
index 0e250ff512..23c83668b5 100644
--- a/conf/airframes/examples/quadrotor_revo.xml
+++ b/conf/airframes/examples/quadrotor_revo.xml
@@ -161,7 +161,6 @@
diff --git a/conf/airframes/examples/trashcan.xml b/conf/airframes/examples/trashcan.xml
index 70a078e6e4..bc3d8c4603 100644
--- a/conf/airframes/examples/trashcan.xml
+++ b/conf/airframes/examples/trashcan.xml
@@ -150,7 +150,6 @@
<-->
-
diff --git a/conf/airframes/max/microjet_lisa_m.xml b/conf/airframes/max/microjet_lisa_m.xml
index 2d47a986ed..ed44ab59c6 100644
--- a/conf/airframes/max/microjet_lisa_m.xml
+++ b/conf/airframes/max/microjet_lisa_m.xml
@@ -213,7 +213,6 @@
diff --git a/conf/airframes/tudelft/aa_quadplane.xml b/conf/airframes/tudelft/aa_quadplane.xml
index 682e1aa1c9..cfbe4ebc3f 100644
--- a/conf/airframes/tudelft/aa_quadplane.xml
+++ b/conf/airframes/tudelft/aa_quadplane.xml
@@ -212,7 +212,6 @@
diff --git a/conf/airframes/tudelft/ardrone2_indi.xml b/conf/airframes/tudelft/ardrone2_indi.xml
index 627b9879c6..bd5fd162e3 100644
--- a/conf/airframes/tudelft/ardrone2_indi.xml
+++ b/conf/airframes/tudelft/ardrone2_indi.xml
@@ -32,7 +32,6 @@
-
@@ -184,7 +183,6 @@
-
diff --git a/conf/airframes/tudelft/ardrone2_opticflow.xml b/conf/airframes/tudelft/ardrone2_opticflow.xml
index da6700a4f4..0a61910a41 100644
--- a/conf/airframes/tudelft/ardrone2_opticflow.xml
+++ b/conf/airframes/tudelft/ardrone2_opticflow.xml
@@ -29,7 +29,6 @@
-
@@ -192,7 +191,6 @@
diff --git a/conf/airframes/tudelft/ardrone2_optitrack.xml b/conf/airframes/tudelft/ardrone2_optitrack.xml
index 4d0aa23e65..13536ac6ed 100644
--- a/conf/airframes/tudelft/ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/ardrone2_optitrack.xml
@@ -185,7 +185,6 @@
-
diff --git a/conf/airframes/tudelft/bebop2_indi.xml b/conf/airframes/tudelft/bebop2_indi.xml
index 9df614cd27..fba9f0912f 100644
--- a/conf/airframes/tudelft/bebop2_indi.xml
+++ b/conf/airframes/tudelft/bebop2_indi.xml
@@ -29,7 +29,6 @@
-
@@ -153,7 +152,6 @@
diff --git a/conf/airframes/tudelft/bebop2_no_damping.xml b/conf/airframes/tudelft/bebop2_no_damping.xml
index b155397acb..4fc9039a7a 100644
--- a/conf/airframes/tudelft/bebop2_no_damping.xml
+++ b/conf/airframes/tudelft/bebop2_no_damping.xml
@@ -27,7 +27,6 @@
-
@@ -176,7 +175,6 @@
diff --git a/conf/airframes/tudelft/bebop2_opticflow.xml b/conf/airframes/tudelft/bebop2_opticflow.xml
index 6977594c5d..9fc4dcea00 100644
--- a/conf/airframes/tudelft/bebop2_opticflow.xml
+++ b/conf/airframes/tudelft/bebop2_opticflow.xml
@@ -26,7 +26,6 @@
-->
@@ -212,7 +211,6 @@
diff --git a/conf/airframes/tudelft/bebop2_optitrack.xml b/conf/airframes/tudelft/bebop2_optitrack.xml
index 180bcfce17..8d631bcfef 100644
--- a/conf/airframes/tudelft/bebop2_optitrack.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack.xml
@@ -30,7 +30,6 @@
-
@@ -198,7 +197,6 @@
diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
index d59359f66a..34c1a58372 100644
--- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -21,7 +21,6 @@
-
@@ -220,7 +219,6 @@
diff --git a/conf/airframes/tudelft/bebop2_undistort_front.xml b/conf/airframes/tudelft/bebop2_undistort_front.xml
index 4c7985faa8..fa7437da35 100644
--- a/conf/airframes/tudelft/bebop2_undistort_front.xml
+++ b/conf/airframes/tudelft/bebop2_undistort_front.xml
@@ -21,7 +21,6 @@
-
@@ -208,7 +207,6 @@
diff --git a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
index eb1f5fd9c9..aac19bb1df 100644
--- a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
+++ b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
@@ -48,7 +48,6 @@
-
@@ -253,7 +252,6 @@
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid.xml b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
index 18b3658bad..e016d13f0c 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
@@ -247,7 +247,6 @@
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
index 9c29079b2c..f57fe0d2ec 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
@@ -265,7 +265,6 @@
diff --git a/conf/airframes/tudelft/bebop_flip.xml b/conf/airframes/tudelft/bebop_flip.xml
index 05914c8038..5ec7389121 100644
--- a/conf/airframes/tudelft/bebop_flip.xml
+++ b/conf/airframes/tudelft/bebop_flip.xml
@@ -31,7 +31,6 @@
-
@@ -203,7 +202,6 @@
diff --git a/conf/airframes/tudelft/bebop_indi.xml b/conf/airframes/tudelft/bebop_indi.xml
index e33525f41f..b4be780759 100644
--- a/conf/airframes/tudelft/bebop_indi.xml
+++ b/conf/airframes/tudelft/bebop_indi.xml
@@ -38,7 +38,6 @@
-
@@ -191,7 +190,6 @@
diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
index 7dbce7b3e6..740baef026 100644
--- a/conf/airframes/tudelft/bebop_indi_actuators.xml
+++ b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -38,7 +38,6 @@
-
@@ -165,7 +164,6 @@
diff --git a/conf/airframes/tudelft/bebop_mav_course_exercise.xml b/conf/airframes/tudelft/bebop_mav_course_exercise.xml
index 253b5f5198..07efb1dc87 100644
--- a/conf/airframes/tudelft/bebop_mav_course_exercise.xml
+++ b/conf/airframes/tudelft/bebop_mav_course_exercise.xml
@@ -248,7 +248,6 @@
diff --git a/conf/airframes/tudelft/bebop_mavlink.xml b/conf/airframes/tudelft/bebop_mavlink.xml
index e19998f940..b1b2692524 100644
--- a/conf/airframes/tudelft/bebop_mavlink.xml
+++ b/conf/airframes/tudelft/bebop_mavlink.xml
@@ -35,7 +35,6 @@
-
@@ -218,7 +217,6 @@
diff --git a/conf/airframes/tudelft/bebop_opticflow.xml b/conf/airframes/tudelft/bebop_opticflow.xml
index 1343cdbe90..93b24cdafd 100644
--- a/conf/airframes/tudelft/bebop_opticflow.xml
+++ b/conf/airframes/tudelft/bebop_opticflow.xml
@@ -30,7 +30,6 @@
-
@@ -195,7 +194,6 @@
diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor.xml b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
index 27ac91584e..f3321a7ed6 100644
--- a/conf/airframes/tudelft/bebop_opticflow_indoor.xml
+++ b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
@@ -186,7 +186,6 @@
diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
index 35511a6a6a..d3d72e0861 100644
--- a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
+++ b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
@@ -216,7 +216,6 @@ pyramid level 2: 21 fps average, min=11fps
diff --git a/conf/airframes/tudelft/bebop_optitrack.xml b/conf/airframes/tudelft/bebop_optitrack.xml
index e73549bb2d..69aac036a8 100644
--- a/conf/airframes/tudelft/bebop_optitrack.xml
+++ b/conf/airframes/tudelft/bebop_optitrack.xml
@@ -158,7 +158,6 @@
diff --git a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
index 5096947e52..5142fa78ff 100644
--- a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
+++ b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
@@ -261,7 +261,6 @@
diff --git a/conf/airframes/tudelft/chouchou_lisa_s.xml b/conf/airframes/tudelft/chouchou_lisa_s.xml
index 68f69f7f14..d9b90c627b 100644
--- a/conf/airframes/tudelft/chouchou_lisa_s.xml
+++ b/conf/airframes/tudelft/chouchou_lisa_s.xml
@@ -156,7 +156,6 @@
@@ -189,6 +188,5 @@
-
diff --git a/conf/airframes/tudelft/cx10.xml b/conf/airframes/tudelft/cx10.xml
index 019400bbb0..7119656fab 100644
--- a/conf/airframes/tudelft/cx10.xml
+++ b/conf/airframes/tudelft/cx10.xml
@@ -226,6 +226,5 @@
-
diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml
index 9510baec6c..cc8b74dbfe 100644
--- a/conf/airframes/tudelft/cyfoam.xml
+++ b/conf/airframes/tudelft/cyfoam.xml
@@ -176,7 +176,6 @@
@@ -265,13 +264,13 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/delfly_lisas.xml b/conf/airframes/tudelft/delfly_lisas.xml
index 972fc85ebc..9a60fb9b7d 100644
--- a/conf/airframes/tudelft/delfly_lisas.xml
+++ b/conf/airframes/tudelft/delfly_lisas.xml
@@ -64,7 +64,6 @@
-
@@ -236,7 +235,6 @@
diff --git a/conf/airframes/tudelft/delfly_nimble.xml b/conf/airframes/tudelft/delfly_nimble.xml
index 8d17c17a2c..5b49342492 100644
--- a/conf/airframes/tudelft/delfly_nimble.xml
+++ b/conf/airframes/tudelft/delfly_nimble.xml
@@ -259,7 +259,6 @@
diff --git a/conf/airframes/tudelft/disco_modified.xml b/conf/airframes/tudelft/disco_modified.xml
index 4047a8f05a..1a9c10403f 100644
--- a/conf/airframes/tudelft/disco_modified.xml
+++ b/conf/airframes/tudelft/disco_modified.xml
@@ -189,7 +189,6 @@
-
diff --git a/conf/airframes/tudelft/fan_demo.xml b/conf/airframes/tudelft/fan_demo.xml
index 56dde27a4d..06e598120a 100644
--- a/conf/airframes/tudelft/fan_demo.xml
+++ b/conf/airframes/tudelft/fan_demo.xml
@@ -30,7 +30,6 @@
-
@@ -180,7 +179,6 @@
diff --git a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
index ecce76fba9..37688c06a4 100644
--- a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
@@ -191,7 +191,6 @@ ARDrone2 with optical_flow landing.
-
diff --git a/conf/airframes/tudelft/iris_indi.xml b/conf/airframes/tudelft/iris_indi.xml
index ddfb43a529..33a06375f7 100644
--- a/conf/airframes/tudelft/iris_indi.xml
+++ b/conf/airframes/tudelft/iris_indi.xml
@@ -51,7 +51,6 @@
-
diff --git a/conf/airframes/tudelft/ladybird_lisa_mxs.xml b/conf/airframes/tudelft/ladybird_lisa_mxs.xml
index c66f518f3e..5c7b8d6ceb 100644
--- a/conf/airframes/tudelft/ladybird_lisa_mxs.xml
+++ b/conf/airframes/tudelft/ladybird_lisa_mxs.xml
@@ -62,7 +62,6 @@
-
@@ -196,7 +195,6 @@
-
diff --git a/conf/airframes/tudelft/ladylisa_bluegiga_stereoboard.xml b/conf/airframes/tudelft/ladylisa_bluegiga_stereoboard.xml
index 0c9db5d9bf..5c8fd95e94 100644
--- a/conf/airframes/tudelft/ladylisa_bluegiga_stereoboard.xml
+++ b/conf/airframes/tudelft/ladylisa_bluegiga_stereoboard.xml
@@ -194,7 +194,6 @@
@@ -234,7 +233,6 @@
-
diff --git a/conf/airframes/tudelft/logo600.xml b/conf/airframes/tudelft/logo600.xml
index 09f41849b1..70932e1e7a 100644
--- a/conf/airframes/tudelft/logo600.xml
+++ b/conf/airframes/tudelft/logo600.xml
@@ -229,7 +229,6 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
diff --git a/conf/airframes/tudelft/mavtec5.xml b/conf/airframes/tudelft/mavtec5.xml
index 381a586bc1..18d372344c 100644
--- a/conf/airframes/tudelft/mavtec5.xml
+++ b/conf/airframes/tudelft/mavtec5.xml
@@ -206,7 +206,6 @@
diff --git a/conf/airframes/tudelft/mentor.xml b/conf/airframes/tudelft/mentor.xml
index 7c853e5314..d3b5e28bb9 100644
--- a/conf/airframes/tudelft/mentor.xml
+++ b/conf/airframes/tudelft/mentor.xml
@@ -223,7 +223,6 @@
-
diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index ad7e066ffe..9b74f37769 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -49,7 +49,7 @@
-
+
@@ -85,20 +85,19 @@
-
-
+
-
+
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
@@ -338,7 +340,6 @@
diff --git a/conf/airframes/tudelft/nederdrone4_tem.xml b/conf/airframes/tudelft/nederdrone4_tem.xml
index 6e81afb1fb..bcab5abc5b 100644
--- a/conf/airframes/tudelft/nederdrone4_tem.xml
+++ b/conf/airframes/tudelft/nederdrone4_tem.xml
@@ -79,7 +79,6 @@
-
diff --git a/conf/airframes/tudelft/nederdrone5.xml b/conf/airframes/tudelft/nederdrone5.xml
index 07b12780b9..ae0441e873 100644
--- a/conf/airframes/tudelft/nederdrone5.xml
+++ b/conf/airframes/tudelft/nederdrone5.xml
@@ -82,7 +82,6 @@
-
diff --git a/conf/airframes/tudelft/nederquad.xml b/conf/airframes/tudelft/nederquad.xml
index 461a222c3e..e58703e113 100644
--- a/conf/airframes/tudelft/nederquad.xml
+++ b/conf/airframes/tudelft/nederquad.xml
@@ -81,7 +81,6 @@
-
diff --git a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
index 9b546cdd10..c487f31cc1 100644
--- a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
+++ b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
@@ -69,7 +69,6 @@
-
@@ -288,7 +287,6 @@
-
diff --git a/conf/airframes/tudelft/quadthopter.xml b/conf/airframes/tudelft/quadthopter.xml
index 861031121e..2f2b6d0416 100644
--- a/conf/airframes/tudelft/quadthopter.xml
+++ b/conf/airframes/tudelft/quadthopter.xml
@@ -169,7 +169,6 @@
diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml
index 6776f8f793..e4049708dc 100644
--- a/conf/airframes/tudelft/robird.xml
+++ b/conf/airframes/tudelft/robird.xml
@@ -49,7 +49,6 @@ Flapping wing frame equiped with
-
diff --git a/conf/airframes/tudelft/splash4.xml b/conf/airframes/tudelft/splash4.xml
index 83622f539f..03b849e5f6 100644
--- a/conf/airframes/tudelft/splash4.xml
+++ b/conf/airframes/tudelft/splash4.xml
@@ -100,7 +100,6 @@
-
diff --git a/conf/airframes/tudelft/splash_pixracer.xml b/conf/airframes/tudelft/splash_pixracer.xml
index c9b09cfb76..44ad3e4f5d 100644
--- a/conf/airframes/tudelft/splash_pixracer.xml
+++ b/conf/airframes/tudelft/splash_pixracer.xml
@@ -54,7 +54,6 @@
-
@@ -210,7 +209,6 @@
diff --git a/conf/airframes/tudelft/splash_px4.xml b/conf/airframes/tudelft/splash_px4.xml
index 7efe962454..0598deff84 100644
--- a/conf/airframes/tudelft/splash_px4.xml
+++ b/conf/airframes/tudelft/splash_px4.xml
@@ -59,7 +59,6 @@
-
diff --git a/conf/airframes/tudelft/walkera_genius_v1.xml b/conf/airframes/tudelft/walkera_genius_v1.xml
index 5f4748e0e2..89b5b130da 100644
--- a/conf/airframes/tudelft/walkera_genius_v1.xml
+++ b/conf/airframes/tudelft/walkera_genius_v1.xml
@@ -172,6 +172,5 @@
-
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index 8a6e35a5c8..b1edd90266 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
settings="settings/rotorcraft_basic.xml"
- settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
+ settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
+
+
diff --git a/conf/modules/imu_apogee.xml b/conf/modules/imu_apogee.xml
index 0d5274b7f8..d641ab8d85 100644
--- a/conf/modules/imu_apogee.xml
+++ b/conf/modules/imu_apogee.xml
@@ -26,8 +26,6 @@
-
-
diff --git a/conf/modules/imu_ardrone2.xml b/conf/modules/imu_ardrone2.xml
index 748727917e..28f2fd972c 100644
--- a/conf/modules/imu_ardrone2.xml
+++ b/conf/modules/imu_ardrone2.xml
@@ -26,8 +26,6 @@
-
-
diff --git a/conf/modules/imu_aspirin_common.xml b/conf/modules/imu_aspirin_common.xml
index 3541ea7592..8d18b1dc15 100644
--- a/conf/modules/imu_aspirin_common.xml
+++ b/conf/modules/imu_aspirin_common.xml
@@ -45,7 +45,6 @@
-
diff --git a/conf/modules/imu_aspirin_i2c_common.xml b/conf/modules/imu_aspirin_i2c_common.xml
index 1436f9cebf..314d40565e 100644
--- a/conf/modules/imu_aspirin_i2c_common.xml
+++ b/conf/modules/imu_aspirin_i2c_common.xml
@@ -37,8 +37,6 @@
-
-
diff --git a/conf/modules/imu_aspirin_v2_common.xml b/conf/modules/imu_aspirin_v2_common.xml
index 763599ec75..3091c9b750 100644
--- a/conf/modules/imu_aspirin_v2_common.xml
+++ b/conf/modules/imu_aspirin_v2_common.xml
@@ -41,8 +41,6 @@
-
-
diff --git a/conf/modules/imu_bebop.xml b/conf/modules/imu_bebop.xml
index b409f7321a..b73be62beb 100644
--- a/conf/modules/imu_bebop.xml
+++ b/conf/modules/imu_bebop.xml
@@ -36,8 +36,6 @@
-
-
@@ -47,7 +45,6 @@
-
diff --git a/conf/modules/imu_bmi088_i2c.xml b/conf/modules/imu_bmi088_i2c.xml
index 19c885bb9a..dcf3746c29 100644
--- a/conf/modules/imu_bmi088_i2c.xml
+++ b/conf/modules/imu_bmi088_i2c.xml
@@ -36,13 +36,10 @@
-
-
-
diff --git a/conf/modules/imu_common.xml b/conf/modules/imu_common.xml
index d73250e262..001c60c2e2 100644
--- a/conf/modules/imu_common.xml
+++ b/conf/modules/imu_common.xml
@@ -4,6 +4,8 @@
Common part for all IMUs.
+
+ This takes the IMU_GYRO_RAW, IMU_ACCEL_RAW and IMU_MAG_RAW ABI messages as input.
@@ -16,6 +18,9 @@
+
+ electrical
+
diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml
new file mode 100644
index 0000000000..eeee20fd29
--- /dev/null
+++ b/conf/modules/imu_cube.xml
@@ -0,0 +1,54 @@
+
+
+
+
+
+ IMU driver for the sensors inside the cube autopilots
+ - IMU1: ICM20948 (non-isolated)
+ - IMU2: ICM20602 (isolated)
+ - IMU3: ICM20649 (isolated)
+
+
+
+ spi_master,imu_common
+ imu
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/imu_disco.xml b/conf/modules/imu_disco.xml
index 132b3ca25b..6c864a1c7a 100644
--- a/conf/modules/imu_disco.xml
+++ b/conf/modules/imu_disco.xml
@@ -33,13 +33,11 @@
-
-
diff --git a/conf/modules/imu_mpu6000.xml b/conf/modules/imu_mpu6000.xml
index e35ace8d22..2c34a9781f 100644
--- a/conf/modules/imu_mpu6000.xml
+++ b/conf/modules/imu_mpu6000.xml
@@ -34,13 +34,10 @@
-
-
-
diff --git a/conf/modules/imu_mpu6000_hmc5883.xml b/conf/modules/imu_mpu6000_hmc5883.xml
index c7615fb763..686f882631 100644
--- a/conf/modules/imu_mpu6000_hmc5883.xml
+++ b/conf/modules/imu_mpu6000_hmc5883.xml
@@ -56,14 +56,11 @@
-
-
-
diff --git a/conf/modules/imu_mpu60x0_i2c.xml b/conf/modules/imu_mpu60x0_i2c.xml
index 5f7feeb1d7..5eb49f8d53 100644
--- a/conf/modules/imu_mpu60x0_i2c.xml
+++ b/conf/modules/imu_mpu60x0_i2c.xml
@@ -32,8 +32,6 @@
-
-
diff --git a/conf/modules/imu_mpu9250_i2c.xml b/conf/modules/imu_mpu9250_i2c.xml
index 9538ff027b..2f5759bfa1 100644
--- a/conf/modules/imu_mpu9250_i2c.xml
+++ b/conf/modules/imu_mpu9250_i2c.xml
@@ -37,14 +37,11 @@
-
-
-
diff --git a/conf/modules/imu_mpu9250_spi.xml b/conf/modules/imu_mpu9250_spi.xml
index fdc4c1685b..8c6c3b864f 100644
--- a/conf/modules/imu_mpu9250_spi.xml
+++ b/conf/modules/imu_mpu9250_spi.xml
@@ -44,13 +44,10 @@
-
-
-
diff --git a/conf/modules/imu_nps.xml b/conf/modules/imu_nps.xml
index d92aeceb2c..280f15bffc 100644
--- a/conf/modules/imu_nps.xml
+++ b/conf/modules/imu_nps.xml
@@ -17,7 +17,6 @@
-
diff --git a/conf/modules/imu_openpilot_revo.xml b/conf/modules/imu_openpilot_revo.xml
index 7526298997..8ae7148aeb 100644
--- a/conf/modules/imu_openpilot_revo.xml
+++ b/conf/modules/imu_openpilot_revo.xml
@@ -57,8 +57,6 @@
-
-
diff --git a/conf/modules/imu_openpilot_revo_nano.xml b/conf/modules/imu_openpilot_revo_nano.xml
index 0fbb8fecdb..a12abc5ee2 100644
--- a/conf/modules/imu_openpilot_revo_nano.xml
+++ b/conf/modules/imu_openpilot_revo_nano.xml
@@ -42,8 +42,6 @@
-
-
diff --git a/conf/modules/imu_px4fmu_v1.7.xml b/conf/modules/imu_px4fmu_v1.7.xml
index d55ed19947..b7d258f6d8 100644
--- a/conf/modules/imu_px4fmu_v1.7.xml
+++ b/conf/modules/imu_px4fmu_v1.7.xml
@@ -29,14 +29,11 @@
-
-
-
diff --git a/conf/modules/imu_px4fmu_v2.4.xml b/conf/modules/imu_px4fmu_v2.4.xml
index f1e7824e16..c02bfe4d96 100644
--- a/conf/modules/imu_px4fmu_v2.4.xml
+++ b/conf/modules/imu_px4fmu_v2.4.xml
@@ -37,14 +37,11 @@
-
-
-
diff --git a/conf/modules/imu_vectornav.xml b/conf/modules/imu_vectornav.xml
index 7471a1163a..e7e10b73f3 100644
--- a/conf/modules/imu_vectornav.xml
+++ b/conf/modules/imu_vectornav.xml
@@ -31,7 +31,5 @@
-
-
diff --git a/conf/modules/imu_xsens.xml b/conf/modules/imu_xsens.xml
index bb8fbc137d..f530275123 100644
--- a/conf/modules/imu_xsens.xml
+++ b/conf/modules/imu_xsens.xml
@@ -33,9 +33,7 @@
-
-
diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index da39dd8930..1520534b55 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -79,7 +79,7 @@
-
+
diff --git a/conf/modules/ins_nps.xml b/conf/modules/ins_nps.xml
index f8a9aaded0..351f3c142b 100644
--- a/conf/modules/ins_nps.xml
+++ b/conf/modules/ins_nps.xml
@@ -19,12 +19,13 @@
-
+
+
diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml
index efb74ad442..34b7906021 100644
--- a/conf/modules/mag_hmc5843.xml
+++ b/conf/modules/mag_hmc5843.xml
@@ -6,7 +6,7 @@
- i2c
+ i2c,@imu
mag
diff --git a/conf/modules/mag_hmc58xx.xml b/conf/modules/mag_hmc58xx.xml
index 26fbbafdd4..ba33838afe 100644
--- a/conf/modules/mag_hmc58xx.xml
+++ b/conf/modules/mag_hmc58xx.xml
@@ -23,7 +23,7 @@
- i2c
+ i2c,@imu
mag
diff --git a/conf/modules/mag_ist8310.xml b/conf/modules/mag_ist8310.xml
index fc0f6eaab9..cc792434b2 100644
--- a/conf/modules/mag_ist8310.xml
+++ b/conf/modules/mag_ist8310.xml
@@ -21,7 +21,7 @@
- i2c
+ i2c,@imu
mag
diff --git a/conf/modules/mag_lis3mdl.xml b/conf/modules/mag_lis3mdl.xml
index cf8c583054..cb1c62b4c6 100644
--- a/conf/modules/mag_lis3mdl.xml
+++ b/conf/modules/mag_lis3mdl.xml
@@ -21,7 +21,7 @@
- i2c
+ i2c,@imu
mag
diff --git a/conf/modules/mag_pitot_uart.xml b/conf/modules/mag_pitot_uart.xml
index 0b583e8b32..cf2a7fd91e 100644
--- a/conf/modules/mag_pitot_uart.xml
+++ b/conf/modules/mag_pitot_uart.xml
@@ -10,7 +10,7 @@
- uart
+ uart,@imu
mag
diff --git a/conf/modules/mag_rm3100.xml b/conf/modules/mag_rm3100.xml
index 5255366bf0..f4aeb88c74 100644
--- a/conf/modules/mag_rm3100.xml
+++ b/conf/modules/mag_rm3100.xml
@@ -23,7 +23,7 @@
- i2c
+ i2c,@imu
mag
diff --git a/conf/modules/send_imu_mag_current.xml b/conf/modules/send_imu_mag_current.xml
deleted file mode 100644
index 98789e4b1b..0000000000
--- a/conf/modules/send_imu_mag_current.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
- Enables sending of IMU_MAG_CURRENT_CALIBRATION message.
- For calibration of changing magnetometer offset due to estimated or measured current.
-
-
-
-
-
-
-
-
diff --git a/conf/simulator/nps/nps_sensors_params_ardrone2.h b/conf/simulator/nps/nps_sensors_params_ardrone2.h
deleted file mode 100644
index 4dc6030b2b..0000000000
--- a/conf/simulator/nps/nps_sensors_params_ardrone2.h
+++ /dev/null
@@ -1,173 +0,0 @@
-/*
- * Copyright (C) 2013 Felix Ruess, Gautier Hattenberger, JP Condomines
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef NPS_SENSORS_PARAMS_H
-#define NPS_SENSORS_PARAMS_H
-
-#include "generated/airframe.h"
-#include "modules/imu/imu.h"
-
-
-#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
-#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
-#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
-
-/*
- * Accelerometer
- */
-/* navdata has 12 bit resolution */
-#define NPS_ACCEL_MIN 0
-#define NPS_ACCEL_MAX 4095
-/* ms-2 */
-/* aka 2^10/ACCEL_X_SENS */
-#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)
-#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)
-#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS)
-
-#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
-/* m2s-4 */
-#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2
-#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
-#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2
-/* ms-2 */
-#define NPS_ACCEL_BIAS_X 0
-#define NPS_ACCEL_BIAS_Y 0
-#define NPS_ACCEL_BIAS_Z 0
-/* s */
-#define NPS_ACCEL_DT (1./200.)
-
-
-
-/*
- * Gyrometer
- */
-/* navdata has 12 bit resolution */
-#define NPS_GYRO_MIN -2047
-#define NPS_GYRO_MAX 2047
-
-/* 2^12/GYRO_X_SENS */
-#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)
-#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)
-#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS)
-
-#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
-#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
-#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
-
-#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
-#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
-#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
-
-#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
-
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
-/* s */
-#define NPS_GYRO_DT (1./200.)
-
-
-
-/*
- * Magnetometer
- */
- /* HMC5883 has 12 bit resolution */
-#define NPS_MAG_MIN -2047
-#define NPS_MAG_MAX 2047
-
-#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
-#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
-#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
-
-#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)
-#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)
-#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)
-
-#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
-#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
-#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
-
-#define NPS_MAG_NOISE_STD_DEV_X 2e-3
-#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
-#define NPS_MAG_NOISE_STD_DEV_Z 2e-3
-
-#define NPS_MAG_DT (1./60.)
-
-
-
-/*
- * Barometer
- */
-/* m */
-#define NPS_BARO_DT (1./5.)
-#define NPS_BARO_NOISE_STD_DEV 5.e-2
-
-/*
- * Sonar (distance to ground in meters)
- */
-#define NPS_SONAR_DT (1./10.)
-#define NPS_SONAR_NOISE_STD_DEV 0.01
-
-
-/*
- * GPS
- */
-
-#ifndef GPS_PERFECT
-#define GPS_PERFECT 1
-#endif
-
-#if GPS_PERFECT
-
-#define NPS_GPS_SPEED_NOISE_STD_DEV 0.
-#define NPS_GPS_SPEED_LATENCY 0.
-#define NPS_GPS_POS_NOISE_STD_DEV 0.001
-#define NPS_GPS_POS_BIAS_INITIAL_X 0.
-#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
-#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
-#define NPS_GPS_POS_LATENCY 0.
-
-#else
-
-#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5
-#define NPS_GPS_SPEED_LATENCY 0.2
-#define NPS_GPS_POS_NOISE_STD_DEV 2
-#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
-#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
-#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
-#define NPS_GPS_POS_LATENCY 0.2
-
-#endif /* GPS_PERFECT */
-
-#define NPS_GPS_DT (1./5.)
-
-#endif /* NPS_SENSORS_PARAMS_H */
-
diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h
index eb105a757c..69471f1653 100644
--- a/conf/simulator/nps/nps_sensors_params_default.h
+++ b/conf/simulator/nps/nps_sensors_params_default.h
@@ -25,10 +25,15 @@
#include "generated/airframe.h"
#include "modules/imu/imu.h"
-
+#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
+#define NPS_BODY_TO_IMU_PHI 0
+#define NPS_BODY_TO_IMU_THETA 0
+#define NPS_BODY_TO_IMU_PSI 0
+#else
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
+#endif
// try to determine propagate frequency
#if defined AHRS_PROPAGATE_FREQUENCY
@@ -38,7 +43,8 @@
#elif defined PERIODIC_FREQUENCY
#define NPS_PROPAGATE PERIODIC_FREQUENCY
#else
-#define 512. // historical magic number
+#warning "NPS_PROPAGATE not defined, using default 512Hz"
+#define NPS_PROPAGATE 512.
#endif
/*
@@ -53,13 +59,16 @@
#endif
/* ms-2 */
/* aka 2^10/ACCEL_X_SENS */
-#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
-#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
-#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
+#define NPS_ACCEL_SENSITIVITY_NUM 981
+#define NPS_ACCEL_SENSITIVITY_DEN 200
+#define NPS_ACCEL_SENSITIVITY ((float)NPS_ACCEL_SENSITIVITY_NUM / (float)NPS_ACCEL_SENSITIVITY_DEN)
+#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
+#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
+#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
-#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_X 0
+#define NPS_ACCEL_NEUTRAL_Y 0
+#define NPS_ACCEL_NEUTRAL_Z 0
/* m2s-4 */
#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2
#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
@@ -87,21 +96,24 @@
#endif
/* 2^12/GYRO_X_SENS */
-#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
-#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
-#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
+#define NPS_GYRO_SENSITIVITY_NUM 36542
+#define NPS_GYRO_SENSITIVITY_DEN 8383
+#define NPS_GYRO_SENSITIVITY ((float)NPS_GYRO_SENSITIVITY_NUM / (float)NPS_GYRO_SENSITIVITY_DEN)
+#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
+#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
+#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
-#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
-#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
-#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
+#define NPS_GYRO_NEUTRAL_P 0
+#define NPS_GYRO_NEUTRAL_Q 0
+#define NPS_GYRO_NEUTRAL_R 0
#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
-#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg(0.0)
+#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg(0.0)
+#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg(0.0)
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
@@ -128,13 +140,16 @@
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
-#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
-#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
-#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
+#define NPS_MAG_SENSITIVITY_NUM 1
+#define NPS_MAG_SENSITIVITY_DEN 1
+#define NPS_MAG_SENSITIVITY ((float)NPS_MAG_SENSITIVITY_NUM / (float)NPS_MAG_SENSITIVITY_DEN)
+#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
+#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
+#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
-#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
-#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
-#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
+#define NPS_MAG_NEUTRAL_X 0
+#define NPS_MAG_NEUTRAL_Y 0
+#define NPS_MAG_NEUTRAL_Z 0
#define NPS_MAG_NOISE_STD_DEV_X 2e-3
#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
diff --git a/conf/simulator/nps/nps_sensors_params_noisy.h b/conf/simulator/nps/nps_sensors_params_noisy.h
index 121969874b..2cc2d9298f 100644
--- a/conf/simulator/nps/nps_sensors_params_noisy.h
+++ b/conf/simulator/nps/nps_sensors_params_noisy.h
@@ -38,7 +38,8 @@
#elif defined PERIODIC_FREQUENCY
#define NPS_PROPAGATE PERIODIC_FREQUENCY
#else
-#define 512. // historical magic number
+#warning "NPS_PROPAGATE not defined, using default 512Hz"
+#define NPS_PROPAGATE 512.
#endif
/*
@@ -53,13 +54,16 @@
#endif
/* ms-2 */
/* aka 2^10/ACCEL_X_SENS */
-#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
-#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
-#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
+#define NPS_ACCEL_SENSITIVITY_NUM 981
+#define NPS_ACCEL_SENSITIVITY_DEN 200
+#define NPS_ACCEL_SENSITIVITY ((float)NPS_ACCEL_SENSITIVITY_NUM / (float)NPS_ACCEL_SENSITIVITY_DEN)
+#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
+#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
+#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY)
-#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
-#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_X 0
+#define NPS_ACCEL_NEUTRAL_Y 0
+#define NPS_ACCEL_NEUTRAL_Z 0
/* m2s-4 */
#define NPS_ACCEL_NOISE_STD_DEV_X .5
#define NPS_ACCEL_NOISE_STD_DEV_Y .5
@@ -86,22 +90,25 @@
#define NPS_GYRO_MAX 65536
#endif
-/* 2^12/GYRO_X_SENS */
-#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
-#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
-#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
+/* 2^12/GYRO_X_SENS */
+#define NPS_GYRO_SENSITIVITY_NUM 36542
+#define NPS_GYRO_SENSITIVITY_DEN 8383
+#define NPS_GYRO_SENSITIVITY ((float)NPS_GYRO_SENSITIVITY_NUM / (float)NPS_GYRO_SENSITIVITY_DEN)
+#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
+#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
+#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY)
-#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
-#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
-#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
+#define NPS_GYRO_NEUTRAL_P 0
+#define NPS_GYRO_NEUTRAL_Q 0
+#define NPS_GYRO_NEUTRAL_R 0
#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(9.)
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(9.)
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(9.)
-#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg(0.0)
+#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg(0.0)
+#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg(0.0)
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
@@ -128,13 +135,16 @@
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
-#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
-#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
-#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
+#define NPS_MAG_SENSITIVITY_NUM 1
+#define NPS_MAG_SENSITIVITY_DEN 1
+#define NPS_MAG_SENSITIVITY ((float)NPS_MAG_SENSITIVITY_NUM / (float)NPS_MAG_SENSITIVITY_DEN)
+#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
+#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
+#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY)
-#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
-#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
-#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
+#define NPS_MAG_NEUTRAL_X 0
+#define NPS_MAG_NEUTRAL_Y 0
+#define NPS_MAG_NEUTRAL_Z 0
#define NPS_MAG_NOISE_STD_DEV_X 2e-1
#define NPS_MAG_NOISE_STD_DEV_Y 2e-1
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index 1d9fd6e392..f2d3480adf 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -50,14 +50,15 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index fd1e98c6f7..b50c37179e 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -44,11 +44,22 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
@@ -68,7 +79,7 @@
-
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 2657e4f9cd..da1ca13c6c 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -9,7 +9,6 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
- release="ac5eaa8da36b999a135f979e598930a9d40f6401"
/>
theta);
#else
float vdot = 0;
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index d254261c55..e6dc415a2e 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -108,11 +108,6 @@ void main_ap_init(void)
modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
-#if USE_IMU
- // send body_to_imu from here for now
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
-#endif
-
// Do a failsafe check first
autopilot_failsafe_checks();
diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c
index 77838e93a3..c254ceba6f 100644
--- a/sw/airborne/math/pprz_algebra_int.c
+++ b/sw/airborne/math/pprz_algebra_int.c
@@ -72,7 +72,7 @@ uint32_t int32_gcd(uint32_t a, uint32_t b)
/** Composition (multiplication) of two rotation matrices.
* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b
*/
-void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
+void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
{
m_a2c->m[0] = (m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6]) >> INT32_TRIG_FRAC;
m_a2c->m[1] = (m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7]) >> INT32_TRIG_FRAC;
@@ -88,7 +88,7 @@ void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct In
/** Composition (multiplication) of two rotation matrices.
* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c
*/
-void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
+void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c, const struct Int32RMat *m_b2c)
{
m_a2b->m[0] = (m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6]) >> INT32_TRIG_FRAC;
m_a2b->m[1] = (m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7]) >> INT32_TRIG_FRAC;
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 25a04747f1..fdd066ec60 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -352,14 +352,14 @@ static inline void int32_rmat_identity(struct Int32RMat *rm)
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
-extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b,
- struct Int32RMat *m_b2c);
+extern void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b,
+ const struct Int32RMat *m_b2c);
/** Composition (multiplication) of two rotation matrices.
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
*/
-extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c,
- struct Int32RMat *m_b2c);
+extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c,
+ const struct Int32RMat *m_b2c);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
diff --git a/sw/airborne/modules/ahrs/ahrs_aligner.c b/sw/airborne/modules/ahrs/ahrs_aligner.c
index 5bfa400649..75b10a68b8 100644
--- a/sw/airborne/modules/ahrs/ahrs_aligner.c
+++ b/sw/airborne/modules/ahrs/ahrs_aligner.c
@@ -65,16 +65,19 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
static void send_aligner(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
- &ahrs_aligner.lp_gyro.p,
- &ahrs_aligner.lp_gyro.q,
- &ahrs_aligner.lp_gyro.r,
- &imu.gyro.p,
- &imu.gyro.q,
- &imu.gyro.r,
- &ahrs_aligner.noise,
- &ahrs_aligner.low_noise_cnt,
- &ahrs_aligner.status);
+ struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false);
+ if(gyro != NULL) {
+ pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
+ &ahrs_aligner.lp_gyro.p,
+ &ahrs_aligner.lp_gyro.q,
+ &ahrs_aligner.lp_gyro.r,
+ &gyro->scaled.p,
+ &gyro->scaled.q,
+ &gyro->scaled.r,
+ &ahrs_aligner.noise,
+ &ahrs_aligner.low_noise_cnt,
+ &ahrs_aligner.status);
+ }
}
#endif
@@ -83,7 +86,7 @@ void ahrs_aligner_init(void)
ahrs_aligner_restart();
// for now: only bind to gyro message and still read from global imu struct
- AbiBindMsgIMU_GYRO_INT32(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_GYRO(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FILTER_ALIGNER, send_aligner);
@@ -111,12 +114,20 @@ void ahrs_aligner_restart(void)
void ahrs_aligner_run(void)
{
+ struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false);
+ struct imu_accel_t *accel = imu_get_accel(AHRS_ALIGNER_IMU_ID, false);
+ struct imu_mag_t *mag = imu_get_mag(AHRS_ALIGNER_IMU_ID, false);
- RATES_ADD(gyro_sum, imu.gyro);
- VECT3_ADD(accel_sum, imu.accel);
- VECT3_ADD(mag_sum, imu.mag);
+ // Could not find all sensors
+ if(gyro == NULL || accel == NULL)
+ return;
+
+ RATES_ADD(gyro_sum, gyro->scaled);
+ VECT3_ADD(accel_sum, accel->scaled);
+ if(mag != NULL)
+ VECT3_ADD(mag_sum, mag->scaled);
- ref_sensor_samples[samples_idx] = imu.accel.z;
+ ref_sensor_samples[samples_idx] = accel->scaled.z;
samples_idx++;
#ifdef AHRS_ALIGNER_LED
@@ -160,7 +171,7 @@ void ahrs_aligner_run(void)
LED_ON(AHRS_ALIGNER_LED);
#endif
uint32_t now_ts = get_sys_time_usec();
- AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro,
+ AbiSendMsgIMU_LOWPASSED(AHRS_ALIGNER_IMU_ID, now_ts, &ahrs_aligner.lp_gyro,
&ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
}
}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl.c b/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
index 1e53484a37..fc9d7d765f 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl.c
@@ -87,13 +87,10 @@ void ahrs_fc_init(void)
ahrs_fc.ltp_vel_norm_valid = false;
ahrs_fc.heading_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- float_quat_identity(&ahrs_fc.ltp_to_imu_quat);
-
- orientationSetIdentity(&ahrs_fc.body_to_imu);
- orientationSetIdentity(&ahrs_fc.ltp_to_body);
-
- FLOAT_RATES_ZERO(ahrs_fc.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ float_quat_identity(&ahrs_fc.ltp_to_body_quat);
+ float_rmat_identity(&ahrs_fc.ltp_to_body_rmat);
+ FLOAT_RATES_ZERO(ahrs_fc.body_rate);
/* set default filter cut-off frequency and damping */
ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA;
@@ -121,16 +118,16 @@ bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag);
+ ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_body_quat, lp_accel, lp_mag);
ahrs_fc.heading_aligned = true;
#else
/* Compute an initial orientation from accel and just set heading to zero */
- ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel);
+ ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_body_quat, lp_accel);
ahrs_fc.heading_aligned = false;
#endif
/* Convert initial orientation from quat to rotation matrix representations. */
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
/* used averaged gyro as initial value for bias */
ahrs_fc.gyro_bias = *lp_gyro;
@@ -151,9 +148,9 @@ void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
- FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), rates, alpha);
+ FLOAT_RATES_LIN_CMB(ahrs_fc.body_rate, ahrs_fc.body_rate, (1. - alpha), rates, alpha);
#else
- RATES_COPY(ahrs_fc.imu_rate, rates);
+ RATES_COPY(ahrs_fc.body_rate, rates);
#endif
/* add correction */
@@ -163,14 +160,14 @@ void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
- float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
- float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
- float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
+ float_rmat_integrate_fi(&ahrs_fc.ltp_to_body_rmat, &omega, dt);
+ float_rmat_reorthogonalize(&ahrs_fc.ltp_to_body_rmat);
+ float_quat_of_rmat(&ahrs_fc.ltp_to_body_quat, &ahrs_fc.ltp_to_body_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
- float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
- float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_quat_integrate(&ahrs_fc.ltp_to_body_quat, &omega, dt);
+ float_quat_normalize(&ahrs_fc.ltp_to_body_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
#endif
// increase accel and mag propagation counters
@@ -185,10 +182,10 @@ void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
return;
}
- /* last column of roation matrix = ltp z-axis in imu-frame */
- struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
+ /* last column of roation matrix = ltp z-axis in body-frame */
+ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 2),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 2),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
};
struct FloatVect3 imu_accel = *accel;
@@ -213,18 +210,11 @@ void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
/* assume tangential velocity along body x-axis */
{ahrs_fc.ltp_vel_norm, 0.0, 0.0};
#endif
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
- VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
+ VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_fc.body_rate, vel_tangential_body);
- /* convert centrifugal acceleration from body to imu frame */
- struct FloatVect3 acc_c_imu;
- float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
-
- /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
- VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_imu);
+ /* centrifugal acceleration subtract it from imu measurement to get a corrected measurement of the gravity vector */
+ VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_body);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel);
@@ -297,7 +287,7 @@ void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
{
struct FloatVect3 expected_imu;
- float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.mag_h);
+ float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_body_rmat, &ahrs_fc.mag_h);
struct FloatVect3 measured_imu = *mag;
struct FloatVect3 residual_imu;
@@ -333,7 +323,7 @@ void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
struct FloatVect3 measured_imu = *mag;
struct FloatVect3 measured_ltp;
- float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu);
+ float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_body_rmat, &measured_imu);
struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
@@ -349,7 +339,7 @@ void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
- float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
+ float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
/* Complementary filter proportional gain.
@@ -372,21 +362,21 @@ void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
{
/* project mag on local tangeant plane */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_rmat(<p_to_body_euler, &ahrs_fc.ltp_to_body_rmat);
- const float cphi = cosf(ltp_to_imu_euler.phi);
- const float sphi = sinf(ltp_to_imu_euler.phi);
- const float ctheta = cosf(ltp_to_imu_euler.theta);
- const float stheta = sinf(ltp_to_imu_euler.theta);
+ const float cphi = cosf(ltp_to_body_euler.phi);
+ const float sphi = sinf(ltp_to_body_euler.phi);
+ const float ctheta = cosf(ltp_to_body_euler.theta);
+ const float stheta = sinf(ltp_to_body_euler.theta);
const float mn = ctheta * mag->x + sphi * stheta * mag->y + cphi * stheta * mag->z;
const float me = 0. * mag->x + cphi * mag->y - sphi * mag->z;
- const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me +
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
- const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1),
- RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
+ const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0) * me +
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 1, 0) * mn;
+ const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 0),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 1),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 2, 2)
};
const float mag_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
@@ -431,14 +421,11 @@ void ahrs_fc_update_heading(float heading)
FLOAT_ANGLE_NORMALIZE(heading);
- ahrs_fc_recompute_ltp_to_body();
- struct FloatRMat *ltp_to_body_rmat = orientationGetRMat_f(&ahrs_fc.ltp_to_body);
-
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp = {
- RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
- RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 0),
+ RMAT_ELMT(ahrs_fc.ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
@@ -449,7 +436,7 @@ void ahrs_fc_update_heading(float heading)
};
struct FloatVect3 residual_imu;
- float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
+ float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_body_rmat, &residual_ltp);
const float heading_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
@@ -481,11 +468,11 @@ void ahrs_fc_realign_heading(float heading)
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
- ahrs_fc_recompute_ltp_to_body();
- struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
+ struct FloatQuat ltp_to_body_quat;
+ QUAT_COPY(ltp_to_body_quat, ahrs_fc.ltp_to_body_quat);
/* quaternion representing current heading only */
- struct FloatQuat q_h = *ltp_to_body_quat;
+ struct FloatQuat q_h = ltp_to_body_quat;
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
@@ -495,37 +482,10 @@ void ahrs_fc_realign_heading(float heading)
float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
/* correct current heading in body frame */
- struct FloatQuat q;
- float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat);
+ float_quat_comp_norm_shortest(&ahrs_fc.ltp_to_body_quat, &q_c, <p_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
- float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
- float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
+ float_rmat_of_quat(&ahrs_fc.ltp_to_body_rmat, &ahrs_fc.ltp_to_body_quat);
ahrs_fc.heading_aligned = true;
}
-
-void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i);
-
- if (!ahrs_fc.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_fc.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_fc.body_to_imu);
- ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu);
- }
-}
-
-void ahrs_fc_recompute_ltp_to_body(void)
-{
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
- struct FloatQuat ltp_to_body_quat;
- float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
- orientationSetQuat_f(&ahrs_fc.ltp_to_body, <p_to_body_quat);
-}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl.h b/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
index 0699c2bceb..e82eaa9991 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl.h
@@ -43,9 +43,9 @@ enum AhrsFCStatus {
struct AhrsFloatCmpl {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
- struct FloatRates imu_rate;
- struct FloatQuat ltp_to_imu_quat;
- struct FloatRMat ltp_to_imu_rmat;
+ struct FloatRates body_rate;
+ struct FloatQuat ltp_to_body_quat;
+ struct FloatRMat ltp_to_body_rmat;
bool correct_gravity; ///< enable gravity correction during coordinated turns
float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns
@@ -69,9 +69,6 @@ struct AhrsFloatCmpl {
uint16_t accel_cnt; ///< number of propagations since last accel update
uint16_t mag_cnt; ///< number of propagations since last mag update
- struct OrientationReps body_to_imu;
- struct OrientationReps ltp_to_body;
-
enum AhrsFCStatus status;
bool is_aligned;
};
@@ -79,9 +76,6 @@ struct AhrsFloatCmpl {
extern struct AhrsFloatCmpl ahrs_fc;
extern void ahrs_fc_init(void);
-extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i);
-extern void ahrs_fc_recompute_ltp_to_body(void);
extern bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_fc_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
index 7418e55d20..b2407ab70a 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
@@ -48,12 +48,12 @@ static void compute_body_orientation_and_rates(void);
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_fc.ltp_to_body_quat);
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&ahrs_fc_id);
}
@@ -68,22 +68,18 @@ static void send_bias(struct transport_tx *trans, struct link_device *dev)
static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
- struct Int32Eulers eulers_imu;
- EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
-
- /* get Eulers in int (body frame) */
- ahrs_fc_recompute_ltp_to_body();
- struct Int32Eulers *eulers_body = orientationGetEulers_i(&ahrs_fc.ltp_to_body);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_fc.ltp_to_body_quat);
+ struct Int32Eulers eulers_body;
+ EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- &eulers_imu.phi,
- &eulers_imu.theta,
- &eulers_imu.psi,
- &eulers_body->phi,
- &eulers_body->theta,
- &eulers_body->psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
&ahrs_fc_id);
}
@@ -131,7 +127,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -235,12 +230,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_fc_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_fc.mag_h = *h;
@@ -266,17 +255,9 @@ static bool ahrs_fc_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_fc_output_enabled) {
- /* recompute LTP to BODY quaternion */
- ahrs_fc_recompute_ltp_to_body();
- struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
/* Set state */
- stateSetNedToBodyQuat_f(ltp_to_body_quat);
-
- /* compute body rates */
- struct FloatRates body_rate;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
- stateSetBodyRates_f(&body_rate);
+ stateSetNedToBodyQuat_f(&ahrs_fc.ltp_to_body_quat);
+ stateSetBodyRates_f(&ahrs_fc.body_rate);
}
}
@@ -289,11 +270,10 @@ void ahrs_fc_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(AHRS_FC_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_GYRO(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(AHRS_FC_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_FC_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(AHRS_FC_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm.c b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
index 3c7d457fda..237ee50425 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm.c
@@ -101,13 +101,14 @@ void ahrs_dcm_init(void)
ahrs_dcm.status = AHRS_DCM_UNINIT;
ahrs_dcm.is_aligned = false;
- /* init ltp_to_imu euler with zero */
- FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler);
-
- FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
+ /* init ltp_to_body euler with zero */
+ FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_body_euler);
+ FLOAT_RATES_ZERO(ahrs_dcm.body_rate);
/* set inital filter dcm */
- set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
+ struct FloatRMat init_rmat;
+ float_rmat_identity(&init_rmat);
+ set_dcm_matrix_from_rmat(&init_rmat);
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
@@ -126,14 +127,14 @@ bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
#else
ahrs_float_get_quat_from_accel(&quat, lp_accel);
#endif
- float_eulers_of_quat(&ahrs_dcm.ltp_to_imu_euler, &quat);
+ float_eulers_of_quat(&ahrs_dcm.ltp_to_body_euler, &quat);
/* Convert initial orientation from quaternion to rotation matrix representations. */
- struct FloatRMat ltp_to_imu_rmat;
- float_rmat_of_quat(<p_to_imu_rmat, &quat);
+ struct FloatRMat ltp_to_body_rmat;
+ float_rmat_of_quat(<p_to_body_rmat, &quat);
/* set filter dcm */
- set_dcm_matrix_from_rmat(<p_to_imu_rmat);
+ set_dcm_matrix_from_rmat(<p_to_body_rmat);
/* use averaged gyro as initial value for bias */
ahrs_dcm.gyro_bias = *lp_gyro;
@@ -148,21 +149,21 @@ bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
{
/* unbias rate measurement */
- RATES_DIFF(ahrs_dcm.imu_rate, *gyro, ahrs_dcm.gyro_bias);
+ RATES_DIFF(ahrs_dcm.body_rate, *gyro, ahrs_dcm.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp = 0, dq = 0, dr = 0;
- dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
- dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
- dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
- dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
- dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
- dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
+ dp += ahrs_dcm.body_rate.q * IMU_GYRO_P_Q;
+ dp += ahrs_dcm.body_rate.r * IMU_GYRO_P_R;
+ dq += ahrs_dcm.body_rate.p * IMU_GYRO_Q_P;
+ dq += ahrs_dcm.body_rate.r * IMU_GYRO_Q_R;
+ dr += ahrs_dcm.body_rate.p * IMU_GYRO_R_P;
+ dr += ahrs_dcm.body_rate.q * IMU_GYRO_R_Q;
- ahrs_dcm.imu_rate.p += dp;
- ahrs_dcm.imu_rate.q += dq;
- ahrs_dcm.imu_rate.r += dr;
+ ahrs_dcm.body_rate.p += dp;
+ ahrs_dcm.body_rate.q += dq;
+ ahrs_dcm.body_rate.r += dr;
#endif
Matrix_update(dt);
@@ -234,10 +235,10 @@ MESSAGE("MAGNETOMETER FEEDBACK NOT TESTED YET")
float cos_pitch;
float sin_pitch;
- cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
- sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
- cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
- sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
+ cos_roll = cosf(ahrs_dcm.ltp_to_body_euler.phi);
+ sin_roll = sinf(ahrs_dcm.ltp_to_body_euler.phi);
+ cos_pitch = cosf(ahrs_dcm.ltp_to_body_euler.theta);
+ sin_pitch = sinf(ahrs_dcm.ltp_to_body_euler.theta);
// Pitch&Roll Compensation:
@@ -358,7 +359,9 @@ void Normalize(void)
// Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
- set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
+ struct FloatRMat init_rmat;
+ float_rmat_identity(&init_rmat);
+ set_dcm_matrix_from_rmat(&init_rmat);
problem = false;
}
}
@@ -485,7 +488,7 @@ void Drift_correction()
void Matrix_update(float dt)
{
- Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega[0], &ahrs_dcm.body_rate.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if OUTPUTMODE==1 // With corrected data (drift correction)
@@ -500,13 +503,13 @@ void Matrix_update(float dt)
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0] = 0;
- Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
- Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
- Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
+ Update_Matrix[0][1] = -dt * ahrs_dcm.body_rate.r; //-z
+ Update_Matrix[0][2] = dt * ahrs_dcm.body_rate.q; //y
+ Update_Matrix[1][0] = dt * ahrs_dcm.body_rate.r; //z
Update_Matrix[1][1] = 0;
- Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
- Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
- Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
+ Update_Matrix[1][2] = -dt * ahrs_dcm.body_rate.p;
+ Update_Matrix[2][0] = -dt * ahrs_dcm.body_rate.q;
+ Update_Matrix[2][1] = dt * ahrs_dcm.body_rate.p;
Update_Matrix[2][2] = 0;
#endif
@@ -522,28 +525,13 @@ void Matrix_update(float dt)
static void compute_ahrs_representations(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
- ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
- ahrs_dcm.ltp_to_imu_euler.psi = 0;
+ ahrs_dcm.ltp_to_body_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
+ ahrs_dcm.ltp_to_body_euler.theta = -asin((accel_float.x) / GRAVITY); // asin(acc_x)
+ ahrs_dcm.ltp_to_body_euler.psi = 0;
#else
- ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
- ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
- ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
- ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
+ ahrs_dcm.ltp_to_body_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
+ ahrs_dcm.ltp_to_body_euler.theta = -asin(DCM_Matrix[2][0]);
+ ahrs_dcm.ltp_to_body_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
+ ahrs_dcm.ltp_to_body_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
}
-
-void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_dcm_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_dcm.body_to_imu, q_b2i);
-
- if (!ahrs_dcm.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_dcm.ltp_to_imu_euler = *orientationGetEulers_f(&ahrs_dcm.body_to_imu);
- }
-}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm.h b/sw/airborne/modules/ahrs/ahrs_float_dcm.h
index e082f6ed74..db2f26f021 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm.h
@@ -38,8 +38,8 @@ struct AhrsFloatDCM {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
- struct FloatEulers ltp_to_imu_euler;
- struct FloatRates imu_rate;
+ struct FloatEulers ltp_to_body_euler;
+ struct FloatRates body_rate;
float gps_speed;
float gps_acceleration;
@@ -47,8 +47,6 @@ struct AhrsFloatDCM {
bool gps_course_valid;
uint8_t gps_age;
- struct OrientationReps body_to_imu;
-
enum AhrsDCMStatus status;
bool is_aligned;
};
@@ -84,8 +82,6 @@ extern float imu_health;
#endif
extern void ahrs_dcm_init(void);
-extern void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_dcm_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
index 37e6d0a28c..f54df181ed 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
@@ -82,7 +82,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event gps_ev;
@@ -156,12 +155,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
-}
-
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
@@ -181,16 +174,12 @@ static bool ahrs_dcm_enable_output(bool enable)
static void set_body_orientation_and_rates(void)
{
if (ahrs_dcm_output_enabled) {
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
-
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
- stateSetBodyRates_f(&body_rate);
-
- struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
- float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
- float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat);
+ /* Set the state */
+ stateSetBodyRates_f(&ahrs_dcm.body_rate);
+ /* Convert eulers to RMaat and set state */
+ struct FloatRMat ltp_to_body_rmat;
+ float_rmat_of_eulers(<p_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
stateSetNedToBodyRMat_f(<p_to_body_rmat);
}
}
@@ -204,11 +193,10 @@ void ahrs_dcm_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_GYRO(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_DCM_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant.c b/sw/airborne/modules/ahrs/ahrs_float_invariant.c
index b2408e0e3a..af88db5acc 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant.c
@@ -178,10 +178,7 @@ void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
}
// fill command vector
- struct FloatRates gyro_meas_body;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
- ahrs_float_inv.cmd.rates = gyro_meas_body;
+ RATES_COPY(ahrs_float_inv.cmd.rates, *gyro);
// update correction gains
error_output(&ahrs_float_inv);
@@ -249,10 +246,8 @@ void ahrs_float_invariant_update_mag(struct FloatVect3* mag)
mag_frozen_count = MAG_FROZEN_COUNT;
}
} else {
- // values are moving
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
// new values in body frame
- float_rmat_transp_vmult(&ahrs_float_inv.meas.mag, body_to_imu_rmat, mag);
+ VECT3_COPY(ahrs_float_inv.meas.mag, *mag);
// reset counter
mag_frozen_count = MAG_FROZEN_COUNT;
}
@@ -380,14 +375,3 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
VECT3_ADD(v2, v1);
QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
}
-
-void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_float_inv.body_to_imu, q_b2i);
-
- if (!ahrs_float_inv.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_float_inv.state.quat = *q_b2i;
- }
-}
-
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant.h b/sw/airborne/modules/ahrs/ahrs_float_invariant.h
index 2301574bbe..50a9382307 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant.h
@@ -96,10 +96,7 @@ struct AhrsFloatInv {
struct inv_correction_gains corr; ///< correction gains
struct inv_gains gains; ///< tuning gains
- bool reset; ///< flag to request reset/reinit the filter
-
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
+ bool reset; ///< flag to request reset/reinit the filte
struct FloatVect3 mag_h;
bool is_aligned;
@@ -108,7 +105,6 @@ struct AhrsFloatInv {
extern struct AhrsFloatInv ahrs_float_inv;
extern void ahrs_float_invariant_init(void);
-extern void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_float_invariant_align(struct FloatRates *lp_gyro,
struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
index eb3b7bdde7..f1a9c0252f 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
@@ -35,7 +35,7 @@
#ifndef AHRS_FINV_OUTPUT_ENABLED
#define AHRS_FINV_OUTPUT_ENABLED TRUE
#endif
-PRINT_CONFIG_VAR(AHRS_INV_OUTPUT_ENABLED)
+PRINT_CONFIG_VAR(AHRS_FINV_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_finv_output_enabled;
@@ -50,25 +50,16 @@ static void compute_body_orientation_and_rates(void);
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
- /* compute eulers in int (IMU frame) */
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat);
- struct Int32Eulers eulers_imu;
- EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
-
/* compute Eulers in int (body frame) */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
- float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_float_inv.state.quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- &eulers_imu.phi,
- &eulers_imu.theta,
- &eulers_imu.psi,
+ &eulers_body.phi,
+ &eulers_body.theta,
+ &eulers_body.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
@@ -115,7 +106,6 @@ static abi_event mag_ev;
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
/**
@@ -193,12 +183,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_float_inv_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_float_inv.mag_h = *h;
@@ -216,20 +200,13 @@ static bool ahrs_float_invariant_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_finv_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
/* Set state */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
+ stateSetNedToBodyQuat_f(&ahrs_float_inv.state.quat);
/* compute body rates */
struct FloatRates body_rate;
RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &body_rate);
stateSetBodyRates_f(&body_rate);
-
}
}
@@ -243,11 +220,10 @@ void ahrs_float_invariant_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_MAG_INT32(AHRS_FINV_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_GYRO_INT32(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(AHRS_FINV_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(AHRS_FINV_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(AHRS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf.c b/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
index badb6e16de..af412ed7c2 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf.c
@@ -64,9 +64,9 @@ void ahrs_mlkf_init(void)
ahrs_mlkf.is_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
- FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ float_quat_identity(&ahrs_mlkf.ltp_to_body_quat);
+ FLOAT_RATES_ZERO(ahrs_mlkf.body_rate);
VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
@@ -88,28 +88,12 @@ void ahrs_mlkf_init(void)
VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
}
-void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
-
- if (!ahrs_mlkf.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_mlkf.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
- }
-}
-
-
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
+ ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_body_quat, lp_accel, lp_mag);
/* used averaged gyro as initial value for bias */
ahrs_mlkf.gyro_bias = *lp_gyro;
@@ -170,14 +154,14 @@ static inline void propagate_ref(struct FloatRates *gyro, float dt)
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
- FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate,
+ FLOAT_RATES_LIN_CMB(ahrs_mlkf.body_rate, ahrs_mlkf.body_rate,
(1. - alpha), rates, alpha);
#else
- RATES_COPY(ahrs_mlkf.imu_rate, rates);
+ RATES_COPY(ahrs_mlkf.body_rate, rates);
#endif
/* propagate reference quaternion */
- float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt);
+ float_quat_integrate(&ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.body_rate, dt);
}
@@ -189,9 +173,9 @@ static inline void propagate_state(float dt)
{
/* predict covariance */
- const float dp = ahrs_mlkf.imu_rate.p * dt;
- const float dq = ahrs_mlkf.imu_rate.q * dt;
- const float dr = ahrs_mlkf.imu_rate.r * dt;
+ const float dp = ahrs_mlkf.body_rate.p * dt;
+ const float dq = ahrs_mlkf.body_rate.q * dt;
+ const float dr = ahrs_mlkf.body_rate.r * dt;
float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
{ -dr, 1., dp, 0., -dt, 0. },
@@ -225,7 +209,7 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
- float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
+ float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
// S = HPH' + JRJ
float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
@@ -294,12 +278,12 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected,
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
- float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
+ float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
/* set roll/pitch errors to zero to only correct heading */
struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
struct FloatVect3 b_yaw;
- float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d);
+ float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_body_quat, &i_h_2d);
// S = HPH' + JRJ
float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
{ 0., 0., b_yaw.y, 0., 0., 0.},
@@ -359,9 +343,9 @@ static inline void reset_state(void)
ahrs_mlkf.gibbs_cor.qi = 2.;
struct FloatQuat q_tmp;
- float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor);
+ float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.gibbs_cor);
float_quat_normalize(&q_tmp);
- ahrs_mlkf.ltp_to_imu_quat = q_tmp;
+ ahrs_mlkf.ltp_to_body_quat = q_tmp;
float_quat_identity(&ahrs_mlkf.gibbs_cor);
}
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf.h b/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
index 74d062c9eb..12c2ebfce8 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf.h
@@ -41,9 +41,8 @@ enum AhrsMlkfStatus {
};
struct AhrsMlkf {
- struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
- struct FloatRates imu_rate; ///< Rotational velocity in IMU frame
+ struct FloatRates body_rate; ///< Rotational velocity in body frame
struct FloatRates gyro_bias;
struct FloatVect3 mag_h;
@@ -54,9 +53,6 @@ struct AhrsMlkf {
float P[6][6];
float lp_accel;
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
-
enum AhrsMlkfStatus status;
bool is_aligned;
};
@@ -64,8 +60,6 @@ struct AhrsMlkf {
extern struct AhrsMlkf ahrs_mlkf;
extern void ahrs_mlkf_init(void);
-extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
extern void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt);
diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
index e212e98ad9..7d08b41814 100644
--- a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
@@ -48,12 +48,12 @@ static void set_body_state_from_quat(void);
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct FloatEulers ltp_to_imu_euler;
- float_eulers_of_quat(<p_to_imu_euler, &ahrs_mlkf.ltp_to_imu_quat);
+ struct FloatEulers ltp_to_body_euler;
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_mlkf.ltp_to_body_quat);
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&ahrs_mlkf_id);
}
@@ -102,7 +102,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
@@ -179,12 +178,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ahrs_mlkf.mag_h = *h;
@@ -202,20 +195,9 @@ static bool ahrs_mlkf_enable_output(bool enable)
static void set_body_state_from_quat(void)
{
if (ahrs_mlkf_output_enabled) {
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
-
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
-
- /* compute body rates */
- struct FloatRates body_rate;
- float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
- /* Set state */
- stateSetBodyRates_f(&body_rate);
+ stateSetNedToBodyQuat_f(&ahrs_mlkf.ltp_to_body_quat);
+ stateSetBodyRates_f(&ahrs_mlkf.body_rate);
}
}
@@ -228,11 +210,10 @@ void ahrs_mlkf_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_GYRO(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_MLKF_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
diff --git a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
index d3ee04ad99..bb8cc73c06 100644
--- a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat.c
@@ -121,10 +121,9 @@ void ahrs_icq_init(void)
ahrs_icq.ltp_vel_norm_valid = false;
ahrs_icq.heading_aligned = false;
- /* init ltp_to_imu quaternion as zero/identity rotation */
- int32_quat_identity(&ahrs_icq.ltp_to_imu_quat);
-
- INT_RATES_ZERO(ahrs_icq.imu_rate);
+ /* init ltp_to_body quaternion as zero/identity rotation */
+ int32_quat_identity(&ahrs_icq.ltp_to_body_quat);
+ INT_RATES_ZERO(ahrs_icq.body_rate);
INT_RATES_ZERO(ahrs_icq.gyro_bias);
INT_RATES_ZERO(ahrs_icq.rate_correction);
@@ -161,12 +160,12 @@ bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_imu_quat,
+ ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_body_quat,
lp_accel, lp_mag);
ahrs_icq.heading_aligned = true;
#else
/* Compute an initial orientation from accel and just set heading to zero */
- ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_imu_quat, lp_accel);
+ ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_body_quat, lp_accel);
ahrs_icq.heading_aligned = false;
// supress unused arg warning
lp_mag = lp_mag;
@@ -194,11 +193,11 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
- RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
- RATES_ADD(ahrs_icq.imu_rate, omega);
- RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
+ RATES_SMUL(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
+ RATES_ADD(ahrs_icq.body_rate, omega);
+ RATES_SDIV(ahrs_icq.body_rate, ahrs_icq.body_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
#else
- RATES_COPY(ahrs_icq.imu_rate, omega);
+ RATES_COPY(ahrs_icq.body_rate, omega);
#endif
/* add correction */
@@ -207,9 +206,9 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
INT_RATES_ZERO(ahrs_icq.rate_correction);
/* integrate quaternion */
- int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat,
+ int32_quat_integrate_fi(&ahrs_icq.ltp_to_body_quat, &ahrs_icq.high_rez_quat,
&omega, freq);
- int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat);
+ int32_quat_normalize(&ahrs_icq.ltp_to_body_quat);
// increase accel and mag propagation counters
ahrs_icq.accel_cnt++;
@@ -246,11 +245,11 @@ void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
}
// c2 = ltp z-axis in imu-frame
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
- struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2),
- RMAT_ELMT(ltp_to_imu_rmat, 1, 2),
- RMAT_ELMT(ltp_to_imu_rmat, 2, 2)
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
+ struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_body_rmat, 0, 2),
+ RMAT_ELMT(ltp_to_body_rmat, 1, 2),
+ RMAT_ELMT(ltp_to_body_rmat, 2, 2)
};
struct Int32Vect3 residual;
@@ -279,20 +278,13 @@ void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
/* assume tangential velocity along body x-axis */
{ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
#endif
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
- struct Int32Rates body_rate;
- int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
struct Int32Vect3 acc_c_body;
- VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
+ VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_icq.body_rate, vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
- /* convert centrifucal acceleration from body to imu frame */
- struct Int32Vect3 acc_c_imu;
- int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
-
- /* and subtract it from imu measurement to get a corrected measurement
+ /* centrifucal acceleration subtract it from imu measurement to get a corrected measurement
* of the gravity vector */
- VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu);
+ VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_body);
} else {
VECT3_COPY(pseudo_gravity_measurement, *accel);
}
@@ -410,11 +402,11 @@ void ahrs_icq_set_mag_gains(void)
static inline void ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
{
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect3 expected_imu;
- int32_rmat_vmult(&expected_imu, <p_to_imu_rmat, &ahrs_icq.mag_h);
+ int32_rmat_vmult(&expected_imu, <p_to_body_rmat, &ahrs_icq.mag_h);
struct Int32Vect3 residual;
VECT3_CROSS_PRODUCT(residual, *mag, expected_imu);
@@ -467,11 +459,11 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
/* normalize expected ltp in 2D (x,y) */
int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC);
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32RMat ltp_to_body_rmat;
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect3 measured_ltp;
- int32_rmat_transp_vmult(&measured_ltp, <p_to_imu_rmat, mag);
+ int32_rmat_transp_vmult(&measured_ltp, <p_to_body_rmat, mag);
/* normalize measured ltp in 2D (x,y) */
struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
@@ -485,15 +477,15 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
};
- struct Int32Vect3 residual_imu;
- int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp);
+ struct Int32Vect3 residual_body;
+ int32_rmat_vmult(&residual_body, <p_to_body_rmat, &residual_ltp);
/* Complementary filter proportionnal gain.
* Kp = 2 * mag_zeta * mag_omega
* final Kp with frequency correction = Kp * ahrs_icq.mag_cnt
* with ahrs_icq.mag_cnt beeing the number of propagations since last update
*
- * residual_imu FRAC = residual_ltp FRAC = 17
+ * residual_body FRAC = residual_ltp FRAC = 17
* rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^17 = 1/32
*
@@ -502,15 +494,15 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
*/
int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_icq.mag_kp * ahrs_icq.mag_cnt));
- ahrs_icq.rate_correction.p += (residual_imu.x / inv_rate_gain);
- ahrs_icq.rate_correction.q += (residual_imu.y / inv_rate_gain);
- ahrs_icq.rate_correction.r += (residual_imu.z / inv_rate_gain);
+ ahrs_icq.rate_correction.p += (residual_body.x / inv_rate_gain);
+ ahrs_icq.rate_correction.q += (residual_body.y / inv_rate_gain);
+ ahrs_icq.rate_correction.r += (residual_body.z / inv_rate_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 * dt
*
- * residual_imu FRAC = residual_ltp FRAC = 17
+ * residual_body FRAC = residual_ltp FRAC = 17
* high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^17 = 2^23
*
@@ -518,9 +510,9 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
*/
int32_t bias_gain = (int32_t)(ahrs_icq.mag_ki * dt * (1 << 23));
- ahrs_icq.high_rez_bias.p -= (residual_imu.x * bias_gain);
- ahrs_icq.high_rez_bias.q -= (residual_imu.y * bias_gain);
- ahrs_icq.high_rez_bias.r -= (residual_imu.z * bias_gain);
+ ahrs_icq.high_rez_bias.p -= (residual_body.x * bias_gain);
+ ahrs_icq.high_rez_bias.q -= (residual_body.y * bias_gain);
+ ahrs_icq.high_rez_bias.r -= (residual_body.z * bias_gain);
INT_RATES_RSHIFT(ahrs_icq.gyro_bias, ahrs_icq.high_rez_bias, 28);
@@ -569,11 +561,8 @@ void ahrs_icq_update_heading(int32_t heading)
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
- struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
- struct Int32Quat ltp_to_body_quat;
- int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
struct Int32RMat ltp_to_body_rmat;
- int32_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat);
+ int32_rmat_of_quat(<p_to_body_rmat, &ahrs_icq.ltp_to_body_quat);
struct Int32Vect2 expected_ltp = {
RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(ltp_to_body_rmat, 0, 1)
@@ -590,18 +579,16 @@ void ahrs_icq_update_heading(int32_t heading)
(expected_ltp.x * heading_y - expected_ltp.y * heading_x) / (1 << INT32_ANGLE_FRAC)
};
- struct Int32Vect3 residual_imu;
- struct Int32RMat ltp_to_imu_rmat;
- int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
- int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp);
+ struct Int32Vect3 residual_body;
+ int32_rmat_vmult(&residual_body, <p_to_body_rmat, &residual_ltp);
// residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^28 * 4.0 = 1/2^14
// (1<qi),
&(quat->qx),
&(quat->qy),
@@ -64,13 +64,13 @@ static void send_quat(struct transport_tx *trans, struct link_device *dev)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
- struct Int32Eulers ltp_to_imu_euler;
- int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat);
+ struct Int32Eulers ltp_to_body_euler;
+ int32_eulers_of_quat(<p_to_body_euler, &ahrs_icq.ltp_to_body_quat);
struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- <p_to_imu_euler.phi,
- <p_to_imu_euler.theta,
- <p_to_imu_euler.psi,
+ <p_to_body_euler.phi,
+ <p_to_body_euler.theta,
+ <p_to_body_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi),
@@ -132,7 +132,6 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -223,12 +222,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_icq_set_body_to_imu_quat(q_b2i_f);
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y),
@@ -252,19 +245,9 @@ static bool ahrs_icq_enable_output(bool enable)
static void set_body_state_from_quat(void)
{
if (ahrs_icq_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct Int32Quat ltp_to_body_quat;
- struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
- int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
/* Set state */
- stateSetNedToBodyQuat_i(<p_to_body_quat);
-
- /* compute body rates */
- struct Int32Rates body_rate;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
- int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
- /* Set state */
- stateSetBodyRates_i(&body_rate);
+ stateSetNedToBodyQuat_i(&ahrs_icq.ltp_to_body_quat);
+ stateSetBodyRates_i(&ahrs_icq.body_rate);
}
}
@@ -277,11 +260,10 @@ void ahrs_icq_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_GYRO(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_LOWPASSED(AHRS_ICQ_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(AHRS_ICQ_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick.c b/sw/airborne/modules/ahrs/ahrs_madgwick.c
index a3175cdc8b..8463a6042d 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick.c
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick.c
@@ -78,11 +78,8 @@ void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt)
init_state();
}
- // unbias and rotate gyro
- struct FloatRates gyro_unbiased;
- RATES_DIFF(gyro_unbiased, *gyro, ahrs_madgwick.bias);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_madgwick.body_to_imu);
- float_rmat_transp_ratemult(&ahrs_madgwick.rates, body_to_imu_rmat, &gyro_unbiased);
+ // unbias gyro
+ RATES_DIFF(ahrs_madgwick.rates, *gyro, ahrs_madgwick.bias);
// Rate of change of quaternion from gyroscope
float_quat_derivative(&qdot, &ahrs_madgwick.rates, &ahrs_madgwick.quat);
@@ -143,14 +140,3 @@ void ahrs_madgwick_update_accel(struct FloatVect3* accel)
{
ahrs_madgwick.accel = *accel;
}
-
-void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_madgwick.body_to_imu, q_b2i);
-
- if (!ahrs_madgwick.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_madgwick.quat = *q_b2i;
- }
-}
-
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick.h b/sw/airborne/modules/ahrs/ahrs_madgwick.h
index 5c27920899..c9a3b53a17 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick.h
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick.h
@@ -40,7 +40,6 @@ struct AhrsMadgwick {
struct FloatRates rates; ///< Measured gyro rates
struct FloatRates bias; ///< Gyro bias (from alignment)
struct FloatVect3 accel; ///< Measured accelerometers
- struct OrientationReps body_to_imu; ///< body_to_imu rotation
bool reset; ///< flag to request reset/reinit the filter
bool is_aligned; ///< aligned flag
};
@@ -48,7 +47,6 @@ struct AhrsMadgwick {
extern struct AhrsMadgwick ahrs_madgwick;
extern void ahrs_madgwick_init(void);
-extern void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel);
extern void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt);
extern void ahrs_madgwick_update_accel(struct FloatVect3* accel);
diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
index 765dee159d..48f083cb93 100644
--- a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
+++ b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
@@ -56,11 +56,8 @@ static void send_att(struct transport_tx *trans, struct link_device *dev)
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
- float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
+ float_eulers_of_quat(<p_to_body_euler, &ahrs_madgwick.quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
@@ -105,7 +102,6 @@ PRINT_CONFIG_VAR(AHRS_MADGWICK_MAG_ID)
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
/**
* Call ahrs_madgwick_propagate on new gyro measurements.
@@ -169,12 +165,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_madgwick_set_body_to_imu_quat(q_b2i_f);
-}
-
static bool ahrs_madgwick_enable_output(bool enable)
{
ahrs_madgwick_output_enabled = enable;
@@ -187,12 +177,8 @@ static bool ahrs_madgwick_enable_output(bool enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_madgwick_output_enabled) {
- /* Compute LTP to BODY quaternion */
- struct FloatQuat ltp_to_body_quat;
- struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
- float_quat_comp_inv(<p_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
/* Set state */
- stateSetNedToBodyQuat_f(<p_to_body_quat);
+ stateSetNedToBodyQuat_f(&ahrs_madgwick.quat);
/* compute body rates */
stateSetBodyRates_f(&ahrs_madgwick.rates);
@@ -209,10 +195,9 @@ void ahrs_madgwick_register(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_GYRO_INT32(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_GYRO(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(AHRS_MADGWICK_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(AHRS_MADGWICK_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
diff --git a/sw/airborne/modules/ahrs/ahrs_vectornav.c b/sw/airborne/modules/ahrs/ahrs_vectornav.c
index 6efba0669c..06e3f34f35 100644
--- a/sw/airborne/modules/ahrs/ahrs_vectornav.c
+++ b/sw/airborne/modules/ahrs/ahrs_vectornav.c
@@ -83,9 +83,9 @@ void ahrs_vectornav_event(void)
uint32_t now_ts = get_sys_time_usec();
// in fixed point for sending as ABI and telemetry msgs
RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
- AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
+ AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1);
ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
- AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1);
}
}
diff --git a/sw/airborne/modules/calibration/mag_calib_ukf.c b/sw/airborne/modules/calibration/mag_calib_ukf.c
index d629be6a91..e0bea922fc 100644
--- a/sw/airborne/modules/calibration/mag_calib_ukf.c
+++ b/sw/airborne/modules/calibration/mag_calib_ukf.c
@@ -141,7 +141,7 @@ void mag_calib_ukf_init(void)
float initial_state[12] = MAG_CALIB_UKF_INITIAL_STATE;
memcpy(&mag_calib.state, &initial_state, 12 * sizeof(float));
#endif
- AbiBindMsgIMU_MAG_INT32(MAG_CALIB_UKF_ABI_BIND_ID, &mag_ev, mag_calib_ukf_run);
+ AbiBindMsgIMU_MAG(MAG_CALIB_UKF_ABI_BIND_ID, &mag_ev, mag_calib_ukf_run);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &h_ev, mag_calib_update_field); ///< GEO_MAG_SENDER_ID is defined in geo_mag.c so unknown
}
@@ -189,7 +189,7 @@ void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag
VERBOSE_PRINT("expected measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n", expected_mag_field[0], expected_mag_field[1], expected_mag_field[2], hypot(hypot(expected_mag_field[0], expected_mag_field[1]), expected_mag_field[2]));
VERBOSE_PRINT("calibrated measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n\n", calibrated_measurement[0], calibrated_measurement[1], calibrated_measurement[2], hypot(hypot(calibrated_measurement[0], calibrated_measurement[1]), calibrated_measurement[2]));
/** Forward calibrated data */
- AbiSendMsgIMU_MAG_INT32(MAG_CALIB_UKF_ID, stamp, &calibrated_mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_CALIB_UKF_ID, stamp, &calibrated_mag);
}
}
}
diff --git a/sw/airborne/modules/calibration/send_imu_mag_current.c b/sw/airborne/modules/calibration/send_imu_mag_current.c
deleted file mode 100644
index 4ecfff4299..0000000000
--- a/sw/airborne/modules/calibration/send_imu_mag_current.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2013 Felix Ruess
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file modules/calibration/send_imu_mag_current.c
- * Enables sending of IMU_MAG_CURRENT_CALIBRATION message.
- */
-
-#include "modules/imu/imu.h"
-#include "modules/energy/electrical.h"
-
-#include "pprzlink/messages.h"
-#include "mcu_periph/uart.h"
-#include "modules/datalink/downlink.h"
-
-void send_imu_mag_current(void)
-{
- DOWNLINK_SEND_IMU_MAG_CURRENT_CALIBRATION(DefaultChannel, DefaultDevice,
- &imu.mag_unscaled.x,
- &imu.mag_unscaled.y,
- &imu.mag_unscaled.z,
- &electrical.current);
-}
-
diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h
index ca84d12db5..f7ce184a86 100644
--- a/sw/airborne/modules/core/abi_sender_ids.h
+++ b/sw/airborne/modules/core/abi_sender_ids.h
@@ -363,6 +363,22 @@
#define IMU_BMI088_ID 19
#endif
+#ifndef IMU_CUBE1_ID
+#define IMU_CUBE1_ID 20
+#endif
+
+#ifndef IMU_CUBE2_ID
+#define IMU_CUBE2_ID 21
+#endif
+
+#ifndef IMU_CUBE3_ID
+#define IMU_CUBE3_ID 22
+#endif
+
+#ifndef IMU_NPS_ID
+#define IMU_NPS_ID 23
+#endif
+
// prefiltering with OneEuro filter
#ifndef IMU_F1E_ID
#define IMU_F1E_ID 30
diff --git a/sw/airborne/modules/dragspeed/dragspeed.c b/sw/airborne/modules/dragspeed/dragspeed.c
index 12c77d2475..8cd07033b6 100644
--- a/sw/airborne/modules/dragspeed/dragspeed.c
+++ b/sw/airborne/modules/dragspeed/dragspeed.c
@@ -99,7 +99,7 @@ void dragspeed_init(void)
// Register callbacks
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DRAGSPEED,
send_dragspeed);
- AbiBindMsgIMU_ACCEL_INT32(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_ACCEL(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb);
}
bool dragspeed_calibrate_coeff(void)
diff --git a/sw/airborne/modules/imu/filter_1euro_imu.c b/sw/airborne/modules/imu/filter_1euro_imu.c
index 0d3c156e07..bc8b5ed89e 100644
--- a/sw/airborne/modules/imu/filter_1euro_imu.c
+++ b/sw/airborne/modules/imu/filter_1euro_imu.c
@@ -111,15 +111,15 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev; // only passthrough
-static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
+static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro, uint8_t samples)
{
if (sender_id == IMU_F1E_ID) {
return; // don't process own data
}
- if (filter_1e_imu.enabled) {
+ if (filter_1e_imu.enabled && samples > 0) {
struct FloatRates gyro_f;
- RATES_FLOAT_OF_BFP(gyro_f, *gyro);
+ RATES_FLOAT_OF_BFP(gyro_f, gyro[samples-1]); // For now only filter last value
// compute filters
#ifdef FILTER_1EURO_FREQ
gyro_f.p = update_1e_filter(&gyro_1e[0], gyro_f.p);
@@ -134,19 +134,19 @@ static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
// send filtered data
struct Int32Rates gyro_i;
RATES_BFP_OF_REAL(gyro_i, gyro_f);
- AbiSendMsgIMU_GYRO_INT32(IMU_F1E_ID, stamp, &gyro_i);
+ AbiSendMsgIMU_GYRO_RAW(IMU_F1E_ID, stamp, &gyro_i, 1);
} else {
- AbiSendMsgIMU_GYRO_INT32(IMU_F1E_ID, stamp, gyro);
+ AbiSendMsgIMU_GYRO_RAW(IMU_F1E_ID, stamp, gyro, samples);
}
}
-static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
+static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel, uint8_t samples)
{
if (sender_id == IMU_F1E_ID) {
return; // don't process own data
}
- if (filter_1e_imu.enabled) {
+ if (filter_1e_imu.enabled && samples > 0) {
struct FloatVect3 accel_f;
ACCELS_FLOAT_OF_BFP(accel_f, *accel);
// compute filters
@@ -163,9 +163,9 @@ static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel
// send filtered data
struct Int32Vect3 accel_i;
ACCELS_BFP_OF_REAL(accel_i, accel_f);
- AbiSendMsgIMU_ACCEL_INT32(IMU_F1E_ID, stamp, &accel_i);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_F1E_ID, stamp, &accel_i, 1);
} else {
- AbiSendMsgIMU_ACCEL_INT32(IMU_F1E_ID, stamp, accel);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_F1E_ID, stamp, accel, samples);
}
}
@@ -176,7 +176,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
return; // don't process own data
}
- AbiSendMsgIMU_MAG_INT32(IMU_F1E_ID, stamp, mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_F1E_ID, stamp, mag);
}
/**
@@ -205,9 +205,9 @@ void filter_1euro_imu_init(void)
filter_1e_imu.accel_dcutoff);
}
- AbiBindMsgIMU_GYRO_INT32(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(IMU_F1E_BIND_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(IMU_F1E_BIND_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO_RAW(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL_RAW(IMU_F1E_BIND_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG_RAW(IMU_F1E_BIND_ID, &mag_ev, mag_cb);
}
/**
@@ -270,4 +270,3 @@ void filter_1euro_imu_update_accel_dcutoff(float dcutoff)
accel_1e[i].dcutoff = dcutoff;
}
}
-
diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c
index 0dd9099780..7218a57076 100644
--- a/sw/airborne/modules/imu/imu.c
+++ b/sw/airborne/modules/imu/imu.c
@@ -1,5 +1,6 @@
/*
- * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2008-2022 The Paparazzi Team
+ * Freek van Tienen
*
* This file is part of paparazzi.
*
@@ -31,107 +32,314 @@
#include "modules/imu/imu.h"
#include "state.h"
#include "modules/core/abi.h"
+#include "modules/energy/electrical.h"
-#ifdef IMU_POWER_GPIO
-#include "mcu_periph/gpio.h"
+/** By default disable IMU integration calculations */
+#ifndef IMU_INTEGRATION
+#define IMU_INTEGRATION false
+#endif
-#ifndef IMU_POWER_GPIO_ON
-#define IMU_POWER_GPIO_ON gpio_set
+/** Default gyro calibration is for single IMU with old format */
+#if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
+#define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
+PRINT_CONFIG_MSG("Using single IMU gyro neutral calibration")
#endif
+
+#if defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
+#define GYRO_SCALE {{IMU_GYRO_P_SENS_NUM, IMU_GYRO_Q_SENS_NUM, IMU_GYRO_R_SENS_NUM}, {IMU_GYRO_P_SENS_DEN, IMU_GYRO_Q_SENS_DEN, IMU_GYRO_R_SENS_DEN}}
+PRINT_CONFIG_MSG("Using single IMU gyro sensitivity calibration")
#endif
+#if defined(GYRO_NEUTRAL) && defined(GYRO_SCALE)
+#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=GYRO_NEUTRAL, .scale=GYRO_SCALE}}
+#elif defined(GYRO_NEUTRAL)
+#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=GYRO_NEUTRAL}}
+#elif defined(GYRO_SCALE)
+#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=GYRO_SCALE}}
+#elif !defined(IMU_GYRO_CALIB)
+#define IMU_GYRO_CALIB {}
+#endif
+
+/** Default accel calibration is for single IMU with old format */
+#if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
+#define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
+PRINT_CONFIG_MSG("Using single IMU accel neutral calibration")
+#endif
+
+#if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
+#define ACCEL_SCALE {{IMU_ACCEL_X_SENS_NUM, IMU_ACCEL_Y_SENS_NUM, IMU_ACCEL_Z_SENS_NUM}, {IMU_ACCEL_X_SENS_DEN, IMU_ACCEL_Y_SENS_DEN, IMU_ACCEL_Z_SENS_DEN}}
+PRINT_CONFIG_MSG("Using single IMU accel sensitivity calibration")
+#endif
+
+#if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
+#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
+#elif defined(ACCEL_NEUTRAL)
+#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
+#elif defined(ACCEL_SCALE)
+#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
+#elif !defined(IMU_ACCEL_CALIB)
+#define IMU_ACCEL_CALIB {}
+#endif
+
+/** Default mag calibration is for single IMU with old format */
+#if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
+#define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
+PRINT_CONFIG_MSG("Using single IMU mag neutral calibration")
+#endif
+
+#if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
+#define MAG_SCALE {{IMU_MAG_X_SENS_NUM, IMU_MAG_Y_SENS_NUM, IMU_MAG_Z_SENS_NUM}, {IMU_MAG_X_SENS_DEN, IMU_MAG_Y_SENS_DEN, IMU_MAG_Z_SENS_DEN}}
+PRINT_CONFIG_MSG("Using single IMU mag sensitivity calibration")
+#endif
+
+#if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
+#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
+#elif defined(MAG_NEUTRAL)
+#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
+#elif defined(MAG_SCALE)
+#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
+#elif !defined(IMU_MAG_CALIB)
+#define IMU_MAG_CALIB {}
+#endif
+
+
+/** Default body to imu is 0 (radians) */
+#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
+#define IMU_BODY_TO_IMU_PHI 0
+#define IMU_BODY_TO_IMU_THETA 0
+#define IMU_BODY_TO_IMU_PSI 0
+#endif
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PHI)
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_THETA)
+PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
+
+
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID,
- &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id,
+ &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
- &imu.accel.x, &imu.accel.y, &imu.accel.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
+ &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_accel(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatVect3 accel_float;
- ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
- pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
+ ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
+ pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
&accel_float.x, &accel_float.y, &accel_float.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID,
- &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id,
+ &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
- &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
+ &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatRates gyro_float;
- RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
- pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
+ RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
+ pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID,
- &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
+ &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID,
- &imu.mag.x, &imu.mag.y, &imu.mag.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
+ &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatVect3 mag_float;
- MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
- pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
+ MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
+ pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
&mag_float.x, &mag_float.y, &mag_float.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
+}
+
+static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
+{
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans, dev, AC_ID,
+ &imu.mags[id].abi_id,
+ &imu.mags[id].unscaled.x,
+ &imu.mags[id].unscaled.y,
+ &imu.mags[id].unscaled.z,
+ &electrical.current);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
#endif /* PERIODIC_TELEMETRY */
-struct Imu imu;
+struct Imu imu = {0};
+static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
+static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples);
+static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples);
+static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
+static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
void imu_init(void)
{
-
-#ifdef IMU_POWER_GPIO
- gpio_setup_output(IMU_POWER_GPIO);
- IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
-#endif
-
- /* initialises neutrals */
- RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
-
- VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
-
-#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
- VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
-#else
-#if USE_MAGNETOMETER && (!defined MAG_CALIB_UKF_H)
- INFO("Magnetometer neutrals are set to zero, you should calibrate!")
-#endif
- INT_VECT3_ZERO(imu.mag_neutral);
-#endif
-
- struct FloatEulers body_to_imu_eulers =
- {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+ // Set the Body to IMU rotation
+ struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
+ struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
+
+
+ // Set the calibrated sensors
+ struct Int32RMat body_to_sensor;
+ const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB;
+ const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB;
+ const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB;
+ const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t);
+ const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t);
+ const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t);
+
+ // Initialize the sensors to default values
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ /* Copy gyro calibration if needed */
+ if(i >= gyro_calib_len) {
+ imu.gyros[i].abi_id = ABI_DISABLE;
+ imu.gyros[i].calibrated.neutral = false;
+ imu.gyros[i].calibrated.scale = false;
+ imu.gyros[i].calibrated.rotation = false;
+ } else {
+ imu.gyros[i] = gyro_calib[i];
+ }
+
+ // Set the default safe values if not calibrated
+ if(!imu.gyros[i].calibrated.neutral) {
+ INT_RATES_ZERO(imu.gyros[i].neutral);
+ }
+ if(!imu.gyros[i].calibrated.scale) {
+ RATES_ASSIGN(imu.gyros[i].scale[0], 1, 1, 1);
+ RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1);
+ }
+ if(!imu.gyros[i].calibrated.rotation) {
+ int32_rmat_identity(&imu.gyros[i].body_to_sensor);
+ }
+ imu.gyros[i].last_stamp = 0;
+ int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
+ RMAT_COPY(imu.gyros[i].body_to_sensor, body_to_sensor);
+
+
+ /* Copy accel calibration if needed */
+ if(i >= accel_calib_len) {
+ imu.accels[i].abi_id = ABI_DISABLE;
+ imu.accels[i].calibrated.neutral = false;
+ imu.accels[i].calibrated.scale = false;
+ imu.accels[i].calibrated.rotation = false;
+ } else {
+ imu.accels[i] = accel_calib[i];
+ }
+
+ // Set the default safe values if not calibrated
+ if(!imu.accels[i].calibrated.neutral) {
+ INT_VECT3_ZERO(imu.accels[i].neutral);
+ }
+ if(!imu.accels[i].calibrated.scale) {
+ VECT3_ASSIGN(imu.accels[i].scale[0], 1, 1, 1);
+ VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1);
+ }
+ if(!imu.accels[i].calibrated.rotation) {
+ int32_rmat_identity(&imu.accels[i].body_to_sensor);
+ }
+ imu.accels[i].last_stamp = 0;
+ int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
+ RMAT_COPY(imu.accels[i].body_to_sensor, body_to_sensor);
+
+
+ /* Copy mag calibrated if needed */
+ if(i >= mag_calib_len) {
+ imu.mags[i].abi_id = ABI_DISABLE;
+ imu.mags[i].calibrated.neutral = false;
+ imu.mags[i].calibrated.scale = false;
+ imu.mags[i].calibrated.rotation = false;
+ imu.mags[i].calibrated.current = false;
+ } else {
+ imu.mags[i] = mag_calib[i];
+ }
+
+ // Set the default safe values if not calibrated
+ if(!imu.mags[i].calibrated.neutral) {
+ INT_VECT3_ZERO(imu.mags[i].neutral);
+ }
+ if(!imu.mags[i].calibrated.scale) {
+ VECT3_ASSIGN(imu.mags[i].scale[0], 1, 1, 1);
+ VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1);
+ }
+ if(!imu.mags[i].calibrated.rotation) {
+ int32_rmat_identity(&imu.mags[i].body_to_sensor);
+ }
+ if(!imu.mags[i].calibrated.current) {
+ INT_VECT3_ZERO(imu.mags[i].current_scale);
+ }
+ int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.mags[i].body_to_sensor);
+ RMAT_COPY(imu.mags[i].body_to_sensor, body_to_sensor);
+ }
+
+ // Bind to raw measurements
+ AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
+ AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
+ AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
@@ -143,18 +351,370 @@ void imu_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, send_mag_raw);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_SCALED, send_mag_scaled);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_CURRENT_CALIBRATION, send_mag_current);
#endif // DOWNLINK
+ imu.initialized = true;
}
+/**
+ * @brief Set the defaults for a gyro sensor
+ * WARNING: Should be called before sensor is publishing messages to ensure correct values
+ * @param abi_id The ABI sender id to set the defaults for
+ * @param imu_to_sensor Imu to sensor rotation matrix
+ * @param neutral Neutral values
+ * @param scale Scale values, 0 index is multiply and 1 index is divide
+ */
+void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
+{
+ // Find the correct gyro
+ struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true);
+ if(gyro == NULL)
+ return;
+
+ // Copy the defaults
+ if(imu_to_sensor != NULL && !gyro->calibrated.rotation) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(gyro->body_to_sensor, body_to_sensor);
+ }
+ if(neutral != NULL && !gyro->calibrated.neutral)
+ RATES_COPY(gyro->neutral, *neutral);
+ if(scale != NULL && !gyro->calibrated.scale) {
+ RATES_COPY(gyro->scale[0], scale[0]);
+ RATES_COPY(gyro->scale[1], scale[1]);
+ }
+}
+
+/**
+ * @brief Set the defaults for a accel sensor
+ * WARNING: Should be called before sensor is publishing messages to ensure correct values
+ * @param abi_id The ABI sender id to set the defaults for
+ * @param imu_to_sensor Imu to sensor rotation matrix
+ * @param neutral Neutral values
+ * @param scale Scale values, 0 index is multiply and 1 index is divide
+ */
+void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
+{
+ // Find the correct accel
+ struct imu_accel_t *accel = imu_get_accel(abi_id, true);
+ if(accel == NULL)
+ return;
+
+ // Copy the defaults
+ if(imu_to_sensor != NULL && !accel->calibrated.rotation) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(accel->body_to_sensor, body_to_sensor);
+ }
+ if(neutral != NULL && !accel->calibrated.neutral)
+ VECT3_COPY(accel->neutral, *neutral);
+ if(scale != NULL && !accel->calibrated.scale) {
+ VECT3_COPY(accel->scale[0], scale[0]);
+ VECT3_COPY(accel->scale[1], scale[1]);
+ }
+}
+
+/**
+ * @brief Set the defaults for a mag sensor
+ * WARNING: Should be called before sensor is publishing messages to ensure correct values
+ * @param abi_id The ABI sender id to set the defaults for
+ * @param imu_to_sensor Imu to sensor rotation matrix
+ * @param neutral Neutral values
+ * @param scale Scale values, 0 index is multiply and 1 index is divide
+ */
+void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
+{
+ // Find the correct mag
+ struct imu_mag_t *mag = imu_get_mag(abi_id, true);
+ if(mag == NULL)
+ return;
+
+ // Copy the defaults
+ if(imu_to_sensor != NULL && !mag->calibrated.rotation) {
+ struct Int32RMat body_to_sensor;
+ struct Int32RMat *body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ int32_rmat_comp(&body_to_sensor, body_to_imu, imu_to_sensor);
+ RMAT_COPY(mag->body_to_sensor, body_to_sensor);
+ }
+ if(neutral != NULL && !mag->calibrated.neutral)
+ VECT3_COPY(mag->neutral, *neutral);
+ if(scale != NULL && !mag->calibrated.scale) {
+ VECT3_COPY(mag->scale[0], scale[0]);
+ VECT3_COPY(mag->scale[1], scale[1]);
+ }
+}
+
+static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples)
+{
+ // Find the correct gyro
+ struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
+ if(gyro == NULL || samples < 1)
+ return;
+
+ // Copy last sample as unscaled
+ RATES_COPY(gyro->unscaled, data[samples-1]);
+
+ // Scale the gyro
+ struct Int32Rates scaled, scaled_rot;
+ scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p;
+ scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q;
+ scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r;
+
+ // Rotate the sensor
+ int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled);
+
+#if IMU_INTEGRATION
+ // Only integrate if we have gotten a previous measurement and didn't overflow the timer
+ if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
+ struct FloatRates integrated;
+ uint16_t dt = stamp - gyro->last_stamp;
+
+ // Trapezoidal integration (TODO: coning correction)
+ integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
+ integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
+ integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
+
+ // Only perform multiple integrations in sensor frame if needed
+ if(samples > 1) {
+ struct FloatRates integrated_sensor;
+ struct FloatRMat body_to_sensor;
+ // Rotate back to sensor frame
+ RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
+ float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
+
+ // Add all the other samples
+ for(uint8_t i = 0; i < samples-1; i++) {
+ integrated_sensor.p += RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
+ integrated_sensor.q += RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
+ integrated_sensor.r += RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
+ }
+
+ // Rotate to body frame
+ float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
+ }
+
+ // Divide by the amount of samples and multiply by the delta time
+ integrated.p = integrated.p / samples * ((float)dt * 1e-6f);
+ integrated.q = integrated.q / samples * ((float)dt * 1e-6f);
+ integrated.r = integrated.r / samples * ((float)dt * 1e-6f);
+
+ // Send the integrated values
+ AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
+ }
+#endif
+
+ // Copy and send
+ RATES_COPY(gyro->scaled, scaled_rot);
+ AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
+ gyro->last_stamp = stamp;
+}
+
+static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples)
+{
+ // Find the correct accel
+ struct imu_accel_t *accel = imu_get_accel(sender_id, true);
+ if(accel == NULL || samples < 1)
+ return;
+
+ // Copy last sample as unscaled
+ VECT3_COPY(accel->unscaled, data[samples-1]);
+
+ // Scale the accel
+ struct Int32Vect3 scaled, scaled_rot;
+ scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x;
+ scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y;
+ scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z;
+
+ // Rotate the sensor
+ int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled);
+
+#if IMU_INTEGRATION
+ // Only integrate if we have gotten a previous measurement and didn't overflow the timer
+ if(accel->last_stamp > 0 && stamp > accel->last_stamp) {
+ struct FloatVect3 integrated;
+ uint16_t dt = stamp - accel->last_stamp;
+
+ // Trapezoidal integration
+ integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
+ integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
+ integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
+
+ // Only perform multiple integrations in sensor frame if needed
+ if(samples > 1) {
+ struct FloatVect3 integrated_sensor;
+ struct FloatRMat body_to_sensor;
+ // Rotate back to sensor frame
+ RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
+ float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
+
+ // Add all the other samples
+ for(uint8_t i = 0; i < samples-1; i++) {
+ integrated_sensor.x += ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
+ integrated_sensor.y += ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
+ integrated_sensor.z += ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
+ }
+
+ // Rotate to body frame
+ float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
+ }
+
+ // Divide by the amount of samples and multiply by the delta time
+ integrated.x = integrated.x / samples * ((float)dt * 1e-6f);
+ integrated.y = integrated.y / samples * ((float)dt * 1e-6f);
+ integrated.z = integrated.z / samples * ((float)dt * 1e-6f);
+
+ // Send the integrated values
+ AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
+ }
+#endif
+
+ // Copy and send
+ VECT3_COPY(accel->scaled, scaled_rot);
+ AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
+ accel->last_stamp = stamp;
+}
+
+static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
+{
+ // Find the correct mag
+ struct imu_mag_t *mag = imu_get_mag(sender_id, true);
+ if(mag == NULL)
+ return;
+
+ // Calculate current compensation
+ struct Int32Vect3 mag_correction;
+ mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
+ mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
+ mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
+
+ // Copy last sample as unscaled
+ VECT3_COPY(mag->unscaled, *data);
+
+ // Scale the mag
+ struct Int32Vect3 scaled;
+ scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale[0].x / mag->scale[1].x;
+ scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale[0].y / mag->scale[1].y;
+ scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale[0].z / mag->scale[1].z;
+
+ // Rotate the sensor
+ int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled);
+ AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
+}
+
+/**
+ * @brief Find or create the gyro in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @param create Create a new index if not found
+ * @return struct imu_gyro_t* The gyro structure if found/created else NULL
+ */
+struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) {
+ // Find the correct gyro or create index
+ // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
+ struct imu_gyro_t *gyro = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.gyros[i].abi_id == sender_id || (create && (imu.gyros[i].abi_id == ABI_BROADCAST || imu.gyros[i].abi_id == ABI_DISABLE))) {
+ gyro = &imu.gyros[i];
+ gyro->abi_id = sender_id;
+ break;
+ } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) {
+ gyro = &imu.gyros[i];
+ }
+ }
+ return gyro;
+}
+
+/**
+ * @brief Find or create the accel in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @param create Create a new index if not found
+ * @return struct imu_accel_t* The accel structure if found/created else NULL
+ */
+struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) {
+ // Find the correct accel
+ // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
+ struct imu_accel_t *accel = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.accels[i].abi_id == sender_id || (create && (imu.accels[i].abi_id == ABI_BROADCAST || imu.accels[i].abi_id == ABI_DISABLE))) {
+ accel = &imu.accels[i];
+ accel->abi_id = sender_id;
+ break;
+ } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) {
+ accel = &imu.accels[i];
+ }
+ }
+
+ return accel;
+}
+
+/**
+ * @brief Find or create the mag in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @param create Create a new index if not found
+ * @return struct imu_mag_t* The mag structure if found/created else NULL
+ */
+struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) {
+ // Find the correct mag
+ // If abi_id was set to broadcast (assuming a single IMU case), the first request takes the index for himself
+ struct imu_mag_t *mag = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.mags[i].abi_id == sender_id || (create && (imu.mags[i].abi_id == ABI_BROADCAST || imu.mags[i].abi_id == ABI_DISABLE))) {
+ mag = &imu.mags[i];
+ mag->abi_id = sender_id;
+ break;
+ } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) {
+ mag = &imu.mags[i];
+ }
+ }
+
+ return mag;
+}
+
+/**
+ * @brief Set the body to IMU rotation in eulers
+ * This will update all the sensor values
+ * @param body_to_imu_eulers 321 Euler angles in radians
+ */
+static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers)
+{
+ struct Int32RMat new_body_to_imu, diff_body_to_imu;
+ struct Int32Eulers body_to_imu_eulers_i;
+ // Convert to RMat
+ struct Int32RMat *old_body_to_imu = orientationGetRMat_i(&imu.body_to_imu);
+ EULERS_BFP_OF_REAL(body_to_imu_eulers_i, *body_to_imu_eulers);
+ int32_rmat_of_eulers(&new_body_to_imu, &body_to_imu_eulers_i);
+
+ // Calculate the difference between old and new
+ int32_rmat_comp_inv(&diff_body_to_imu, &new_body_to_imu, old_body_to_imu);
+
+ // Apply the difference to all sensors
+ struct Int32RMat old_rmat;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ old_rmat = imu.gyros[i].body_to_sensor;
+ int32_rmat_comp(&imu.gyros[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+
+ old_rmat = imu.accels[i].body_to_sensor;
+ int32_rmat_comp(&imu.accels[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+
+ old_rmat = imu.mags[i].body_to_sensor;
+ int32_rmat_comp(&imu.mags[i].body_to_sensor, &diff_body_to_imu, &old_rmat);
+ }
+
+ // Set the current body to imu
+ orientationSetEulers_f(&imu.body_to_imu, body_to_imu_eulers);
+}
void imu_SetBodyToImuPhi(float phi)
{
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.phi = phi;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuTheta(float theta)
@@ -162,8 +722,7 @@ void imu_SetBodyToImuTheta(float theta)
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.theta = theta;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuPsi(float psi)
@@ -171,8 +730,7 @@ void imu_SetBodyToImuPsi(float psi)
struct FloatEulers body_to_imu_eulers;
body_to_imu_eulers = *orientationGetEulers_f(&imu.body_to_imu);
body_to_imu_eulers.psi = psi;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
void imu_SetBodyToImuCurrent(float set)
@@ -187,71 +745,14 @@ void imu_SetBodyToImuCurrent(float set)
// adjust imu_to_body roll and pitch by current NedToBody roll and pitch
body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
} else {
// indicate that we couldn't set to current roll/pitch
imu.b2i_set_current = false;
}
} else {
// reset to BODY_TO_IMU as defined in airframe file
- struct FloatEulers body_to_imu_eulers =
- {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
- orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
- AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+ struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+ imu_set_body_to_imu_eulers(&body_to_imu_eulers);
}
}
-
-
-// weak functions, used if not explicitly provided by implementation
-
-void WEAK imu_scale_gyro(struct Imu *_imu)
-{
- RATES_COPY(_imu->gyro_prev, _imu->gyro);
- _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
- IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
- _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
- IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
- _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
- IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
-}
-
-void WEAK imu_scale_accel(struct Imu *_imu)
-{
- VECT3_COPY(_imu->accel_prev, _imu->accel);
- _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN *
- IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
- _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN *
- IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN;
- _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN *
- IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN;
-}
-
-#if !defined SITL && defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
-#include "modules/energy/electrical.h"
-void WEAK imu_scale_mag(struct Imu *_imu)
-{
- struct Int32Vect3 mag_correction;
- mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current);
- mag_correction.y = (int32_t)(IMU_MAG_Y_CURRENT_COEF * (float) electrical.current);
- mag_correction.z = (int32_t)(IMU_MAG_Z_CURRENT_COEF * (float) electrical.current);
- _imu->mag.x = (((_imu->mag_unscaled.x - mag_correction.x) - _imu->mag_neutral.x) *
- IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
- _imu->mag.y = (((_imu->mag_unscaled.y - mag_correction.y) - _imu->mag_neutral.y) *
- IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
- _imu->mag.z = (((_imu->mag_unscaled.z - mag_correction.z) - _imu->mag_neutral.z) *
- IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
-}
-#elif USE_MAGNETOMETER
-void WEAK imu_scale_mag(struct Imu *_imu)
-{
- _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN *
- IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
- _imu->mag.y = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN *
- IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
- _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN *
- IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
-}
-#else
-void WEAK imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
-#endif /* MAG_x_CURRENT_COEF */
diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h
index 026dd9af89..09f037ddde 100644
--- a/sw/airborne/modules/imu/imu.h
+++ b/sw/airborne/modules/imu/imu.h
@@ -1,5 +1,6 @@
/*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2008-2022 The Paparazzi Team
+ * Freek van Tienen
*
* This file is part of paparazzi.
*
@@ -32,21 +33,58 @@
#include "math/pprz_orientation_conversion.h"
#include "generated/airframe.h"
+#ifndef IMU_MAX_SENSORS
+#define IMU_MAX_SENSORS 3
+#endif
+
+struct imu_calib_t {
+ bool neutral: 1; ///< Neutral values calibrated
+ bool scale: 1; ///< Scale calibrated
+ bool rotation: 1; ///< Rotation calibrated
+ bool current: 1; ///< Current calibrated
+};
+
+struct imu_gyro_t {
+ uint8_t abi_id; ///< ABI sensor ID
+ uint32_t last_stamp; ///< Last measurement timestamp for integration
+ struct imu_calib_t calibrated; ///< Calibration bitmask
+ struct Int32Rates scaled; ///< Last scaled values in body frame
+ struct Int32Rates unscaled; ///< Last unscaled values in sensor frame
+ struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled
+ struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
+};
+
+struct imu_accel_t {
+ uint8_t abi_id; ///< ABI sensor ID
+ uint32_t last_stamp; ///< Last measurement timestamp for integration
+ struct imu_calib_t calibrated; ///< Calibration bitmask
+ struct Int32Vect3 scaled; ///< Last scaled values in body frame
+ struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
+ struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
+ struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
+};
+
+struct imu_mag_t {
+ uint8_t abi_id; ///< ABI sensor ID
+ struct imu_calib_t calibrated; ///< Calibration bitmask
+ struct Int32Vect3 scaled; ///< Last scaled values in body frame
+ struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
+ struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
+ struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
+ struct FloatVect3 current_scale; ///< Current scaling multiplying
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
+};
+
/** abstract IMU interface providing fixed point interface */
struct Imu {
- struct Int32Rates gyro; ///< gyroscope measurements in rad/s in BFP with #INT32_RATE_FRAC
- struct Int32Vect3 accel; ///< accelerometer measurements in m/s^2 in BFP with #INT32_ACCEL_FRAC
- struct Int32Vect3 mag; ///< magnetometer measurements scaled to 1 in BFP with #INT32_MAG_FRAC
- struct Int32Rates gyro_prev; ///< previous gyroscope measurements
- struct Int32Vect3 accel_prev; ///< previous accelerometer measurements
- struct Int32Rates gyro_neutral; ///< static gyroscope bias from calibration in raw/unscaled units
- struct Int32Vect3 accel_neutral; ///< static accelerometer bias from calibration in raw/unscaled units
- struct Int32Vect3 mag_neutral; ///< magnetometer neutral readings (bias) in raw/unscaled units
- struct Int32Rates gyro_unscaled; ///< unscaled gyroscope measurements
- struct Int32Vect3 accel_unscaled; ///< unscaled accelerometer measurements
- struct Int32Vect3 mag_unscaled; ///< unscaled magnetometer measurements
- struct OrientationReps body_to_imu; ///< rotation from body to imu frame
+ bool initialized; ///< Check if the IMU is initialized
+ struct imu_gyro_t gyros[IMU_MAX_SENSORS]; ///< The gyro sensors
+ struct imu_accel_t accels[IMU_MAX_SENSORS]; ///< The accelerometer sensors
+ struct imu_mag_t mags[IMU_MAX_SENSORS]; ///< The magnetometer sensors
+ struct OrientationReps body_to_imu; ///< Rotation from body to imu (all sensors) frame
/** flag for adjusting body_to_imu via settings.
* if FALSE, reset to airframe values, if TRUE set current roll/pitch
@@ -57,62 +95,21 @@ struct Imu {
/** global IMU state */
extern struct Imu imu;
-/* underlying hardware */
-#ifdef IMU_TYPE_H
-#include IMU_TYPE_H
-#endif
-
+/** External functions */
extern void imu_init(void);
+
+extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale);
+extern void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale);
+extern void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale);
+
+extern struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create);
+extern struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create);
+extern struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create);
+
extern void imu_SetBodyToImuPhi(float phi);
extern void imu_SetBodyToImuTheta(float theta);
extern void imu_SetBodyToImuPsi(float psi);
extern void imu_SetBodyToImuCurrent(float set);
extern void imu_ResetBodyToImu(float reset);
-/* can be provided implementation */
-extern void imu_scale_gyro(struct Imu *_imu);
-extern void imu_scale_accel(struct Imu *_imu);
-extern void imu_scale_mag(struct Imu *_imu);
-
-#if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
-#define IMU_BODY_TO_IMU_PHI 0
-#define IMU_BODY_TO_IMU_THETA 0
-#define IMU_BODY_TO_IMU_PSI 0
-#endif
-
-#if !defined IMU_GYRO_P_NEUTRAL && !defined IMU_GYRO_Q_NEUTRAL && !defined IMU_GYRO_R_NEUTRAL
-#define IMU_GYRO_P_NEUTRAL 0
-#define IMU_GYRO_Q_NEUTRAL 0
-#define IMU_GYRO_R_NEUTRAL 0
-#endif
-
-#if !defined IMU_ACCEL_X_NEUTRAL && !defined IMU_ACCEL_Y_NEUTRAL && !defined IMU_ACCEL_Z_NEUTRAL
-#define IMU_ACCEL_X_NEUTRAL 0
-#define IMU_ACCEL_Y_NEUTRAL 0
-#define IMU_ACCEL_Z_NEUTRAL 0
-#endif
-
-#if !defined IMU_MAG_X_NEUTRAL && !defined IMU_MAG_Y_NEUTRAL && !defined IMU_MAG_Z_NEUTRAL
-#define IMU_MAG_X_NEUTRAL 0
-#define IMU_MAG_Y_NEUTRAL 0
-#define IMU_MAG_Z_NEUTRAL 0
-#endif
-
-#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN 1
-#define IMU_GYRO_Q_SIGN 1
-#define IMU_GYRO_R_SIGN 1
-#endif
-#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN 1
-#define IMU_ACCEL_Y_SIGN 1
-#define IMU_ACCEL_Z_SIGN 1
-#endif
-#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
-#define IMU_MAG_X_SIGN 1
-#define IMU_MAG_Y_SIGN 1
-#define IMU_MAG_Z_SIGN 1
-#endif
-
-
#endif /* IMU_H */
diff --git a/sw/airborne/modules/imu/imu_ardrone2.c b/sw/airborne/modules/imu/imu_ardrone2.c
index 0c3683f522..8d8f464119 100644
--- a/sw/airborne/modules/imu/imu_ardrone2.c
+++ b/sw/airborne/modules/imu/imu_ardrone2.c
@@ -24,10 +24,8 @@
* IMU implementation for ardrone2.
*/
-#include "modules/imu/imu.h"
#include "navdata.h"
#include "imu_ardrone2.h"
-#include "mcu_periph/uart.h"
void imu_ardrone2_init(void)
{
diff --git a/sw/airborne/modules/imu/imu_ardrone2.h b/sw/airborne/modules/imu/imu_ardrone2.h
index bc46ebf3d0..dfd31934f1 100644
--- a/sw/airborne/modules/imu/imu_ardrone2.h
+++ b/sw/airborne/modules/imu/imu_ardrone2.h
@@ -27,63 +27,9 @@
#ifndef IMU_ARDRONE2_H_
#define IMU_ARDRONE2_H_
-#include "generated/airframe.h"
+#include "std.h"
#include "boards/ardrone/navdata.h"
-
-/** default gyro sensitivy and neutral from the datasheet
- * MPU with 2000 deg/s
- */
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS 4.359
-#define IMU_GYRO_P_SENS_NUM 4359
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS 4.359
-#define IMU_GYRO_Q_SENS_NUM 4359
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS 4.359
-#define IMU_GYRO_R_SENS_NUM 4359
-#define IMU_GYRO_R_SENS_DEN 1000
-#endif
-
-/** default accel sensitivy from the datasheet
- * 512 LSB/g
- */
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS 19.5
-#define IMU_ACCEL_X_SENS_NUM 195
-#define IMU_ACCEL_X_SENS_DEN 10
-#define IMU_ACCEL_Y_SENS 19.5
-#define IMU_ACCEL_Y_SENS_NUM 195
-#define IMU_ACCEL_Y_SENS_DEN 10
-#define IMU_ACCEL_Z_SENS 19.5
-#define IMU_ACCEL_Z_SENS_NUM 195
-#define IMU_ACCEL_Z_SENS_DEN 10
-#endif
-
-#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
-#define IMU_ACCEL_X_NEUTRAL 2048
-#define IMU_ACCEL_Y_NEUTRAL 2048
-#define IMU_ACCEL_Z_NEUTRAL 2048
-#endif
-
-#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
-#define IMU_MAG_X_SENS 16.0
-#define IMU_MAG_X_SENS_NUM 16
-#define IMU_MAG_X_SENS_DEN 1
-#define IMU_MAG_Y_SENS 16.0
-#define IMU_MAG_Y_SENS_NUM 16
-#define IMU_MAG_Y_SENS_DEN 1
-#define IMU_MAG_Z_SENS 16.0
-#define IMU_MAG_Z_SENS_NUM 16
-#define IMU_MAG_Z_SENS_DEN 1
-#endif
-
-/*
- * we include imh.h after the definitions of the neutrals
- */
-#include "modules/imu/imu.h"
-
extern void imu_ardrone2_init(void);
#endif /* IMU_ARDRONE2_H_ */
diff --git a/sw/airborne/modules/imu/imu_aspirin.c b/sw/airborne/modules/imu/imu_aspirin.c
index f30a4dc1dc..3031db0218 100644
--- a/sw/airborne/modules/imu/imu_aspirin.c
+++ b/sw/airborne/modules/imu/imu_aspirin.c
@@ -25,6 +25,7 @@
* Driver for the Aspirin v1.x IMU using SPI for the accelerometer.
*/
+#include "modules/imu/imu_aspirin.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
@@ -67,10 +68,6 @@ struct ImuAspirin imu_aspirin;
void imu_aspirin_init(void)
{
- imu_aspirin.accel_valid = false;
- imu_aspirin.gyro_valid = false;
- imu_aspirin.mag_valid = false;
-
/* Set accel configuration */
adxl345_spi_init(&imu_aspirin.acc_adxl, &(ASPIRIN_SPI_DEV), ASPIRIN_SPI_SLAVE_IDX);
// set the data rate
@@ -90,6 +87,27 @@ void imu_aspirin_init(void)
/* interrupt on data ready, idle high, latch until read any register */
//itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
+ // Default scaling values
+#ifdef IMU_ASPIRIN_VERSION_1_5
+ const struct Int32Rates gyro_scale[2] = {
+ {4359, 4359, 4359},
+ {1000, 1000, 1000}
+ };
+#else
+ const struct Int32Rates gyro_scale[2] = {
+ {4973, 4973, 4973},
+ {1000, 1000, 1000}
+ };
+#endif
+ const struct Int32Vect3 accel_scale[2] = {
+ {3791, 3791, 3791},
+ {100, 100, 100}
+ };
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale);
+ imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale);
+
/* initialize mag and set default options */
hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR);
#ifdef IMU_ASPIRIN_VERSION_1_0
@@ -121,46 +139,29 @@ void imu_aspirin_event(void)
adxl345_spi_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
- VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1);
imu_aspirin.acc_adxl.data_available = false;
- imu_aspirin.accel_valid = true;
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
- RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
+ AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1);
imu_aspirin.gyro_itg.data_available = false;
- imu_aspirin.gyro_valid = true;
}
/* HMC58XX event task */
hmc58xx_event(&imu_aspirin.mag_hmc);
if (imu_aspirin.mag_hmc.data_available) {
+ struct Int32Vect3 mag;
#ifdef IMU_ASPIRIN_VERSION_1_0
- VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
+ VECT3_COPY(mag, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
- imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y;
- imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
- imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z;
+ mag.x = imu_aspirin.mag_hmc.data.vect.y;
+ mag.y = -imu_aspirin.mag_hmc.data.vect.x;
+ mag.z = imu_aspirin.mag_hmc.data.vect.z;
#endif
imu_aspirin.mag_hmc.data_available = false;
- imu_aspirin.mag_valid = true;
- }
-
- if (imu_aspirin.gyro_valid) {
- imu_aspirin.gyro_valid = false;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
- }
- if (imu_aspirin.accel_valid) {
- imu_aspirin.accel_valid = false;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
- }
- if (imu_aspirin.mag_valid) {
- imu_aspirin.mag_valid = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN_ID, now_ts, &mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_aspirin.h b/sw/airborne/modules/imu/imu_aspirin.h
index d05dea0476..8dfb6fe5c3 100644
--- a/sw/airborne/modules/imu/imu_aspirin.h
+++ b/sw/airborne/modules/imu/imu_aspirin.h
@@ -36,13 +36,8 @@
#include "peripherals/hmc58xx.h"
#include "peripherals/adxl345_spi.h"
-/* include default aspirin sensitivity definitions */
-#include "modules/imu/imu_aspirin_defaults.h"
struct ImuAspirin {
- volatile uint8_t accel_valid;
- volatile uint8_t gyro_valid;
- volatile uint8_t mag_valid;
struct Adxl345_Spi acc_adxl;
struct Itg3200 gyro_itg;
struct Hmc58xx mag_hmc;
diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.c b/sw/airborne/modules/imu/imu_aspirin_2_spi.c
index 73032875dd..01a7ecde6b 100644
--- a/sw/airborne/modules/imu/imu_aspirin_2_spi.c
+++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.c
@@ -24,6 +24,7 @@
* Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
*/
+#include "modules/imu/imu_aspirin_2_spi.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
@@ -105,6 +106,10 @@ void imu_aspirin2_init(void)
imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE]);
+
/* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */
imu_aspirin2.mpu.config.nb_bytes = 21;
@@ -159,31 +164,33 @@ void imu_aspirin2_event(void)
if (imu_aspirin2.mpu.data_available) {
#if !ASPIRIN_2_DISABLE_MAG
/* HMC5883 has xzy order of axes in returned data */
- struct Int32Vect3 mag;
+ struct Int32Vect3 mag, mag_rot;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#endif
/* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */
+ struct Int32Rates gyro;
+ struct Int32Vect3 accel;
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
- RATES_ASSIGN(imu.gyro_unscaled,
+ RATES_ASSIGN(gyro,
imu_aspirin2.mpu.data_rates.rates.p,
-imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled,
+ VECT3_ASSIGN(accel,
imu_aspirin2.mpu.data_accel.vect.x,
-imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
- VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
+ VECT3_ASSIGN(mag_rot, mag.x, -mag.y, -mag.z);
#endif
#else
- RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
- VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
+ RATES_COPY(gyro, imu_aspirin2.mpu.data_rates.rates);
+ VECT3_COPY(accel, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
- VECT3_COPY(imu.mag_unscaled, mag);
+ VECT3_COPY(mag_rot, mag);
#endif
#endif
#else
@@ -192,37 +199,37 @@ void imu_aspirin2_event(void)
* IMU.
*/
#ifdef LISA_M_OR_MX_21
- RATES_ASSIGN(imu.gyro_unscaled,
+ RATES_ASSIGN(gyro,
-imu_aspirin2.mpu.data_rates.rates.q,
imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled,
+ VECT3_ASSIGN(accel,
-imu_aspirin2.mpu.data_accel.vect.y,
imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
- VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z);
+ VECT3_ASSIGN(mag_rot, -mag.y, mag.x, mag.z);
#endif
#else
/* Handle real Aspirin IMU axis assignement. */
#ifdef LISA_M_LONGITUDINAL_X
- RATES_ASSIGN(imu.gyro_unscaled,
+ RATES_ASSIGN(gyro,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled,
+ VECT3_ASSIGN(accel,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
- VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
+ VECT3_ASSIGN(mag_rot, -mag.x, -mag.y, mag.z);
#endif
#else
- RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
- VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
+ RATES_COPY(gyro, imu_aspirin2.mpu.data_rates.rates);
+ VECT3_COPY(accel, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
- VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
+ VECT3_ASSIGN(mag_rot, mag.y, -mag.x, mag.z)
#endif
#endif
#endif
@@ -230,13 +237,10 @@ void imu_aspirin2_event(void)
imu_aspirin2.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1);
#if !ASPIRIN_2_DISABLE_MAG
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN2_ID, now_ts, &mag_rot);
#endif
}
}
diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.h b/sw/airborne/modules/imu/imu_aspirin_2_spi.h
index 7c4fb21432..f77d445f79 100644
--- a/sw/airborne/modules/imu/imu_aspirin_2_spi.h
+++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.h
@@ -41,32 +41,6 @@
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
-#endif
-
struct ImuAspirin2Spi {
struct Mpu60x0_Spi mpu;
diff --git a/sw/airborne/modules/imu/imu_aspirin_defaults.h b/sw/airborne/modules/imu/imu_aspirin_defaults.h
deleted file mode 100644
index bb47a630c4..0000000000
--- a/sw/airborne/modules/imu/imu_aspirin_defaults.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * Copyright (C) 2010-2013 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file modules/imu/imu_aspirin_defaults.h
- * Default sensitivity definitions for IMU Aspirin 1x.
- */
-
-
-#ifndef IMU_ASPIRIN_DEFAULTS_H
-#define IMU_ASPIRIN_DEFAULTS_H
-
-#include "generated/airframe.h"
-
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#ifdef IMU_ASPIRIN_VERSION_1_5
-/** default gyro sensitivy and neutral from the datasheet
- * IMU-3000 has 16.4 LSB/(deg/s) at 2000deg/s range
- * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
- * sens = 1/16.4 * pi/180 * 4096 = 4.359066229
- */
-#define IMU_GYRO_P_SENS 4.359
-#define IMU_GYRO_P_SENS_NUM 4359
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS 4.359
-#define IMU_GYRO_Q_SENS_NUM 4359
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS 4.359
-#define IMU_GYRO_R_SENS_NUM 4359
-#define IMU_GYRO_R_SENS_DEN 1000
-#else
-/** default gyro sensitivy and neutral from the datasheet
- * ITG3200 has 14.375 LSB/(deg/s)
- * sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC
- * sens = 1/14.375 * pi/180 * 4096 = 4.973126
- */
-#define IMU_GYRO_P_SENS 4.973
-#define IMU_GYRO_P_SENS_NUM 4973
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS 4.973
-#define IMU_GYRO_Q_SENS_NUM 4973
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS 4.973
-#define IMU_GYRO_R_SENS_NUM 4973
-#define IMU_GYRO_R_SENS_DEN 1000
-#endif // IMU_ASPIRIN_VERSION_1_5
-#endif
-
-
-/** default accel sensitivy from the ADXL345 datasheet
- * sensitivity of x & y axes depends on supply voltage:
- * - 256 LSB/g @ 2.5V
- * - 265 LSB/g @ 3.3V
- * z sensitivity stays at 256 LSB/g
- * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
- * x/y sens = 9.81 / 265 * 1024 = 37.91
- * z sens = 9.81 / 256 * 1024 = 39.24
- *
- * what about the offset at 3.3V?
- */
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS 37.91
-#define IMU_ACCEL_X_SENS_NUM 3791
-#define IMU_ACCEL_X_SENS_DEN 100
-#define IMU_ACCEL_Y_SENS 37.91
-#define IMU_ACCEL_Y_SENS_NUM 3791
-#define IMU_ACCEL_Y_SENS_DEN 100
-#define IMU_ACCEL_Z_SENS 39.24
-#define IMU_ACCEL_Z_SENS_NUM 3924
-#define IMU_ACCEL_Z_SENS_DEN 100
-#endif
-
-#endif /* IMU_ASPIRIN_DEFAULTS_H */
diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.c b/sw/airborne/modules/imu/imu_aspirin_i2c.c
index 1e7d0fa98c..513ca9cd12 100644
--- a/sw/airborne/modules/imu/imu_aspirin_i2c.c
+++ b/sw/airborne/modules/imu/imu_aspirin_i2c.c
@@ -21,10 +21,11 @@
*/
/**
- * @file modules/imu/imu_aspirin.c
+ * @file modules/imu/imu_aspirin_i2c.c
* Driver for the Aspirin v1.x IMU using I2C for the accelerometer.
*/
+#include "modules/imu/imu_aspirin_i2c.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
@@ -107,6 +108,27 @@ void imu_aspirin_i2c_init(void)
/* interrupt on data ready, idle high, latch until read any register */
//itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
+ // Default scaling values
+#ifdef IMU_ASPIRIN_VERSION_1_5
+ const struct Int32Rates gyro_scale[2] = {
+ {4359, 4359, 4359},
+ {1000, 1000, 1000}
+ };
+#else
+ const struct Int32Rates gyro_scale[2] = {
+ {4973, 4973, 4973},
+ {1000, 1000, 1000}
+ };
+#endif
+ const struct Int32Vect3 accel_scale[2] = {
+ {3791, 3791, 3791},
+ {100, 100, 100}
+ };
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale);
+ imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale);
+
/* initialize mag and set default options */
hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR);
#ifdef IMU_ASPIRIN_VERSION_1_0
@@ -132,33 +154,29 @@ void imu_aspirin_i2c_event(void)
adxl345_i2c_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
- VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1);
imu_aspirin.acc_adxl.data_available = false;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
- RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
+ AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1);
imu_aspirin.gyro_itg.data_available = false;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
hmc58xx_event(&imu_aspirin.mag_hmc);
if (imu_aspirin.mag_hmc.data_available) {
+ struct Int32Vect3 mag;
#ifdef IMU_ASPIRIN_VERSION_1_0
- VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect);
+ VECT3_COPY(mag, imu_aspirin.mag_hmc.data.vect);
#else // aspirin 1.5 with hmc5883
- imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y;
- imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x;
- imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z;
+ mag.x = imu_aspirin.mag_hmc.data.vect.y;
+ mag.y = -imu_aspirin.mag_hmc.data.vect.x;
+ mag.z = imu_aspirin.mag_hmc.data.vect.z;
#endif
imu_aspirin.mag_hmc.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN_ID, now_ts, &mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.h b/sw/airborne/modules/imu/imu_aspirin_i2c.h
index 203c524cc9..8481cf8505 100644
--- a/sw/airborne/modules/imu/imu_aspirin_i2c.h
+++ b/sw/airborne/modules/imu/imu_aspirin_i2c.h
@@ -21,7 +21,7 @@
*/
/**
- * @file modules/imu/imu_aspirin.h
+ * @file modules/imu/imu_aspirin_i2c.h
* Interface for the Aspirin v1.x IMU using I2C for the accelerometer.
*/
@@ -36,9 +36,6 @@
#include "peripherals/hmc58xx.h"
#include "peripherals/adxl345_i2c.h"
-/* include default aspirin sensitivity/channel definitions */
-#include "modules/imu/imu_aspirin_defaults.h"
-
struct ImuAspirinI2c {
struct Adxl345_I2c acc_adxl;
diff --git a/sw/airborne/modules/imu/imu_bebop.c b/sw/airborne/modules/imu/imu_bebop.c
index 410e732fe3..61e4f1053c 100644
--- a/sw/airborne/modules/imu/imu_bebop.c
+++ b/sw/airborne/modules/imu/imu_bebop.c
@@ -24,6 +24,7 @@
* Driver for the Bebop (2) magnetometer, accelerometer and gyroscope
*/
+#include "modules/imu/imu_bebop.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
@@ -80,6 +81,10 @@ void imu_bebop_init(void)
imu_bebop.mpu.config.gyro_range = BEBOP_GYRO_RANGE;
imu_bebop.mpu.config.accel_range = BEBOP_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE]);
+
/* AKM8963 */
ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR);
@@ -118,35 +123,35 @@ void imu_bebop_event(void)
if (imu_bebop.mpu.data_available) {
/* default orientation of the MPU is upside down sor corrigate this here */
- RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q,
+ struct Int32Rates gyro;
+ struct Int32Vect3 accel;
+ RATES_ASSIGN(gyro, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q,
-imu_bebop.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y,
+ VECT3_ASSIGN(accel, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y,
-imu_bebop.mpu.data_accel.vect.z);
imu_bebop.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
}
/* AKM8963 event task */
ak8963_event(&imu_bebop.ak);
if (imu_bebop.ak.data_available) {
+ struct Int32Vect3 mag;
#if BEBOP_VERSION2
struct Int32Vect3 mag_temp;
// In the second bebop version the magneto is turned 90 degrees
VECT3_ASSIGN(mag_temp, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);
// Rotate the magneto
struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop);
- int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp);
+ int32_rmat_vmult(&mag, imu_to_mag_rmat, &mag_temp);
#else //BEBOP regular first verion
- VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
+ VECT3_ASSIGN(mag, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
#endif
imu_bebop.ak.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_bebop.h b/sw/airborne/modules/imu/imu_bebop.h
index 2a97146839..b2e9346d00 100644
--- a/sw/airborne/modules/imu/imu_bebop.h
+++ b/sw/airborne/modules/imu/imu_bebop.h
@@ -42,33 +42,6 @@
#define BEBOP_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
-#endif
-
-
/** Everything that is in the bebop IMU */
struct ImuBebop {
struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device
diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.c b/sw/airborne/modules/imu/imu_bmi088_i2c.c
index 958835a440..e72f22739b 100644
--- a/sw/airborne/modules/imu/imu_bmi088_i2c.c
+++ b/sw/airborne/modules/imu/imu_bmi088_i2c.c
@@ -25,6 +25,7 @@
*
*/
+#include "modules/imu/imu_bmi088_i2c.h"
#include "modules/imu/imu.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/sys_time.h"
@@ -98,6 +99,10 @@ void imu_bmi088_init(void)
imu_bmi088.bmi.config.accel_range = IMU_BMI088_ACCEL_RANGE;
imu_bmi088.bmi.config.accel_odr = IMU_BMI088_ACCEL_ODR;
imu_bmi088.bmi.config.accel_bw = IMU_BMI088_ACCEL_BW;
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_BMI088_ID, NULL, NULL, BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_BMI088_ID, NULL, NULL, BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE]);
}
void imu_bmi088_periodic(void)
@@ -118,13 +123,9 @@ void imu_bmi088_event(void)
IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Y]),
IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Z])
};
- // unscaled vector
- RATES_COPY(imu.gyro_unscaled, rates);
imu_bmi088.bmi.gyro_available = false;
-
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_BMI088_ID, now_ts, &imu.gyro);
+ AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1);
}
if (imu_bmi088.bmi.accel_available) {
// set channel order
@@ -133,13 +134,9 @@ void imu_bmi088_event(void)
IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Y]),
IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Z])
};
- // unscaled vector
- VECT3_COPY(imu.accel_unscaled, accel);
imu_bmi088.bmi.accel_available = false;
-
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_BMI088_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1);
}
}
diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.h b/sw/airborne/modules/imu/imu_bmi088_i2c.h
index ac0d504245..384ff8ec85 100644
--- a/sw/airborne/modules/imu/imu_bmi088_i2c.h
+++ b/sw/airborne/modules/imu/imu_bmi088_i2c.h
@@ -43,33 +43,6 @@
#define IMU_BMI088_ACCEL_RANGE BMI088_ACCEL_RANGE_6G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
-#endif
-
-
struct ImuBmi088 {
struct Bmi088_I2c bmi;
};
diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c
new file mode 100644
index 0000000000..6c4cf6b49c
--- /dev/null
+++ b/sw/airborne/modules/imu/imu_cube.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2022 Freek van tieen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/imu/imu_cube.c
+ * Driver for the IMU's in the Cube autopilots.
+ */
+
+#include "modules/imu/imu.h"
+#include "modules/core/abi.h"
+#include "mcu_periph/spi.h"
+#include "peripherals/invensense2.h"
+#include "peripherals/mpu60x0_spi.h"
+
+
+static struct invensense2_t imu1;
+static struct Mpu60x0_Spi imu2;
+static struct invensense2_t imu3;
+
+void imu_cube_init(void)
+{
+ struct Int32RMat rmat;
+ struct Int32Eulers eulers;
+
+ /* IMU 1 (ICM20649 not isolated) */
+ imu1.abi_id = IMU_CUBE1_ID;
+ imu1.bus = INVENSENSE2_SPI;
+ imu1.spi.p = &CUBE_IMU1_SPI_DEV;
+ imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
+ imu1.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
+ imu1.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
+ imu1.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
+ imu1.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
+ invensense2_init(&imu1);
+
+ // Rotation
+ eulers.phi = ANGLE_BFP_OF_REAL(0);
+ eulers.theta = ANGLE_BFP_OF_REAL(0);
+ eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
+ int32_rmat_of_eulers(&rmat, &eulers);
+ imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL);
+ imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL);
+
+ /* IMU 2 (ICM20602 isolated) */
+ mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX);
+ // change the default configuration
+ imu2.config.smplrt_div = 3;
+ imu2.config.dlpf_cfg = MPU60X0_DLPF_256HZ;
+ imu2.config.dlpf_cfg_acc = MPU60X0_DLPF_ACC_218HZ; // only for ICM sensors
+ imu2.config.gyro_range = MPU60X0_GYRO_RANGE_2000;
+ imu2.config.accel_range = MPU60X0_ACCEL_RANGE_16G;
+
+ // Rotation
+ eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180)),
+ eulers.theta = ANGLE_BFP_OF_REAL(0);
+ eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
+ int32_rmat_of_eulers(&rmat, &eulers);
+ imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_GYRO_SENS_FRAC[MPU60X0_GYRO_RANGE_2000]);
+ imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_ACCEL_SENS_FRAC[MPU60X0_ACCEL_RANGE_16G]);
+
+ /* IMU 3 (ICM2094 isolated) */
+ imu3.abi_id = IMU_CUBE3_ID;
+ imu3.bus = INVENSENSE2_SPI;
+ imu3.spi.p = &CUBE_IMU3_SPI_DEV;
+ imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
+ imu3.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
+ imu3.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
+ imu3.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
+ imu3.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
+ invensense2_init(&imu3);
+
+ // Rotation
+ eulers.phi = ANGLE_BFP_OF_REAL(0),
+ eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
+ eulers.psi = ANGLE_BFP_OF_REAL(0);
+ int32_rmat_of_eulers(&rmat, &eulers);
+ imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
+ imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
+}
+
+void imu_cube_periodic(void)
+{
+ invensense2_periodic(&imu1);
+ mpu60x0_spi_periodic(&imu2);
+ invensense2_periodic(&imu3);
+}
+
+void imu_cube_event(void)
+{
+ invensense2_event(&imu1);
+
+ mpu60x0_spi_event(&imu2);
+ if (imu2.data_available) {
+ uint32_t now_ts = get_sys_time_usec();
+
+ // set channel order
+ struct Int32Vect3 accel = {
+ (int32_t)(imu2.data_accel.value[1]),
+ (int32_t)(imu2.data_accel.value[0]),
+ -(int32_t)(imu2.data_accel.value[2])
+ };
+ struct Int32Rates rates = {
+ (int32_t)(imu2.data_rates.value[1]),
+ (int32_t)(imu2.data_rates.value[0]),
+ -(int32_t)(imu2.data_rates.value[2])
+ };
+
+ imu2.data_available = false;
+
+ // Send the scaled values over ABI
+ AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1);
+ }
+
+ invensense2_event(&imu3);
+}
diff --git a/sw/airborne/modules/calibration/send_imu_mag_current.h b/sw/airborne/modules/imu/imu_cube.h
similarity index 70%
rename from sw/airborne/modules/calibration/send_imu_mag_current.h
rename to sw/airborne/modules/imu/imu_cube.h
index 1484340d0c..bfdb9ad311 100644
--- a/sw/airborne/modules/calibration/send_imu_mag_current.h
+++ b/sw/airborne/modules/imu/imu_cube.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2013 Felix Ruess
+ * Copyright (C) 2022 Freek van Tienen
*
* This file is part of paparazzi.
*
@@ -20,13 +20,17 @@
*/
/**
- * @file modules/calibration/send_imu_mag_current.h
- * Enables sending of IMU_MAG_CURRENT_CALIBRATION message.
+ * @file modules/imu/imu_cube.h
+ * Driver for the IMU's in the Cube autopilots.
*/
-#ifndef SEND_IMU_MAG_CURRENT_H
-#define SEND_IMU_MAG_CURRENT_H
+#ifndef IMU_CUBE_H
+#define IMU_CUBE_H
-extern void send_imu_mag_current(void);
+#include "std.h"
-#endif
+extern void imu_cube_init(void);
+extern void imu_cube_periodic(void);
+extern void imu_cube_event(void);
+
+#endif /* IMU_CUBE_H */
diff --git a/sw/airborne/modules/imu/imu_disco.c b/sw/airborne/modules/imu/imu_disco.c
index c6529720cd..2656a6f09a 100644
--- a/sw/airborne/modules/imu/imu_disco.c
+++ b/sw/airborne/modules/imu/imu_disco.c
@@ -24,6 +24,7 @@
* Driver for the Disco magnetometer, accelerometer and gyroscope
*/
+#include "modules/imu/imu_disco.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
@@ -78,6 +79,10 @@ void imu_disco_init(void)
imu_disco.mpu.config.gyro_range = DISCO_GYRO_RANGE;
imu_disco.mpu.config.accel_range = DISCO_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE]);
+
/* AKM8963 */
ak8963_init(&imu_disco.ak, &(DISCO_MAG_I2C_DEV), AK8963_ADDR);
}
@@ -114,33 +119,33 @@ void imu_disco_event(void)
if (imu_disco.mpu.data_available) {
/* set correct orientation here */
- RATES_ASSIGN(imu.gyro_unscaled,
+ struct Int32Rates gyro;
+ struct Int32Vect3 accel;
+ RATES_ASSIGN(gyro,
-imu_disco.mpu.data_rates.rates.p,
-imu_disco.mpu.data_rates.rates.q,
imu_disco.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled,
+ VECT3_ASSIGN(accel,
-imu_disco.mpu.data_accel.vect.x,
-imu_disco.mpu.data_accel.vect.y,
imu_disco.mpu.data_accel.vect.z);
imu_disco.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
}
/* AKM8963 event task */
ak8963_event(&imu_disco.ak);
if (imu_disco.ak.data_available) {
- VECT3_ASSIGN(imu.mag_unscaled,
+ struct Int32Vect3 mag;
+ VECT3_ASSIGN(mag,
imu_disco.ak.data.vect.x,
imu_disco.ak.data.vect.y,
imu_disco.ak.data.vect.z);
+ AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
imu_disco.ak.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_disco.h b/sw/airborne/modules/imu/imu_disco.h
index 18eaf0a1ad..ba5103ce95 100644
--- a/sw/airborne/modules/imu/imu_disco.h
+++ b/sw/airborne/modules/imu/imu_disco.h
@@ -42,32 +42,6 @@
#define DISCO_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
-#endif
-
/** Everything that is in the disco IMU */
struct ImuDisco {
struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device
diff --git a/sw/airborne/modules/imu/imu_mpu6000.c b/sw/airborne/modules/imu/imu_mpu6000.c
index dcf2e1b332..098063b9dd 100644
--- a/sw/airborne/modules/imu/imu_mpu6000.c
+++ b/sw/airborne/modules/imu/imu_mpu6000.c
@@ -25,6 +25,7 @@
* Driver for IMU with only MPU6000 via SPI.
*/
+#include "modules/imu/imu_mpu6000.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/spi.h"
@@ -126,6 +127,10 @@ void imu_mpu_spi_init(void)
imu_mpu_spi.mpu.config.dlpf_cfg_acc = IMU_MPU_ACCEL_LOWPASS_FILTER; // only for ICM sensors
imu_mpu_spi.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
imu_mpu_spi.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_MPU6000_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_MPU6000_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]);
}
void imu_mpu_spi_periodic(void)
@@ -156,18 +161,11 @@ void imu_mpu_spi_event(void)
UpdateMedianFilterVect3Int(medianfilter_accel, accel);
UpdateMedianFilterRatesInt(medianfilter_rates, rates);
#endif
- // unscaled vector
- VECT3_COPY(imu.accel_unscaled, accel);
- RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu_spi.mpu.data_available = false;
- // Scale the gyro and accelerometer
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
-
// Send the scaled values over ABI
- AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1);
}
}
diff --git a/sw/airborne/modules/imu/imu_mpu6000.h b/sw/airborne/modules/imu/imu_mpu6000.h
index a753133bdd..919157550f 100644
--- a/sw/airborne/modules/imu/imu_mpu6000.h
+++ b/sw/airborne/modules/imu/imu_mpu6000.h
@@ -33,6 +33,7 @@
#include "peripherals/mpu60x0_spi.h"
+/* Default range configurations */
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
@@ -41,33 +42,6 @@
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#endif
-
-
struct ImuMpu6000 {
struct Mpu60x0_Spi mpu;
};
diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
index 77fbcd5e6f..1f2fe1bdd5 100644
--- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
+++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
@@ -24,6 +24,7 @@
* Driver for IMU with MPU6000 via SPI and HMC5883 via I2c.
*/
+#include "modules/imu/imu_mpu6000_hmc5883.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/spi.h"
@@ -128,6 +129,10 @@ void imu_mpu_hmc_init(void)
imu_mpu_hmc.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
imu_mpu_hmc.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]);
+
/* initialize mag and set default options */
hmc58xx_init(&imu_mpu_hmc.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR);
}
@@ -160,26 +165,22 @@ void imu_mpu_hmc_event(void)
IMU_MPU_Y_SIGN * (int32_t)(imu_mpu_hmc.mpu.data_rates.value[IMU_MPU_CHAN_Y]),
IMU_MPU_Z_SIGN * (int32_t)(imu_mpu_hmc.mpu.data_rates.value[IMU_MPU_CHAN_Z])
};
- // unscaled vector
- VECT3_COPY(imu.accel_unscaled, accel);
- RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu_hmc.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1);
}
/* HMC58XX event task */
hmc58xx_event(&imu_mpu_hmc.hmc);
if (imu_mpu_hmc.hmc.data_available) {
/* mag by default rotated by 90deg around z axis relative to MPU */
- imu.mag_unscaled.x = IMU_HMC_X_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_X];
- imu.mag_unscaled.y = IMU_HMC_Y_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Y];
- imu.mag_unscaled.z = IMU_HMC_Z_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Z];
+ struct Int32Vect3 mag = {
+ IMU_HMC_X_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_X],
+ IMU_HMC_Y_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Y],
+ IMU_HMC_Z_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Z]
+ };
imu_mpu_hmc.hmc.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_MPU6000_HMC_ID, now_ts, &mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h
index 7a669512ca..277cd37301 100644
--- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h
+++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h
@@ -43,33 +43,6 @@
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
-#endif
-
-
struct ImuMpu6000Hmc5883 {
struct Mpu60x0_Spi mpu;
struct Hmc58xx hmc;
diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c
index 3fe377be36..cce76c47f3 100644
--- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c
+++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c
@@ -25,6 +25,7 @@
*/
#include
+#include "modules/imu/imu_mpu60x0_i2c.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
@@ -70,6 +71,10 @@ void imu_mpu_i2c_init(void)
imu_mpu_i2c.mpu.config.dlpf_cfg = IMU_MPU60X0_LOWPASS_FILTER;
imu_mpu_i2c.mpu.config.gyro_range = IMU_MPU60X0_GYRO_RANGE;
imu_mpu_i2c.mpu.config.accel_range = IMU_MPU60X0_ACCEL_RANGE;
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE]);
}
void imu_mpu_i2c_periodic(void)
@@ -84,12 +89,8 @@ void imu_mpu_i2c_event(void)
// If the MPU60X0 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_mpu_i2c.mpu);
if (imu_mpu_i2c.mpu.data_available) {
- RATES_COPY(imu.gyro_unscaled, imu_mpu_i2c.mpu.data_rates.rates);
- VECT3_COPY(imu.accel_unscaled, imu_mpu_i2c.mpu.data_accel.vect);
+ AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1);
imu_mpu_i2c.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_MPU60X0_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_MPU60X0_ID, now_ts, &imu.accel);
}
}
diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.h b/sw/airborne/modules/imu/imu_mpu60x0_i2c.h
index 8a8eb213e1..cfae20c564 100644
--- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.h
+++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.h
@@ -41,32 +41,6 @@
#define IMU_MPU60X0_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
-#endif
-
struct ImuMpu60x0 {
struct Mpu60x0_I2c mpu;
};
diff --git a/sw/airborne/modules/imu/imu_mpu9250.c b/sw/airborne/modules/imu/imu_mpu9250.c
index b9b40e575c..63663d7c0a 100644
--- a/sw/airborne/modules/imu/imu_mpu9250.c
+++ b/sw/airborne/modules/imu/imu_mpu9250.c
@@ -51,28 +51,31 @@ void imu_mpu9250_event(void)
#include "math/pprz_algebra_int.h"
#include "modules/datalink/downlink.h"
+#include "modules/core/abi.h"
void imu_mpu9250_report(void)
{
+ uint8_t id = IMU_MPU9250_ID;
+
struct Int32Vect3 accel = {
(int32_t)(mpu9250.data_accel.vect.x),
(int32_t)(mpu9250.data_accel.vect.y),
(int32_t)(mpu9250.data_accel.vect.z)
};
- DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &accel.x, &accel.y, &accel.z);
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &id, &accel.x, &accel.y, &accel.z);
struct Int32Rates rates = {
(int32_t)(mpu9250.data_rates.rates.p),
(int32_t)(mpu9250.data_rates.rates.q),
(int32_t)(mpu9250.data_rates.rates.r)
};
- DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &rates.p, &rates.q, &rates.r);
+ DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &id, &rates.p, &rates.q, &rates.r);
struct Int32Vect3 mag = {
(int32_t)(mpu9250.akm.data.vect.x),
(int32_t)(mpu9250.akm.data.vect.y),
(int32_t)(mpu9250.akm.data.vect.z)
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.c b/sw/airborne/modules/imu/imu_mpu9250_i2c.c
index 46e3b25fde..4dd45fd817 100644
--- a/sw/airborne/modules/imu/imu_mpu9250_i2c.c
+++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.c
@@ -25,6 +25,7 @@
*
*/
+#include "modules/imu/imu_mpu9250_i2c.h"
#include "modules/imu/imu.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/sys_time.h"
@@ -109,6 +110,10 @@ void imu_mpu9250_init(void)
imu_mpu9250.mpu.config.dlpf_accel_cfg = IMU_MPU9250_ACCEL_LOWPASS_FILTER;
imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE;
imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE;
+
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]);
}
void imu_mpu9250_periodic(void)
@@ -135,16 +140,10 @@ void imu_mpu9250_event(void)
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
- // unscaled vector
- VECT3_COPY(imu.accel_unscaled, accel);
- RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu9250.mpu.data_available = false;
-
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1);
}
#if IMU_MPU9250_READ_MAG
// Test if mag data are updated
@@ -154,10 +153,8 @@ void imu_mpu9250_event(void)
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
-IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
};
- VECT3_COPY(imu.mag_unscaled, mag);
imu_mpu9250.mpu.akm.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_MPU9250_ID, now_ts, &mag);
}
#endif
diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.h b/sw/airborne/modules/imu/imu_mpu9250_i2c.h
index 746a0e474b..f044f138a3 100644
--- a/sw/airborne/modules/imu/imu_mpu9250_i2c.h
+++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.h
@@ -43,33 +43,6 @@
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#endif
-
-
struct ImuMpu9250 {
struct Mpu9250_I2c mpu;
};
diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.c b/sw/airborne/modules/imu/imu_mpu9250_spi.c
index 469dce6a0e..133c789a7a 100644
--- a/sw/airborne/modules/imu/imu_mpu9250_spi.c
+++ b/sw/airborne/modules/imu/imu_mpu9250_spi.c
@@ -25,6 +25,7 @@
*
*/
+#include "modules/imu/imu_mpu9250_spi.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
@@ -122,6 +123,9 @@ void imu_mpu9250_init(void)
imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE;
imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]);
/* "internal" ak8963 magnetometer as I2C slave */
#if IMU_MPU9250_READ_MAG
@@ -183,9 +187,6 @@ void imu_mpu9250_event(void)
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
- // unscaled vector
- VECT3_COPY(imu.accel_unscaled, accel);
- RATES_COPY(imu.gyro_unscaled, rates);
#if IMU_MPU9250_READ_MAG
if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
@@ -194,18 +195,13 @@ void imu_mpu9250_event(void)
mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
- VECT3_COPY(imu.mag_unscaled, mag);
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_MPU9250_ID, now_ts, &mag);
}
#endif
imu_mpu9250.mpu.data_available = false;
-
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1);
}
}
diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.h b/sw/airborne/modules/imu/imu_mpu9250_spi.h
index 7527427e15..836bfb0511 100644
--- a/sw/airborne/modules/imu/imu_mpu9250_spi.h
+++ b/sw/airborne/modules/imu/imu_mpu9250_spi.h
@@ -43,33 +43,6 @@
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
-#endif
-
-
struct ImuMpu9250 {
struct Mpu9250_Spi mpu;
diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c
index 28480c22d7..a3eb863f06 100644
--- a/sw/airborne/modules/imu/imu_nps.c
+++ b/sw/airborne/modules/imu/imu_nps.c
@@ -19,6 +19,7 @@
* Boston, MA 02111-1307, USA.
*/
+#include "modules/imu/imu_nps.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "generated/airframe.h"
@@ -34,14 +35,39 @@ void imu_nps_init(void)
imu_nps.mag_available = false;
imu_nps.accel_available = false;
+ // Set the default scaling
+ const struct Int32Rates gyro_scale[2] = {
+ {NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM},
+ {NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN}
+ };
+ const struct Int32Rates gyro_neutral = {
+ NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R
+ };
+ const struct Int32Vect3 accel_scale[2] = {
+ {NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM},
+ {NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN}
+ };
+ const struct Int32Vect3 accel_neutral = {
+ NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z
+ };
+ const struct Int32Vect3 mag_scale[2] = {
+ {NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM},
+ {NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN}
+ };
+ const struct Int32Vect3 mag_neutral = {
+ NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z
+ };
+ imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale);
+ imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, accel_scale);
+ imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale);
}
void imu_feed_gyro_accel(void)
{
- RATES_ASSIGN(imu.gyro_unscaled, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z);
- VECT3_ASSIGN(imu.accel_unscaled, sensors.accel.value.x, sensors.accel.value.y, sensors.accel.value.z);
+ RATES_ASSIGN(imu_nps.gyro, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z);
+ VECT3_ASSIGN(imu_nps.accel, sensors.accel.value.x, sensors.accel.value.y, sensors.accel.value.z);
// set availability flags...
imu_nps.accel_available = true;
@@ -53,7 +79,7 @@ void imu_feed_gyro_accel(void)
void imu_feed_mag(void)
{
- VECT3_ASSIGN(imu.mag_unscaled, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z);
+ VECT3_ASSIGN(imu_nps.mag, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z);
imu_nps.mag_available = true;
}
@@ -62,18 +88,15 @@ void imu_nps_event(void)
{
uint32_t now_ts = get_sys_time_usec();
if (imu_nps.gyro_available) {
+ AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1);
imu_nps.gyro_available = false;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_nps.accel_available) {
+ AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1);
imu_nps.accel_available = false;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
if (imu_nps.mag_available) {
+ AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_nps.mag);
imu_nps.mag_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_nps.h b/sw/airborne/modules/imu/imu_nps.h
index 8155b7ef37..72edec50fa 100644
--- a/sw/airborne/modules/imu/imu_nps.h
+++ b/sw/airborne/modules/imu/imu_nps.h
@@ -26,53 +26,14 @@
#include "generated/airframe.h"
-/** we just define some defaults for aspirin v1.5 for now
- */
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS 4.359
-#define IMU_GYRO_P_SENS_NUM 4359
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS 4.359
-#define IMU_GYRO_Q_SENS_NUM 4359
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS 4.359
-#define IMU_GYRO_R_SENS_NUM 4359
-#define IMU_GYRO_R_SENS_DEN 1000
-#endif
-
-
-/** we just define some defaults for aspirin v1.5 for now
- */
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS 37.91
-#define IMU_ACCEL_X_SENS_NUM 3791
-#define IMU_ACCEL_X_SENS_DEN 100
-#define IMU_ACCEL_Y_SENS 37.91
-#define IMU_ACCEL_Y_SENS_NUM 3791
-#define IMU_ACCEL_Y_SENS_DEN 100
-#define IMU_ACCEL_Z_SENS 39.24
-#define IMU_ACCEL_Z_SENS_NUM 3924
-#define IMU_ACCEL_Z_SENS_DEN 100
-#endif
-
-
-#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
-#define IMU_MAG_X_SENS 3.5
-#define IMU_MAG_X_SENS_NUM 7
-#define IMU_MAG_X_SENS_DEN 2
-#define IMU_MAG_Y_SENS 3.5
-#define IMU_MAG_Y_SENS_NUM 7
-#define IMU_MAG_Y_SENS_DEN 2
-#define IMU_MAG_Z_SENS 3.5
-#define IMU_MAG_Z_SENS_NUM 7
-#define IMU_MAG_Z_SENS_DEN 2
-#endif
-
-
struct ImuNps {
uint8_t mag_available;
uint8_t accel_available;
uint8_t gyro_available;
+
+ struct Int32Rates gyro;
+ struct Int32Vect3 accel;
+ struct Int32Vect3 mag;
};
extern struct ImuNps imu_nps;
diff --git a/sw/airborne/modules/imu/imu_px4_defaults.h b/sw/airborne/modules/imu/imu_px4_defaults.h
deleted file mode 100644
index 11bc0702b1..0000000000
--- a/sw/airborne/modules/imu/imu_px4_defaults.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Copyright (C) 2013 Felix Ruess
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file modules/imu/imu_px4_defaults.h
- * Default sensitivity definitions for the Pixhawk IMU using the l3d20 gyro and lsm303dlc acc.
- */
-
-#ifndef IMU_PX4_DEFAULTS_H
-#define IMU_PX4_DEFAULTS_H
-
-#include "generated/airframe.h"
-
-/** default gyro sensitivy and neutral from the datasheet
- * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range
- * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC
- * sens = (70e-3 / 180.0f) * pi * 4096
- */
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS_NUM 5004
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS_NUM 5004
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS_NUM 5004
-#define IMU_GYRO_R_SENS_DEN 1000
-#endif
-
-/** default accel sensitivy from the datasheet
- * LSM303DLHC has 732 LSB/g
- * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC
- * sens = 9.81 / 732 * 1024 = 13.72
- */
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-
-#define IMU_ACCEL_X_SENS 13.723
-#define IMU_ACCEL_X_SENS_NUM 13723
-#define IMU_ACCEL_X_SENS_DEN 1000
-#define IMU_ACCEL_Y_SENS 13.723
-#define IMU_ACCEL_Y_SENS_NUM 13723
-#define IMU_ACCEL_Y_SENS_DEN 1000
-#define IMU_ACCEL_Z_SENS 13.723
-#define IMU_ACCEL_Z_SENS_NUM 13723
-#define IMU_ACCEL_Z_SENS_DEN 1000
-#endif
-
-#endif /* IMU_PX4_DEFAULTS_H */
diff --git a/sw/airborne/modules/imu/imu_px4fmu.c b/sw/airborne/modules/imu/imu_px4fmu.c
index a0d9554cc3..6d9feb0486 100644
--- a/sw/airborne/modules/imu/imu_px4fmu.c
+++ b/sw/airborne/modules/imu/imu_px4fmu.c
@@ -24,6 +24,7 @@
* Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883.
*/
+#include "modules/imu/imu_px4fmu.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/spi.h"
@@ -68,6 +69,10 @@ void imu_px4fmu_init(void)
imu_px4fmu.mpu.config.gyro_range = PX4FMU_GYRO_RANGE;
imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE;
+ // Set the default scaling
+ imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE]);
+ imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE]);
+
/* initialize mag on i2c2 and set default options */
hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR);
}
@@ -87,30 +92,31 @@ void imu_px4fmu_event(void)
mpu60x0_spi_event(&imu_px4fmu.mpu);
if (imu_px4fmu.mpu.data_available) {
- RATES_ASSIGN(imu.gyro_unscaled,
- imu_px4fmu.mpu.data_rates.rates.q,
- imu_px4fmu.mpu.data_rates.rates.p,
- -imu_px4fmu.mpu.data_rates.rates.r);
- VECT3_ASSIGN(imu.accel_unscaled,
- imu_px4fmu.mpu.data_accel.vect.y,
- imu_px4fmu.mpu.data_accel.vect.x,
- -imu_px4fmu.mpu.data_accel.vect.z);
+ struct Int32Rates gyro = {
+ imu_px4fmu.mpu.data_rates.rates.q,
+ imu_px4fmu.mpu.data_rates.rates.p,
+ -imu_px4fmu.mpu.data_rates.rates.r
+ };
+ struct Int32Vect3 accel = {
+ imu_px4fmu.mpu.data_accel.vect.y,
+ imu_px4fmu.mpu.data_accel.vect.x,
+ -imu_px4fmu.mpu.data_accel.vect.z
+ };
imu_px4fmu.mpu.data_available = false;
- imu_scale_gyro(&imu);
- imu_scale_accel(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
}
/* HMC58XX event task */
hmc58xx_event(&imu_px4fmu.hmc);
if (imu_px4fmu.hmc.data_available) {
- imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y;
- imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x;
- imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z;
+ struct Int32Vect3 mag = {
+ imu_px4fmu.hmc.data.vect.y,
+ imu_px4fmu.hmc.data.vect.x,
+ -imu_px4fmu.hmc.data.vect.z
+ };
imu_px4fmu.hmc.data_available = false;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
}
}
diff --git a/sw/airborne/modules/imu/imu_px4fmu.h b/sw/airborne/modules/imu/imu_px4fmu.h
index 4993aec414..08748849cf 100644
--- a/sw/airborne/modules/imu/imu_px4fmu.h
+++ b/sw/airborne/modules/imu/imu_px4fmu.h
@@ -37,38 +37,11 @@
#ifndef PX4FMU_GYRO_RANGE
#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
-PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE)
#ifndef PX4FMU_ACCEL_RANGE
#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
-// Set default sensitivity based on range if needed
-#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
-#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
-#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
-#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
-#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
-#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
-#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
-#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
-#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
-#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
-#endif
-
-// Set default sensitivity based on range if needed
-#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
-#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
-#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
-#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
-#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
-#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
-#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
-#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
-#endif
-
struct ImuPx4fmu {
struct Mpu60x0_Spi mpu;
struct Hmc58xx hmc;
diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c
index 7a40b8c678..959fa1b98f 100644
--- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c
+++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c
@@ -25,12 +25,10 @@
* Driver for pixhawk IMU's.
* L3GD20H + LSM303D (both on spi)
*/
+#include "modules/imu/imu_px4fmu_v2.4.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/spi.h"
-#include "peripherals/l3gd20_regs.h"
-#include "peripherals/lsm303d_regs.h"
-//#include "peripherals/lsm303d_spi.h"
/* SPI defaults set in subsystem makefile, can be configured from airframe file */
PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
@@ -44,9 +42,23 @@ void imu_px4_init(void) {
/* initialize gyro and set default options */
l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX);
- /* LSM303d acc + magneto init */
+ /* LSM303d acc init */
lsm303d_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_ACC);
+
+ // Set the default scaling
+ const struct Int32Rates gyro_scale[2] = {
+ {L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM},
+ {L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN}
+ };
+ const struct Int32Vect3 accel_scale[2] = {
+ {LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM},
+ {LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN}
+ };
+ imu_set_defaults_gyro(IMU_PX4_ID, NULL, NULL, gyro_scale);
+ imu_set_defaults_accel(IMU_PX4_ID, NULL, NULL, accel_scale);
+
#if !IMU_PX4_DISABLE_MAG
+ /* LSM303d mag init */
lsm303d_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_MAG);
#endif
@@ -72,29 +84,33 @@ void imu_px4_event(void) {
l3gd20_spi_event(&imu_px4.l3g);
if (imu_px4.l3g.data_available) {
//the p and q seem to be swapped on the Pixhawk board compared to the acc
- imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q;
- imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p;
- imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r;
+ struct Int32Rates gyro = {
+ imu_px4.l3g.data_rates.rates.q,
+ -imu_px4.l3g.data_rates.rates.p,
+ imu_px4.l3g.data_rates.rates.r
+ };
imu_px4.l3g.data_available = FALSE;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
+ AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1);
}
/* LSM303d event task */
lsm303d_spi_event(&imu_px4.lsm_acc);
if (imu_px4.lsm_acc.data_available_acc) {
- VECT3_COPY(imu.accel_unscaled, imu_px4.lsm_acc.data_accel.vect);
+ struct Int32Vect3 accel;
+ VECT3_COPY(accel, imu_px4.lsm_acc.data_accel.vect);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1);
imu_px4.lsm_acc.data_available_acc = FALSE;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
}
#if !IMU_PX4_DISABLE_MAG
lsm303d_spi_event(&imu_px4.lsm_mag);
if (imu_px4.lsm_mag.data_available_mag) {
- VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect);
+ struct Int32Vect3 mag;
+ VECT3_ASSIGN(mag,
+ imu_px4.lsm_mag.data_mag.vect.x,
+ imu_px4.lsm_mag.data_mag.vect.y,
+ imu_px4.lsm_mag.data_mag.vect.z);
+ AbiSendMsgIMU_MAG_RAW(IMU_PX4_ID, now_ts, &mag);
imu_px4.lsm_mag.data_available_mag = FALSE;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag);
}
#endif
diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.h b/sw/airborne/modules/imu/imu_px4fmu_v2.4.h
index e091816b8d..32a997b8e7 100644
--- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.h
+++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.h
@@ -32,7 +32,6 @@
#include "generated/airframe.h"
#include "modules/imu/imu.h"
-#include "modules/imu/imu_px4_defaults.h"
#include "peripherals/l3gd20_spi.h"
#include "peripherals/lsm303d_spi.h"
diff --git a/sw/airborne/modules/imu/imu_vectornav.c b/sw/airborne/modules/imu/imu_vectornav.c
index a3103ef171..0d62f7e927 100644
--- a/sw/airborne/modules/imu/imu_vectornav.c
+++ b/sw/airborne/modules/imu/imu_vectornav.c
@@ -39,10 +39,6 @@
struct ImuVectornav imu_vn;
-/* no scaling */
-void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
-void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
-
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -151,12 +147,8 @@ void imu_vectornav_propagate(void)
uint64_t tmp = imu_vn.vn_data.nanostamp / 1000;
uint32_t now_ts = (uint32_t) tmp;
- // Rates and accel
- RATES_BFP_OF_REAL(imu.gyro, imu_vn.vn_data.gyro);
- ACCELS_BFP_OF_REAL(imu.accel, imu_vn.vn_data.accel);
-
// Send accel and gyro data separate for other AHRS algorithms
- AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &imu.gyro);
- AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1);
}
diff --git a/sw/airborne/modules/imu/imu_vectornav.h b/sw/airborne/modules/imu/imu_vectornav.h
index e95c87a514..fbc3b80b01 100644
--- a/sw/airborne/modules/imu/imu_vectornav.h
+++ b/sw/airborne/modules/imu/imu_vectornav.h
@@ -52,29 +52,4 @@ void imu_vectornav_event(void);
void imu_vectornav_periodic(void);
void imu_vectornav_propagate(void);
-/* no scaling (the WEAK attribute has no effect */
-#ifndef IMU_GYRO_P_SENS_NUM
-#define IMU_GYRO_P_SENS_NUM 1
-#endif
-
-#ifndef IMU_GYRO_P_SENS_DEN
-#define IMU_GYRO_P_SENS_DEN 1
-#endif
-
-#ifndef IMU_GYRO_Q_SENS_NUM
-#define IMU_GYRO_Q_SENS_NUM 1
-#endif
-
-#ifndef IMU_GYRO_Q_SENS_DEN
-#define IMU_GYRO_Q_SENS_DEN 1
-#endif
-
-#ifndef IMU_GYRO_R_SENS_NUM
-#define IMU_GYRO_R_SENS_NUM 1
-#endif
-
-#ifndef IMU_GYRO_R_SENS_DEN
-#define IMU_GYRO_R_SENS_DEN 1
-#endif
-
#endif /* IMU_VECTORNAV_H */
diff --git a/sw/airborne/modules/ins/imu_xsens.c b/sw/airborne/modules/ins/imu_xsens.c
index 07c62f9680..05a431d7a2 100644
--- a/sw/airborne/modules/ins/imu_xsens.c
+++ b/sw/airborne/modules/ins/imu_xsens.c
@@ -55,41 +55,59 @@ static void handle_ins_msg(void)
uint32_t now_ts = get_sys_time_usec();
#ifdef XSENS_BACKWARDS
if (xsens.gyro_available) {
- RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(xsens.gyro.p), -RATE_BFP_OF_REAL(xsens.gyro.q), RATE_BFP_OF_REAL(xsens.gyro.r));
- xsens.gyro_available = FALSE;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
+ struct Int32Rates gyro = {
+ -RATE_BFP_OF_REAL(xsens.gyro.p),
+ -RATE_BFP_OF_REAL(xsens.gyro.q),
+ RATE_BFP_OF_REAL(xsens.gyro.r)
+ };
+ xsens.gyro_available = FALSE
+ AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1);
}
if (xsens.accel_available) {
- VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(xsens.accel.ax), -ACCEL_BFP_OF_REAL(xsens.accel.ay), ACCEL_BFP_OF_REAL(xsens.accel.az));
+ struct Int32Vect3 accel = {
+ -ACCEL_BFP_OF_REAL(xsens.accel.x),
+ -ACCEL_BFP_OF_REAL(xsens.accel.y),
+ ACCEL_BFP_OF_REAL(xsens.accel.z)
+ };
xsens.accel_available = FALSE;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
+ AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1);
}
if (xsens.mag_available) {
- VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(xsens.mag.mx), -MAG_BFP_OF_REAL(xsens.mag.my), MAG_BFP_OF_REAL(xsens.mag.mz));
+ struct Int32Vect3 mag = {
+ -MAG_BFP_OF_REAL(xsens.mag.x),
+ -MAG_BFP_OF_REAL(xsens.mag.y),
+ MAG_BFP_OF_REAL(xsens.mag.z)
+ };
xsens.mag_available = FALSE;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &mag);
}
#else
if (xsens.gyro_available) {
- RATES_BFP_OF_REAL(imu.gyro_unscaled, xsens.gyro);
+ struct Int32Rates gyro = {
+ RATE_BFP_OF_REAL(xsens.gyro.p),
+ RATE_BFP_OF_REAL(xsens.gyro.q),
+ RATE_BFP_OF_REAL(xsens.gyro.r)
+ };
+ AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1);
xsens.gyro_available = FALSE;
- imu_scale_gyro(&imu);
- AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
}
if (xsens.accel_available) {
- ACCELS_BFP_OF_REAL(imu.accel_unscaled, xsens.accel);
+ struct Int32Vect3 accel = {
+ ACCEL_BFP_OF_REAL(xsens.accel.x),
+ ACCEL_BFP_OF_REAL(xsens.accel.y),
+ ACCEL_BFP_OF_REAL(xsens.accel.z)
+ };
+ AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1);
xsens.accel_available = FALSE;
- imu_scale_accel(&imu);
- AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
}
if (xsens.mag_available) {
- MAGS_BFP_OF_REAL(imu.mag_unscaled, xsens.mag);
+ struct Int32Vect3 mag = {
+ MAG_BFP_OF_REAL(xsens.mag.x),
+ MAG_BFP_OF_REAL(xsens.mag.y),
+ MAG_BFP_OF_REAL(xsens.mag.z)
+ };
+ AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &mag);
xsens.mag_available = FALSE;
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
}
#endif /* XSENS_BACKWARDS */
}
diff --git a/sw/airborne/modules/ins/ins_alt_float.c b/sw/airborne/modules/ins/ins_alt_float.c
index 83555d45e8..8683945b14 100644
--- a/sw/airborne/modules/ins/ins_alt_float.c
+++ b/sw/airborne/modules/ins/ins_alt_float.c
@@ -88,10 +88,6 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
-static abi_event body_to_imu_ev;
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
-static struct OrientationReps body_to_imu;
-
static void alt_kalman_reset(void);
static void alt_kalman_init(void);
static void alt_kalman(float z_meas, float dt);
@@ -110,10 +106,6 @@ void ins_alt_float_init(void)
ins_altf.origin_initialized = false;
#endif
- // set initial body to imu to 0
- struct Int32Eulers b2i0 = { 0, 0, 0 };
- orientationSetEulers_i(&body_to_imu, &b2i0);
-
alt_kalman_init();
#if USE_BAROMETER
@@ -131,8 +123,7 @@ void ins_alt_float_init(void)
AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb);
#endif
AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_ACCEL(INS_ALT_IMU_ID, &accel_ev, accel_cb);
}
/** Reset the geographic reference to the current GPS fix */
@@ -373,17 +364,10 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *accel)
{
// untilt accel and remove gravity
- struct Int32Vect3 accel_body, accel_ned;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
- int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
+ struct Int32Vect3 accel_ned;
+ stateSetAccelBody_i(accel);
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
- int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
+ int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
}
-
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&body_to_imu, q_b2i_f);
-}
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index fc394f997c..86677c38e5 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -280,22 +280,20 @@ PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
static abi_event baro_ev;
static abi_event temperature_ev;
static abi_event agl_ev;
-static abi_event gyro_ev;
-static abi_event accel_ev;
+static abi_event gyro_int_ev;
+static abi_event accel_int_ev;
static abi_event mag_ev;
static abi_event gps_ev;
-static abi_event body_to_imu_ev;
static abi_event optical_flow_ev;
/* All ABI callbacks */
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
static void temperature_cb(uint8_t sender_id, float temp);
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
-static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
-static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
+static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt);
+static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x,
int32_t flow_der_y, float quality, float size_divergence);
@@ -511,8 +509,6 @@ void ins_ekf2_init(void)
/* Initialize struct */
ekf2.ltp_stamp = 0;
- ekf2.accel_stamp = 0;
- ekf2.gyro_stamp = 0;
ekf2.flow_stamp = 0;
ekf2.gyro_valid = false;
ekf2.accel_valid = false;
@@ -565,11 +561,10 @@ void ins_ekf2_init(void)
AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
- AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
+ AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
+ AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
}
@@ -700,9 +695,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
imuSample imu_sample = {};
imu_sample.time_us = stamp;
imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
- imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt;
+ imu_sample.delta_ang = Vector3f{ekf2.delta_gyro.p, ekf2.delta_gyro.q, ekf2.delta_gyro.r};
imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
- imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt;
+ imu_sample.delta_vel = Vector3f{ekf2.delta_accel.x, ekf2.delta_accel.y, ekf2.delta_accel.z};
ekf.setIMUData(imu_sample);
if (ekf.attitude_valid()) {
@@ -743,9 +738,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get in-run gyro bias */
struct FloatRates body_rates;
Vector3f gyro_bias{ekf.getGyroBias()};
- body_rates.p = ekf2.gyro.p - gyro_bias(0);
- body_rates.q = ekf2.gyro.q - gyro_bias(1);
- body_rates.r = ekf2.gyro.r - gyro_bias(2);
+ body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
+ body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
+ body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
// Publish it to the state
stateSetBodyRates_f(&body_rates);
@@ -753,9 +748,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get the in-run acceleration bias */
struct Int32Vect3 accel;
Vector3f accel_bias{ekf.getAccelBias()};
- accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias(0));
- accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias(1));
- accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias(2));
+ accel.x = ACCEL_BFP_OF_REAL((ekf2.delta_accel.x / (ekf2.accel_dt * 1e-6f)) - accel_bias(0));
+ accel.y = ACCEL_BFP_OF_REAL((ekf2.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
+ accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
// Publish it to the state
stateSetAccelBody_i(&accel);
@@ -799,24 +794,13 @@ static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, fl
}
/* Update INS based on Gyro information */
-static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
- uint32_t stamp, struct Int32Rates *gyro)
+static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
+ uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
{
- FloatRates imu_rate;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
-
- // Convert Gyro information to float
- RATES_FLOAT_OF_BFP(imu_rate, *gyro);
-
- // Rotate with respect to Body To IMU
- float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
-
- // Calculate the Gyro interval
- if (ekf2.gyro_stamp > 0) {
- ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
- ekf2.gyro_valid = true;
- }
- ekf2.gyro_stamp = stamp;
+ // Copy and save the gyro data
+ RATES_COPY(ekf2.delta_gyro, *delta_gyro);
+ ekf2.gyro_dt = dt;
+ ekf2.gyro_valid = true;
/* When Gyro and accelerometer are valid enter it into the EKF */
if (ekf2.gyro_valid && ekf2.accel_valid) {
@@ -825,24 +809,13 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
}
/* Update INS based on Accelerometer information */
-static void accel_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp, struct Int32Vect3 *accel)
+static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
{
- struct FloatVect3 accel_imu;
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
-
- // Convert Accelerometer information to float
- ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
-
- // Rotate with respect to Body To IMU
- float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
-
- // Calculate the Accelerometer interval
- if (ekf2.accel_stamp > 0) {
- ekf2.accel_dt = stamp - ekf2.accel_stamp;
- ekf2.accel_valid = true;
- }
- ekf2.accel_stamp = stamp;
+ // Copy and save the gyro data
+ VECT3_COPY(ekf2.delta_accel, *delta_accel);
+ ekf2.accel_dt = dt;
+ ekf2.accel_valid = true;
/* When Gyro and accelerometer are valid enter it into the EKF */
if (ekf2.gyro_valid && ekf2.accel_valid) {
@@ -855,8 +828,7 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp,
struct Int32Vect3 *mag)
{
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
- struct FloatVect3 mag_gauss, mag_body;
+ struct FloatVect3 mag_gauss;
magSample sample;
sample.time_us = stamp;
@@ -866,13 +838,10 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id,
mag_gauss.y *= 0.4f;
mag_gauss.z *= 0.4f;
- // Rotate with respect to Body To IMU
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
-
// Publish information to the EKF
- sample.mag(0) = mag_body.x;
- sample.mag(1) = mag_body.y;
- sample.mag(2) = mag_body.z;
+ sample.mag(0) = mag_gauss.x;
+ sample.mag(1) = mag_gauss.y;
+ sample.mag(2) = mag_gauss.z;
ekf.setMagData(sample);
ekf2.got_imu_data = true;
@@ -910,13 +879,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ekf.setGpsData(gps_msg);
}
-/* Save the Body to IMU information */
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&ekf2.body_to_imu, q_b2i_f);
-}
-
/* Update INS based on Optical Flow information */
static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp,
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index 6894c91041..4a817b6ec0 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -38,24 +38,20 @@ extern "C" {
/* Main EKF2 structure for keeping track of the status and use cross messaging */
struct ekf2_t {
- uint32_t gyro_stamp; ///< Gyroscope last abi message timestamp
- uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages
- uint32_t accel_stamp; ///< Accelerometer last abi message timestamp
- uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages
- uint32_t flow_stamp; ///< Optic flow last abi message timestamp
+ struct FloatRates delta_gyro; ///< Last gyroscope measurements
+ struct FloatVect3 delta_accel; ///< Last accelerometer measurements
+ uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages (us)
+ uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages (us)
+ bool gyro_valid; ///< If we received a gyroscope measurement
+ bool accel_valid; ///< If we received a acceleration measurement
+ uint32_t flow_stamp; ///< Optic flow last abi message timestamp
- struct FloatRates gyro; ///< Last gyroscope measurements
- struct FloatVect3 accel; ///< Last accelerometer measurements
- bool gyro_valid; ///< If we received a gyroscope measurement
- bool accel_valid; ///< If we received a acceleration measurement
-
- float temp; ///< Latest temperature measurement in degrees celcius
- float qnh; ///< QNH value in hPa
- uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
- uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
- struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counterEKF2
- struct OrientationReps body_to_imu; ///< Body to IMU rotation
- bool got_imu_data; ///< If we received valid IMU data (any sensor)
+ float temp; ///< Latest temperature measurement in degrees celcius
+ float qnh; ///< QNH value in hPa
+ uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
+ uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
+ struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counter EKF2
+ bool got_imu_data; ///< If we received valid IMU data (any sensor)
int32_t mag_fusion_type;
int32_t fusion_mode;
diff --git a/sw/airborne/modules/ins/ins_float_invariant.c b/sw/airborne/modules/ins/ins_float_invariant.c
index fb443bcef8..44ea15a56f 100644
--- a/sw/airborne/modules/ins/ins_float_invariant.c
+++ b/sw/airborne/modules/ins/ins_float_invariant.c
@@ -353,9 +353,8 @@ void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* a
}
// fill command vector in body frame
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
- float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
- float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
+ RATES_COPY(ins_float_inv.cmd.rates, *gyro);
+ VECT3_COPY(ins_float_inv.cmd.accel, *accel);
struct Int32Vect3 body_accel_i;
ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
@@ -575,10 +574,8 @@ void ins_float_invariant_update_mag(struct FloatVect3* mag)
mag_frozen_count = MAG_FROZEN_COUNT;
}
} else {
- // values are moving
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
// new values in body frame
- float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
+ VECT3_COPY(ins_float_inv.meas.mag, *mag);
// reset counter
mag_frozen_count = MAG_FROZEN_COUNT;
}
@@ -747,13 +744,3 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
VECT3_ADD(v2, v1);
QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
}
-
-void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ins_float_inv.body_to_imu, q_b2i);
-
- if (!ins_float_inv.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ins_float_inv.state.quat = *q_b2i;
- }
-}
diff --git a/sw/airborne/modules/ins/ins_float_invariant.h b/sw/airborne/modules/ins/ins_float_invariant.h
index a6516ff07c..f95770ff34 100644
--- a/sw/airborne/modules/ins/ins_float_invariant.h
+++ b/sw/airborne/modules/ins/ins_float_invariant.h
@@ -112,9 +112,6 @@ struct InsFloatInv {
bool reset; ///< flag to request reset/reinit the filter
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
-
struct FloatVect3 mag_h;
bool is_aligned;
};
@@ -122,7 +119,6 @@ struct InsFloatInv {
extern struct InsFloatInv ins_float_inv;
extern void ins_float_invariant_init(void);
-extern void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ins_float_invariant_align(struct FloatRates *lp_gyro,
struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag);
diff --git a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
index aa09aebcfa..069ee88b88 100644
--- a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
+++ b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c
@@ -113,7 +113,6 @@ static abi_event baro_ev;
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
#if USE_MAGNETOMETER
static abi_event mag_ev;
static abi_event geo_mag_ev;
@@ -186,12 +185,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ins_float_inv_set_body_to_imu_quat(q_b2i_f);
-}
-
#if USE_MAGNETOMETER
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
@@ -229,12 +222,11 @@ void ins_float_invariant_wrapper_init(void)
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
- AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_GYRO(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(INS_FINV_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
#if USE_MAGNETOMETER
- AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_MAG(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#endif
AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ins/ins_gps_passthrough.c b/sw/airborne/modules/ins/ins_gps_passthrough.c
index 43627a4ad4..bc8bccfa0b 100644
--- a/sw/airborne/modules/ins/ins_gps_passthrough.c
+++ b/sw/airborne/modules/ins/ins_gps_passthrough.c
@@ -66,10 +66,6 @@ struct InsGpsPassthrough ins_gp;
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
-static abi_event body_to_imu_ev;
-static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
-static struct OrientationReps body_to_imu;
-
/** ABI binding for gps data.
* Used for GPS ABI messages.
@@ -170,8 +166,7 @@ void ins_gps_passthrough_init(void)
#endif
AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_PT_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_PT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
+ AbiBindMsgIMU_ACCEL(INS_PT_IMU_ID, &accel_ev, accel_cb);
}
void ins_reset_local_origin(void)
@@ -200,20 +195,11 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *accel)
{
// untilt accel and remove gravity
- struct Int32Vect3 accel_body, accel_ned;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
- int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_body);
+ struct Int32Vect3 accel_ned;
+ stateSetAccelBody_i(accel);
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
- int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
+ int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
VECT3_COPY(ins_gp.ltp_accel, accel_ned);
}
-
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&body_to_imu, q_b2i_f);
-}
-
diff --git a/sw/airborne/modules/ins/ins_int.c b/sw/airborne/modules/ins/ins_int.c
index d5a62ee57f..8016ebd35b 100644
--- a/sw/airborne/modules/ins/ins_int.c
+++ b/sw/airborne/modules/ins/ins_int.c
@@ -120,12 +120,14 @@ static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
#ifndef INS_INT_IMU_ID
#define INS_INT_IMU_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_IMU_ID)
static abi_event accel_ev;
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
#ifndef INS_INT_GPS_ID
#define INS_INT_GPS_ID GPS_MULTI_ID
#endif
+PRINT_CONFIG_VAR(INS_INT_GPS_ID)
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
@@ -135,6 +137,7 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
#ifndef INS_INT_VEL_ID
#define INS_INT_VEL_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_VEL_ID)
static abi_event vel_est_ev;
static void vel_est_cb(uint8_t sender_id,
uint32_t stamp,
@@ -143,6 +146,7 @@ static void vel_est_cb(uint8_t sender_id,
#ifndef INS_INT_POS_ID
#define INS_INT_POS_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_POS_ID)
static abi_event pos_est_ev;
static void pos_est_cb(uint8_t sender_id,
uint32_t stamp,
@@ -155,6 +159,7 @@ static void pos_est_cb(uint8_t sender_id,
#ifndef INS_INT_AGL_ID
#define INS_INT_AGL_ID ABI_BROADCAST
#endif
+PRINT_CONFIG_VAR(INS_INT_AGL_ID)
static abi_event agl_ev; ///< The agl ABI event
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
@@ -234,7 +239,7 @@ void ins_int_init(void)
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
- AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_ACCEL(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
@@ -283,12 +288,8 @@ void ins_reset_altitude_ref(void)
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
{
/* untilt accels */
- struct Int32Vect3 accel_meas_body;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
- int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_meas_body);
struct Int32Vect3 accel_meas_ltp;
- int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
+ int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
index f866fc2ccc..2c277b5496 100644
--- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
+++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
@@ -206,7 +206,6 @@ static abi_event mag_ev;
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
@@ -302,11 +301,8 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Rates *gyro)
{
if (ins_mekf_wind.is_aligned) {
- struct FloatRates gyro_f, gyro_body;
- RATES_FLOAT_OF_BFP(gyro_f, *gyro);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
+ struct FloatRates gyro_body;
+ RATES_FLOAT_OF_BFP(gyro_body, *gyro);
#if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
@@ -383,11 +379,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
- struct FloatVect3 accel_f;
- ACCELS_FLOAT_OF_BFP(accel_f, *accel);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_vmult(&ins_mekf_wind_accel, body_to_imu_rmat, &accel_f);
+ ACCELS_FLOAT_OF_BFP(ins_mekf_wind_accel, *accel);
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
@@ -395,11 +387,9 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_mekf_wind.is_aligned) {
- struct FloatVect3 mag_f, mag_body;
- MAGS_FLOAT_OF_BFP(mag_f, *mag);
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
- // new values in body frame
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
+ struct FloatVect3 mag_body;
+ MAGS_FLOAT_OF_BFP(mag_body, *mag);
+
// only correct attitude if GPS is not initialized
ins_mekf_wind_update_mag(&mag_body, !ins_mekf_wind.gps_initialized);
@@ -420,19 +410,14 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag)
{
if (!ins_mekf_wind.is_aligned) {
- struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
+ struct FloatRates gyro_body;
+ RATES_FLOAT_OF_BFP(gyro_body, *lp_gyro);
- struct FloatRates gyro_f, gyro_body;
- RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
- float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
+ struct FloatVect3 accel_body;
+ ACCELS_FLOAT_OF_BFP(accel_body, *lp_accel);
- struct FloatVect3 accel_f, accel_body;
- ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
- float_rmat_transp_vmult(&accel_body, body_to_imu_rmat, &accel_f);
-
- struct FloatVect3 mag_f, mag_body;
- MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
- float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
+ struct FloatVect3 mag_body;
+ MAGS_FLOAT_OF_BFP(mag_body, *lp_mag);
struct FloatQuat quat;
ahrs_float_get_quat_from_accel_mag(&quat, &accel_body, &mag_body);
@@ -445,16 +430,6 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ins_mekf_wind.body_to_imu, q_b2i);
- if (!ins_mekf_wind.is_aligned) {
- // set ltp_to_imu so that body is zero
- ins_mekf_wind_set_quat(q_b2i);
- }
-}
-
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ins_mekf_wind_set_mag_h(h);
@@ -589,11 +564,10 @@ void ins_mekf_wind_wrapper_init(void)
AbiBindMsgBARO_DIFF(INS_MEKF_WIND_AIRSPEED_ID, &pressure_diff_ev, pressure_diff_cb);
AbiBindMsgAIRSPEED(INS_MEKF_WIND_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
AbiBindMsgINCIDENCE(INS_MEKF_WIND_INCIDENCE_ID, &incidence_ev, incidence_cb);
- AbiBindMsgIMU_MAG_INT32(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_GYRO_INT32(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(INS_MEKF_WIND_IMU_ID, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_MEKF_WIND_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(INS_MEKF_WIND_GPS_ID, &gps_ev, gps_cb);
diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
index 7c4b9db998..f8c1394215 100644
--- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
+++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.h
@@ -33,7 +33,6 @@
/** filter structure
*/
struct InsMekfWind {
- struct OrientationReps body_to_imu;
bool is_aligned;
bool baro_initialized;
bool gps_initialized;
diff --git a/sw/airborne/modules/ins/ins_skeleton.c b/sw/airborne/modules/ins/ins_skeleton.c
index 049e7fef13..2279da4010 100644
--- a/sw/airborne/modules/ins/ins_skeleton.c
+++ b/sw/airborne/modules/ins/ins_skeleton.c
@@ -49,7 +49,7 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#endif
PRINT_CONFIG_VAR(INS_MODULE_BARO_ID)
-/** IMU (accel, body_to_imu) */
+/** IMU (accel) */
#ifndef INS_MODULE_IMU_ID
#define INS_MODULE_IMU_ID ABI_BROADCAST
#endif
@@ -66,7 +66,6 @@ PRINT_CONFIG_VAR(INS_MODULE_GPS_ID)
static abi_event baro_ev;
static abi_event accel_ev;
static abi_event gps_ev;
-static abi_event body_to_imu_ev;
struct InsModuleInt ins_module;
@@ -137,11 +136,6 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
last_stamp = stamp;
}
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- orientationSetQuat_f(&ins_module.body_to_imu, q_b2i_f);
-}
/*********************************************************************
* weak functions that are used if not implemented in a module
********************************************************************/
@@ -185,12 +179,9 @@ void WEAK ins_module_update_gps(struct GpsState *gps_s, float dt __attribute__((
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused)))
{
/* untilt accels */
- struct Int32Vect3 accel_meas_body;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu);
- int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
- stateSetAccelBody_i(&accel_meas_body);
+ stateSetAccelBody_i(accel);
struct Int32Vect3 accel_meas_ltp;
- int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
+ int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp);
}
@@ -237,8 +228,7 @@ void ins_module_wrapper_init(void)
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_ACCEL(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
diff --git a/sw/airborne/modules/ins/ins_skeleton.h b/sw/airborne/modules/ins/ins_skeleton.h
index 8214251284..3a4c39291b 100644
--- a/sw/airborne/modules/ins/ins_skeleton.h
+++ b/sw/airborne/modules/ins/ins_skeleton.h
@@ -48,9 +48,6 @@ struct InsModuleInt {
/** internal copy of last GPS message */
struct GpsState gps;
-
- /** body_to_imu rotation */
- struct OrientationReps body_to_imu;
};
/** global INS state */
diff --git a/sw/airborne/modules/ins/ins_vectornav.c b/sw/airborne/modules/ins/ins_vectornav.c
index 450b421a89..9c8fea28bc 100644
--- a/sw/airborne/modules/ins/ins_vectornav.c
+++ b/sw/airborne/modules/ins/ins_vectornav.c
@@ -74,25 +74,29 @@ static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
static void send_accel(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
+ uint8_t id = IMU_VECTORNAV_ID;
+ pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &id,
&ins_vn.vn_data.accel.x, &ins_vn.vn_data.accel.y, &ins_vn.vn_data.accel.z);
}
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
+ uint8_t id = IMU_VECTORNAV_ID;
+ pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &id,
&ins_vn.vn_data.gyro.p, &ins_vn.vn_data.gyro.q, &ins_vn.vn_data.gyro.r);
}
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
+ uint8_t id = IMU_VECTORNAV_ID;
+ pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &id,
&ins_vn.accel_i.x, &ins_vn.accel_i.y, &ins_vn.accel_i.z);
}
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
+ uint8_t id = IMU_VECTORNAV_ID;
+ pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &id,
&ins_vn.gyro_i.p, &ins_vn.gyro_i.q, &ins_vn.gyro_i.r);
}
#endif
diff --git a/sw/airborne/modules/meteo/wind_estimator.c b/sw/airborne/modules/meteo/wind_estimator.c
index 23fadbbd41..aa4c1df104 100644
--- a/sw/airborne/modules/meteo/wind_estimator.c
+++ b/sw/airborne/modules/meteo/wind_estimator.c
@@ -314,11 +314,10 @@ void wind_estimator_periodic(void)
// float_rmat_vmult(&accel_body, ned_to_body, &accel_ned);
///// IMU test
- struct FloatVect3 accel_body = {
- .x = ACCEL_FLOAT_OF_BFP(imu.accel.x),
- .y = ACCEL_FLOAT_OF_BFP(imu.accel.y),
- .z = ACCEL_FLOAT_OF_BFP(imu.accel.z)
- };
+ struct Int32Vect3 *accel_body_i = stateGetAccelBody_i();
+ struct FloatVect3 accel_body;
+ ACCELS_FLOAT_OF_BFP(accel_body, *accel_body_i);
+
struct FloatVect3 tmp = accel_body;
//struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
//int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
diff --git a/sw/airborne/modules/pose_history/pose_history.c b/sw/airborne/modules/pose_history/pose_history.c
index a1408f4e88..92fac78efb 100644
--- a/sw/airborne/modules/pose_history/pose_history.c
+++ b/sw/airborne/modules/pose_history/pose_history.c
@@ -56,12 +56,12 @@ struct pose_t get_rotation_at_timestamp(uint32_t timestamp)
pthread_mutex_lock(&pose_mutex);
#endif
uint32_t index_history = 0;
- uint32_t closestTimeDiff = abs(timestamp - location_history.ring_data[0].timestamp);
+ uint32_t closestTimeDiff = timestamp - location_history.ring_data[0].timestamp;
uint32_t closestIndex = 0;
// Seach for the timestamp closes to the timestamp argument of this function.
for (index_history = 0; index_history < location_history.ring_size; index_history++) {
- uint32_t diff = abs(timestamp - location_history.ring_data[index_history].timestamp);
+ uint32_t diff = timestamp - location_history.ring_data[index_history].timestamp;
if (diff < closestTimeDiff) {
closestIndex = index_history;
closestTimeDiff = diff;
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
index bb24333243..0817c5149d 100644
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -22,7 +22,7 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include
-
+#include "modules/core/abi.h"
#include "../../peripherals/hmc5843.h"
@@ -43,7 +43,8 @@ void hmc5843_module_periodic(void)
mag_x = hmc5843.data.value[0];
mag_y = hmc5843.data.value[1];
mag_z = hmc5843.data.value[2];
- RunOnceEvery(30, DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag_x, &mag_y, &mag_z));
+ uint8_t id = MAG_HMC58XX_SENDER_ID;
+ RunOnceEvery(30, DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag_x, &mag_y, &mag_z));
}
void hmc5843_module_event(void)
diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index 28c1d1dbf2..51e4de52e5 100644
--- a/sw/airborne/modules/sensors/mag_hmc58xx.c
+++ b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -30,6 +30,7 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include "generated/airframe.h"
+#include "modules/core/abi.h"
#ifndef HMC58XX_CHAN_X
#define HMC58XX_CHAN_X 0
@@ -51,8 +52,6 @@
#endif
#if MODULE_HMC58XX_UPDATE_AHRS
-#include "modules/imu/imu.h"
-#include "modules/core/abi.h"
#if defined HMC58XX_MAG_TO_IMU_PHI && defined HMC58XX_MAG_TO_IMU_THETA && defined HMC58XX_MAG_TO_IMU_PSI
#define USE_MAG_TO_IMU 1
@@ -104,15 +103,10 @@ void mag_hmc58xx_module_event(void)
// rotate data from mag frame to imu frame
int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
// unscaled vector
- VECT3_COPY(imu.mag_unscaled, imu_mag);
-#else
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, mag);
+ VECT3_COPY(mag, imu_mag);
#endif
- // scale vector
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_HMC58XX_SENDER_ID, now_ts, &mag);
#endif
#if MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx_report();
@@ -125,10 +119,11 @@ void mag_hmc58xx_module_event(void)
void mag_hmc58xx_report(void)
{
+ uint8_t id = MAG_HMC58XX_SENDER_ID;
struct Int32Vect3 mag = {
HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/modules/sensors/mag_ist8310.c b/sw/airborne/modules/sensors/mag_ist8310.c
index 23e1b90d2b..9eff18c6df 100644
--- a/sw/airborne/modules/sensors/mag_ist8310.c
+++ b/sw/airborne/modules/sensors/mag_ist8310.c
@@ -29,6 +29,7 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include "generated/airframe.h"
+#include "modules/core/abi.h"
#ifndef IST8310_CHAN_X
#define IST8310_CHAN_X 1
@@ -49,31 +50,22 @@
#define IST8310_CHAN_Z_SIGN +
#endif
-#if MODULE_IST8310_UPDATE_AHRS
-#include "modules/imu/imu.h"
-#include "modules/core/abi.h"
-
-#if defined IST8310_MAG_TO_IMU_PHI && defined IST8310_MAG_TO_IMU_THETA && defined IST8310_MAG_TO_IMU_PSI
-#define USE_MAG_TO_IMU 1
-static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
-#else
-#define USE_MAG_TO_IMU 0
-#endif
-#endif
-
struct IST8310 mag_ist8310;
void mag_ist8310_module_init(void)
{
ist8310_init(&mag_ist8310, &(MAG_IST8310_I2C_DEV), IST8310_ADDR);
-#if MODULE_IST8310_UPDATE_AHRS && USE_MAG_TO_IMU
+#if MODULE_IST8310_UPDATE_AHRS && defined(IST8310_MAG_TO_IMU_PHI) && defined(IST8310_MAG_TO_IMU_THETA) && defined(IST8310_MAG_TO_IMU_PSI)
+ struct Int32RMat mag_to_imu;
struct Int32Eulers mag_to_imu_eulers = {
ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PSI)
};
int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
+
+ imu_set_defaults_mag(MAG_IST8310_SENDER_ID, &mag_to_imu, NULL, NULL)
#endif
}
@@ -97,21 +89,8 @@ void mag_ist8310_module_event(void)
IST8310_CHAN_Y_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Y]),
IST8310_CHAN_Z_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Z])
};
- // only rotate if needed
-#if USE_MAG_TO_IMU
- struct Int32Vect3 imu_mag;
- // rotate data from mag frame to imu frame
- int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, imu_mag);
-#else
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, mag);
-#endif
- // scale vector
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(MAG_IST8310_SENDER_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_IST8310_SENDER_ID, now_ts, &mag);
#endif
#if MODULE_IST8310_SYNC_SEND
mag_ist8310_report();
@@ -124,10 +103,11 @@ void mag_ist8310_module_event(void)
void mag_ist8310_report(void)
{
+ uint8_t id = MAG_IST8310_SENDER_ID;
struct Int32Vect3 mag = {
IST8310_CHAN_X_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_X]),
IST8310_CHAN_Y_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Y]),
IST8310_CHAN_Z_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Z])
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/modules/sensors/mag_lis3mdl.c b/sw/airborne/modules/sensors/mag_lis3mdl.c
index 1716ba9db2..9ad4d2b2f7 100644
--- a/sw/airborne/modules/sensors/mag_lis3mdl.c
+++ b/sw/airborne/modules/sensors/mag_lis3mdl.c
@@ -29,6 +29,7 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include "generated/airframe.h"
+#include "modules/core/abi.h"
#ifndef LIS3MDL_CHAN_X
#define LIS3MDL_CHAN_X 0
@@ -49,18 +50,6 @@
#define LIS3MDL_CHAN_Z_SIGN +
#endif
-#if MODULE_LIS3MDL_UPDATE_AHRS
-#include "modules/imu/imu.h"
-#include "modules/core/abi.h"
-
-#if defined LIS3MDL_MAG_TO_IMU_PHI && defined LIS3MDL_MAG_TO_IMU_THETA && defined LIS3MDL_MAG_TO_IMU_PSI
-#define USE_MAG_TO_IMU 1
-static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
-#else
-#define USE_MAG_TO_IMU 0
-#endif
-#endif
-
struct Lis3mdl mag_lis3mdl;
void mag_lis3mdl_module_init(void)
@@ -70,15 +59,6 @@ void mag_lis3mdl_module_init(void)
LIS3MDL_SCALE_4_GAUSS,
LIS3MDL_MODE_CONTINUOUS,
LIS3MDL_PERFORMANCE_ULTRA_HIGH);
-
-#if MODULE_LIS3MDL_UPDATE_AHRS && USE_MAG_TO_IMU
- struct Int32Eulers mag_to_imu_eulers = {
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PHI),
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_THETA),
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PSI)
- };
- int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
-#endif
}
void mag_lis3mdl_module_periodic(void)
@@ -101,21 +81,8 @@ void mag_lis3mdl_module_event(void)
LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]),
LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Z])
};
- // only rotate if needed
-#if USE_MAG_TO_IMU
- struct Int32Vect3 imu_mag;
- // rotate data from mag frame to imu frame
- int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, imu_mag);
-#else
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, mag);
-#endif
- // scale vector
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(MAG_LIS3MDL_SENDER_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_LIS3MDL_SENDER_ID, now_ts, &mag);
#endif
#if MODULE_LIS3MDL_SYNC_SEND
mag_lis3mdl_report();
@@ -128,10 +95,11 @@ void mag_lis3mdl_module_event(void)
void mag_lis3mdl_report(void)
{
+ uint8_t id = MAG_LIS3MDL_SENDER_ID;
struct Int32Vect3 mag = {
LIS3MDL_CHAN_X_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_X]),
LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]),
LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Z])
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/modules/sensors/mag_pitot_uart.c b/sw/airborne/modules/sensors/mag_pitot_uart.c
index f333e01b52..7fe1932bc1 100644
--- a/sw/airborne/modules/sensors/mag_pitot_uart.c
+++ b/sw/airborne/modules/sensors/mag_pitot_uart.c
@@ -40,17 +40,6 @@ static struct mag_pitot_t mag_pitot = {
};
static uint8_t mp_msg_buf[128] __attribute__((aligned)); ///< The message buffer for the Magneto and pitot
-
-#if PERIODIC_TELEMETRY
-#include "modules/datalink/telemetry.h"
-
-static void mag_pitot_raw_downlink(struct transport_tx *trans, struct link_device *dev)
-{
- pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mag_unscaled.x, &imu.mag_unscaled.y,
- &imu.mag_unscaled.z);
-}
-#endif
-
/* Initialize the magneto and pitot */
void mag_pitot_init() {
// Initialize transport protocol
@@ -60,10 +49,6 @@ void mag_pitot_init() {
struct FloatEulers imu_to_mag_eulers =
{IMU_TO_MAG_PHI, IMU_TO_MAG_THETA, IMU_TO_MAG_PSI};
orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers);
-
-#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, mag_pitot_raw_downlink);
-#endif
}
/* Parse the InterMCU message */
@@ -81,7 +66,7 @@ static inline void mag_pitot_parse_msg(void)
switch (msg_id) {
/* Got a magneto message */
case DL_IMCU_REMOTE_MAG: {
- struct Int32Vect3 raw_mag;
+ struct Int32Vect3 raw_mag, mag;
// Read the raw magneto information
raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
@@ -90,11 +75,10 @@ static inline void mag_pitot_parse_msg(void)
// Rotate the magneto
struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
- int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);
+ int32_rmat_vmult(&mag, imu_to_mag_rmat, &raw_mag);
// Send and set the magneto IMU data
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(IMU_MAG_PITOT_ID, now_ts, &mag);
break;
}
diff --git a/sw/airborne/modules/sensors/mag_rm3100.c b/sw/airborne/modules/sensors/mag_rm3100.c
index 507a828a2b..6f98bbe7ec 100644
--- a/sw/airborne/modules/sensors/mag_rm3100.c
+++ b/sw/airborne/modules/sensors/mag_rm3100.c
@@ -29,6 +29,7 @@
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#include "generated/airframe.h"
+#include "modules/core/abi.h"
#ifndef RM3100_CHAN_X
#define RM3100_CHAN_X 0
@@ -58,8 +59,6 @@
#endif
#if MODULE_RM3100_UPDATE_AHRS
-#include "modules/imu/imu.h"
-#include "modules/core/abi.h"
#if defined RM3100_MAG_TO_IMU_PHI && defined RM3100_MAG_TO_IMU_THETA && defined RM3100_MAG_TO_IMU_PSI
#define USE_MAG_TO_IMU 1
@@ -111,15 +110,10 @@ void mag_rm3100_module_event(void)
// rotate data from mag frame to imu frame
int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
// unscaled vector
- VECT3_COPY(imu.mag_unscaled, imu_mag);
-#else
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, mag);
+ VECT3_COPY(mag, imu_mag);
#endif
- // scale vector
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(MAG_RM3100_SENDER_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_RM3100_SENDER_ID, now_ts, &mag);
#endif
#if MODULE_RM3100_SYNC_SEND
mag_rm3100_report();
@@ -132,10 +126,11 @@ void mag_rm3100_module_event(void)
void mag_rm3100_report(void)
{
+ uint8_t id = MAG_RM3100_SENDER_ID;
struct Int32Vect3 mag = {
RM3100_CHAN_X_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_X]),
RM3100_CHAN_Y_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_Y]),
RM3100_CHAN_Z_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_Z])
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/peripherals/bmi088.c b/sw/airborne/peripherals/bmi088.c
index ee14de28de..3bd9915683 100644
--- a/sw/airborne/peripherals/bmi088.c
+++ b/sw/airborne/peripherals/bmi088.c
@@ -36,12 +36,17 @@ const float BMI088_GYRO_SENS[5] = {
BMI088_GYRO_SENS_125,
};
-const int32_t BMI088_GYRO_SENS_FRAC[5][2] = {
- { BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_DEN },
- { BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_DEN },
- { BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_DEN },
- { BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_DEN },
- { BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_DEN },
+const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2] = {
+ { {BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM},
+ {BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN} },
+ { {BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM},
+ {BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN} },
+ { {BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM},
+ {BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN} },
+ { {BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM},
+ {BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN} },
+ { {BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM},
+ {BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN} }
};
const float BMI088_ACCEL_SENS[4] = {
@@ -51,11 +56,15 @@ const float BMI088_ACCEL_SENS[4] = {
BMI088_ACCEL_SENS_24G
};
-const int32_t BMI088_ACCEL_SENS_FRAC[4][2] = {
- { BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_DEN },
- { BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_DEN },
- { BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_DEN },
- { BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_DEN }
+const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2] = {
+ { {BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM},
+ {BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN} },
+ { {BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM},
+ {BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN} },
+ { {BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM},
+ {BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN} },
+ { {BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM},
+ {BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN} }
};
void bmi088_set_default_config(struct Bmi088Config *c)
diff --git a/sw/airborne/peripherals/bmi088.h b/sw/airborne/peripherals/bmi088.h
index 17fb183d2c..a8de841205 100644
--- a/sw/airborne/peripherals/bmi088.h
+++ b/sw/airborne/peripherals/bmi088.h
@@ -28,6 +28,7 @@
#define BMI088_H
#include "std.h"
+#include "math/pprz_algebra_int.h"
/* Include address and register definition */
#include "peripherals/bmi088_regs.h"
@@ -67,7 +68,7 @@
// Get default sensitivity from a table
extern const float BMI088_GYRO_SENS[5];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t BMI088_GYRO_SENS_FRAC[5][2];
+extern const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
@@ -91,7 +92,7 @@ extern const int32_t BMI088_GYRO_SENS_FRAC[5][2];
// Get default sensitivity from a table
extern const float BMI088_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t BMI088_ACCEL_SENS_FRAC[4][2];
+extern const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2];
enum Bmi088ConfStatus {
BMI088_CONF_UNINIT,
diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c
new file mode 100644
index 0000000000..fbc83fea61
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2.c
@@ -0,0 +1,562 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2.c
+ *
+ * Driver for the Invensense V2 IMUs
+ * ICM20948, ICM20648 and ICM20649
+ */
+
+#include "peripherals/invensense2.h"
+#include "peripherals/invensense2_regs.h"
+#include "math/pprz_isa.h"
+#include "math/pprz_algebra_int.h"
+#include "modules/imu/imu.h"
+#include "modules/core/abi.h"
+#include "mcu_periph/gpio_arch.h"
+
+
+/* Local functions */
+static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *data, uint16_t len);
+static void invensense2_fix_config(struct invensense2_t *inv);
+static bool invensense2_register_write(struct invensense2_t *inv, uint16_t bank_reg, uint8_t value);
+static bool invensense2_register_read(struct invensense2_t *inv, uint16_t bank_reg, uint16_t size);
+static bool invensense2_select_bank(struct invensense2_t *inv, uint8_t bank);
+static bool invensense2_config(struct invensense2_t *inv);
+
+/* Default gyro scalings */
+static const struct Int32Rates invensense2_gyro_scale[5][2] = {
+ { {30267, 30267, 30267},
+ {55463, 55463, 55463} }, // 250DPS
+ { {60534, 60534, 60534},
+ {55463, 55463, 55463} }, // 500DPS
+ { {40147, 40147, 40147},
+ {18420, 18420, 18420} }, // 1000DPS
+ { {40147, 40147, 40147},
+ {9210, 9210, 9210} }, // 2000DPS
+ { {40147, 40147, 40147},
+ {4605, 4605, 4605} } // 4000DPS
+};
+
+/* Default accel scalings */
+static const struct Int32Vect3 invensense2_accel_scale[5][2] = {
+ { {3189, 3189, 3189},
+ {5203, 5203, 5203} }, // 2G
+ { {6378, 6378, 6378},
+ {5203, 5203, 5203} }, // 4G
+ { {12756, 12756, 12756},
+ {5203, 5203, 5203} }, // 8G
+ { {25512, 25512, 25512},
+ {5203, 5203, 5203} }, // 16G
+ { {51024, 51024, 51024},
+ {5203, 5203, 5203} } // 30G
+};
+
+/**
+ * @brief Initialize the invensense v2 sensor instance
+ *
+ * @param inv The structure containing the configuratio of the invensense v2 instance
+ */
+void invensense2_init(struct invensense2_t *inv) {
+ /* General setup */
+ inv->status = INVENSENSE2_IDLE;
+ inv->device = INVENSENSE2_UNKOWN;
+ inv->register_bank = 0xFF;
+ inv->config_idx = 0;
+
+ /* SPI setup */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.cpol = SPICpolIdleHigh;
+ inv->spi.trans.cpha = SPICphaEdge2;
+ inv->spi.trans.dss = SPIDss8bit;
+ inv->spi.trans.bitorder = SPIMSBFirst;
+ inv->spi.trans.cdiv = SPIDiv16;
+
+ inv->spi.trans.select = SPISelectUnselect;
+ inv->spi.trans.slave_idx = inv->spi.slave_idx;
+ inv->spi.trans.output_length = 0;
+ inv->spi.trans.input_length = 0;
+ inv->spi.trans.before_cb = NULL;
+ inv->spi.trans.after_cb = NULL;
+ inv->spi.trans.input_buf = inv->spi.rx_buf;
+ inv->spi.trans.output_buf = inv->spi.tx_buf;
+ inv->spi.trans.status = SPITransDone;
+ }
+ /* I2C setup */
+ else {
+ inv->i2c.trans.slave_addr = inv->i2c.slave_addr;
+ inv->i2c.trans.status = I2CTransDone;
+ }
+}
+
+/**
+ * @brief Should be called periodically to request sensor readings
+ * - First detects the sensor using WHO_AM_I reading
+ * - Configures the sensor according the users requested configuration
+ * - Requests a sensor reading by reading the FIFO_COUNT register
+ *
+ * @param inv The invensense v2 instance
+ */
+void invensense2_periodic(struct invensense2_t *inv) {
+ /* Idle */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransDone) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransDone)) {
+
+ /* Depending on the status choose what to do */
+ switch(inv->status) {
+ case INVENSENSE2_IDLE:
+ /* Request WHO_AM_I */
+ invensense2_register_read(inv, INV2REG_WHO_AM_I, 1);
+ break;
+ case INVENSENSE2_CONFIG:
+ /* Start configuring */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_RUNNING:
+ /* Request a sensor reading */
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2);
+ break;
+ }
+ }
+}
+
+/**
+ * @brief Should be called in the event thread
+ * - Checks the response of the WHO_AM_I reading
+ * - Configures the sensor and reads the responses
+ * - Parse and request the sensor data from the FIFO buffers
+ *
+ * @param inv The invensense v2 instance
+ */
+void invensense2_event(struct invensense2_t *inv) {
+ volatile uint8_t *rx_buffer, *tx_buffer;
+ uint16_t rx_length = 0;
+
+ /* Set the buffers depending on the interface */
+ if(inv->bus == INVENSENSE2_SPI) {
+ rx_buffer = inv->spi.rx_buf;
+ tx_buffer = inv->spi.tx_buf;
+ rx_length = inv->spi.trans.input_length;
+ }
+ else {
+ rx_buffer = inv->i2c.trans.buf;
+ tx_buffer = inv->i2c.trans.buf;
+ rx_length = inv->i2c.trans.len_r;
+ }
+
+ /* Successful transfer */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransSuccess) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransSuccess)) {
+
+ /* Update the register bank */
+ if(tx_buffer[0] == INV2REG_BANK_SEL)
+ inv->register_bank = inv->spi.tx_buf[1] >> 4;
+
+ /* Set the transaction as done and update register bank if needed */
+ if(inv->bus == INVENSENSE2_SPI)
+ inv->spi.trans.status = SPITransDone;
+ else
+ inv->i2c.trans.status = I2CTransDone;
+
+ /* Look at the results */
+ switch(inv->status) {
+ case INVENSENSE2_IDLE:
+ /* Check the response of the WHO_AM_I */
+ if(rx_buffer[1] == INV2_WHOAMI_ICM20648) {
+ inv->device = INVENSENSE2_ICM20648;
+ inv->status = INVENSENSE2_CONFIG;
+ } else if(rx_buffer[1] == INV2_WHOAMI_ICM20649) {
+ inv->device = INVENSENSE2_ICM20649;
+ inv->status = INVENSENSE2_CONFIG;
+ } else if(rx_buffer[1] == INV2_WHOAMI_ICM20948) {
+ inv->device = INVENSENSE2_ICM20948;
+ inv->status = INVENSENSE2_CONFIG;
+ }
+
+ /* Fix the configuration and set the scaling */
+ if(inv->status == INVENSENSE2_CONFIG)
+ invensense2_fix_config(inv);
+ break;
+ case INVENSENSE2_CONFIG:
+ /* Apply the next configuration register */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_RUNNING: {
+ /* Parse the results */
+ static const uint16_t max_bytes = sizeof(inv->spi.rx_buf) - 3;
+ uint16_t fifo_bytes = (uint16_t)rx_buffer[1] << 8 | rx_buffer[2];
+
+ // We read an incorrect length (try again)
+ if(fifo_bytes > 4096) {
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2);
+ return;
+ }
+
+ // Parse the data
+ if((rx_length - 3) > 0) {
+ uint16_t valid_bytes = ((rx_length - 3) < fifo_bytes)? (rx_length - 3) : fifo_bytes;
+ invensense2_parse_data(inv, &rx_buffer[3], valid_bytes);
+ inv->timer -= valid_bytes;
+ } else {
+ fifo_bytes -= fifo_bytes%INVENSENSE2_SAMPLE_SIZE;
+ inv->timer = fifo_bytes;
+ }
+
+ // If we have more data request more
+ if(inv->timer > 0) {
+ uint16_t read_bytes = (inv->timer > max_bytes)? max_bytes : inv->timer;
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2 + read_bytes);
+ }
+ break;
+ }
+ }
+ }
+ /* Failed transaction */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransFailed) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransFailed)) {
+
+ /* Set the transaction as done and update register bank if needed */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.status = SPITransDone;
+ }
+ else {
+ inv->i2c.trans.status = I2CTransDone;
+ }
+
+ /* Retry or ignore */
+ switch(inv->status) {
+ case INVENSENSE2_CONFIG:
+ /* If was not a bus switch decrease the index */
+ if(inv->config_idx > 0 && ((inv->bus == INVENSENSE2_SPI && inv->spi.tx_buf[0] != INV2REG_BANK_SEL) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.buf[0] != INV2REG_BANK_SEL))) {
+ inv->config_idx--;
+ }
+ /* Try again */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_IDLE:
+ case INVENSENSE2_RUNNING:
+ /* Ignore while idle/running */
+ break;
+ }
+ }
+}
+
+/**
+ * @brief Parse the FIFO buffer data
+ *
+ * @param inv The invensense v2 instance
+ * @param data The FIFO buffer data to parse
+ * @param len The length of the FIFO buffer
+ */
+static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *data, uint16_t len) {
+ uint8_t samples = len / INVENSENSE2_SAMPLE_SIZE;
+ static struct Int32Vect3 accel[INVENSENSE2_SAMPLE_CNT] = {0};
+ static struct Int32Rates gyro[INVENSENSE2_SAMPLE_CNT] = {0};
+
+ if(samples > INVENSENSE2_SAMPLE_CNT)
+ samples = INVENSENSE2_SAMPLE_CNT;
+
+ uint16_t gyro_samplerate = 9000;
+ if(inv->gyro_dlpf != INVENSENSE2_GYRO_DLPF_OFF)
+ gyro_samplerate = 1125;
+
+ uint16_t accel_samplerate = 4500;
+ if(inv->accel_dlpf != INVENSENSE2_ACCEL_DLPF_OFF)
+ accel_samplerate = 1125;
+
+ // Go through the different samples
+ uint8_t j = 0;
+ uint8_t downsample = gyro_samplerate / accel_samplerate;
+ int32_t temp = 0;
+ for(uint8_t i = 0; i < samples; i++) {
+ if(i % downsample == 0) {
+ accel[j].x = (int16_t)((uint16_t)data[2] << 8 | data[3]);
+ accel[j].y = (int16_t)((uint16_t)data[0] << 8 | data[1]);
+ accel[j].z = -(int16_t)((uint16_t)data[4] << 8 | data[5]);
+ j++;
+ }
+
+ gyro[i].p = (int16_t)((uint16_t)data[8] << 8 | data[9]);
+ gyro[i].q = (int16_t)((uint16_t)data[6] << 8 | data[7]);
+ gyro[i].r = -(int16_t)((uint16_t)data[10] << 8 | data[11]);
+
+ temp += (int16_t)((uint16_t)data[12] << 8 | data[13]);
+ data += INVENSENSE2_SAMPLE_SIZE;
+ }
+
+ float temp_f = ((float)temp / samples) / 333.87f - 21.f;
+
+ // Send the scaled values over ABI
+ uint32_t now_ts = get_sys_time_usec();
+ AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples);
+ AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j);
+ AbiSendMsgTEMPERATURE(inv->abi_id, temp_f);
+}
+
+/**
+ * @brief This fixes the configuration errors and sets the correct scalings
+ *
+ * @param inv The invensense v2 instance
+ */
+static void invensense2_fix_config(struct invensense2_t *inv) {
+ /* Fix wrong configuration settings (prevent user error) */
+ if(inv->device == INVENSENSE2_ICM20649) {
+ if(inv->gyro_range == INVENSENSE2_GYRO_RANGE_250DPS)
+ inv->gyro_range = INVENSENSE2_GYRO_RANGE_500DPS;
+ if(inv->accel_range == INVENSENSE2_ACCEL_RANGE_2G)
+ inv->accel_range = INVENSENSE2_ACCEL_RANGE_4G;
+ }
+ else {
+ if(inv->gyro_range == INVENSENSE2_GYRO_RANGE_4000DPS)
+ inv->gyro_range = INVENSENSE2_GYRO_RANGE_2000DPS;
+ if(inv->accel_range == INVENSENSE2_ACCEL_RANGE_30G)
+ inv->accel_range = INVENSENSE2_ACCEL_RANGE_16G;
+ }
+
+ /* Set the default values */
+ imu_set_defaults_gyro(inv->abi_id, NULL, NULL, invensense2_gyro_scale[inv->gyro_range]);
+ imu_set_defaults_accel(inv->abi_id, NULL, NULL, invensense2_accel_scale[inv->accel_range]);
+}
+
+/**
+ * @brief Write a register based on a combined bank/regsiter value
+ *
+ * @param inv The invensense v2 instance
+ * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF
+ * @param value The value to write to the register
+ * @return true Whenever the register write was started
+ * @return false First we are busy switching the register bank
+ */
+static bool invensense2_register_write(struct invensense2_t *inv, uint16_t bank_reg, uint8_t value) {
+ /* Split the register and bank */
+ uint8_t bank = bank_reg >> 8;
+ uint8_t reg = bank_reg & 0xFF;
+
+ /* Switch the register bank if needed */
+ if(invensense2_select_bank(inv, bank)) {
+ return false;
+ }
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 0;
+ inv->spi.tx_buf[0] = reg;
+ inv->spi.tx_buf[1] = value;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = reg;
+ inv->i2c.trans.buf[1] = value;
+ i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
+ }
+
+ return true;
+}
+
+/**
+ * @brief Read a register based on a combined bank/regsiter value
+ *
+ * @param inv The invensense v2 instance
+ * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF
+ * @param size The size to read (already 1 is added for the transmission of the register to read)
+ * @return true If we initiated the register read succesfully
+ * @return false First we are busy switching the register bank
+ */
+static bool invensense2_register_read(struct invensense2_t *inv, uint16_t bank_reg, uint16_t size) {
+ /* Split the register and bank */
+ uint8_t bank = bank_reg >> 8;
+ uint8_t reg = bank_reg & 0xFF;
+
+ /* Switch the register bank if needed */
+ if(invensense2_select_bank(inv, bank)) {
+ return false;
+ }
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 1 + size;
+ inv->spi.tx_buf[0] = reg | INV2_READ_FLAG;
+ inv->spi.tx_buf[1] = 0;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = reg | INV2_READ_FLAG;
+ i2c_transceive(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 1, (1 + size));
+ }
+
+ return true;
+}
+
+/**
+ * @brief Select the correct register bank
+ *
+ * @param inv The invensense v2 instance
+ * @param bank The bank ID to select
+ * @return true The bank change has been requested
+ * @return false The register bank is already correct
+ */
+static bool invensense2_select_bank(struct invensense2_t *inv, uint8_t bank) {
+ /* If we already selected the correct bank continue */
+ if(inv->register_bank == bank)
+ return false;
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 0;
+ inv->spi.tx_buf[0] = INV2REG_BANK_SEL;
+ inv->spi.tx_buf[1] = bank << 4;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = INV2REG_BANK_SEL;
+ inv->i2c.trans.buf[1] = bank << 4;
+ i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
+ }
+
+ return true;
+}
+
+/**
+ * @brief Configure the Invensense 2 device register by register
+ *
+ * @param inv The invensense v2 instance
+ * @return true When the configuration is completed
+ * @return false Still busy configuring
+ */
+static bool invensense2_config(struct invensense2_t *inv) {
+ switch(inv->config_idx) {
+ case 0:
+ /* Reset the device */
+ if(invensense2_register_write(inv, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET))
+ inv->config_idx++;
+ inv->timer = get_sys_time_usec();
+ break;
+ case 1: {
+ /* Reset I2C and FIFO SRAM, disable I2C if using SPI */
+ uint8_t user_ctrl = BIT_USER_CTRL_I2C_MST_RESET | BIT_USER_CTRL_SRAM_RESET;
+ if(inv->bus == INVENSENSE2_SPI)
+ user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
+
+ /* Because reset takes time wait ~100ms */
+ if((get_sys_time_usec() - inv->timer) < 100000)
+ break;
+
+ if(invensense2_register_write(inv, INV2REG_USER_CTRL, user_ctrl))
+ inv->config_idx++;
+ break;
+ }
+ case 2:
+ /* Wakeup the device in auto clock mode */
+ if(invensense2_register_write(inv, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_AUTO))
+ inv->config_idx++;
+ inv->timer = get_sys_time_usec();
+ break;
+ case 3: {
+ /* Configure gyro */
+ uint8_t gyro_config = 0;
+ if(inv->gyro_dlpf != INVENSENSE2_GYRO_DLPF_OFF)
+ gyro_config |= BIT_GYRO_DLPF_ENABLE | ((inv->gyro_dlpf - 1) << GYRO_DLPF_CFG_SHIFT);
+ if((inv->device == INVENSENSE2_ICM20649 && inv->gyro_range > 0) || inv->gyro_range > 3)
+ gyro_config |= (inv->gyro_range - 1) << GYRO_FS_SEL_SHIFT;
+ else
+ gyro_config |= inv->gyro_range << GYRO_FS_SEL_SHIFT;
+
+ /* Because reset takes time wait ~100ms */
+ if((get_sys_time_usec() - inv->timer) < 100000)
+ break;
+
+ if(invensense2_register_write(inv, INV2REG_GYRO_CONFIG_1, gyro_config))
+ inv->config_idx++;
+ break;
+ }
+ case 4: {
+ /* Configure accelerometer */
+ uint8_t accel_config = 0;
+ if(inv->accel_dlpf != INVENSENSE2_ACCEL_DLPF_OFF)
+ accel_config |= BIT_ACCEL_DLPF_ENABLE | ((inv->accel_dlpf - 1) << ACCEL_DLPF_CFG_SHIFT);
+ if((inv->device == INVENSENSE2_ICM20649 && inv->accel_range > 0) || inv->accel_range > 3)
+ accel_config |= (inv->accel_range - 1) << ACCEL_FS_SEL_SHIFT;
+ else
+ accel_config |= inv->accel_range << ACCEL_FS_SEL_SHIFT;
+ if(invensense2_register_write(inv, INV2REG_ACCEL_CONFIG, accel_config))
+ inv->config_idx++;
+ break;
+ }
+ case 5:
+ /* Set the FIFO mode */
+ if(invensense2_register_write(inv, INV2REG_FIFO_MODE, 0xF))
+ inv->config_idx++;
+ break;
+ case 6:
+ /* Set the GYRO sample rate divider */
+ if(invensense2_register_write(inv, INV2REG_GYRO_SMPLRT_DIV, 0))
+ inv->config_idx++;
+ break;
+ case 7:
+ /* FIFO reset 1 */
+ if(invensense2_register_write(inv, INV2REG_FIFO_RST, 0x0F))
+ inv->config_idx++;
+ break;
+ case 8:
+ /* FIFO reset 2 */
+ if(invensense2_register_write(inv, INV2REG_FIFO_RST, 0x00))
+ inv->config_idx++;
+ break;
+ case 9: {
+ /* Enable FIFO */
+ uint8_t user_ctrl = BIT_USER_CTRL_FIFO_EN;
+ if(inv->bus == INVENSENSE2_SPI)
+ user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
+ if(invensense2_register_write(inv, INV2REG_USER_CTRL, user_ctrl))
+ inv->config_idx++;
+ break;
+ }
+ case 10:
+ /* Cofigure FIFO enable */
+ if(invensense2_register_write(inv, INV2REG_FIFO_EN_2, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
+ BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN))
+ inv->config_idx++;
+ break;
+ case 11:
+ /* Enable interrupt pin/status */
+ if(invensense2_register_write(inv, INV2REG_INT_ENABLE_1, 0x1))
+ inv->config_idx++;
+ break;
+ default:
+ inv->timer = 0;
+ return true;
+ }
+ return false;
+}
diff --git a/sw/airborne/peripherals/invensense2.h b/sw/airborne/peripherals/invensense2.h
new file mode 100644
index 0000000000..80ab174dff
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2.h
+ *
+ * Driver for the Invensense V2 IMUs
+ * ICM20948, ICM20648 and ICM20649
+ */
+
+#ifndef INVENSENSE2_H
+#define INVENSENSE2_H
+
+#include "std.h"
+#include "mcu_periph/spi.h"
+#include "mcu_periph/i2c.h"
+
+// Hold 22 measurements and 3 for the register address and length
+#define INVENSENSE2_SAMPLE_CNT 22
+#define INVENSENSE2_SAMPLE_SIZE 14
+#define INVENSENSE2_BUFFER_SIZE ((INVENSENSE2_SAMPLE_SIZE*INVENSENSE2_SAMPLE_CNT) + 3)
+
+/* Invensense v2 SPI peripheral */
+struct invensense2_spi_t {
+ struct spi_periph *p; ///< Peripheral device for communication
+ uint8_t slave_idx; ///< Slave index used for Slave Select
+ struct spi_transaction trans; ///< Transaction used during configuration and measurements
+
+ volatile uint8_t tx_buf[2]; ///< Transmit buffer
+ volatile uint8_t rx_buf[INVENSENSE2_BUFFER_SIZE]; ///< Receive buffer
+};
+
+/* Invensense v2 I2C peripheral */
+struct invensense2_i2c_t {
+ struct i2c_periph *p; ///< Peripheral device for communication
+ uint8_t slave_addr; ///< The I2C slave address on the bus
+ struct i2c_transaction trans; ///< TRansaction used during configuration and measurements
+};
+
+/* Possible communication busses for the invense V2 driver */
+enum invensense2_bus_t {
+ INVENSENSE2_SPI,
+ INVENSENSE2_I2C
+};
+
+/* Different states the invensense driver can be in */
+enum invensense2_status_t {
+ INVENSENSE2_IDLE,
+ INVENSENSE2_CONFIG,
+ INVENSENSE2_RUNNING
+};
+
+/* Different device types compatible with the invensense V2 driver */
+enum invensense2_device_t {
+ INVENSENSE2_UNKOWN,
+ INVENSENSE2_ICM20648,
+ INVENSENSE2_ICM20649,
+ INVENSENSE2_ICM20948
+};
+
+/* The gyro digital low pass filter bandwidth configuration */
+enum invensense2_gyro_dlpf_t {
+ INVENSENSE2_GYRO_DLPF_OFF,
+ INVENSENSE2_GYRO_DLPF_229HZ,
+ INVENSENSE2_GYRO_DLPF_188HZ,
+ INVENSENSE2_GYRO_DLPF_154HZ,
+ INVENSENSE2_GYRO_DLPF_73HZ,
+ INVENSENSE2_GYRO_DLPF_36HZ,
+ INVENSENSE2_GYRO_DLPF_18HZ,
+ INVENSENSE2_GYRO_DLPF_9HZ,
+ INVENSENSE2_GYRO_DLPF_377HZ
+};
+
+/* The gyro range in degrees per second(dps) */
+enum invensense2_gyro_range_t {
+ INVENSENSE2_GYRO_RANGE_250DPS, ///< Not possible for ICM20649
+ INVENSENSE2_GYRO_RANGE_500DPS,
+ INVENSENSE2_GYRO_RANGE_1000DPS,
+ INVENSENSE2_GYRO_RANGE_2000DPS,
+ INVENSENSE2_GYRO_RANGE_4000DPS ///< Only possible for ICM20649
+};
+
+/* The accelerometer digital low pass filter bandwidth configuration */
+enum invensense2_accel_dlpf_t {
+ INVENSENSE2_ACCEL_DLPF_OFF,
+ INVENSENSE2_ACCEL_DLPF_265HZ,
+ INVENSENSE2_ACCEL_DLPF_136HZ,
+ INVENSENSE2_ACCEL_DLPF_69HZ,
+ INVENSENSE2_ACCEL_DLPF_34HZ,
+ INVENSENSE2_ACCEL_DLPF_17HZ,
+ INVENSENSE2_ACCEL_DLPF_8HZ,
+ INVENSENSE2_ACCEL_DLPF_499HZ
+};
+
+/* The accelerometer range in G */
+enum invensense2_accel_range_t {
+ INVENSENSE2_ACCEL_RANGE_2G, ///< Not possible for ICM20649
+ INVENSENSE2_ACCEL_RANGE_4G,
+ INVENSENSE2_ACCEL_RANGE_8G,
+ INVENSENSE2_ACCEL_RANGE_16G,
+ INVENSENSE2_ACCEL_RANGE_30G ///< Only possible for ICM20649
+};
+
+/* Main invensense V2 device structure */
+struct invensense2_t {
+ uint8_t abi_id; ///< The ABI id used to broadcast the device measurements
+ enum invensense2_status_t status; ///< Status of the invensense V2 device
+ enum invensense2_device_t device; ///< The device type detected
+
+ enum invensense2_bus_t bus; ///< The communication bus used to connect the device SPI/I2C
+ union {
+ struct invensense2_spi_t spi; ///< SPI specific configuration
+ struct invensense2_i2c_t i2c; ///< I2C specific configuration
+ };
+
+ uint8_t register_bank; ///< The current register bank communicating with
+ uint8_t config_idx; ///< The current configuration index
+ uint32_t timer; ///< Used to time operations during configuration (samples left during measuring)
+
+ enum invensense2_gyro_dlpf_t gyro_dlpf; ///< Gyro DLPF configuration
+ enum invensense2_gyro_range_t gyro_range; ///< Gyro range configuration
+ enum invensense2_accel_dlpf_t accel_dlpf; ///< Accelerometer DLPF configuration
+ enum invensense2_accel_range_t accel_range; ///< Accelerometer range configuration
+
+ // float temp; ///< temperature in degrees Celcius
+};
+
+/* External functions */
+void invensense2_init(struct invensense2_t *inv);
+void invensense2_periodic(struct invensense2_t *inv);
+void invensense2_event(struct invensense2_t *inv);
+
+#endif // INVENSENSE2_H
diff --git a/sw/airborne/peripherals/invensense2_regs.h b/sw/airborne/peripherals/invensense2_regs.h
new file mode 100644
index 0000000000..b9513783c6
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2_regs.h
@@ -0,0 +1,242 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2_regs.h
+ *
+ * Register and address definitions for the Invensense V2 from ardupilot.
+ */
+
+#ifndef INVENSENSE2_REGS_H
+#define INVENSENSE2_REGS_H
+
+#define INV2_BANK0 0x00U
+#define INV2_BANK1 0x01U
+#define INV2_BANK2 0x02U
+#define INV2_BANK3 0x03U
+
+
+#define INV2REG(b, r) ((((uint16_t)b) << 8)|(r))
+#define INV2_READ_FLAG 0x80
+
+//Register Map
+#define INV2REG_WHO_AM_I INV2REG(INV2_BANK0,0x00U)
+#define INV2REG_USER_CTRL INV2REG(INV2_BANK0,0x03U)
+# define BIT_USER_CTRL_I2C_MST_RESET 0x02 // reset I2C Master (only applicable if I2C_MST_EN bit is set)
+# define BIT_USER_CTRL_SRAM_RESET 0x04 // Reset (i.e. clear) FIFO buffer
+# define BIT_USER_CTRL_DMP_RESET 0x08 // Reset DMP
+# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
+# define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
+# define BIT_USER_CTRL_FIFO_EN 0x40 // Enable FIFO operations
+# define BIT_USER_CTRL_DMP_EN 0x80 // Enable DMP operations
+#define INV2REG_LP_CONFIG INV2REG(INV2_BANK0,0x05U)
+#define INV2REG_PWR_MGMT_1 INV2REG(INV2_BANK0,0x06U)
+# define BIT_PWR_MGMT_1_CLK_INTERNAL 0x00 // clock set to internal 8Mhz oscillator
+# define BIT_PWR_MGMT_1_CLK_AUTO 0x01 // PLL with X axis gyroscope reference
+# define BIT_PWR_MGMT_1_CLK_STOP 0x07 // Stops the clock and keeps the timing generator in reset
+# define BIT_PWR_MGMT_1_TEMP_DIS 0x08 // disable temperature sensor
+# define BIT_PWR_MGMT_1_SLEEP 0x40 // put sensor into low power sleep mode
+# define BIT_PWR_MGMT_1_DEVICE_RESET 0x80 // reset entire device
+#define INV2REG_PWR_MGMT_2 INV2REG(INV2_BANK0,0x07U)
+#define INV2REG_INT_PIN_CFG INV2REG(INV2_BANK0,0x0FU)
+# define BIT_BYPASS_EN 0x02
+# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
+# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
+#define INV2REG_INT_ENABLE INV2REG(INV2_BANK0,0x10U)
+# define BIT_PLL_RDY_EN 0x04
+#define INV2REG_INT_ENABLE_1 INV2REG(INV2_BANK0,0x11U)
+#define INV2REG_INT_ENABLE_2 INV2REG(INV2_BANK0,0x12U)
+#define INV2REG_INT_ENABLE_3 INV2REG(INV2_BANK0,0x13U)
+#define INV2REG_I2C_MST_STATUS INV2REG(INV2_BANK0,0x17U)
+#define INV2REG_INT_STATUS INV2REG(INV2_BANK0,0x19U)
+
+#define INV2REG_INT_STATUS_1 INV2REG(INV2_BANK0,0x1AU)
+#define INV2REG_INT_STATUS_2 INV2REG(INV2_BANK0,0x1BU)
+#define INV2REG_INT_STATUS_3 INV2REG(INV2_BANK0,0x1CU)
+#define INV2REG_DELAY_TIMEH INV2REG(INV2_BANK0,0x28U)
+#define INV2REG_DELAY_TIMEL INV2REG(INV2_BANK0,0x29U)
+#define INV2REG_ACCEL_XOUT_H INV2REG(INV2_BANK0,0x2DU)
+#define INV2REG_ACCEL_XOUT_L INV2REG(INV2_BANK0,0x2EU)
+#define INV2REG_ACCEL_YOUT_H INV2REG(INV2_BANK0,0x2FU)
+#define INV2REG_ACCEL_YOUT_L INV2REG(INV2_BANK0,0x30U)
+#define INV2REG_ACCEL_ZOUT_H INV2REG(INV2_BANK0,0x31U)
+#define INV2REG_ACCEL_ZOUT_L INV2REG(INV2_BANK0,0x32U)
+#define INV2REG_GYRO_XOUT_H INV2REG(INV2_BANK0,0x33U)
+#define INV2REG_GYRO_XOUT_L INV2REG(INV2_BANK0,0x34U)
+#define INV2REG_GYRO_YOUT_H INV2REG(INV2_BANK0,0x35U)
+#define INV2REG_GYRO_YOUT_L INV2REG(INV2_BANK0,0x36U)
+#define INV2REG_GYRO_ZOUT_H INV2REG(INV2_BANK0,0x37U)
+#define INV2REG_GYRO_ZOUT_L INV2REG(INV2_BANK0,0x38U)
+#define INV2REG_TEMP_OUT_H INV2REG(INV2_BANK0,0x39U)
+#define INV2REG_TEMP_OUT_L INV2REG(INV2_BANK0,0x3AU)
+#define INV2REG_EXT_SLV_SENS_DATA_00 INV2REG(INV2_BANK0,0x3BU)
+#define INV2REG_EXT_SLV_SENS_DATA_01 INV2REG(INV2_BANK0,0x3CU)
+#define INV2REG_EXT_SLV_SENS_DATA_02 INV2REG(INV2_BANK0,0x3DU)
+#define INV2REG_EXT_SLV_SENS_DATA_03 INV2REG(INV2_BANK0,0x3EU)
+#define INV2REG_EXT_SLV_SENS_DATA_04 INV2REG(INV2_BANK0,0x3FU)
+#define INV2REG_EXT_SLV_SENS_DATA_05 INV2REG(INV2_BANK0,0x40U)
+#define INV2REG_EXT_SLV_SENS_DATA_06 INV2REG(INV2_BANK0,0x41U)
+#define INV2REG_EXT_SLV_SENS_DATA_07 INV2REG(INV2_BANK0,0x42U)
+#define INV2REG_EXT_SLV_SENS_DATA_08 INV2REG(INV2_BANK0,0x43U)
+#define INV2REG_EXT_SLV_SENS_DATA_09 INV2REG(INV2_BANK0,0x44U)
+#define INV2REG_EXT_SLV_SENS_DATA_10 INV2REG(INV2_BANK0,0x45U)
+#define INV2REG_EXT_SLV_SENS_DATA_11 INV2REG(INV2_BANK0,0x46U)
+#define INV2REG_EXT_SLV_SENS_DATA_12 INV2REG(INV2_BANK0,0x47U)
+#define INV2REG_EXT_SLV_SENS_DATA_13 INV2REG(INV2_BANK0,0x48U)
+#define INV2REG_EXT_SLV_SENS_DATA_14 INV2REG(INV2_BANK0,0x49U)
+#define INV2REG_EXT_SLV_SENS_DATA_15 INV2REG(INV2_BANK0,0x4AU)
+#define INV2REG_EXT_SLV_SENS_DATA_16 INV2REG(INV2_BANK0,0x4BU)
+#define INV2REG_EXT_SLV_SENS_DATA_17 INV2REG(INV2_BANK0,0x4CU)
+#define INV2REG_EXT_SLV_SENS_DATA_18 INV2REG(INV2_BANK0,0x4DU)
+#define INV2REG_EXT_SLV_SENS_DATA_19 INV2REG(INV2_BANK0,0x4EU)
+#define INV2REG_EXT_SLV_SENS_DATA_20 INV2REG(INV2_BANK0,0x4FU)
+#define INV2REG_EXT_SLV_SENS_DATA_21 INV2REG(INV2_BANK0,0x50U)
+#define INV2REG_EXT_SLV_SENS_DATA_22 INV2REG(INV2_BANK0,0x51U)
+#define INV2REG_EXT_SLV_SENS_DATA_23 INV2REG(INV2_BANK0,0x52U)
+#define INV2REG_FIFO_EN_1 INV2REG(INV2_BANK0,0x66U)
+# define BIT_SLV3_FIFO_EN 0x08
+# define BIT_SLV2_FIFO_EN 0x04
+# define BIT_SLV1_FIFO_EN 0x02
+# define BIT_SLV0_FIFI_EN0 0x01
+#define INV2REG_FIFO_EN_2 INV2REG(INV2_BANK0,0x67U)
+# define BIT_ACCEL_FIFO_EN 0x10
+# define BIT_ZG_FIFO_EN 0x08
+# define BIT_YG_FIFO_EN 0x04
+# define BIT_XG_FIFO_EN 0x02
+# define BIT_TEMP_FIFO_EN 0x01
+#define INV2REG_FIFO_RST INV2REG(INV2_BANK0,0x68U)
+#define INV2REG_FIFO_MODE INV2REG(INV2_BANK0,0x69U)
+#define INV2REG_FIFO_COUNTH INV2REG(INV2_BANK0,0x70U)
+#define INV2REG_FIFO_COUNTL INV2REG(INV2_BANK0,0x71U)
+#define INV2REG_FIFO_R_W INV2REG(INV2_BANK0,0x72U)
+#define INV2REG_DATA_RDY_STATUS INV2REG(INV2_BANK0,0x74U)
+#define INV2REG_FIFO_CFG INV2REG(INV2_BANK0,0x76U)
+
+#define INV2REG_SELF_TEST_X_GYRO INV2REG(INV2_BANK1,0x02U)
+#define INV2REG_SELF_TEST_Y_GYRO INV2REG(INV2_BANK1,0x03U)
+#define INV2REG_SELF_TEST_Z_GYRO INV2REG(INV2_BANK1,0x04U)
+#define INV2REG_SELF_TEST_X_ACCEL INV2REG(INV2_BANK1,0x0EU)
+#define INV2REG_SELF_TEST_Y_ACCEL INV2REG(INV2_BANK1,0x0FU)
+#define INV2REG_SELF_TEST_Z_ACCEL INV2REG(INV2_BANK1,0x10U)
+#define INV2REG_XA_OFFS_H INV2REG(INV2_BANK1,0x14U)
+#define INV2REG_XA_OFFS_L INV2REG(INV2_BANK1,0x15U)
+#define INV2REG_YA_OFFS_H INV2REG(INV2_BANK1,0x17U)
+#define INV2REG_YA_OFFS_L INV2REG(INV2_BANK1,0x18U)
+#define INV2REG_ZA_OFFS_H INV2REG(INV2_BANK1,0x1AU)
+#define INV2REG_ZA_OFFS_L INV2REG(INV2_BANK1,0x1BU)
+#define INV2REG_TIMEBASE_CORRECTIO INV2REG(INV2_BANK1,0x28U)
+
+#define INV2REG_GYRO_SMPLRT_DIV INV2REG(INV2_BANK2,0x00U)
+#define INV2REG_GYRO_CONFIG_1 INV2REG(INV2_BANK2,0x01U)
+# define BIT_GYRO_NODLPF_9KHZ 0x00
+# define BIT_GYRO_DLPF_ENABLE 0x01
+# define GYRO_DLPF_CFG_229HZ 0x00
+# define GYRO_DLPF_CFG_188HZ 0x01
+# define GYRO_DLPF_CFG_154HZ 0x02
+# define GYRO_DLPF_CFG_73HZ 0x03
+# define GYRO_DLPF_CFG_35HZ 0x04
+# define GYRO_DLPF_CFG_17HZ 0x05
+# define GYRO_DLPF_CFG_9HZ 0x06
+# define GYRO_DLPF_CFG_377HZ 0x07
+# define GYRO_DLPF_CFG_SHIFT 0x03
+# define GYRO_FS_SEL_250DPS 0x00
+# define GYRO_FS_SEL_500DPS 0x01
+# define GYRO_FS_SEL_1000DPS 0x02
+# define GYRO_FS_SEL_2000DPS 0x03
+# define GYRO_FS_SEL_SHIFT 0x01
+#define INV2REG_GYRO_CONFIG_2 INV2REG(INV2_BANK2,0x02U)
+#define INV2REG_XG_OFFS_USRH INV2REG(INV2_BANK2,0x03U)
+#define INV2REG_XG_OFFS_USRL INV2REG(INV2_BANK2,0x04U)
+#define INV2REG_YG_OFFS_USRH INV2REG(INV2_BANK2,0x05U)
+#define INV2REG_YG_OFFS_USRL INV2REG(INV2_BANK2,0x06U)
+#define INV2REG_ZG_OFFS_USRH INV2REG(INV2_BANK2,0x07U)
+#define INV2REG_ZG_OFFS_USRL INV2REG(INV2_BANK2,0x08U)
+#define INV2REG_ODR_ALIGN_EN INV2REG(INV2_BANK2,0x09U)
+#define INV2REG_ACCEL_SMPLRT_DIV_1 INV2REG(INV2_BANK2,0x10U)
+#define INV2REG_ACCEL_SMPLRT_DIV_2 INV2REG(INV2_BANK2,0x11U)
+#define INV2REG_ACCEL_INTEL_CTRL INV2REG(INV2_BANK2,0x12U)
+#define INV2REG_ACCEL_WOM_THR INV2REG(INV2_BANK2,0x13U)
+#define INV2REG_ACCEL_CONFIG INV2REG(INV2_BANK2,0x14U)
+# define BIT_ACCEL_NODLPF_4_5KHZ 0x00
+# define BIT_ACCEL_DLPF_ENABLE 0x01
+# define ACCEL_DLPF_CFG_265HZ 0x00
+# define ACCEL_DLPF_CFG_136HZ 0x02
+# define ACCEL_DLPF_CFG_69HZ 0x03
+# define ACCEL_DLPF_CFG_34HZ 0x04
+# define ACCEL_DLPF_CFG_17HZ 0x05
+# define ACCEL_DLPF_CFG_8HZ 0x06
+# define ACCEL_DLPF_CFG_499HZ 0x07
+# define ACCEL_DLPF_CFG_SHIFT 0x03
+# define ACCEL_FS_SEL_2G 0x00
+# define ACCEL_FS_SEL_4G 0x01
+# define ACCEL_FS_SEL_8G 0x02
+# define ACCEL_FS_SEL_16G 0x03
+# define ACCEL_FS_SEL_SHIFT 0x01
+#define INV2REG_FSYNC_CONFIG INV2REG(INV2_BANK2,0x52U)
+# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01
+# define FSYNC_CONFIG_EXT_SYNC_GX 0x02
+# define FSYNC_CONFIG_EXT_SYNC_GY 0x03
+# define FSYNC_CONFIG_EXT_SYNC_GZ 0x04
+# define FSYNC_CONFIG_EXT_SYNC_AX 0x05
+# define FSYNC_CONFIG_EXT_SYNC_AY 0x06
+# define FSYNC_CONFIG_EXT_SYNC_AZ 0x07
+#define INV2REG_TEMP_CONFIG INV2REG(INV2_BANK2,0x53U)
+#define INV2REG_MOD_CTRL_USR INV2REG(INV2_BANK2,0x54U)
+
+#define INV2REG_I2C_MST_ODR_CONFIG INV2REG(INV2_BANK3,0x00U)
+#define INV2REG_I2C_MST_CTRL INV2REG(INV2_BANK3,0x01U)
+# define BIT_I2C_MST_P_NSR 0x10
+# define BIT_I2C_MST_CLK_400KHZ 0x0D
+#define INV2REG_I2C_MST_DELAY_CTRL INV2REG(INV2_BANK3,0x02U)
+# define BIT_I2C_SLV0_DLY_EN 0x01
+# define BIT_I2C_SLV1_DLY_EN 0x02
+# define BIT_I2C_SLV2_DLY_EN 0x04
+# define BIT_I2C_SLV3_DLY_EN 0x08
+#define INV2REG_I2C_SLV0_ADDR INV2REG(INV2_BANK3,0x03U)
+#define INV2REG_I2C_SLV0_REG INV2REG(INV2_BANK3,0x04U)
+#define INV2REG_I2C_SLV0_CTRL INV2REG(INV2_BANK3,0x05U)
+#define INV2REG_I2C_SLV0_DO INV2REG(INV2_BANK3,0x06U)
+#define INV2REG_I2C_SLV1_ADDR INV2REG(INV2_BANK3,0x07U)
+#define INV2REG_I2C_SLV1_REG INV2REG(INV2_BANK3,0x08U)
+#define INV2REG_I2C_SLV1_CTRL INV2REG(INV2_BANK3,0x09U)
+#define INV2REG_I2C_SLV1_DO INV2REG(INV2_BANK3,0x0AU)
+#define INV2REG_I2C_SLV2_ADDR INV2REG(INV2_BANK3,0x0BU)
+#define INV2REG_I2C_SLV2_REG INV2REG(INV2_BANK3,0x0CU)
+#define INV2REG_I2C_SLV2_CTRL INV2REG(INV2_BANK3,0x0DU)
+#define INV2REG_I2C_SLV2_DO INV2REG(INV2_BANK3,0x0EU)
+#define INV2REG_I2C_SLV3_ADDR INV2REG(INV2_BANK3,0x0FU)
+#define INV2REG_I2C_SLV3_REG INV2REG(INV2_BANK3,0x10U)
+#define INV2REG_I2C_SLV3_CTRL INV2REG(INV2_BANK3,0x11U)
+#define INV2REG_I2C_SLV3_DO INV2REG(INV2_BANK3,0x12U)
+#define INV2REG_I2C_SLV4_ADDR INV2REG(INV2_BANK3,0x13U)
+#define INV2REG_I2C_SLV4_REG INV2REG(INV2_BANK3,0x14U)
+#define INV2REG_I2C_SLV4_CTRL INV2REG(INV2_BANK3,0x15U)
+#define INV2REG_I2C_SLV4_DO INV2REG(INV2_BANK3,0x16U)
+#define INV2REG_I2C_SLV4_DI INV2REG(INV2_BANK3,0x17U)
+
+#define INV2REG_BANK_SEL 0x7F
+
+// WHOAMI values
+#define INV2_WHOAMI_ICM20648 0xe0
+#define INV2_WHOAMI_ICM20948 0xea
+#define INV2_WHOAMI_ICM20649 0xe1
+
+#endif /* INVENSENSE2_REGS_H */
diff --git a/sw/airborne/peripherals/l3gd20.h b/sw/airborne/peripherals/l3gd20.h
index 93e5afe475..98dfe6848f 100644
--- a/sw/airborne/peripherals/l3gd20.h
+++ b/sw/airborne/peripherals/l3gd20.h
@@ -31,6 +31,15 @@
/* Include address and register definition */
#include "peripherals/l3gd20_regs.h"
+
+/** default gyro sensitivy and neutral from the datasheet
+ * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range
+ * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = (70e-3 / 180.0f) * pi * 4096
+ */
+#define L3GD20_SENS_2000_NUM 5004
+#define L3GD20_SENS_2000_DEN 1000
+
enum L3gd20ConfStatus {
L3G_CONF_UNINIT = 0,
L3G_CONF_WHO_AM_I = 1,
diff --git a/sw/airborne/peripherals/lsm303d.h b/sw/airborne/peripherals/lsm303d.h
index 165306cc79..af25f1c1e6 100644
--- a/sw/airborne/peripherals/lsm303d.h
+++ b/sw/airborne/peripherals/lsm303d.h
@@ -53,6 +53,14 @@
#define LSM303D_DEFAULT_MD (LSM303D_MAG_MODE_CONTINOUS_CONVERSION << 0) // Magneto continious conversion mode
#endif
+/** default accel sensitivy from the datasheet
+ * LSM303DLHC has 732 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * sens = 9.81 / 732 * 1024 = 13.72
+ */
+#define LSM303D_ACCEL_SENS_16G_NUM 13723
+#define LSM303D_ACCEL_SENS_16G_DEN 1000
+
struct Lsm303dConfig {
uint8_t acc_rate; ///< Data Output Rate (Hz)
uint8_t acc_scale; ///< full scale selection (m/s²)
diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
index e094243fe0..915829de37 100644
--- a/sw/airborne/peripherals/mpu60x0.c
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -14,9 +14,8 @@
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
*/
/**
@@ -36,11 +35,15 @@ const float MPU60X0_GYRO_SENS[4] = {
MPU60X0_GYRO_SENS_2000
};
-const int32_t MPU60X0_GYRO_SENS_FRAC[4][2] = {
- { MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_DEN },
- { MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_DEN },
- { MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_DEN },
- { MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_DEN }
+const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2] = {
+ { {MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM},
+ {MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN} },
+ { {MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM},
+ {MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN} },
+ { {MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM},
+ {MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN} },
+ { {MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM},
+ {MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN} }
};
const float MPU60X0_ACCEL_SENS[4] = {
@@ -50,11 +53,15 @@ const float MPU60X0_ACCEL_SENS[4] = {
MPU60X0_ACCEL_SENS_16G
};
-const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2] = {
- { MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_DEN },
- { MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_DEN },
- { MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_DEN },
- { MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_DEN }
+const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2] = {
+ { {MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM},
+ {MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN} },
+ { {MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM},
+ {MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN} },
+ { {MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM},
+ {MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN} },
+ { {MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM},
+ {MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN} }
};
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index fe7032f220..15225b9bce 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -14,9 +14,8 @@
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
*/
/**
@@ -29,6 +28,7 @@
#define MPU60X0_H
#include "std.h"
+#include "math/pprz_algebra_int.h"
/* Include address and register definition */
#include "peripherals/mpu60x0_regs.h"
@@ -74,7 +74,7 @@
// Get default sensitivity from a table
extern const float MPU60X0_GYRO_SENS[4];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t MPU60X0_GYRO_SENS_FRAC[4][2];
+extern const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
@@ -97,7 +97,7 @@ extern const int32_t MPU60X0_GYRO_SENS_FRAC[4][2];
// Get default sensitivity from a table
extern const float MPU60X0_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2];
+extern const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2];
/** MPU60x0 sensor type
*/
diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c
index c79ea9e224..7177bb36c9 100644
--- a/sw/airborne/peripherals/mpu9250.c
+++ b/sw/airborne/peripherals/mpu9250.c
@@ -36,11 +36,15 @@ const float MPU9250_GYRO_SENS[4] = {
MPU9250_GYRO_SENS_2000
};
-const int32_t MPU9250_GYRO_SENS_FRAC[4][2] = {
- { MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_DEN },
- { MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_DEN },
- { MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_DEN },
- { MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_DEN }
+const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2] = {
+ { {MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM},
+ {MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN} },
+ { {MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM},
+ {MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN} },
+ { {MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM},
+ {MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN} },
+ { {MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM},
+ {MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN} }
};
const float MPU9250_ACCEL_SENS[4] = {
@@ -50,11 +54,15 @@ const float MPU9250_ACCEL_SENS[4] = {
MPU9250_ACCEL_SENS_16G
};
-const int32_t MPU9250_ACCEL_SENS_FRAC[4][2] = {
- { MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_DEN },
- { MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_DEN },
- { MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_DEN },
- { MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_DEN }
+const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2] = {
+ { {MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM},
+ {MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN} },
+ { {MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM},
+ {MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN} },
+ { {MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM},
+ {MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN} },
+ { {MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM},
+ {MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN} }
};
void mpu9250_set_default_config(struct Mpu9250Config *c)
diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h
index db19d9d061..18bcf8afaf 100644
--- a/sw/airborne/peripherals/mpu9250.h
+++ b/sw/airborne/peripherals/mpu9250.h
@@ -28,6 +28,7 @@
#define MPU9250_H
#include "std.h"
+#include "math/pprz_algebra_int.h"
/* Include address and register definition */
#include "peripherals/mpu9250_regs.h"
@@ -73,7 +74,7 @@
// Get default sensitivity from a table
extern const float MPU9250_GYRO_SENS[4];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t MPU9250_GYRO_SENS_FRAC[4][2];
+extern const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
@@ -96,7 +97,7 @@ extern const int32_t MPU9250_GYRO_SENS_FRAC[4][2];
// Get default sensitivity from a table
extern const float MPU9250_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
-extern const int32_t MPU9250_ACCEL_SENS_FRAC[4][2];
+extern const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2];
enum Mpu9250ConfStatus {
MPU9250_CONF_UNINIT,
diff --git a/sw/airborne/test/modules/test_imu.c b/sw/airborne/test/modules/test_imu.c
index 69ceb2e8da..eae686b074 100644
--- a/sw/airborne/test/modules/test_imu.c
+++ b/sw/airborne/test/modules/test_imu.c
@@ -39,6 +39,7 @@
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
+#include "modules/energy/electrical.h"
#include "generated/modules.h"
@@ -59,6 +60,9 @@ static inline void main_init(void);
static inline void main_periodic_task(void);
static inline void main_event_task(void);
+// Bypass electrical
+struct Electrical electrical;
+
int main(void)
{
main_init();
@@ -84,9 +88,9 @@ static inline void main_init(void)
downlink_init();
pprz_dl_init();
- AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(ABI_BROADCAST, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO(ABI_BROADCAST, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(ABI_BROADCAST, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(ABI_BROADCAST, &mag_ev, mag_cb);
}
static inline void led_toggle(void)
@@ -173,73 +177,44 @@ static inline void main_event_task(void)
modules_event_task();
}
-static void accel_cb(uint8_t sender_id __attribute__((unused)),
+static void accel_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
#if USE_LED_3
RunOnceEvery(50, LED_TOGGLE(3));
#endif
- static uint8_t cnt;
- cnt++;
- if (cnt > 15) { cnt = 0; }
- if (cnt == 0) {
- DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
- &imu.accel_unscaled.x,
- &imu.accel_unscaled.y,
- &imu.accel_unscaled.z);
- } else if (cnt == 7) {
- DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
- &accel->x,
- &accel->y,
- &accel->z);
- }
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
+ &sender_id,
+ &accel->x,
+ &accel->y,
+ &accel->z);
}
-static void gyro_cb(uint8_t sender_id __attribute__((unused)),
+static void gyro_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *gyro)
{
#if USE_LED_2
RunOnceEvery(50, LED_TOGGLE(2));
#endif
- static uint8_t cnt;
- cnt++;
- if (cnt > 15) { cnt = 0; }
-
- if (cnt == 0) {
- DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
- &imu.gyro_unscaled.p,
- &imu.gyro_unscaled.q,
- &imu.gyro_unscaled.r);
- } else if (cnt == 7) {
- DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
- &gyro->p,
- &gyro->q,
- &gyro->r);
- }
+ DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
+ &sender_id,
+ &gyro->p,
+ &gyro->q,
+ &gyro->r);
}
-static void mag_cb(uint8_t sender_id __attribute__((unused)),
+static void mag_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
- static uint8_t cnt;
- cnt++;
- if (cnt > 10) { cnt = 0; }
-
- if (cnt == 0) {
- DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
- &mag->x,
- &mag->y,
- &mag->z);
- } else if (cnt == 5) {
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
- &imu.mag_unscaled.x,
- &imu.mag_unscaled.y,
- &imu.mag_unscaled.z);
- }
+ DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
+ &sender_id,
+ &mag->x,
+ &mag->y,
+ &mag->z);
}
void dl_parse_msg(struct link_device *dev __attribute__((unused)),
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 20cf56bcd0..a796278507 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 20cf56bcd0d24896a81abc682ed4b082b7391d78
+Subproject commit a7962785072cbd78fb34e94e5265ce3ed4c1597e
diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c
index 6de8c4212b..eafddf7067 100644
--- a/sw/simulator/nps/nps_ivy.c
+++ b/sw/simulator/nps/nps_ivy.c
@@ -26,8 +26,6 @@
#include "modules/gps/gps.h"
#endif
-#include NPS_SENSORS_PARAMS
-
pthread_t th_ivy_main; // runs main Ivy loop
static MsgRcvPtr ivyPtr = NULL;
static int seq = 1;
diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c
index d75427962d..d96fcd600d 100644
--- a/sw/simulator/nps/nps_sensor_accel.c
+++ b/sw/simulator/nps/nps_sensor_accel.c
@@ -1,10 +1,8 @@
#include "nps_sensor_accel.h"
-#include "generated/airframe.h" /* to get NPS_SENSORS_PARAMS */
-
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
#include "math/pprz_algebra_int.h"
void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
diff --git a/sw/simulator/nps/nps_sensor_airspeed.c b/sw/simulator/nps/nps_sensor_airspeed.c
index bf3ebfcb82..6d28aa0546 100644
--- a/sw/simulator/nps/nps_sensor_airspeed.c
+++ b/sw/simulator/nps/nps_sensor_airspeed.c
@@ -33,7 +33,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
/// 10Hz default
#ifndef NPS_AIRSPEED_DT
diff --git a/sw/simulator/nps/nps_sensor_aoa.c b/sw/simulator/nps/nps_sensor_aoa.c
index 5524cb69e6..a8894d940a 100644
--- a/sw/simulator/nps/nps_sensor_aoa.c
+++ b/sw/simulator/nps/nps_sensor_aoa.c
@@ -32,7 +32,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
/// 10Hz default
#ifndef NPS_AOA_DT
diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c
index a56f6ae3ce..1d612c4561 100644
--- a/sw/simulator/nps/nps_sensor_baro.c
+++ b/sw/simulator/nps/nps_sensor_baro.c
@@ -5,7 +5,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
#ifndef NPS_BARO_NOISE_STD_DEV
#define NPS_BARO_NOISE_STD_DEV 0.
diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c
index 9465c09df4..d7ec26689a 100644
--- a/sw/simulator/nps/nps_sensor_gps.c
+++ b/sw/simulator/nps/nps_sensor_gps.c
@@ -5,7 +5,7 @@
#include "nps_fdm.h"
#include "nps_random.h"
#include "nps_sensors_utils.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
{
diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c
index 1bf66296f7..6362a7d89c 100644
--- a/sw/simulator/nps/nps_sensor_gyro.c
+++ b/sw/simulator/nps/nps_sensor_gyro.c
@@ -2,7 +2,7 @@
#include "generated/airframe.h"
#include "nps_fdm.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
#include "math/pprz_algebra_int.h"
#include "nps_random.h"
diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c
index 1612c776bb..be0a1114ff 100644
--- a/sw/simulator/nps/nps_sensor_mag.c
+++ b/sw/simulator/nps/nps_sensor_mag.c
@@ -2,7 +2,7 @@
#include "generated/airframe.h"
#include "nps_fdm.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
#include "math/pprz_algebra_int.h"
void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
diff --git a/sw/simulator/nps/nps_sensor_sideslip.c b/sw/simulator/nps/nps_sensor_sideslip.c
index f2fbab3c43..5e38af752d 100644
--- a/sw/simulator/nps/nps_sensor_sideslip.c
+++ b/sw/simulator/nps/nps_sensor_sideslip.c
@@ -33,7 +33,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
/// 10Hz default
#ifndef NPS_SIDESLIP_DT
diff --git a/sw/simulator/nps/nps_sensor_sonar.c b/sw/simulator/nps/nps_sensor_sonar.c
index ea33050888..d7b2e32cc0 100644
--- a/sw/simulator/nps/nps_sensor_sonar.c
+++ b/sw/simulator/nps/nps_sensor_sonar.c
@@ -33,7 +33,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
/// 10Hz default
#ifndef NPS_SONAR_DT
diff --git a/sw/simulator/nps/nps_sensor_temperature.c b/sw/simulator/nps/nps_sensor_temperature.c
index 1dd2791913..8af96bbcd1 100644
--- a/sw/simulator/nps/nps_sensor_temperature.c
+++ b/sw/simulator/nps/nps_sensor_temperature.c
@@ -5,7 +5,7 @@
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
-#include NPS_SENSORS_PARAMS
+#include "nps_sensors.h"
/// 10Hz default
#ifndef NPS_TEMPERATURE_DT
diff --git a/sw/simulator/nps/nps_sensors.c b/sw/simulator/nps/nps_sensors.c
index 11744c8e7d..d59c615795 100644
--- a/sw/simulator/nps/nps_sensors.c
+++ b/sw/simulator/nps/nps_sensors.c
@@ -1,8 +1,5 @@
#include "nps_sensors.h"
-#include "generated/airframe.h"
-#include NPS_SENSORS_PARAMS
-
struct NpsSensors sensors;
void nps_sensors_init(double time)
diff --git a/sw/simulator/nps/nps_sensors.h b/sw/simulator/nps/nps_sensors.h
index 19f6d1bc18..93e66b72f9 100644
--- a/sw/simulator/nps/nps_sensors.h
+++ b/sw/simulator/nps/nps_sensors.h
@@ -2,6 +2,12 @@
#define NPS_SENSORS_H
#include "math/pprz_algebra.h"
+
+#ifndef NPS_SENSORS_PARAMS
+#include "nps_sensors_params_default.h"
+#else
+#include NPS_SENSORS_PARAMS
+#endif /* NPS_SENSORS_PARAMS */
#include "nps_sensor_gyro.h"
#include "nps_sensor_accel.h"
#include "nps_sensor_mag.h"
diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py
index 2ae5c22a30..d33a958929 100755
--- a/sw/tools/calibration/calibrate.py
+++ b/sw/tools/calibration/calibrate.py
@@ -37,6 +37,9 @@ def main():
parser.add_option("-i", "--id", dest="ac_id",
action="store",
help="aircraft id to use")
+ parser.add_option("-j", "--sid", dest="sensor_id",
+ action="store",
+ help="sensor id to use")
parser.add_option("-s", "--sensor", dest="sensor",
type="choice", choices=["ACCEL", "MAG"],
help="sensor to calibrate (ACCEL, MAG)",
@@ -75,6 +78,15 @@ def main():
if options.verbose:
print("Using aircraft id "+options.ac_id)
+ sensor_ids = calibration_utils.get_sensor_ids(options.ac_id, filename, options.sensor)
+ if options.sensor_id is None:
+ if len(sensor_ids) == 1:
+ options.sensor_id = sensor_ids[0]
+ else:
+ parser.error("More than one sensor id found in log file. Specify the id to use.")
+ if options.verbose:
+ print("Using sensor id "+options.sensor_id)
+
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
@@ -87,10 +99,10 @@ def main():
noise_threshold = options.noise_threshold
if options.verbose:
- print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor)
+ print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor +" with sensor id "+options.sensor_id)
# read raw measurements from log file
- measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
+ measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor, options.sensor_id)
if len(measurements) == 0:
print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!")
sys.exit(1)
@@ -155,7 +167,7 @@ def main():
print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
if not optimze_failed:
- calibration_utils.print_xml(p1, options.sensor, sensor_res)
+ calibration_utils.print_xml(p1, options.sensor, options.sensor_id, sensor_res)
if options.plot:
# if we are calibrating a mag, just draw first plot (non-blocking), then show the second
diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py
index 2e36f8cd5b..523fc219c8 100644
--- a/sw/tools/calibration/calibration_utils.py
+++ b/sw/tools/calibration/calibration_utils.py
@@ -22,9 +22,11 @@
from __future__ import print_function, division
import re
+from telnetlib import theNULL
import numpy as np
from numpy import sin, cos
from scipy import linalg, stats
+from fractions import Fraction
import matplotlib
import matplotlib.pyplot as plt
@@ -47,11 +49,25 @@ def get_ids_in_log(filename):
ids.append(ac_id)
return ids
+def get_sensor_ids(ac_id, filename, sensor):
+ f = open(filename, 'r')
+ ids = []
+ pattern = re.compile("\S+ "+ac_id+" IMU_"+sensor+"_RAW (\S+) \S+ \S+ \S+")
+ while True:
+ line = f.readline().strip()
+ if line == '':
+ break
+ m = re.match(pattern, line)
+ if m:
+ sensor_id = m.group(1)
+ if not sensor_id in ids:
+ ids.append(sensor_id)
+ return ids
-def read_log(ac_id, filename, sensor):
+def read_log(ac_id, filename, sensor, sensor_id):
"""Extracts raw sensor measurements from a log."""
f = open(filename, 'r')
- pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) (\S+)")
+ pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW "+sensor_id+" (\S+) (\S+) (\S+)")
list_meas = []
while True:
line = f.readline().strip()
@@ -63,10 +79,10 @@ def read_log(ac_id, filename, sensor):
return np.array(list_meas)
-def read_log_scaled(ac_id, filename, sensor, t_start, t_end):
+def read_log_scaled(ac_id, filename, sensor, sensor_id, t_start, t_end):
"""Extracts scaled sensor measurements from a log."""
f = open(filename, 'r')
- pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_SCALED (\S+) (\S+) (\S+)")
+ pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_SCALED "+sensor_id+" (\S+) (\S+) (\S+)")
list_meas = []
while True:
line = f.readline().strip()
@@ -141,9 +157,43 @@ def estimate_mag_current_relation(meas):
offset.append(intercept)
return coefficient, offset
+def continious_frac(v):
+ max_val = 2**16
+ if v > 0:
+ s = 1
+ else:
+ v = -v
+ s = -1
+ return _continious_frac(v, max_val, int(v), v, (1, int(v)), (0,1), s)
-def print_xml(p, sensor, res):
+def _continious_frac(v, max_val, a, x, num, den, s):
+ x1 = 1 / (x - a)
+ a1 = int(x1)
+ (num1, num2) = num
+ num3 = a1 * num2 + num1
+ (den1, den2) = den
+ den3 = a1 * den2 + den1
+ if num3 > max_val or den3 > max_val:
+ return (num2, s*den2)
+ elif (num3 / den3) == v:
+ return (num3, s*den3)
+ else:
+ return _continious_frac(v, max_val, a1, x1, (num2, num3), (den2, den3), s)
+
+def print_xml(p, sensor, sensor_id, res):
"""Print xml for airframe file."""
+ x_sens = continious_frac(p[3]*2**res)
+ y_sens = continious_frac(p[4]*2**res)
+ z_sens = continious_frac(p[5]*2**res)
+
+ struct = "{{.abi_id="+sensor_id+", .calibrated={.neutral=true, .scale=true},"
+ struct += ".neutral={"+str(int(round(p[0])))+","+str(int(round(p[1])))+","+str(int(round(p[2])))+"}, "
+ struct += ".scale={{"+str(x_sens[0])+","+str(y_sens[0])+","+str(z_sens[0])+"},"
+ struct += "{"+str(x_sens[1])+","+str(y_sens[1])+","+str(z_sens[1])+"}}"
+ struct += "}}"
+
+ print("")
+ print("")
print("")
print("")
print("")
@@ -151,7 +201,6 @@ def print_xml(p, sensor, res):
print("")
print("")
print("")
- print("")
def print_imu_scaled(sensor, measurements, attrs):
diff --git a/sw/tools/px4/set_target.py b/sw/tools/px4/set_target.py
index ef57bf4379..18f221be71 100755
--- a/sw/tools/px4/set_target.py
+++ b/sw/tools/px4/set_target.py
@@ -74,7 +74,7 @@ if mode == -1: # no pprz cdc was found, look for PX4
if mode == -1:
print("No original PX4 CDC firmware found either.")
print("Error: no compatible usb device found...")
- sys.exit(1)
+ #sys.exit(1)
if target == "fbw":
print("Error: original firmware cannot be used to upload the fbw code. Wait for the PX4 bootloader to exit (takes 5 seconds), or in case this is the first upload; first upload the Paparazzi ap target.")
diff --git a/tests/modules/generated/airframe.h b/tests/modules/generated/airframe.h
index 65b06a5da2..2b11ce1b88 100644
--- a/tests/modules/generated/airframe.h
+++ b/tests/modules/generated/airframe.h
@@ -37,39 +37,6 @@
#define FAILSAFE_HOME_RADIUS 100
#define SECTION_IMU 1
-#define IMU_GYRO_P_SENS 4.359
-#define IMU_GYRO_P_SENS_NUM 4359
-#define IMU_GYRO_P_SENS_DEN 1000
-#define IMU_GYRO_Q_SENS 4.359
-#define IMU_GYRO_Q_SENS_NUM 4359
-#define IMU_GYRO_Q_SENS_DEN 1000
-#define IMU_GYRO_R_SENS 4.359
-#define IMU_GYRO_R_SENS_NUM 4359
-#define IMU_GYRO_R_SENS_DEN 1000
-#define IMU_ACCEL_X_NEUTRAL -85
-#define IMU_ACCEL_Y_NEUTRAL 51
-#define IMU_ACCEL_Z_NEUTRAL -76
-#define IMU_ACCEL_X_SENS 2.44493965424
-#define IMU_ACCEL_X_SENS_NUM 61989
-#define IMU_ACCEL_X_SENS_DEN 25354
-#define IMU_ACCEL_Y_SENS 2.43424121497
-#define IMU_ACCEL_Y_SENS_NUM 61579
-#define IMU_ACCEL_Y_SENS_DEN 25297
-#define IMU_ACCEL_Z_SENS 2.44715908645
-#define IMU_ACCEL_Z_SENS_NUM 4307
-#define IMU_ACCEL_Z_SENS_DEN 1760
-#define IMU_MAG_X_NEUTRAL 0
-#define IMU_MAG_Y_NEUTRAL 0
-#define IMU_MAG_Z_NEUTRAL 0
-#define IMU_MAG_X_SENS 7.28514789391
-#define IMU_MAG_X_SENS_NUM 23888
-#define IMU_MAG_X_SENS_DEN 3279
-#define IMU_MAG_Y_SENS 7.33022132691
-#define IMU_MAG_Y_SENS_NUM 21199
-#define IMU_MAG_Y_SENS_DEN 2892
-#define IMU_MAG_Z_SENS 7.57102035692
-#define IMU_MAG_Z_SENS_NUM 63589
-#define IMU_MAG_Z_SENS_DEN 8399
#define IMU_BODY_TO_IMU_PHI 0.
#define IMU_BODY_TO_IMU_THETA 0.
#define IMU_BODY_TO_IMU_PSI 0