diff --git a/conf/airframes/examples/demo_module.xml b/conf/airframes/examples/demo_module.xml
deleted file mode 100644
index 94ef391cea..0000000000
--- a/conf/airframes/examples/demo_module.xml
+++ /dev/null
@@ -1,211 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-CONFIG = \"tiny_2_1_1.h\"
-
-include $(PAPARAZZI_SRC)/conf/firmwares/tiny.makefile
-
-FLASH_MODE=IAP
-
-ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DSYS_TIME_LED=1
-ap.srcs = mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c $(SRC_FIRMWARE)/main.c
-
-ap.CFLAGS += -DINTER_MCU
-ap.srcs += inter_mcu.c
-
-ap.srcs += commands.c
-
-########## RC actuators + radio
-ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
-ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
-
-ap.CFLAGS += -DRADIO_CONTROL
-ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-
-########## Modem
-ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
-
-########## ADC
-ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3
-ap.srcs += $(SRC_ARCH)/adc_hw.c
-
-########## GPS
-ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART0 -DGPS_LINK=Uart0 -DUART0_BAUD=B38400 -DGPS_USE_LATLONG
-ap.srcs += gps_ubx.c gps.c latlong.c
-
-########## IR sensors
-ap.CFLAGS += -DUSE_INFRARED -DALT_KALMAN
-ap.srcs += infrared.c estimator.c
-
-########## Nav
-ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
-ap.srcs += subsystems/nav.c fw_h_ctl.c fw_v_ctl.c
-ap.srcs += subsystems/navigation/nav_survey_rectangle.c
-ap.srcs += subsystems/navigation/nav_line.c
-
-
-# Config for SITL simulation
-include $(PAPARAZZI_SRC)/conf/firmwares/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-
-sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
-
-
-
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index b2fcae195d..a394a3d966 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -200,6 +200,8 @@ endif
ap.srcs += $(SRC_FIRMWARE)/autopilot.c
+ap.srcs += state.c
+
ap.srcs += $(SRC_FIRMWARE)/stabilization.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index 56b83ad623..0e91400c10 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -162,8 +162,23 @@ fbw_srcs += $(SRC_FIRMWARE)/fbw_downlink.c
ap_CFLAGS += -DAP
ap_srcs += $(SRC_FIRMWARE)/main_ap.c
-ap_srcs += $(SRC_FIXEDWING)/estimator.c
+ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float.h\"
+ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c
+ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_float.c
ap_srcs += $(SRC_FIRMWARE)/ap_downlink.c
+ap_srcs += state.c
+
+# BARO
+ifeq ($(BOARD), umarim)
+ifeq ($(BOARD_VERSION), 1.0)
+ap_srcs += boards/umarim/baro_board.c
+ap_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
+ap_CFLAGS += -DADS1114_I2C_DEVICE=i2c1
+ap_srcs += peripherals/ads1114.c
+endif
+else ifeq ($(BOARD), lisa_l)
+ap_CFLAGS += -DUSE_I2C2
+endif
######################################################################
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 28d76e543f..5dda2f2a5c 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -107,6 +107,8 @@ nps.srcs += subsystems/electrical.c
nps.srcs += $(SRC_FIRMWARE)/autopilot.c
+nps.srcs += state.c
+
#
# in makefile section of airframe xml
# include $(CFG_BOOZ)/subsystems/booz2_ahrs_lkf.makefile
diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile
index 7340c078a2..c23c0333ca 100644
--- a/conf/firmwares/subsystems/rotorcraft/ins.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile
@@ -2,7 +2,9 @@
# simple INS with float vertical filter
#
+$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c
# vertical filter float version
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
index e872ac1f77..4928c4c495 100644
--- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
@@ -2,7 +2,9 @@
# extended INS with vertical filter using sonar in a better way (flap ground)
#
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins_extended.c
+$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\"
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c
# vertical filter float version
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c
diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml
index 4e10939332..560eabb11d 100644
--- a/conf/flight_plans/EMAV2008.xml
+++ b/conf/flight_plans/EMAV2008.xml
@@ -48,7 +48,7 @@
-
+
@@ -84,14 +84,14 @@
-
+
-
+
-
+
@@ -114,7 +114,7 @@
-
+
diff --git a/conf/flight_plans/EMAV2009.xml b/conf/flight_plans/EMAV2009.xml
index fbb603488b..5cbeaab4c3 100644
--- a/conf/flight_plans/EMAV2009.xml
+++ b/conf/flight_plans/EMAV2009.xml
@@ -26,9 +26,9 @@
-
+
-
+
diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml
index dddd2b9cb5..c04ad1b09f 100644
--- a/conf/flight_plans/IS.xml
+++ b/conf/flight_plans/IS.xml
@@ -79,8 +79,8 @@
-
-
+
+
@@ -107,15 +107,15 @@
-
+
-
+
-
+
@@ -148,12 +148,12 @@
-
+
diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml
index 151f18a087..382e64ab26 100644
--- a/conf/flight_plans/basic.xml
+++ b/conf/flight_plans/basic.xml
@@ -36,9 +36,9 @@
-
+
-
+
@@ -78,10 +78,10 @@
-
+
-
+
diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml
index d913ac6c92..ea161cde0a 100644
--- a/conf/flight_plans/ccc_gl.xml
+++ b/conf/flight_plans/ccc_gl.xml
@@ -45,7 +45,7 @@
-
+
@@ -66,19 +66,19 @@
-
+
-
+
-
+
-
+
@@ -112,10 +112,10 @@
-
+
-
+
@@ -155,11 +155,11 @@
-
+
-
+
diff --git a/conf/flight_plans/corsica.xml b/conf/flight_plans/corsica.xml
index b429892412..02db9b1e70 100644
--- a/conf/flight_plans/corsica.xml
+++ b/conf/flight_plans/corsica.xml
@@ -33,9 +33,9 @@
-
+
-
+
@@ -63,10 +63,10 @@
-
+
-
+
diff --git a/conf/flight_plans/creidlitz.xml b/conf/flight_plans/creidlitz.xml
index c53a001f24..8b79b6ca84 100644
--- a/conf/flight_plans/creidlitz.xml
+++ b/conf/flight_plans/creidlitz.xml
@@ -17,7 +17,7 @@
-
+
@@ -34,9 +34,9 @@
-
+
-
+
@@ -46,7 +46,7 @@
-
+
diff --git a/conf/flight_plans/form_follow.xml b/conf/flight_plans/form_follow.xml
index 010cbc178d..41355289ce 100644
--- a/conf/flight_plans/form_follow.xml
+++ b/conf/flight_plans/form_follow.xml
@@ -35,8 +35,8 @@
-
-
+
+
diff --git a/conf/flight_plans/form_leader.xml b/conf/flight_plans/form_leader.xml
index c22cbf6813..a98d81c56e 100644
--- a/conf/flight_plans/form_leader.xml
+++ b/conf/flight_plans/form_leader.xml
@@ -45,8 +45,8 @@
-
-
+
+
diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml
index e676f007c6..c72620f3d9 100644
--- a/conf/flight_plans/fp_tp_auto.xml
+++ b/conf/flight_plans/fp_tp_auto.xml
@@ -44,7 +44,7 @@
-
+
@@ -65,19 +65,19 @@
-
+
-
+
-
+
-
+
@@ -111,10 +111,10 @@
-
+
-
+
diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml
index dafbcfced3..b1fc8ab8ea 100644
--- a/conf/flight_plans/generic.xml
+++ b/conf/flight_plans/generic.xml
@@ -24,13 +24,13 @@
-
+
-
+
-
+
diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml
index 31be09e05c..7741ca2013 100644
--- a/conf/flight_plans/grosslobke_demo.xml
+++ b/conf/flight_plans/grosslobke_demo.xml
@@ -24,7 +24,7 @@
-
+
@@ -55,7 +55,7 @@
-
+
@@ -77,10 +77,10 @@
-
+
-
+
@@ -114,7 +114,7 @@
diff --git a/conf/flight_plans/grosslobke_kreise.xml b/conf/flight_plans/grosslobke_kreise.xml
index 8a6d83dfb1..a6732defbf 100644
--- a/conf/flight_plans/grosslobke_kreise.xml
+++ b/conf/flight_plans/grosslobke_kreise.xml
@@ -12,7 +12,7 @@
-
+
@@ -43,7 +43,7 @@
-
+
diff --git a/conf/flight_plans/hsif.xml b/conf/flight_plans/hsif.xml
index 26825eb6e7..8cff597509 100644
--- a/conf/flight_plans/hsif.xml
+++ b/conf/flight_plans/hsif.xml
@@ -19,15 +19,15 @@
-
+
-
+
-
+
-
+
diff --git a/conf/flight_plans/huit.xml b/conf/flight_plans/huit.xml
index 520e714a92..0819bb0fe8 100644
--- a/conf/flight_plans/huit.xml
+++ b/conf/flight_plans/huit.xml
@@ -13,8 +13,8 @@
-
-
+
+
diff --git a/conf/flight_plans/ingolfsskali.xml b/conf/flight_plans/ingolfsskali.xml
index 2666e2b307..5b811161c1 100644
--- a/conf/flight_plans/ingolfsskali.xml
+++ b/conf/flight_plans/ingolfsskali.xml
@@ -15,7 +15,7 @@
-
+
@@ -23,7 +23,7 @@
-
+
@@ -66,11 +66,11 @@
-
+
-
+
@@ -79,7 +79,7 @@
-
+
diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml
index c1efecaf3f..b39d6694d5 100644
--- a/conf/flight_plans/joystick.xml
+++ b/conf/flight_plans/joystick.xml
@@ -34,9 +34,9 @@
-
+
-
+
@@ -80,10 +80,10 @@
-
+
-
+
diff --git a/conf/flight_plans/kalscott.xml b/conf/flight_plans/kalscott.xml
index b7401c953a..49d59220b2 100644
--- a/conf/flight_plans/kalscott.xml
+++ b/conf/flight_plans/kalscott.xml
@@ -26,47 +26,47 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
diff --git a/conf/flight_plans/kv_svalbard.xml b/conf/flight_plans/kv_svalbard.xml
index 153104cc99..499614ccdf 100644
--- a/conf/flight_plans/kv_svalbard.xml
+++ b/conf/flight_plans/kv_svalbard.xml
@@ -18,7 +18,7 @@
-
+
@@ -36,9 +36,9 @@
-
+
-
+
@@ -48,7 +48,7 @@
-
+
diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml
index 953b26464b..f1f2aed593 100644
--- a/conf/flight_plans/landing.xml
+++ b/conf/flight_plans/landing.xml
@@ -18,10 +18,10 @@
-
+
-
+
diff --git a/conf/flight_plans/mav06.xml b/conf/flight_plans/mav06.xml
index 864c6ccd0e..464950ca4e 100644
--- a/conf/flight_plans/mav06.xml
+++ b/conf/flight_plans/mav06.xml
@@ -30,11 +30,11 @@
-
+
-
+
@@ -50,7 +50,7 @@
-
+
-->
@@ -89,7 +89,7 @@
-
+
-->
diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml
index d4e3429ae0..176f4e2bb9 100644
--- a/conf/flight_plans/mav07.xml
+++ b/conf/flight_plans/mav07.xml
@@ -57,7 +57,7 @@
-
+
@@ -73,7 +73,7 @@
-
+
@@ -130,10 +130,10 @@
-
+
-
+
@@ -144,11 +144,11 @@
-
+
-
+
diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml
index 084723612c..3424afc1a9 100644
--- a/conf/flight_plans/mav08.xml
+++ b/conf/flight_plans/mav08.xml
@@ -42,9 +42,9 @@
-
+
-
+
@@ -98,10 +98,10 @@
-
+
-
+
diff --git a/conf/flight_plans/muret_for.xml b/conf/flight_plans/muret_for.xml
index 233cf0308a..961a2816c4 100644
--- a/conf/flight_plans/muret_for.xml
+++ b/conf/flight_plans/muret_for.xml
@@ -9,8 +9,8 @@
-
-
+
+
diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml
index ae8584808c..7d06ace28b 100644
--- a/conf/flight_plans/nordlys.xml
+++ b/conf/flight_plans/nordlys.xml
@@ -20,13 +20,13 @@
-
+
-
+
@@ -38,7 +38,7 @@
-
+
@@ -71,7 +71,7 @@
-
+
@@ -85,10 +85,10 @@
-
+
-
+
diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml
index 539367ea89..0d0e539340 100644
--- a/conf/flight_plans/poles.xml
+++ b/conf/flight_plans/poles.xml
@@ -32,9 +32,9 @@
-
+
-
+
@@ -70,10 +70,10 @@
-
+
-
+
diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml
index 7719308239..e9798d0de1 100644
--- a/conf/flight_plans/rotorcraft_basic.xml
+++ b/conf/flight_plans/rotorcraft_basic.xml
@@ -34,7 +34,7 @@
-
+
diff --git a/conf/flight_plans/sinsat.xml b/conf/flight_plans/sinsat.xml
index beb5132f5a..306e5d616a 100644
--- a/conf/flight_plans/sinsat.xml
+++ b/conf/flight_plans/sinsat.xml
@@ -6,8 +6,8 @@
-
-
+
+
diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml
index 92d84fe58c..86a3f046e7 100644
--- a/conf/flight_plans/slayer_training.xml
+++ b/conf/flight_plans/slayer_training.xml
@@ -36,7 +36,7 @@
-
+
@@ -75,10 +75,10 @@
-
+
-
+
diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml
index 0dd2e92c73..7dc6d806a0 100644
--- a/conf/flight_plans/snav.xml
+++ b/conf/flight_plans/snav.xml
@@ -34,9 +34,9 @@
-
+
-
+
@@ -55,10 +55,10 @@
-
+
-
+
diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml
index e2b3045e20..ab79ecda5d 100644
--- a/conf/flight_plans/tcas.xml
+++ b/conf/flight_plans/tcas.xml
@@ -34,9 +34,9 @@
-
+
-
+
@@ -72,10 +72,10 @@
-
+
-
+
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index 2200a31631..dcf3d90931 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -44,7 +44,7 @@
-
+
@@ -65,25 +65,25 @@
-
+
-
+
-
+
-
+
-
+
@@ -119,10 +119,10 @@
-
+
-
+
@@ -162,11 +162,11 @@
-
+
-
+
diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml
index 1a0aeafbc5..1427adf72c 100644
--- a/conf/flight_plans/versatile_airspeed.xml
+++ b/conf/flight_plans/versatile_airspeed.xml
@@ -45,7 +45,7 @@
-
+
@@ -69,25 +69,25 @@
-
+
-
+
-
+
-
+
-
+
@@ -134,7 +134,7 @@
-
+
@@ -177,11 +177,11 @@
-
+
-
+
diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
index 0c52f2d526..d89466c368 100644
--- a/conf/flight_plans/versatile_carto_fixe_muret.xml
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -61,7 +61,7 @@ float varsweepsize=110;
-
+
@@ -112,7 +112,7 @@ float varsweepsize=110;
-
+
@@ -140,7 +140,7 @@ float varsweepsize=110;
-
+
diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml
index 2179cf4094..0214c4f467 100644
--- a/conf/flight_plans/xsens_cachejunction.xml
+++ b/conf/flight_plans/xsens_cachejunction.xml
@@ -24,9 +24,9 @@
-
+
-
+
@@ -89,10 +89,10 @@
-
+
-
+
diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml
index e378dea4ce..b87687e982 100644
--- a/conf/modules/baro_board.xml
+++ b/conf/modules/baro_board.xml
@@ -2,30 +2,14 @@
- Temporary hack to use baro interface on fixedwing
+ Allow to use baro interface on fixedwing with external barometers
-
-
-
-
-
-
-ifeq ($(BOARD), navgo)
- ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
- ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1
- ap.srcs += peripherals/ads1114.c
-else ifeq ($(BOARD), umarim)
- ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
- ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1
- ap.srcs += peripherals/ads1114.c
-else ifeq ($(BOARD), lisa_l)
- ap.CFLAGS += -DUSE_I2C2
-endif
-
+
+
diff --git a/conf/settings/estimation/fw_baro_kalman.xml b/conf/settings/estimation/fw_baro_kalman.xml
index 34f11f32de..66012cb48d 100644
--- a/conf/settings/estimation/fw_baro_kalman.xml
+++ b/conf/settings/estimation/fw_baro_kalman.xml
@@ -6,7 +6,7 @@
-
+
diff --git a/conf/settings/fixedwing_basic.xml b/conf/settings/fixedwing_basic.xml
index abdfe89a29..84eb2165f5 100644
--- a/conf/settings/fixedwing_basic.xml
+++ b/conf/settings/fixedwing_basic.xml
@@ -8,7 +8,7 @@
-
+
diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml
index 89fee5251a..fcfa6320f3 100644
--- a/conf/telemetry/default_fixedwing.xml
+++ b/conf/telemetry/default_fixedwing.xml
@@ -62,6 +62,13 @@
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c
index f489ac35a4..94ca3e4715 100644
--- a/sw/airborne/arch/sim/jsbsim_gps.c
+++ b/sw/airborne/arch/sim/jsbsim_gps.c
@@ -9,7 +9,6 @@
#include "generated/flight_plan.h"
#include "autopilot.h"
#include "subsystems/gps.h"
-#include "estimator.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h
index 1202dce646..fa32784663 100644
--- a/sw/airborne/arch/sim/jsbsim_hw.h
+++ b/sw/airborne/arch/sim/jsbsim_hw.h
@@ -31,7 +31,6 @@
#include "std.h"
#include "inter_mcu.h"
#include "firmwares/fixedwing/autopilot.h"
-#include "estimator.h"
#include "subsystems/gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/flight_plan.h"
diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c
index f850dd9574..1259852536 100644
--- a/sw/airborne/arch/sim/jsbsim_ir.c
+++ b/sw/airborne/arch/sim/jsbsim_ir.c
@@ -7,7 +7,6 @@
#include "jsbsim_hw.h"
#include
-#include "estimator.h"
#ifndef JSBSIM_IR_ROLL_NEUTRAL
#define JSBSIM_IR_ROLL_NEUTRAL 0.
diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c
index 0fd1d8bace..4ff9503db8 100644
--- a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c
+++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c
@@ -4,7 +4,7 @@
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
// Arduimu empty implementation
#include "modules/ins/ins_arduimu.h"
@@ -25,8 +25,12 @@ extern float sim_theta;
void ArduIMU_init( void ) {}
void ArduIMU_periodic( void ) {
// Feed directly the estimator
- estimator_phi = sim_phi - ins_roll_neutral;
- estimator_theta = sim_theta - ins_pitch_neutral;
+ struct FloatEulers att = {
+ sim_phi - ins_roll_neutral,
+ sim_theta - ins_pitch_neutral,
+ 0.
+ };
+ stateSetNedToBodyEulers_f(&att);
}
void ArduIMU_periodicGPS( void ) {}
void IMU_Daten_verarbeiten( void ) {}
diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
index 5ba78b7070..6b130d49f2 100644
--- a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
@@ -4,7 +4,7 @@
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
// Arduimu empty implementation
#include "modules/ins/ins_arduimu_basic.h"
@@ -27,11 +27,14 @@ extern float sim_r;
void ArduIMU_init( void ) {}
void ArduIMU_periodic( void ) {
// Feed directly the estimator
- estimator_phi = sim_phi - ins_roll_neutral;
- estimator_theta = sim_theta - ins_pitch_neutral;
- estimator_p = sim_p;
- estimator_q = sim_q;
- estimator_r = sim_r;
+ struct FloatEulers att = {
+ sim_phi - ins_roll_neutral,
+ sim_theta - ins_pitch_neutral,
+ 0.
+ };
+ stateSetNedToBodyEulers_f(&att);
+ struct FloatRates rates = { sim_p, sim_q, sim_r };
+ stateSetBodyRates_f(&rates);
}
void ArduIMU_periodicGPS( void ) {}
void ArduIMU_event( void ) {}
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index c0ad122a24..7eaffac836 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -12,7 +12,6 @@
#include "std.h"
#include "inter_mcu.h"
#include "autopilot.h"
-#include "estimator.h"
#include "subsystems/gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/settings.h"
diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c
index 923961727b..cf6221bb29 100644
--- a/sw/airborne/arch/sim/sim_baro.c
+++ b/sw/airborne/arch/sim/sim_baro.c
@@ -1,5 +1,5 @@
#include
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "subsystems/gps.h"
#include "baro_MS5534A.h"
@@ -32,6 +32,6 @@ void baro_MS5534A_send(void) {
void baro_MS5534A_event_task( void ) {
baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX);
- baro_MS5534A_temp = 10 + estimator_z;
+ baro_MS5534A_temp = 10 + stateGetPositionUtm_f()->alt;
baro_MS5534A_available = TRUE;
}
diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c
index c329f6b76e..70a5a8c18e 100644
--- a/sw/airborne/arch/sim/sim_gps.c
+++ b/sw/airborne/arch/sim/sim_gps.c
@@ -9,7 +9,6 @@
#include "generated/flight_plan.h"
#include "autopilot.h"
#include "subsystems/gps.h"
-#include "estimator.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
@@ -24,6 +23,8 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
gps.course = Double_val(c) * 1e7;
gps.hmsl = Double_val(a) * 1000.;
gps.gspeed = Double_val(s) * 100.;
+ gps.ned_vel.x = gps.gspeed * cos(Double_val(c));
+ gps.ned_vel.y = gps.gspeed * sin(Double_val(c));
gps.ned_vel.z = -Double_val(cl) * 100.;
gps.week = 0; // FIXME
gps.tow = Double_val(t) * 1000.;
diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c
index b42b69ee3a..58d72edadd 100644
--- a/sw/airborne/boards/navgo/baro_board.c
+++ b/sw/airborne/boards/navgo/baro_board.c
@@ -33,7 +33,7 @@
struct Baro baro;
/* Counter to init mcp355x at startup */
-#define STARTUP_COUNTER 200
+#define BARO_STARTUP_COUNTER 200
uint16_t startup_cnt;
void baro_init( void ) {
@@ -45,7 +45,7 @@ void baro_init( void ) {
#ifdef ROTORCRAFT_BARO_LED
LED_OFF(ROTORCRAFT_BARO_LED);
#endif
- startup_cnt = STARTUP_COUNTER;
+ startup_cnt = BARO_STARTUP_COUNTER;
}
void baro_periodic( void ) {
diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h
index 72065a498a..56137a6806 100644
--- a/sw/airborne/boards/navgo/baro_board.h
+++ b/sw/airborne/boards/navgo/baro_board.h
@@ -26,8 +26,8 @@
-#ifndef BOARDS_UMARIM_BARO_H
-#define BOARDS_UMARIM_BARO_H
+#ifndef BOARDS_NAVGO_BARO_H
+#define BOARDS_NAVGO_BARO_H
#include "std.h"
@@ -36,10 +36,10 @@
#define BaroEvent(_b_abs_handler, _b_diff_handler) { \
mcp355x_event(); \
if (mcp355x_data_available) { \
- baro.absolute = mcp355x_data; \
+ baro.absolute = mcp355x_data; \
_b_abs_handler(); \
mcp355x_data_available = FALSE; \
} \
}
-#endif // BOARDS_UMARIM_BARO_H
+#endif // BOARDS_NAVGO_BARO_H
diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c
index 4bc9d9767f..c83675d17b 100644
--- a/sw/airborne/boards/umarim/baro_board.c
+++ b/sw/airborne/boards/umarim/baro_board.c
@@ -31,41 +31,28 @@
/* Common Baro struct */
struct Baro baro;
-#if USE_BARO_AS_ALTIMETER
-/* Number of values to compute an offset at startup */
-#define OFFSET_NBSAMPLES_AVRG 100
-
-/* Weight for offset IIR filter */
-#define OFFSET_FILTER 7
-
-float baro_alt;
-float baro_alt_offset;
-uint16_t offset_cnt;
-#endif
+/* Counter to init ads1114 at startup */
+#define BARO_STARTUP_COUNTER 200
+uint16_t startup_cnt;
void baro_init( void ) {
ads1114_init();
baro.status = BS_UNINITIALIZED;
baro.absolute = 0;
baro.differential = 0; /* not handled on this board, use extra module (ex: airspeed_ads1114) */
-#if USE_BARO_AS_ALTIMETER
- baro_alt = 0.;
- baro_alt_offset = 0.;
- offset_cnt = OFFSET_NBSAMPLES_AVRG;
-#endif
+ startup_cnt = BARO_STARTUP_COUNTER;
}
void baro_periodic( void ) {
-#if USE_BARO_AS_ALTIMETER
if (baro.status == BS_UNINITIALIZED && BARO_ABS_ADS.data_available) {
- // IIR filter to compute an initial offset
- baro_alt_offset = (OFFSET_FILTER * baro_alt_offset + (float)baro.absolute) / (OFFSET_FILTER + 1);
- // decrease init counter
- --offset_cnt;
- if (offset_cnt == 0) baro.status = BS_RUNNING;
+ // Run some loops to get correct readings from the adc
+ --startup_cnt;
+ BARO_ABS_ADS.data_available = FALSE;
+ if (startup_cnt == 0) {
+ baro.status = BS_RUNNING;
+ }
}
-#endif
// Read the ADC
ads1114_read(&BARO_ABS_ADS);
}
diff --git a/sw/airborne/boards/umarim/baro_board.h b/sw/airborne/boards/umarim/baro_board.h
index 62f09ca49c..a296e6a28c 100644
--- a/sw/airborne/boards/umarim/baro_board.h
+++ b/sw/airborne/boards/umarim/baro_board.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2010 ENAC
+ * Copyright (C) 2010-2012 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -33,24 +33,15 @@
#include "std.h"
#include "peripherals/ads1114.h"
-#define BARO_FILTER_GAIN 5
-
/* There is no differential pressure on the board but
* it can be available from an external sensor
* */
-#define DIFF_FILTER_GAIN 5
-
-#if USE_BARO_AS_ALTIMETER
-extern float baro_alt;
-extern float baro_alt_offset;
-#define BaroAltHandler() { baro_alt = BARO_SENS*(baro_alt_offset - (float)baro.absolute); }
-#endif
#define BARO_ABS_ADS ads1114_1
#define BaroAbs(_ads, _handler) { \
if (_ads.data_available) { \
- baro.absolute = (baro.absolute + BARO_FILTER_GAIN*Ads1114GetValue(_ads)) / (BARO_FILTER_GAIN+1); \
+ baro.absolute = Ads1114GetValue(_ads); \
if (baro.status == BS_RUNNING) { \
_handler(); \
_ads.data_available = FALSE; \
@@ -65,14 +56,14 @@ extern float baro_alt_offset;
#ifndef BARO_DIFF_ADS
#define BARO_DIFF_ADS ads1114_2
#endif
-#define BaroDiff(_ads, _handler) { \
- if (_ads.data_available) { \
- baro.differential = (baro.differential + DIFF_FILTER_GAIN*Ads1114GetValue(_ads)) / (DIFF_FILTER_GAIN+1); \
- if (baro.status == BS_RUNNING) { \
- _handler(); \
- _ads.data_available = FALSE; \
- } \
- } \
+#define BaroDiff(_ads, _handler) { \
+ if (_ads.data_available) { \
+ baro.differential = Ads1114GetValue(_ads); \
+ if (baro.status == BS_RUNNING) { \
+ _handler(); \
+ _ads.data_available = FALSE; \
+ } \
+ } \
}
#else // Not using differential with ADS1114
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
deleted file mode 100644
index 2d01172f43..0000000000
--- a/sw/airborne/estimator.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
- *
- * 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 estimator.h
- * \brief State estimation, fusioning sensors
- */
-
-#ifndef ESTIMATOR_H
-#define ESTIMATOR_H
-
-#include
-
-#include "std.h"
-#ifdef BARO_MS5534A
-#include "baro_MS5534A.h"
-#endif
-
-#if USE_BARO_ETS
-#include "modules/sensors/baro_ets.h"
-#endif
-
-#if USE_BARO_BMP
-#include "modules/sensors/baro_bmp.h"
-#endif
-
-/* position in meters, ENU frame, relative to reference */
-extern float estimator_x; ///< east position in meters
-extern float estimator_y; ///< north position in meters
-extern float estimator_z; ///< altitude above MSL in meters
-
-/* attitude in radians */
-extern float estimator_phi; ///< roll angle in rad, + = right
-extern float estimator_psi; ///< heading in rad, CW, 0 = N
-extern float estimator_theta; ///< pitch angle in rad, + = up
-
-/* speed in meters per second */
-extern float estimator_z_dot;
-
-/* rates in radians per second */
-extern float estimator_p;
-extern float estimator_q;
-extern float estimator_r;
-
-/** flight time in seconds. */
-extern uint16_t estimator_flight_time;
-extern float estimator_t;
-
-/* horizontal ground speed in module and dir (m/s, rad (CW/North)) */
-extern float estimator_hspeed_mod; ///< module of horizontal ground speed in m/s
-extern float estimator_hspeed_dir; ///< direction of horizontal ground speed in rad (CW/North)
-
-/* Wind and airspeed estimation sent by the GCS */
-extern float wind_east, wind_north; /* m/s */
-extern float estimator_airspeed; ///< m/s
-
-extern float estimator_AOA; ///< angle of attack in rad
-
-void estimator_init( void );
-
-void estimator_update_state_gps( void );
-
-extern bool_t alt_kalman_enabled;
-#ifdef ALT_KALMAN
-extern void alt_kalman_reset( void );
-extern void alt_kalman_init( void );
-extern void alt_kalman( float );
-#endif
-
-
-#define GetPosX() (estimator_x)
-#define GetPosY() (estimator_y)
-#define GetPosAlt() (estimator_z)
-
-
-#ifdef ALT_KALMAN
-#define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
-
-#if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP
-/* Kalman filter cannot be disabled in this mode (no z_dot) */
-#define EstimatorSetAlt(z) alt_kalman(z)
-#else /* USE_BARO_x */
-#define EstimatorSetAlt(z) { \
- if (!alt_kalman_enabled) { \
- estimator_z = z; \
- } else { \
- alt_kalman(z); \
- } \
-}
-#endif /* ! USE_BARO_x */
-
-#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
- estimator_hspeed_mod = vhmod; \
- estimator_hspeed_dir = vhdir; \
- if (!alt_kalman_enabled) estimator_z_dot = vz; \
-}
-#else /* ALT_KALMAN */
-#define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
-#define EstimatorSetAlt(z) { estimator_z = z; }
-#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
- estimator_hspeed_mod = vhmod; \
- estimator_hspeed_dir = vhdir; \
- estimator_z_dot = vz; \
-}
-
-#endif
-
-#define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; }
-#define EstimatorSetAOA(AOA) { estimator_AOA = AOA; }
-
-#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
-#define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
-
-#define EstimatorSetRate(p, q, r) { estimator_p = p; estimator_q = q; estimator_r = r; }
-
-
-#endif /* ESTIMATOR_H */
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
index f309f37094..a8cdeb4cd3 100644
--- a/sw/airborne/firmwares/fixedwing/ap_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h
@@ -38,6 +38,7 @@
#include
#include "generated/airframe.h"
+#include "state.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -62,7 +63,7 @@
&v_ctl_throttle_slewed, \
&vsupply, \
&s, \
- &estimator_flight_time, \
+ &autopilot_flight_time, \
&kill_throttle, \
&block_time, \
&stage_time, \
@@ -86,7 +87,8 @@
#define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \
- DOWNLINK_SEND_ATTITUDE(_trans, _dev, &estimator_phi, &estimator_psi, &estimator_theta); \
+ struct FloatEulers* att = stateGetNedToBodyEulers_f(); \
+ DOWNLINK_SEND_ATTITUDE(_trans, _dev, &(att->phi), &(att->psi), &(att->theta)); \
})
@@ -186,9 +188,13 @@
#define PERIODIC_SEND_IMU(_trans, _dev) {}
#endif
-#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &estimator_z, &estimator_z_dot)
+#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z))
-#define SEND_NAVIGATION(_trans, _dev) Downlink({ uint8_t _circle_count = NavCircleCount(); DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &estimator_x, &estimator_y, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count);})
+#define SEND_NAVIGATION(_trans, _dev) Downlink({ \
+ uint8_t _circle_count = NavCircleCount(); \
+ struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
+ DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \
+ })
#define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev)
@@ -210,7 +216,7 @@
#define PERIODIC_SEND_RANGEFINDER(_trans, _dev) DOWNLINK_SEND_RANGEFINDER(_trans, _dev, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
-#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
+#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
#if USE_GPS || defined SITL
#define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
@@ -284,8 +290,7 @@
#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {}
#endif
-//FIXME: we need a better switch here...
-#if BOARD_HAS_BARO && USE_BARO_AS_ALTIMETER
+#if USE_BAROMETER
#include "subsystems/sensors/baro.h"
#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \
DOWNLINK_SEND_BARO_RAW(_trans, _dev, \
@@ -297,9 +302,12 @@
#endif
#ifdef MEASURE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed)
+#define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \
+ float* airspeed = stateGetAirspeed_f(); \
+ DOWNLINK_SEND_AIRSPEED (_trans, _dev, airspeed, airspeed, airspeed, airspeed); \
+}
#elif USE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
+#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
#else
#define PERIODIC_SEND_AIRSPEED(_trans, _dev) {}
#endif
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index a7d7e16a1e..bd8f20c1dd 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -33,7 +33,6 @@
#include
#include "std.h"
#include "mcu_periph/sys_time.h"
-#include "estimator.h"
#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
@@ -55,6 +54,9 @@
extern uint8_t pprz_mode;
extern bool_t kill_throttle;
+/** flight time in seconds. */
+extern uint16_t autopilot_flight_time;
+
// FIXME, move to control
#define LATERAL_MODE_MANUAL 0
@@ -95,7 +97,7 @@ extern bool_t power_switch;
#endif // POWER_SWITCH_LED
#define autopilot_ResetFlightTimeAndLaunch(_) { \
- estimator_flight_time = 0; launch = FALSE; \
+ autopilot_flight_time = 0; launch = FALSE; \
}
/* CONTROL_RATE will be removed in the next release
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 553bb828df..ae0195f0cd 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -40,7 +40,7 @@
#endif // TRAFFIC_INFO
#if defined NAV || defined WIND_INFO
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#endif
@@ -134,13 +134,16 @@ void dl_parse_msg(void) {
#endif /** NAV */
#ifdef WIND_INFO
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
- wind_east = DL_WIND_INFO_east(dl_buffer);
- wind_north = DL_WIND_INFO_north(dl_buffer);
+ struct FloatVect2 wind;
+ wind.x = DL_WIND_INFO_north(dl_buffer);
+ wind.y = DL_WIND_INFO_east(dl_buffer);
+ stateSetHorizontalWindspeed_f(&wind);
#if !USE_AIRSPEED
- estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
+ float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
+ stateSetAirspeed_f(&airspeed);
#endif
#ifdef WIND_INFO_RET
- DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, &estimator_airspeed);
+ DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
#endif
} else
#endif /** WIND_INFO */
diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
index 669d9680c9..d33c72d60f 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
@@ -53,7 +53,7 @@
#pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!"
#include "firmwares/fixedwing/guidance/energy_ctrl.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
@@ -146,13 +146,13 @@ static void ac_char_update(float throttle, float pitch, float climb, float accel
{
ac_char_climb_count++;
ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count );
- ac_char_average(&ac_char_climb_max , estimator_z_dot, ac_char_climb_count );
+ ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count );
}
else if (throttle <= 0.0f)
{
ac_char_descend_count++;
ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count );
- ac_char_average(&ac_char_descend_max , estimator_z_dot , ac_char_descend_count );
+ ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count );
}
else if ((climb > -0.125) && (climb < 0.125))
{
@@ -195,7 +195,7 @@ void v_ctl_altitude_loop( void )
if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
// Altitude Controller
- v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
+ v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ;
BoundAbs(sp, v_ctl_max_climb);
@@ -229,7 +229,7 @@ static float low_pass_vdot(float v)
void v_ctl_climb_loop( void )
{
// Airspeed outerloop: positive means we need to accelerate
- float speed_error = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
+ float speed_error = v_ctl_auto_airspeed_setpoint - (*stateGetAirspeed_f());
// Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
@@ -248,7 +248,7 @@ void v_ctl_climb_loop( void )
float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
// Flight Path Outerloop: positive means needs to climb more: needs extra energy
- float gamma_err = (v_ctl_climb_setpoint - estimator_z_dot) / v_ctl_auto_airspeed_setpoint;
+ float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_setpoint;
// Total Energy Error: positive means energy should be added
float en_tot_err = gamma_err + vdot_err;
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
index 1b79c6ebbf..5b86be06bc 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
@@ -26,7 +26,7 @@
*/
#include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
@@ -206,7 +206,7 @@ void v_ctl_altitude_loop( void ) {
}
#endif
- v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
+ v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error
+ v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction;
BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb);
@@ -253,7 +253,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
static float last_err;
float f_throttle = 0;
- float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
@@ -326,14 +326,14 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
// Pitch control (input: rate of climb error, output: pitch setpoint)
- float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
v_ctl_auto_pitch_sum_err += err;
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
(err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
- float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod);
+ float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f());
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
@@ -345,7 +345,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
}
// Airspeed control loop (input: airspeed controlled, output: throttle controlled)
- float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed);
+ float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f());
v_ctl_auto_airspeed_sum_err += err_airspeed;
BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
@@ -367,7 +367,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
*/
#ifdef V_CTL_AUTO_PITCH_PGAIN
inline static void v_ctl_climb_auto_pitch_loop(void) {
- float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_auto_pitch_sum_err += err;
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 8a05ad6217..7bbcd6d54b 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -27,7 +27,7 @@
#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "firmwares/fixedwing/guidance/guidance_v_n.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
@@ -184,7 +184,7 @@ void v_ctl_altitude_loop( void ) {
//static float last_lead_input = 0.;
// Altitude error
- v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
+ v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb;
// Lead controller
@@ -208,7 +208,7 @@ static inline void v_ctl_set_pitch ( void ) {
v_ctl_auto_pitch_sum_err = 0;
// Compute errors
- float err = v_ctl_climb_setpoint - estimator_z_dot;
+ float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
@@ -233,7 +233,7 @@ static inline void v_ctl_set_throttle( void ) {
v_ctl_auto_throttle_sum_err = 0;
// Compute errors
- float err = v_ctl_climb_setpoint - estimator_z_dot;
+ float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
@@ -263,7 +263,7 @@ static inline void v_ctl_set_airspeed( void ) {
Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
// Compute errors
- float err_vz = v_ctl_climb_setpoint - estimator_z_dot;
+ float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD;
last_err_vz = err_vz;
if (v_ctl_auto_throttle_igain > 0.) {
@@ -275,7 +275,7 @@ static inline void v_ctl_set_airspeed( void ) {
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
}
- float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
+ float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f();
float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD;
last_err_as = err_airspeed;
if (v_ctl_auto_airspeed_throttle_igain > 0.) {
@@ -320,7 +320,7 @@ static inline void v_ctl_set_airspeed( void ) {
static inline void v_ctl_set_groundspeed( void ) {
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
- float err_groundspeed = v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod;
+ float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f();
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain;
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index da35197ddc..7db3b69cdc 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -53,10 +53,14 @@
#if USE_AHRS_ALIGNER
#include "subsystems/ahrs/ahrs_aligner.h"
#endif
+#if USE_BAROMETER
+#include "subsystems/sensors/baro.h"
+#endif
// autopilot & control
+#include "state.h"
#include "firmwares/fixedwing/autopilot.h"
-#include "estimator.h"
+#include "subsystems/ins.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include CTRL_TYPE_H
#include "subsystems/nav.h"
@@ -95,6 +99,10 @@ volatile uint8_t ahrs_timeout_counter = 0;
static inline void on_gps_solution( void );
#endif
+#if USE_BAROMETER
+static inline void on_baro_abs_event( void );
+static inline void on_baro_dif_event( void );
+#endif
bool_t power_switch;
@@ -114,6 +122,9 @@ bool_t kill_throttle = FALSE;
bool_t launch = FALSE;
+/* flight time in seconds */
+uint16_t autopilot_flight_time = 0;
+
/** Supply voltage in deciVolt.
* This the ap copy of the measurement from fbw
@@ -167,10 +178,14 @@ void init_ap( void ) {
ahrs_init();
#endif
-#if USE_INS
- ins_init();
+#if USE_BAROMETER
+ baro_init();
#endif
+ ins_init();
+
+ stateInit();
+
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
@@ -182,10 +197,6 @@ void init_ap( void ) {
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
- estimator_init();
-#ifdef ALT_KALMAN
- alt_kalman_init();
-#endif
nav_init();
modules_init();
@@ -375,7 +386,7 @@ static inline void telecommand_task( void ) {
current = fbw_state->current;
#ifdef RADIO_CONTROL
- if (!estimator_flight_time) {
+ if (!autopilot_flight_time) {
if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) {
launch = TRUE;
}
@@ -512,7 +523,7 @@ void attitude_loop( void ) {
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
- if (kill_throttle || (!estimator_flight_time && !launch))
+ if (kill_throttle || (!autopilot_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
@@ -549,9 +560,11 @@ void sensors_task( void ) {
ahrs_propagate();
#endif
-#if USE_INS
- ins_periodic_task();
+#if USE_BAROMETER
+ baro_periodic();
#endif
+
+ ins_periodic();
}
@@ -570,8 +583,8 @@ void sensors_task( void ) {
/** monitor stuff run at 1Hz */
void monitor_task( void ) {
- if (estimator_flight_time)
- estimator_flight_time++;
+ if (autopilot_flight_time)
+ autopilot_flight_time++;
#if defined DATALINK || defined SITL
datalink_time++;
#endif
@@ -584,9 +597,9 @@ void monitor_task( void ) {
kill_throttle |= (t >= LOW_BATTERY_DELAY);
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
- if (!estimator_flight_time &&
- estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
- estimator_flight_time = 1;
+ if (!autopilot_flight_time &&
+ *stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
+ autopilot_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
@@ -619,6 +632,9 @@ void event_task_ap( void ) {
GpsEvent(on_gps_solution);
#endif /* USE_GPS */
+#if USE_BAROMETER
+ BaroEvent(on_baro_abs_event, on_baro_dif_event);
+#endif
DatalinkEvent();
@@ -649,7 +665,7 @@ void event_task_ap( void ) {
#if USE_GPS
static inline void on_gps_solution( void ) {
- estimator_update_state_gps();
+ ins_update_gps();
#if USE_AHRS
ahrs_update_gps();
#endif
@@ -687,7 +703,6 @@ static inline void on_gyro_event( void ) {
ahrs_propagate();
ahrs_update_accel();
- ahrs_update_fw_estimator();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
@@ -727,8 +742,6 @@ static inline void on_gyro_event( void ) {
ahrs_update_accel();
}
- ahrs_update_fw_estimator();
-
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
#endif
@@ -751,3 +764,14 @@ static inline void on_mag_event(void)
#endif // USE_AHRS
+#if USE_BAROMETER
+
+static inline void on_baro_abs_event( void ) {
+ ins_update_baro();
+}
+
+static inline void on_baro_dif_event( void ) {
+
+}
+
+#endif // USE_BAROMETER
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
index 708d5cf02b..8c41aa28e6 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -30,7 +30,7 @@
#include "led.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include CTRL_TYPE_H
@@ -201,14 +201,14 @@ void h_ctl_course_loop ( void ) {
static float last_err;
// Ground path error
- float err = h_ctl_course_setpoint - estimator_hspeed_dir;
+ float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
- float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+ float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
@@ -223,7 +223,7 @@ static inline void compute_airspeed_ratio( void ) {
if (use_airspeed_ratio) {
// low pass airspeed
static float airspeed = 0.;
- airspeed = ( 15*airspeed + estimator_airspeed ) / 16;
+ airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16;
// compute ratio
float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
@@ -285,13 +285,14 @@ inline static void h_ctl_roll_loop( void ) {
#endif
// Compute errors
- float err = h_ctl_ref_roll_angle - estimator_phi;
+ float err = h_ctl_ref_roll_angle - stateGetNedToBodyEulers_f()->phi;
+ struct FloatRates* body_rate = stateGetBodyRates_f();
#ifdef SITL
static float last_err = 0;
- estimator_p = (err - last_err)/(1/60.);
+ body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim
last_err = err;
#endif
- float d_err = h_ctl_ref_roll_rate - estimator_p;
+ float d_err = h_ctl_ref_roll_rate - body_rate->p;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
@@ -325,7 +326,7 @@ inline static void loiter(void) {
float pitch_trim;
#if USE_AIRSPEED
- if (estimator_airspeed > NOMINAL_AIRSPEED) {
+ if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
} else {
pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
@@ -355,7 +356,7 @@ inline static void h_ctl_pitch_loop( void ) {
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
- h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
+ h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
#if USE_PITCH_TRIM
loiter();
#endif
@@ -382,9 +383,9 @@ inline static void h_ctl_pitch_loop( void ) {
#endif
// Compute errors
- float err = h_ctl_ref_pitch_angle - estimator_theta;
+ float err = h_ctl_ref_pitch_angle - stateGetNedToBodyEulers_f()->theta;
#if USE_GYRO_PITCH_RATE
- float d_err = h_ctl_ref_pitch_rate - estimator_q;
+ float d_err = h_ctl_ref_pitch_rate - stateGetBodyRates_f()->q;
#else // soft derivation
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
index c7c8158f55..b868d790b6 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
@@ -31,7 +31,7 @@
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "std.h"
#include "led.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include CTRL_TYPE_H
@@ -179,17 +179,17 @@ void h_ctl_course_loop ( void ) {
static float last_err;
// Ground path error
- float err = estimator_hspeed_dir - h_ctl_course_setpoint;
+ float err = *stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
- float advance = cos(err) * estimator_hspeed_mod / reference_advance;
+ float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance;
if (
(advance < 1.) && // Path speed is small
- (estimator_hspeed_mod < reference_advance) // Small path speed is due to wind (small groundspeed)
+ ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed)
)
{
/*
@@ -208,7 +208,7 @@ void h_ctl_course_loop ( void ) {
*/
// Heading error
- float herr = estimator_psi - h_ctl_course_setpoint; //+crab);
+ float herr = *stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
NormRadAngle(herr);
if (advance < -0.5) // 90 degree turn
@@ -266,7 +266,7 @@ void h_ctl_course_loop ( void ) {
}
#endif
- float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+ float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
@@ -314,14 +314,15 @@ void h_ctl_attitude_loop ( void ) {
#ifdef H_CTL_ROLL_ATTITUDE_GAIN
inline static void h_ctl_roll_loop( void ) {
- float err = estimator_phi - h_ctl_roll_setpoint;
+ float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
+ struct FloatRates* body_rate = stateGetBodyRates_f();
#ifdef SITL
static float last_err = 0;
- estimator_p = (err - last_err)/(1/60.);
+ body_rate->p = (err - last_err)/(1/60.);
last_err = err;
#endif
float cmd = h_ctl_roll_attitude_gain * err
- + h_ctl_roll_rate_gain * estimator_p
+ + h_ctl_roll_rate_gain * body_rate->p
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -331,7 +332,7 @@ inline static void h_ctl_roll_loop( void ) {
/** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
inline static void h_ctl_roll_loop( void ) {
- float err = estimator_phi - h_ctl_roll_setpoint;
+ float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
float cmd = h_ctl_roll_pgain * err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -355,7 +356,7 @@ inline static void h_ctl_roll_loop( void ) {
#ifdef H_CTL_RATE_LOOP
static inline void h_ctl_roll_rate_loop() {
- float err = estimator_p - h_ctl_roll_rate_setpoint;
+ float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
/* I term calculation */
static float roll_rate_sum_err = 0.;
@@ -417,28 +418,29 @@ inline static float loiter(void) {
inline static void h_ctl_pitch_loop( void ) {
static float last_err;
+ struct FloatEulers* att = stateGetNedToBodyEulers_f();
/* sanity check */
if (h_ctl_elevator_of_roll <0.)
h_ctl_elevator_of_roll = 0.;
- h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
+ h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
float err = 0;
#ifdef USE_AOA
switch (h_ctl_pitch_mode){
case H_CTL_PITCH_MODE_THETA:
- err = estimator_theta - h_ctl_pitch_loop_setpoint;
+ err = att->theta - h_ctl_pitch_loop_setpoint;
break;
case H_CTL_PITCH_MODE_AOA:
- err = estimator_AOA - h_ctl_pitch_loop_setpoint;
+ err = (*stateGetAngleOfAttack_f()) - h_ctl_pitch_loop_setpoint;
break;
default:
- err = estimator_theta - h_ctl_pitch_loop_setpoint;
+ err = att->theta - h_ctl_pitch_loop_setpoint;
break;
}
#else //NO_AOA
- err = estimator_theta - h_ctl_pitch_loop_setpoint;
+ err = att->theta - h_ctl_pitch_loop_setpoint;
#endif
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 6c2f4cb13f..d3fbfb1df3 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -114,8 +114,9 @@ extern uint16_t autopilot_flight_time;
#endif
static inline void DetectGroundEvent(void) {
if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) {
- if (ins_ltp_accel.z < -THRESHOLD_GROUND_DETECT ||
- ins_ltp_accel.z > THRESHOLD_GROUND_DETECT) {
+ struct NedCoor_i* accel = stateGetAccelNed_i();
+ if (accel->z < -THRESHOLD_GROUND_DETECT ||
+ accel->z > THRESHOLD_GROUND_DETECT) {
autopilot_detect_ground = TRUE;
autopilot_detect_ground_once = FALSE;
}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index df5a66fcde..c7189d4918 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -30,10 +30,10 @@
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
-#include "subsystems/ahrs.h"
-#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
+#include "state.h"
+
#include "generated/airframe.h"
uint8_t guidance_h_mode;
@@ -209,7 +209,7 @@ void guidance_h_run(bool_t in_flight) {
stab_att_sp_euler.phi = nav_roll;
stab_att_sp_euler.theta = nav_pitch;
/* FIXME: heading can't be set via attitude block yet, use current heading for now */
- stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
+ stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
@@ -266,12 +266,12 @@ static inline void guidance_h_update_reference(bool_t use_ref) {
static inline void guidance_h_traj_run(bool_t in_flight) {
/* compute position error */
- VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, ins_ltp_pos);
+ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i());
/* saturate it */
VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, ins_ltp_speed);
+ VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, *stateGetSpeedNed_i());
/* saturate it */
VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -300,8 +300,9 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
/* Rotate to body frame */
int32_t s_psi, c_psi;
- PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi);
- PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
+ int32_t psi = stateGetNedToBodyEulers_i()->psi;
+ PPRZ_ITRIG_SIN(s_psi, psi);
+ PPRZ_ITRIG_COS(c_psi, psi);
// Restore angle ref resolution after rotation
guidance_h_command_body.phi =
@@ -326,9 +327,9 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
static inline void guidance_h_hover_enter(void) {
- VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos);
+ VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i());
- guidance_h_rc_sp.psi = ahrs.ltp_to_body_euler.psi;
+ guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i()->psi;
reset_psi_ref_from_body();
INT_VECT2_ZERO(guidance_h_pos_err_sum);
@@ -340,13 +341,13 @@ static inline void guidance_h_nav_enter(void) {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
struct Int32Vect2 pos,speed,zero;
INT_VECT2_ZERO(zero);
- VECT2_COPY(pos, ins_ltp_pos);
- VECT2_COPY(speed, ins_ltp_speed);
+ VECT2_COPY(pos, *stateGetPositionNed_i());
+ VECT2_COPY(speed, *stateGetSpeedNed_i());
GuidanceHSetRef(pos, speed, zero);
/* reset psi reference, set psi setpoint to current psi */
reset_psi_ref_from_body();
- nav_heading = ahrs.ltp_to_body_euler.psi;
+ nav_heading = stateGetNedToBodyEulers_i()->psi;
INT_VECT2_ZERO(guidance_h_pos_err_sum);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 308cf016f6..7664543650 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -31,11 +31,11 @@
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
// #include "booz_fms.h" FIXME
#include "firmwares/rotorcraft/navigation.h"
-#include "subsystems/ins.h"
+#include "state.h"
+
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
@@ -133,9 +133,9 @@ void guidance_v_mode_changed(uint8_t new_mode) {
switch (new_mode) {
case GUIDANCE_V_MODE_HOVER:
- guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint
+ guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
guidance_v_z_sum_err = 0;
- GuidanceVSetRef(ins_ltp_pos.z, 0, 0);
+ GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
break;
case GUIDANCE_V_MODE_RC_CLIMB:
@@ -143,7 +143,7 @@ void guidance_v_mode_changed(uint8_t new_mode) {
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
- GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
+ GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
default:
@@ -167,13 +167,13 @@ void guidance_v_run(bool_t in_flight) {
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
if (in_flight) {
- gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
+ gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
}
switch (guidance_v_mode) {
case GUIDANCE_V_MODE_RC_DIRECT:
- guidance_v_z_sp = ins_ltp_pos.z; // for display only
+ guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
break;
@@ -261,9 +261,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
/* compute the error to our reference */
- int32_t err_z = guidance_v_z_ref - ins_ltp_pos.z;
+ int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
- int32_t err_zd = guidance_v_zd_ref - ins_ltp_speed.z;
+ int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
@@ -284,8 +284,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
guidance_v_ff_cmd = g_m_zdd / inv_m;
int32_t cphi,ctheta,cphitheta;
- PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
- PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
+ struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i();
+ PPRZ_ITRIG_COS(cphi, att_euler->phi);
+ PPRZ_ITRIG_COS(ctheta, att_euler->theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
/* feed forward command */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 8eb40f3f9d..9d7e2a37c4 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -54,6 +54,8 @@
#include "subsystems/ahrs.h"
#include "subsystems/ins.h"
+#include "state.h"
+
#include "firmwares/rotorcraft/main.h"
#ifdef SITL
@@ -95,6 +97,8 @@ STATIC_INLINE void main_init( void ) {
electrical_init();
+ stateInit();
+
actuators_init();
radio_control_init();
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index aa6227c4e9..e5a9668526 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -28,6 +28,7 @@
#include "pprz_debug.h"
#include "subsystems/gps.h"
#include "subsystems/ins.h"
+#include "state.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "generated/modules.h"
@@ -111,7 +112,7 @@ void nav_run(void) {
/* compute a vector to the waypoint */
struct Int32Vect2 path_to_waypoint;
- VECT2_DIFF(path_to_waypoint, navigation_target, ins_enu_pos);
+ VECT2_DIFF(path_to_waypoint, navigation_target, *stateGetPositionEnu_i());
/* saturate it */
VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
@@ -127,7 +128,7 @@ void nav_run(void) {
struct Int32Vect2 path_to_carrot;
VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
- VECT2_SUM(navigation_carrot, path_to_carrot, ins_enu_pos);
+ VECT2_SUM(navigation_carrot, path_to_carrot, *stateGetPositionEnu_i());
}
#else
// if H_REF is used, CARROT_DIST is not used
@@ -143,7 +144,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
}
else {
struct Int32Vect2 pos_diff;
- VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_center]);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
@@ -187,7 +188,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
void nav_route(uint8_t wp_start, uint8_t wp_end) {
struct Int32Vect2 wp_diff,pos_diff;
VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
- VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_start]);
// go back to metric precision or values are too large
INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
@@ -222,7 +223,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
int32_t dist_to_point;
struct Int32Vect2 diff;
static uint8_t time_at_wp = 0;
- VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos);
+ VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_VECT2_NORM(dist_to_point, diff);
//printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
@@ -263,13 +264,14 @@ unit_t nav_reset_alt( void ) {
#if USE_GPS
ins_ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl;
+ stateSetLocalOrigin_i(&ins_ltp_def);
#endif
return 0;
}
void nav_init_stage( void ) {
- INT32_VECT3_COPY(nav_last_point, ins_enu_pos);
+ INT32_VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
stage_time = 0;
nav_circle_radians = 0;
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index d71cff7586..5df3d485f4 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -86,7 +86,7 @@ void nav_home(void);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
-#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos); FALSE; })
+#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); FALSE; })
#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })
#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
@@ -192,9 +192,9 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
}
-#define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x)
-#define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y)
-#define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
+#define GetPosX() (stateGetPositionEnu_f()->x)
+#define GetPosY() (stateGetPositionEnu_f()->y)
+#define GetPosAlt() (stateGetPositionEnu_f()->z+ground_alt)
extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp );
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
index 173fe3454f..ac14499dcf 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
@@ -9,7 +9,7 @@
*
*/
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "stabilization/quat_setpoint_int.h"
@@ -94,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
// update setpoint by rotating by incremental yaw command
INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat);
} else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */
- reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &ahrs.ltp_to_body_quat);
+ reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i());
}
// update euler setpoints for telemetry
@@ -104,5 +104,5 @@ void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
void stabilization_attitude_sp_enter(void)
{
// reset setpoint to "hover"
- reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat);
+ reset_sp_quat(0., 0., stateGetNedToBodyQuat_i());
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
index 6139806670..4ca9c4aded 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
@@ -24,7 +24,7 @@
#include "firmwares/rotorcraft/stabilization.h"
#include "math/pprz_algebra_float.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "subsystems/radio_control.h"
#include "generated/airframe.h"
@@ -111,10 +111,9 @@ void stabilization_attitude_run(bool_t in_flight) {
/* Compute feedback */
/* attitude error */
- struct FloatEulers att_float;
- EULERS_FLOAT_OF_BFP(att_float, ahrs.ltp_to_body_euler);
+ struct FloatEulers att_float* = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
- EULERS_DIFF(att_err, stab_att_ref_euler, att_float);
+ EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -127,10 +126,9 @@ void stabilization_attitude_run(bool_t in_flight) {
}
/* rate error */
- struct FloatRates rate_float;
- RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
+ struct FloatRates* rate_float = stateGetBodyRates_f();
struct FloatRates rate_err;
- RATES_DIFF(rate_err, stab_att_ref_rate, rate_float);
+ RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
/* PID */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index a0d42477d3..c204263705 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -20,7 +20,7 @@
*/
#include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
@@ -87,7 +87,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
void stabilization_attitude_enter(void) {
- stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
+ stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
reset_psi_ref_from_body();
INT_EULERS_ZERO( stabilization_att_sum_err );
@@ -120,7 +120,8 @@ void stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) };
struct Int32Eulers att_err;
- EULERS_DIFF(att_err, att_ref_scaled, ahrs.ltp_to_body_euler);
+ struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
+ EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
INT32_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -138,7 +139,8 @@ void stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
- RATES_DIFF(rate_err, rate_ref_scaled, ahrs.body_rate);
+ struct Int32Rates* body_rate = stateGetBodyRates_i();
+ RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
index 011a486ae1..71d64b961b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -30,7 +30,7 @@
#include
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "generated/airframe.h"
#include "stabilization_attitude_float.h"
#include "stabilization_attitude_rc_setpoint.h"
@@ -40,6 +40,8 @@ struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_
struct FloatQuat stabilization_att_sum_err_quat;
struct FloatEulers stabilization_att_sum_err_eulers;
+struct FloatRates last_body_rate;
+
float stabilization_att_fb_cmd[COMMANDS_NB];
float stabilization_att_ff_cmd[COMMANDS_NB];
@@ -101,6 +103,7 @@ void stabilization_attitude_init(void) {
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
+ FLOAT_RAYES_ZERO( last_body_rate );
}
void stabilization_attitude_gain_schedule(uint8_t idx)
@@ -197,13 +200,19 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* attitude error */
struct FloatQuat att_err;
- FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat);
+ struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
+ FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
- RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);
+ struct FloatRates* body_rate = stateGetBodyRates_f();
+ RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
+ /* rate_d error */
+ struct FloatRates body_rate_d;
+ RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
+ RATES_COPY(last_body_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
@@ -225,7 +234,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
- attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);
+ attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, body_rate_d, &stabilization_att_sum_err_quat);
// FIXME: this is very dangerous! only works if this really includes all commands
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index 1aec849712..d2f14e8c0e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -33,7 +33,7 @@
#include
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "generated/airframe.h"
struct Int32AttitudeGains stabilization_gains = {
@@ -80,7 +80,7 @@ void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
- stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
+ stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
#endif
stabilization_attitude_ref_enter();
@@ -135,14 +135,16 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* attitude error */
struct Int32Quat att_err;
- INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat);
+ struct Int32Quat* att_quat = stateGetNedToBodyQuat_i();
+ INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
INT32_QUAT_WRAP_SHORTEST(att_err);
INT32_QUAT_NORMALIZE(att_err);
/* rate error */
struct Int32Rates rate_err;
- RATES_DIFF(rate_err, stab_att_ref_rate, ahrs.body_rate);
+ struct Int32Rates* body_rate = stateGetBodyRates_i();
+ RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
@@ -194,7 +196,7 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
- FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi));
+ FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));
/* apply roll and pitch commands with respect to current heading */
struct FloatQuat q_sp;
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
index 03ce96df9b..f1a5107ca0 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
@@ -32,7 +32,7 @@
#include "math/pprz_algebra_float.h"
#include "subsystems/radio_control.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#ifdef STABILISATION_ATTITUDE_TYPE_INT
#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
@@ -80,7 +80,7 @@ static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eu
}
}
else { /* if not flying, use current yaw as setpoint */
- sp->psi = ahrs.ltp_to_body_euler.psi;
+ sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
index bd3a6d2a49..c45c8292c2 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
@@ -59,7 +59,7 @@
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
- _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \
+ _sp.psi = stateGetNedToBodyEulers_f()->psi; \
} \
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
index 8026b06828..5be7bae51e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
@@ -29,7 +29,7 @@
#include "math/pprz_algebra_int.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
@@ -58,7 +58,7 @@ extern struct Int32RefModel stab_att_ref_model;
static inline void reset_psi_ref_from_body(void) {
- stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+ stab_att_ref_euler.psi = stateGetNedToBodyEulers_i()->psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
index f31f758b4c..09a2891acc 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
@@ -28,7 +28,7 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "stabilization_attitude_ref_float.h"
@@ -55,7 +55,7 @@ static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R;
static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R;
static void reset_psi_ref_from_body(void) {
- stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi;
+ stab_att_ref_euler.psi = stateGetNedToBodyEulers_f()->psi;
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
index c49529e168..7352be5239 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -26,7 +26,6 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
#include "stabilization_attitude_ref_int.h"
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
index 79c2328e3a..b1f39edc4f 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
@@ -28,7 +28,7 @@
#include "firmwares/rotorcraft/stabilization.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "subsystems/imu.h"
#include "subsystems/radio_control.h"
@@ -193,7 +193,8 @@ void stabilization_rate_run(bool_t in_flight) {
OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates _error;
- RATES_DIFF(_error, _ref_scaled, ahrs.body_rate);
+ struct Int32Rates* body_rate = stateGetBodyRates_i();
+ RATES_DIFF(_error, _ref_scaled, (*body_rate));
if (in_flight) {
/* update integrator */
RATES_ADD(stabilization_rate_sum_err, _error);
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 25511638e4..4c4695a7a2 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -35,6 +35,8 @@
#include "subsystems/radio_control.h"
#endif
+#include "state.h"
+
#include "firmwares/rotorcraft/autopilot.h"
#include "firmwares/rotorcraft/guidance.h"
@@ -211,30 +213,28 @@
}
#ifdef STABILISATION_ATTITUDE_TYPE_INT
-#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
- DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \
- &ahrs.body_rate.p, \
- &ahrs.body_rate.q, \
- &ahrs.body_rate.r, \
- &ahrs.ltp_to_body_euler.phi, \
- &ahrs.ltp_to_body_euler.theta, \
- &ahrs.ltp_to_body_euler.psi, \
- &stab_att_sp_euler.phi, \
- &stab_att_sp_euler.theta, \
- &stab_att_sp_euler.psi, \
- &stabilization_att_sum_err.phi, \
- &stabilization_att_sum_err.theta, \
- &stabilization_att_sum_err.psi, \
- &stabilization_att_fb_cmd[COMMAND_ROLL], \
- &stabilization_att_fb_cmd[COMMAND_PITCH], \
- &stabilization_att_fb_cmd[COMMAND_YAW], \
- &stabilization_att_ff_cmd[COMMAND_ROLL], \
- &stabilization_att_ff_cmd[COMMAND_PITCH], \
- &stabilization_att_ff_cmd[COMMAND_YAW], \
- &stabilization_cmd[COMMAND_ROLL], \
- &stabilization_cmd[COMMAND_PITCH], \
- &stabilization_cmd[COMMAND_YAW]); \
- }
+#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
+ struct Int32Rates* body_rate = stateGetBodyRates_i(); \
+ struct Int32Eulers* att = stateGetNedToBodyEulers_i(); \
+ DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \
+ &(body_rate->p), &(body_rate->q), &(body_rate->r), \
+ &(att->phi), &(att->theta), &(att->psi), \
+ &stab_att_sp_euler.phi, \
+ &stab_att_sp_euler.theta, \
+ &stab_att_sp_euler.psi, \
+ &stabilization_att_sum_err.phi, \
+ &stabilization_att_sum_err.theta, \
+ &stabilization_att_sum_err.psi, \
+ &stabilization_att_fb_cmd[COMMAND_ROLL], \
+ &stabilization_att_fb_cmd[COMMAND_PITCH], \
+ &stabilization_att_fb_cmd[COMMAND_YAW], \
+ &stabilization_att_ff_cmd[COMMAND_ROLL], \
+ &stabilization_att_ff_cmd[COMMAND_PITCH], \
+ &stabilization_att_ff_cmd[COMMAND_YAW], \
+ &stabilization_cmd[COMMAND_ROLL], \
+ &stabilization_cmd[COMMAND_PITCH], \
+ &stabilization_cmd[COMMAND_YAW]); \
+}
#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \
@@ -255,32 +255,29 @@
#endif /* STABILISATION_ATTITUDE_TYPE_INT */
#ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
-#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
- DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \
- &ahrs_float.body_rate.p, \
- &ahrs_float.body_rate.q, \
- &ahrs_float.body_rate.r, \
- &ahrs_float.ltp_to_body_euler.phi, \
- &ahrs_float.ltp_to_body_euler.theta, \
- &ahrs_float.ltp_to_body_euler.psi, \
- &stab_att_ref_euler.phi, \
- &stab_att_ref_euler.theta, \
- &stab_att_ref_euler.psi, \
- &stabilization_att_sum_err_eulers.phi, \
- &stabilization_att_sum_err_eulers.theta, \
- &stabilization_att_sum_err_eulers.psi, \
- &stabilization_att_fb_cmd[COMMAND_ROLL], \
- &stabilization_att_fb_cmd[COMMAND_PITCH], \
- &stabilization_att_fb_cmd[COMMAND_YAW], \
- &stabilization_att_ff_cmd[COMMAND_ROLL], \
- &stabilization_att_ff_cmd[COMMAND_PITCH], \
- &stabilization_att_ff_cmd[COMMAND_YAW], \
- &stabilization_cmd[COMMAND_ROLL], \
- &stabilization_cmd[COMMAND_PITCH], \
- &stabilization_cmd[COMMAND_YAW], \
- &ahrs_float.body_rate_d.p, \
- &ahrs_float.body_rate_d.q, \
- &ahrs_float.body_rate_d.r); \
+#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \
+ struct FloatRates* body_rate = stateGetBodyRates_f(); \
+ struct FloatEulers* att = stateGetNedToBodyEulers_f(); \
+ float foo; \
+ DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \
+ &(body_rate->p), &(body_rate->q), &(body_rate->r), \
+ &(att->phi), &(att->theta), &(att->psi), \
+ &stab_att_ref_euler.phi, \
+ &stab_att_ref_euler.theta, \
+ &stab_att_ref_euler.psi, \
+ &stabilization_att_sum_err_eulers.phi, \
+ &stabilization_att_sum_err_eulers.theta, \
+ &stabilization_att_sum_err_eulers.psi, \
+ &stabilization_att_fb_cmd[COMMAND_ROLL], \
+ &stabilization_att_fb_cmd[COMMAND_PITCH], \
+ &stabilization_att_fb_cmd[COMMAND_YAW], \
+ &stabilization_att_ff_cmd[COMMAND_ROLL], \
+ &stabilization_att_ff_cmd[COMMAND_PITCH], \
+ &stabilization_att_ff_cmd[COMMAND_YAW], \
+ &stabilization_cmd[COMMAND_ROLL], \
+ &stabilization_cmd[COMMAND_PITCH], \
+ &stabilization_cmd[COMMAND_YAW], \
+ &foo, &foo, &foo); \
}
#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \
@@ -330,9 +327,9 @@
#include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
#define PERIODIC_SEND_FILTER(_trans, _dev) { \
DOWNLINK_SEND_FILTER(_trans, _dev, \
- &ahrs.ltp_to_imu_euler.phi, \
- &ahrs.ltp_to_imu_euler.theta, \
- &ahrs.ltp_to_imu_euler.psi, \
+ &ahrs_impl.ltp_to_imu_euler.phi, \
+ &ahrs_impl.ltp_to_imu_euler.theta, \
+ &ahrs_impl.ltp_to_imu_euler.psi, \
&ahrs_impl.measure.phi, \
&ahrs_impl.measure.theta, \
&ahrs_impl.measure.psi, \
@@ -418,64 +415,65 @@
#endif
#if defined STABILISATION_ATTITUDE_TYPE_QUAT && defined STABILISATION_ATTITUDE_TYPE_INT
-#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \
- DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \
- &stab_att_ref_quat.qi, \
- &stab_att_ref_quat.qx, \
- &stab_att_ref_quat.qy, \
- &stab_att_ref_quat.qz, \
- &ahrs.ltp_to_body_quat.qi, \
- &ahrs.ltp_to_body_quat.qx, \
- &ahrs.ltp_to_body_quat.qy, \
- &ahrs.ltp_to_body_quat.qz); \
+#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \
+ DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \
+ &stab_att_ref_quat.qi, \
+ &stab_att_ref_quat.qx, \
+ &stab_att_ref_quat.qy, \
+ &stab_att_ref_quat.qz, \
+ &(stateGetNedToBodyQuat_i()->qi), \
+ &(stateGetNedToBodyQuat_i()->qx), \
+ &(stateGetNedToBodyQuat_i()->qy), \
+ &(stateGetNedToBodyQuat_i()->qz)); \
}
#else
#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) {}
#endif /* STABILISATION_ATTITUDE_TYPE_QUAT */
-#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \
- DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \
- &ahrs.ltp_to_imu_quat.qi, \
- &ahrs.ltp_to_imu_quat.qx, \
- &ahrs.ltp_to_imu_quat.qy, \
- &ahrs.ltp_to_imu_quat.qz, \
- &ahrs.ltp_to_body_quat.qi, \
- &ahrs.ltp_to_body_quat.qx, \
- &ahrs.ltp_to_body_quat.qy, \
- &ahrs.ltp_to_body_quat.qz); \
+#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \
+ DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \
+ &ahrs_impl.ltp_to_imu_quat.qi, \
+ &ahrs_impl.ltp_to_imu_quat.qx, \
+ &ahrs_impl.ltp_to_imu_quat.qy, \
+ &ahrs_impl.ltp_to_imu_quat.qz, \
+ &(stateGetNedToBodyQuat_i()->qi), \
+ &(stateGetNedToBodyQuat_i()->qx), \
+ &(stateGetNedToBodyQuat_i()->qy), \
+ &(stateGetNedToBodyQuat_i()->qz)); \
}
-#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \
- DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \
- &ahrs.ltp_to_imu_euler.phi, \
- &ahrs.ltp_to_imu_euler.theta, \
- &ahrs.ltp_to_imu_euler.psi, \
- &ahrs.ltp_to_body_euler.phi, \
- &ahrs.ltp_to_body_euler.theta, \
- &ahrs.ltp_to_body_euler.psi); \
+#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \
+ DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \
+ &ahrs_impl.ltp_to_imu_euler.phi, \
+ &ahrs_impl.ltp_to_imu_euler.theta, \
+ &ahrs_impl.ltp_to_imu_euler.psi, \
+ &(stateGetNedToBodyEulers_i()->phi), \
+ &(stateGetNedToBodyEulers_i()->theta), \
+ &(stateGetNedToBodyEulers_i()->psi)); \
}
-#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \
- DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \
- &ahrs.ltp_to_imu_rmat.m[0], \
- &ahrs.ltp_to_imu_rmat.m[1], \
- &ahrs.ltp_to_imu_rmat.m[2], \
- &ahrs.ltp_to_imu_rmat.m[3], \
- &ahrs.ltp_to_imu_rmat.m[4], \
- &ahrs.ltp_to_imu_rmat.m[5], \
- &ahrs.ltp_to_imu_rmat.m[6], \
- &ahrs.ltp_to_imu_rmat.m[7], \
- &ahrs.ltp_to_imu_rmat.m[8], \
- &ahrs.ltp_to_body_rmat.m[0], \
- &ahrs.ltp_to_body_rmat.m[1], \
- &ahrs.ltp_to_body_rmat.m[2], \
- &ahrs.ltp_to_body_rmat.m[3], \
- &ahrs.ltp_to_body_rmat.m[4], \
- &ahrs.ltp_to_body_rmat.m[5], \
- &ahrs.ltp_to_body_rmat.m[6], \
- &ahrs.ltp_to_body_rmat.m[7], \
- &ahrs.ltp_to_body_rmat.m[8]); \
- }
+#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \
+ struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); \
+ DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \
+ &ahrs_impl.ltp_to_imu_rmat.m[0], \
+ &ahrs_impl.ltp_to_imu_rmat.m[1], \
+ &ahrs_impl.ltp_to_imu_rmat.m[2], \
+ &ahrs_impl.ltp_to_imu_rmat.m[3], \
+ &ahrs_impl.ltp_to_imu_rmat.m[4], \
+ &ahrs_impl.ltp_to_imu_rmat.m[5], \
+ &ahrs_impl.ltp_to_imu_rmat.m[6], \
+ &ahrs_impl.ltp_to_imu_rmat.m[7], \
+ &ahrs_impl.ltp_to_imu_rmat.m[8], \
+ &(att_rmat->m[0]), \
+ &(att_rmat->m[1]), \
+ &(att_rmat->m[2]), \
+ &(att_rmat->m[3]), \
+ &(att_rmat->m[4]), \
+ &(att_rmat->m[5]), \
+ &(att_rmat->m[6]), \
+ &(att_rmat->m[7]), \
+ &(att_rmat->m[8])); \
+}
@@ -650,21 +648,21 @@
#define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) { \
int32_t carrot_up = -guidance_v_z_sp; \
DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev, \
- &ins_enu_pos.x, \
- &ins_enu_pos.y, \
- &ins_enu_pos.z, \
- &ins_enu_speed.x, \
- &ins_enu_speed.y, \
- &ins_enu_speed.z, \
- &ahrs.ltp_to_body_euler.phi, \
- &ahrs.ltp_to_body_euler.theta, \
- &ahrs.ltp_to_body_euler.psi, \
- &guidance_h_pos_sp.y, \
- &guidance_h_pos_sp.x, \
- &carrot_up, \
- &guidance_h_command_body.psi, \
- &stabilization_cmd[COMMAND_THRUST], \
- &autopilot_flight_time); \
+ &(stateGetPositionEnu_i()->x), \
+ &(stateGetPositionEnu_i()->y), \
+ &(stateGetPositionEnu_i()->z), \
+ &(stateGetSpeedEnu_i()->x), \
+ &(stateGetSpeedEnu_i()->y), \
+ &(stateGetSpeedEnu_i()->z), \
+ &(stateGetNedToBodyEulers_i()->phi), \
+ &(stateGetNedToBodyEulers_i()->theta), \
+ &(stateGetNedToBodyEulers_i()->psi), \
+ &guidance_h_pos_sp.y, \
+ &guidance_h_pos_sp.x, \
+ &carrot_up, \
+ &guidance_h_command_body.psi, \
+ &stabilization_cmd[COMMAND_THRUST], \
+ &autopilot_flight_time); \
}
#if USE_GPS
@@ -747,12 +745,12 @@
&stabilization_cmd[COMMAND_PITCH], \
&stabilization_cmd[COMMAND_YAW], \
&stabilization_cmd[COMMAND_THRUST], \
- &ahrs.ltp_to_imu_euler.phi, \
- &ahrs.ltp_to_imu_euler.theta, \
- &ahrs.ltp_to_imu_euler.psi, \
- &ahrs.ltp_to_body_euler.phi, \
- &ahrs.ltp_to_body_euler.theta, \
- &ahrs.ltp_to_body_euler.psi); \
+ &ahrs_impl.ltp_to_imu_euler.phi, \
+ &ahrs_impl.ltp_to_imu_euler.theta, \
+ &ahrs_impl.ltp_to_imu_euler.psi, \
+ &(stateGetNedToBodyEulers_i()->phi), \
+ &(stateGetNedToBodyEulers_i()->theta), \
+ &(stateGetNedToBodyEulers_i()->psi)); \
}
#ifdef USE_I2C0
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 9bc9b54c8a..f9d2dbc7eb 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -628,6 +628,18 @@
(_ri).r = RATE_BFP_OF_REAL((_rf).r); \
}
+#define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
+ (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
+ (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
+ (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \
+ }
+
+#define SPEEDS_BFP_OF_REAL(_ef, _ei) { \
+ (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \
+ (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \
+ (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \
+ }
+
#define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
(_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index e02194a019..e48aca10dd 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -123,6 +123,10 @@ struct FloatRates {
/* _vo = _vi * _s */
#define FLOAT_VECT2_SMUL(_vo, _vi, _s) VECT2_SMUL(_vo, _vi, _s)
+#define FLOAT_VECT2_NORM(n, v) { \
+ n = sqrtf((v).x*(v).x + (v).y*(v).y); \
+ }
+
/*
* Dimension 3 Vectors
diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h
index 08f0a84ed7..717cf1a86c 100644
--- a/sw/airborne/math/pprz_geodetic.h
+++ b/sw/airborne/math/pprz_geodetic.h
@@ -20,4 +20,24 @@
(_pos1).alt = (_pos2).alt; \
}
+#define LTP_DEF_COPY(_def1,_def2){ \
+ LLA_COPY((_def1).lla, (_def2).lla); \
+ VECT3_COPY((_def1).ecef, (_def2).ecef); \
+ RMAT_COPY((_def1).ltp_of_ecef, (_def2).ltp_of_ecef); \
+ (_def1).hmsl = (_def2).hmsl; \
+ }
+
+#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2) { \
+ (_pos).x = (_utm1).east - (_utm2).east; \
+ (_pos).y = (_utm1).north - (_utm2).north; \
+ (_pos).z = (_utm1).alt - (_utm2).alt; \
+}
+
+#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2) { \
+ (_pos).x = (_utm1).north - (_utm2).north; \
+ (_pos).y = (_utm1).east - (_utm2).east; \
+ (_pos).z = -(_utm1).alt + (_utm2).alt; \
+}
+
+
#endif /* PPRZ_GEODETIC_H */
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 7813bff7a7..149acb1ad7 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -123,13 +123,15 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st
#define EM7RAD_OF_RAD(_r) ((_r)*1e7)
#define RAD_OF_EM7RAD(_r) ((_r)/1e7)
-#define INT32_VECT3_ENU_OF_NED(_o, _i) { \
- (_o).x = (_i).y; \
- (_o).y = (_i).x; \
- (_o).z = -(_i).z; \
+#define VECT3_ENU_OF_NED(_o, _i) { \
+ (_o).x = (_i).y; \
+ (_o).y = (_i).x; \
+ (_o).z = -(_i).z; \
}
-#define INT32_VECT3_NED_OF_ENU(_o, _i) INT32_VECT3_ENU_OF_NED(_o,_i)
+#define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
+#define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i)
+#define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i)
#define ECEF_BFP_OF_REAL(_o, _i) { \
(_o).x = (int32_t)CM_OF_M((_i).x); \
diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
index 608b9b242c..c40a5cb84f 100644
--- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
+++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
@@ -23,7 +23,7 @@
#include "subsystems/navigation/common_nav.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/navigation/traffic_info.h"
#include "airborne_ant_track.h"
@@ -85,9 +85,9 @@ float airborne_ant_pan_servo = 0;
static MATRIX smRotation;
- svPlanePosition.fx = estimator_y;
- svPlanePosition.fy = estimator_x;
- svPlanePosition.fz = estimator_z;
+ svPlanePosition.fx = stateGetPositionEnu_f()->y;
+ svPlanePosition.fy = stateGetPositionEnu_f()->x;
+ svPlanePosition.fz = stateGetPositionEnu_f()->z;
Home_Position.fx = WaypointY(WP_HOME);
Home_Position.fy = WaypointX(WP_HOME);
@@ -97,8 +97,8 @@ float airborne_ant_pan_servo = 0;
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
- smRotation.fx1 = (float)(cos(estimator_hspeed_dir));
- smRotation.fx2 = (float)(sin(estimator_hspeed_dir));
+ smRotation.fx1 = (float)(cos((*stateGetHorizontalSpeedDir_f())));
+ smRotation.fx2 = (float)(sin((*stateGetHorizontalSpeedDir_f())));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c
index be1c0700cc..7c352bc5d1 100644
--- a/sw/airborne/modules/benchmark/flight_benchmark.c
+++ b/sw/airborne/modules/benchmark/flight_benchmark.c
@@ -7,7 +7,7 @@
#include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "estimator.h"
+#include "state.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "mcu_periph/uart.h"
@@ -60,7 +60,7 @@ void flight_benchmark_periodic( void ) {
if (benchm_go){
#if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
- Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint);
+ Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint);
if (Err_airspeed>ToleranceAispeed){
Err_airspeed = Err_airspeed-ToleranceAispeed;
SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
@@ -68,7 +68,7 @@ void flight_benchmark_periodic( void ) {
#endif
#ifdef BENCHMARK_ALTITUDE
- Err_altitude = fabs(estimator_z - v_ctl_altitude_setpoint);
+ Err_altitude = fabs(stateGetPositionEnu_f()->z - v_ctl_altitude_setpoint);
if (Err_altitude>ToleranceAltitude){
Err_altitude = Err_altitude-ToleranceAltitude;
SquareSumErr_altitude += (Err_altitude * Err_altitude);
@@ -79,7 +79,7 @@ void flight_benchmark_periodic( void ) {
//---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) -----------------
- // err_temp = waypoints[target].x - estimator_x;
+ // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x;
float deltaPlaneX = 0;
float deltaPlaneY = 0;
float Err_position_segment = 0;
@@ -89,16 +89,16 @@ void flight_benchmark_periodic( void ) {
float deltaX = nav_segment_x_2 - nav_segment_x_1;
float deltaY = nav_segment_y_2 - nav_segment_y_1;
float anglePath = atan2(deltaX,deltaY);
- deltaPlaneX = nav_segment_x_2 - estimator_x;
- deltaPlaneY = nav_segment_y_2 - estimator_y;
+ deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x;
+ deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y;
float anglePlane = atan2(deltaPlaneX,deltaPlaneY);
float angleDiff = fabs(anglePlane - anglePath);
Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY));
// }
// if (nav_in_circle){
- deltaPlaneX = nav_circle_x - estimator_x;
- deltaPlaneY = nav_circle_y - estimator_y;
+ deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x;
+ deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y;
Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius);
// }
if (Err_position_circle < Err_position_segment){
diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c
index 14904ca88d..ddd5dd3451 100644
--- a/sw/airborne/modules/cam_control/booz_cam.c
+++ b/sw/airborne/modules/cam_control/booz_cam.c
@@ -24,9 +24,8 @@
#include "cam_control/booz_cam.h"
#include "modules/core/booz_pwm_arch.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "firmwares/rotorcraft/navigation.h"
-#include "subsystems/ins.h"
#include "generated/flight_plan.h"
#include "std.h"
@@ -103,7 +102,7 @@ void booz_cam_periodic(void) {
booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
#endif
#ifdef BOOZ_CAM_USE_PAN
- booz_cam_pan = ahrs.ltp_to_body_euler.psi;
+ booz_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
break;
case BOOZ_CAM_MODE_MANUAL:
@@ -126,14 +125,14 @@ void booz_cam_periodic(void) {
#ifdef WP_CAM
{
struct Int32Vect2 diff;
- VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos);
+ VECT2_DIFF(diff, waypoints[WP_CAM], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
nav_heading = booz_cam_pan;
#ifdef BOOZ_CAM_USE_TILT_ANGLES
int32_t dist, height;
INT32_VECT2_NORM(dist, diff);
- height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC;
+ height = (waypoints[WP_CAM].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
INT32_ATAN2(booz_cam_tilt, height, dist);
Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
index 6e5c1877c8..6a738754ff 100644
--- a/sw/airborne/modules/cam_control/cam.c
+++ b/sw/airborne/modules/cam_control/cam.c
@@ -31,7 +31,7 @@
#include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt
#include "autopilot.h"
#include "generated/flight_plan.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/navigation/traffic_info.h"
#ifdef POINT_CAM
#include "point.h"
@@ -234,8 +234,10 @@ void cam_target( void ) {
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#else
- vPoint(estimator_x, estimator_y, estimator_z,
- estimator_phi, estimator_theta, estimator_hspeed_dir,
+ struct EnuCoor_f* pos = stateGetPositionEnu_f();
+ struct FloatEulers* att = stateGetNedToBodyEulers_f();
+ vPoint(pos->x, pos->y, pos->z,
+ att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#endif
@@ -244,12 +246,13 @@ void cam_target( void ) {
/** Point straight down */
void cam_nadir( void ) {
+ struct EnuCoor_f* pos = stateGetPositionEnu_f();
#ifdef TEST_CAM
cam_target_x = test_cam_estimator_x;
cam_target_y = test_cam_estimator_y;
#else
- cam_target_x = estimator_x;
- cam_target_y = estimator_y;
+ cam_target_x = pos->x;
+ cam_target_y = pos->y;
#endif
cam_target_alt = -10;
cam_target();
diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c
index 39cd30bbac..dd3a86acf1 100644
--- a/sw/airborne/modules/cam_control/cam_roll.c
+++ b/sw/airborne/modules/cam_control/cam_roll.c
@@ -30,7 +30,7 @@
#include "subsystems/nav.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
-#include "estimator.h"
+#include "state.h"
#include "inter_mcu.h"
#ifndef CAM_PHI_MAX
@@ -62,7 +62,7 @@ void cam_init( void ) {
void cam_periodic( void ) {
switch (cam_roll_mode) {
case MODE_STABILIZED:
- phi_c = cam_roll_phi + estimator_phi;
+ phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi;
break;
case MODE_MANUAL:
phi_c = cam_roll_phi;
diff --git a/sw/airborne/modules/cam_control/cam_segment.c b/sw/airborne/modules/cam_control/cam_segment.c
index 1b6d104044..67e39c69e9 100644
--- a/sw/airborne/modules/cam_control/cam_segment.c
+++ b/sw/airborne/modules/cam_control/cam_segment.c
@@ -30,7 +30,6 @@
#include "modules/cam_control/cam_segment.h"
#include "modules/cam_control/cam.h"
#include "subsystems/nav.h"
-#include "estimator.h"
void cam_segment_init( void ) {
}
diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c
index 78167c838a..055123659b 100644
--- a/sw/airborne/modules/cam_control/cam_track.c
+++ b/sw/airborne/modules/cam_control/cam_track.c
@@ -25,7 +25,7 @@
#include "cam_track.h"
#include "subsystems/ins.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#if USE_HFF
#include "subsystems/ins/hf_float.h"
@@ -72,7 +72,8 @@ void track_periodic_task(void) {
cmd_msg[c++] = 'A';
cmd_msg[c++] = ' ';
- float phi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.phi);
+ struct FloatEulers* att = stateGetNedToBodyEulers_f();
+ float phi = att->phi;
if (phi > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; phi = -phi; }
cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
@@ -81,7 +82,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
cmd_msg[c++] = ' ';
- float theta = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.theta);
+ float theta = att->theta;
if (theta > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; theta = -theta; }
cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
@@ -90,7 +91,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
cmd_msg[c++] = ' ';
- float psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi);
+ float psi = att->psi;
if (psi > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; psi = -psi; }
cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
@@ -99,7 +100,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
cmd_msg[c++] = ' ';
- float alt = -POS_FLOAT_OF_BFP(ins_ltp_pos.z);
+ float alt = stateGetPositionEnu_f()->z;
//alt = 0.40;
if (alt > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; alt = -alt; }
@@ -133,7 +134,7 @@ void track_event(void) {
pos.y = -target_pos_ned.y;
ins_realign_h(pos, zero);
}
- const stuct FlotVect2 measuremet_noise = { 10.0, 10.0);
+ const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 };
b2_hff_update_pos(-target_pos_ned, measurement_noise);
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
@@ -141,6 +142,8 @@ void track_event(void) {
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+
+ INS_NED_TO_STATE();
#else
// store pos in ins
ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
@@ -149,6 +152,8 @@ void track_event(void) {
// TODO get delta T
// store last pos
VECT3_COPY(last_pos_ned, target_pos_ned);
+
+ stateSetPositionNed_i(&ins_ltp_pos);
#endif
b2_hff_lost_counter = 0;
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
index 1fa3aa897f..6c56ba2e4c 100644
--- a/sw/airborne/modules/cartography/cartography.c
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -29,7 +29,7 @@
-#include "estimator.h"
+#include "state.h"
#include "stdio.h"
#include "subsystems/nav.h"
@@ -210,10 +210,10 @@ bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, u
}
////////////////////////////////////////////////////////////////////////////////////////////////
-bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf);
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf);
-bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf)
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf)
//return if the projection of the estimator on the AB line is located inside the AB interval
{
float a,b,c,xa,xb,xc,ya,yb,yc;
@@ -234,8 +234,8 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi
xc=pointBf.x;
yc=pointBf.y;
- xa=estimator_xf;
- ya=estimator_yf;
+ xa=pos_xf;
+ ya=pos_yf;
//calcul des parametres de la droite pointAf pointBf
a = yc - yb;
@@ -427,8 +427,8 @@ bool_t nav_survey_losange_carto(void)
PRTDEB(d,railnumberSinceBoot)
- //PRTDEB(f,estimator_x)
- //PRTDEB(f,estimator_y)
+ //PRTDEB(f,stateGetPositionEnu_f()->x)
+ //PRTDEB(f,stateGetPositionEnu_f()->y)
//sortir du bloc si données abhérantes
if (norm13<1e-15)
@@ -465,15 +465,15 @@ bool_t nav_survey_losange_carto(void)
//the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
- //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
- //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
+ //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
+ //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
- nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+ nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
- if ((DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+ if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
{
- nav_route_xy(estimator_x, estimator_y,pointA.x,pointA.y);
+ nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y);
//nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
}
else
@@ -519,15 +519,15 @@ bool_t nav_survey_losange_carto(void)
// PRTDEB(f,pointA.y)
// PRTDEB(f,pointB.x)
// PRTDEB(f,pointB.y)
- ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+ ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
- // if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) >DISTLIMIT) &&
+ // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) &&
// (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
- if (! ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
+ if (! ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
// (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
{
nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
@@ -536,7 +536,7 @@ bool_t nav_survey_losange_carto(void)
//est ce que l'avion est dans la zone ou il doit prendre des images?
//DEJA APPELE AVANT LE IF
- // nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+ // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
if ( (normAM> distplus) && (normBM> distplus) && (distancefromrail M_PI) angle_between -= 2 * M_PI;
while (angle_between < -M_PI) angle_between += 2 * M_PI;
@@ -618,7 +618,7 @@ bool_t nav_survey_losange_carto(void)
//if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
NavCircleWaypoint(0,signforturn*tempcircleradius);
- if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) )
{
//l'avion fait le rail suivant
survey_losange_uturn=FALSE;
@@ -652,9 +652,9 @@ bool_t nav_survey_losange_carto(void)
course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
PRTDEB(f,course_next_rail )
- PRTDEB(f,estimator_hspeed_dir)
+ PRTDEB(f,(*stateGetHorizontalSpeedDir_f()))
- angle_between=(course_next_rail-estimator_hspeed_dir);
+ angle_between=(course_next_rail-(*stateGetHorizontalSpeedDir_f()));
while (angle_between > M_PI) angle_between -= 2 * M_PI;
while (angle_between < -M_PI) angle_between += 2 * M_PI;
@@ -663,7 +663,7 @@ bool_t nav_survey_losange_carto(void)
//if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius);
- if (( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) )
{
//l'avion fait le rail suivant
survey_losange_uturn=FALSE;
@@ -684,7 +684,7 @@ bool_t nav_survey_losange_carto(void)
pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
- if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) commands[COMMAND_THROTTLE]*100/MAX_PPRZ);
com_trans.buf[20] = pprz_mode;
com_trans.buf[21] = nav_block;
- FillBufWith16bit(com_trans.buf, 22, estimator_flight_time);
+ FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
I2CTransmit(GENERIC_COM_I2C_DEV, com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);
}
diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
index a6bc9ae2f7..0350714b7e 100644
--- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
+++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
@@ -39,7 +39,6 @@
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "estimator.h"
// In I2C mode we can not inline this function:
void dc_send_command(uint8_t cmd)
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index fb3e0ccf45..ec133836f5 100644
--- a/sw/airborne/modules/digital_cam/dc.c
+++ b/sw/airborne/modules/digital_cam/dc.c
@@ -55,13 +55,13 @@ uint16_t dc_buffer = 0;
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/gps.h"
void dc_send_shot_position(void)
{
- int16_t phi = DegOfRad(estimator_phi*10.0f);
- int16_t theta = DegOfRad(estimator_theta*10.0f);
+ int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
+ int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
float gps_z = ((float)gps.hmsl) / 1000.0f;
int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
int16_t photo_nr = -1;
@@ -88,11 +88,11 @@ void dc_send_shot_position(void)
uint8_t dc_info(void) {
#ifdef DOWNLINK_SEND_DC_INFO
- float course = DegOfRad(estimator_psi);
+ float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
&dc_autoshoot,
- &estimator_x,
- &estimator_y,
+ &stateGetPositionEnu_f()->x,
+ &stateGetPositionEnu_f()->y,
&course,
&dc_buffer,
&dc_gps_dist,
@@ -115,7 +115,7 @@ uint8_t dc_circle(float interval, float start) {
dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
if(start == DC_IGNORE) {
- start = DegOfRad(estimator_psi);
+ start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
}
dc_circle_start_angle = fmodf(start, 360.);
@@ -136,8 +136,8 @@ uint8_t dc_survey(float interval, float x, float y) {
dc_gps_dist = interval;
if (x == DC_IGNORE && y == DC_IGNORE) {
- dc_gps_x = estimator_x;
- dc_gps_y = estimator_y;
+ dc_gps_x = stateGetPositionEnu_f()->x;
+ dc_gps_y = stateGetPositionEnu_f()->y;
} else if (y == DC_IGNORE) {
dc_gps_x = waypoints[(uint8_t)x].x;
dc_gps_y = waypoints[(uint8_t)x].y;
diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h
index 48f4be2b55..fe47e1865c 100644
--- a/sw/airborne/modules/digital_cam/dc.h
+++ b/sw/airborne/modules/digital_cam/dc.h
@@ -40,7 +40,7 @@
#include "float.h"
#include "std.h"
#include "led.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "subsystems/gps.h"
@@ -252,7 +252,7 @@ static inline void dc_periodic_4Hz( void )
break;
case DC_AUTOSHOOT_CIRCLE: {
- float course = DegOfRad(estimator_psi) - dc_circle_start_angle;
+ float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
if (course < 0.)
course += 360.;
float current_block = floorf(course/dc_circle_interval);
@@ -272,8 +272,8 @@ static inline void dc_periodic_4Hz( void )
break;
case DC_AUTOSHOOT_SURVEY : {
- float dist_x = dc_gps_x - estimator_x;
- float dist_y = dc_gps_y - estimator_y;
+ float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
+ float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
if (dc_probing) {
if (dist_x*dist_x + dist_y*dist_y < dc_gps_dist*dc_gps_dist) {
diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
index d577dfb4b1..1902a84af8 100644
--- a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
+++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
@@ -36,7 +36,7 @@
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "estimator.h"
+#include "state.h"
void atmega_i2c_cam_ctrl_init(void)
diff --git a/sw/airborne/modules/enose/anemotaxis.c b/sw/airborne/modules/enose/anemotaxis.c
index 055816bb84..d9ec30630c 100644
--- a/sw/airborne/modules/enose/anemotaxis.c
+++ b/sw/airborne/modules/enose/anemotaxis.c
@@ -1,6 +1,6 @@
#include "modules/enose/anemotaxis.h"
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "std.h"
#include "subsystems/nav.h"
#include "generated/flight_plan.h"
@@ -13,12 +13,13 @@ static int8_t sign;
static struct point last_plume;
static void last_plume_was_here( void ) {
- last_plume.x = estimator_x;
- last_plume.y = estimator_y;
+ last_plume.x = stateGetPositionEnu_f()->x;
+ last_plume.y = stateGetPositionEnu_f()->y;
}
bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) {
- float wind_dir = atan2(wind_north, wind_east);
+ struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+ float wind_dir = atan2(wind->x, wind->y);
waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir);
waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir);
return FALSE;
@@ -27,9 +28,10 @@ bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) {
bool_t nav_anemotaxis_init( uint8_t c ) {
status = UTURN;
sign = 1;
- float wind_dir = atan2(wind_north, wind_east);
- waypoints[c].x = estimator_x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
- waypoints[c].y = estimator_y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
+ struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+ float wind_dir = atan2(wind->x, wind->y);
+ waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
+ waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
last_plume_was_here();
return FALSE;
}
@@ -37,12 +39,13 @@ bool_t nav_anemotaxis_init( uint8_t c ) {
bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
if (chemo_sensor) {
last_plume_was_here();
- waypoints[plume].x = estimator_x;
- waypoints[plume].y = estimator_y;
+ waypoints[plume].x = stateGetPositionEnu_f()->x;
+ waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
}
- float wind_dir = atan2(wind_north, wind_east) + M_PI;
+ struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+ float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
@@ -57,7 +60,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
- float width = Max(2*ScalarProduct(upwind_x, upwind_y, estimator_x-last_plume.x, estimator_y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
+ float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
@@ -84,8 +87,8 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
}
if (chemo_sensor) {
- waypoints[c].x = estimator_x + DEFAULT_CIRCLE_RADIUS*upwind_x;
- waypoints[c].y = estimator_y + DEFAULT_CIRCLE_RADIUS*upwind_y;
+ waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x;
+ waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
diff --git a/sw/airborne/modules/enose/chemotaxis.c b/sw/airborne/modules/enose/chemotaxis.c
index bbb6504da2..6433f930d6 100644
--- a/sw/airborne/modules/enose/chemotaxis.c
+++ b/sw/airborne/modules/enose/chemotaxis.c
@@ -1,6 +1,6 @@
#include "modules/enose/chemotaxis.h"
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "std.h"
#include "subsystems/nav.h"
#include "generated/flight_plan.h"
@@ -28,14 +28,14 @@ bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
if (chemo_sensor > last_plume_value) {
/* Move the circle in this direction */
- float x = estimator_x - waypoints[plume].x;
- float y = estimator_y - waypoints[plume].y;
+ float x = stateGetPositionEnu_f()->x - waypoints[plume].x;
+ float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
waypoints[c].x = waypoints[plume].x + ALPHA * x;
waypoints[c].y = waypoints[plume].y + ALPHA * y;
// DownlinkSendWp(c);
/* Turn in the right direction */
- float dir_x = cos(M_PI_2 - estimator_hspeed_dir);
- float dir_y = sin(M_PI_2 - estimator_hspeed_dir);
+ float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
+ float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float pvect = dir_x*y - dir_y*x;
sign = (pvect > 0 ? -1 : 1);
/* Reduce the radius */
@@ -43,8 +43,8 @@ bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
/* Store this plume */
- waypoints[plume].x = estimator_x;
- waypoints[plume].y = estimator_y;
+ waypoints[plume].x = stateGetPositionEnu_f()->x;
+ waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
last_plume_value = chemo_sensor;
}
diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c
index 5f2c10212f..4389f87100 100644
--- a/sw/airborne/modules/gsm/gsm.c
+++ b/sw/airborne/modules/gsm/gsm.c
@@ -41,7 +41,7 @@ Reporting:
In: OK
Out: AT+CMGS=\"GCS_NUMBER\"
In: >
- Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi CTRLZ
+ Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi CTRLZ
Receiving:
In: +CMTI: ...,
@@ -71,7 +71,6 @@ Receiving:
#include "ap_subsystems/datalink/downlink.h"
#include "subsystems/gps.h"
#include "autopilot.h"
-#include "estimator.h"
//#include "subsystems/navigation/common_nav.h" //why is should this be needed?
#include "generated/settings.h"
@@ -409,9 +408,9 @@ void gsm_send_report_continue(void)
uint8_t rssi = atoi(gsm_buf + strlen("+CSQ: "));
// Donnee GPS :ne sont pas envoyes gps_mode, gps.tow, gps.utm_pos.zone, gps_nb_ovrn
- // Donnees batterie (seuls vsupply et estimator_flight_time sont envoyes)
+ // Donnees batterie (seuls vsupply et autopilot_flight_time sont envoyes)
// concatenation de toutes les infos en un seul message à transmettre
- sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi);
+ sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi);
// send the number and wait for the prompt
char buf[32];
diff --git a/sw/airborne/modules/ins/fw_ins_vn100.c b/sw/airborne/modules/ins/fw_ins_vn100.c
index c5f01c08d3..fe595cbfe2 100644
--- a/sw/airborne/modules/ins/fw_ins_vn100.c
+++ b/sw/airborne/modules/ins/fw_ins_vn100.c
@@ -28,7 +28,7 @@
#include "modules/ins/ins_vn100.h"
#include "mcu_periph/spi.h"
-#include "estimator.h"
+#include "state.h"
#include "generated/airframe.h"
#ifndef INS_YAW_NEUTRAL_DEFAULT
@@ -113,7 +113,13 @@ void ins_event_task( void ) {
#ifndef INS_VN100_READ_ONLY
// Update estimator
// FIXME Use a proper rotation matrix here
- EstimatorSetAtt((ins_eulers.phi - ins_roll_neutral), ins_eulers.psi, (ins_eulers.theta - ins_pitch_neutral));
+ struct FloatEulers att = {
+ ins_eulers.phi - ins_roll_neutral,
+ ins_eulers.theta - ins_pitch_neutral,
+ ins_eulers.psi
+ };
+ stateSetNedToBodyEulers_f(&att);
+ stateSetBodyRates(&ins_rates);
#endif
//uint8_t s = 4+VN100_REG_QMR_SIZE;
//DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c
index 5f574befd2..5790c891ee 100644
--- a/sw/airborne/modules/ins/ins_arduimu.c
+++ b/sw/airborne/modules/ins/ins_arduimu.c
@@ -11,7 +11,7 @@ Autoren@ZHAW: schmiemi
#include "mcu_periph/i2c.h"
// test
-#include "estimator.h"
+#include "state.h"
// für das Senden von GPS-Daten an den ArduIMU
#ifndef UBX
@@ -209,12 +209,12 @@ void IMU_Daten_verarbeiten( void ) {
ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
// test
- estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral;
- estimator_theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral;
+ struct FloatEulers att;
+ att.phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral;
+ att.theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral;
+ att.psi = 0.;
imu_daten_angefordert = FALSE;
+ stateSetNedToBodyEulers_f(&att);
- {
- float psi=0;
- RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &estimator_phi, &estimator_theta, &psi));
- }
+ RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi));
}
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index 5285492d6a..441452936d 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -28,7 +28,7 @@
#include "mcu_periph/i2c.h"
// Estimator interface
-#include "estimator.h"
+#include "state.h"
// GPS data for ArduIMU
#include "subsystems/gps.h"
@@ -112,14 +112,15 @@ void ArduIMU_periodicGPS( void ) {
// Test for high acceleration:
// - low speed
// - high thrust
- if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
+ float speed = *stateGetHorizontalSpeedNorm_f();
+ if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
- if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
+ if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
- if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
+ if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
@@ -174,8 +175,8 @@ void ArduIMU_event( void ) {
recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
- arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
- arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
+ arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral;
+ arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral;
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
@@ -185,11 +186,9 @@ void ArduIMU_event( void ) {
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
- estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
- estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
- estimator_p = arduimu_rates.p;
- estimator_q = arduimu_rates.q;
- estimator_r = arduimu_rates.r;
+ stateSetNedToBodyEulers_f(&arduimu_eulers);
+ stateSetBodyRates_f(&arduimu_rates);
+ stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel));
ardu_ins_trans.status = I2CTransDone;
#ifdef ARDUIMU_SYNC_SEND
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index 07ba9c35e5..8674e1e039 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -10,7 +10,7 @@
#include "mcu_periph/spi_slave_hs_arch.h"
// Output
-#include "estimator.h"
+#include "state.h"
// For centripedal corrections
#include "subsystems/gps.h"
@@ -81,8 +81,18 @@ void parse_ins_msg( void )
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
- EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
- EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r
+ struct FloatEulers att = {
+ CHIMU_DATA.m_attitude.euler.phi,
+ CHIMU_DATA.m_attitude.euler.theta,
+ CHIMU_DATA.m_attitude.euler.psi
+ };
+ stateSetNedToBodyEulers_f(&att);
+ struct FloatRates rates = {
+ CHIMU_DATA.m_sensor.rate[0],
+ CHIMU_DATA.m_attrates.euler.theta,
+ 0.
+ }; // FIXME rate r
+ stateSetBodyRates_f(&rates);
}
else if(CHIMU_DATA.m_MsgID==0x02) {
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index baa3af3779..7a05dde04f 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -10,7 +10,7 @@
// Output
-#include "estimator.h"
+#include "state.h"
// For centripedal corrections
#include "subsystems/gps.h"
@@ -82,8 +82,12 @@ void parse_ins_msg( void )
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
- EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
- //EstimatorSetRate(ins_p,ins_q,ins_r);
+ struct FloatEulers att = {
+ CHIMU_DATA.m_attitude.euler.phi,
+ CHIMU_DATA.m_attitude.euler.theta,
+ CHIMU_DATA.m_attitude.euler.psi
+ };
+ stateSetNedToBodyEulers_f(&att);
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index bfe3790d9d..343ce1cef6 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -306,17 +306,35 @@ void ins_periodic_task( void ) {
}
#if USE_INS
-#include "estimator.h"
+#include "state.h"
static inline void update_fw_estimator(void) {
// Send to Estimator (Control)
#ifdef XSENS_BACKWARDS
- EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
- EstimatorSetRate(-ins_p,-ins_q, ins_r);
+ struct FloatEulers att = {
+ -ins_phi+ins_roll_neutral,
+ -ins_theta+ins_pitch_neutral,
+ ins_psi + RadOfDeg(180)
+ };
+ struct FloatRates rates = {
+ -ins_p,
+ -ins_q,
+ ins_r
+ };
#else
- EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
- EstimatorSetRate(ins_p, ins_q, ins_r);
+ struct FloatEulers att = {
+ ins_phi+ins_roll_neutral,
+ ins_theta+ins_pitch_neutral,
+ ins_psi
+ };
+ struct FloatRates rates = {
+ ins_p,
+ ins_q,
+ ins_r
+ };
#endif
+ stateSetNedToBodyEulers_f(&att);
+ stateSetBodyRates_f(&rates);
}
#endif /* USE_INS */
diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c
index 0377137ffd..300ea47b8e 100644
--- a/sw/airborne/modules/multi/formation.c
+++ b/sw/airborne/modules/multi/formation.c
@@ -12,7 +12,7 @@
#include "subsystems/datalink/downlink.h"
#include "multi/formation.h"
-#include "estimator.h"
+#include "state.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "autopilot.h"
@@ -128,21 +128,21 @@ int formation_flight(void) {
static uint8_t _1Hz = 0;
uint8_t nb = 0, i;
- float ch = cosf(estimator_hspeed_dir);
- float sh = sinf(estimator_hspeed_dir);
+ float ch = cosf((*stateGetHorizontalSpeedDir_f()));
+ float sh = sinf((*stateGetHorizontalSpeedDir_f()));
form_n = 0.;
form_e = 0.;
form_a = 0.;
- form_speed = estimator_hspeed_mod;
- form_speed_n = estimator_hspeed_mod * ch;
- form_speed_e = estimator_hspeed_mod * sh;
+ form_speed = (*stateGetHorizontalSpeedNorm_f());
+ form_speed_n = (*stateGetHorizontalSpeedNorm_f()) * ch;
+ form_speed_e = (*stateGetHorizontalSpeedNorm_f()) * sh;
if (AC_ID == leader_id) {
- estimator_x += formation[the_acs_id[AC_ID]].east;
- estimator_y += formation[the_acs_id[AC_ID]].north;
+ stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
+ stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
}
// set info for this AC
- SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps.tow);
+ SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), stateGetPositionEnu_f()->z, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow);
// broadcast info
uint8_t ac_id = AC_ID;
@@ -188,12 +188,12 @@ int formation_flight(void) {
}
else formation[i].status = ACTIVE;
// compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
- if (formation[i].status == ACTIVE && fabs(estimator_z - ac->alt) < form_prox && ac->alt > 0) {
- form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - estimator_x)
+ if (formation[i].status == ACTIVE && fabs(stateGetPositionEnu_f()->z - ac->alt) < form_prox && ac->alt > 0) {
+ form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - stateGetPositionEnu_f()->x)
- (form[i].east - form[the_acs_id[AC_ID]].east);
- form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - estimator_y)
+ form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - stateGetPositionEnu_f()->y)
- (form[i].north - form[the_acs_id[AC_ID]].north);
- form_a += (ac->alt - estimator_z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
+ form_a += (ac->alt - stateGetPositionEnu_f()->z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
form_speed += ac->gspeed;
//form_speed_e += ac->gspeed * sinf(ac->course);
//form_speed_n += ac->gspeed * cosf(ac->course);
@@ -204,9 +204,9 @@ int formation_flight(void) {
form_n /= _nb;
form_e /= _nb;
form_a /= _nb;
- form_speed = form_speed / (nb+1) - estimator_hspeed_mod;
- //form_speed_e = form_speed_e / (nb+1) - estimator_hspeed_mod * sh;
- //form_speed_n = form_speed_n / (nb+1) - estimator_hspeed_mod * ch;
+ form_speed = form_speed / (nb+1) - (*stateGetHorizontalSpeedNorm_f());
+ //form_speed_e = form_speed_e / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * sh;
+ //form_speed_n = form_speed_n / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * ch;
// set commands
NavVerticalAutoThrottleMode(0.);
@@ -230,7 +230,7 @@ int formation_flight(void) {
desired_y = leader->north + dy;
// lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
- //float diff_course = leader->course - estimator_hspeed_dir;
+ //float diff_course = leader->course - (*stateGetHorizontalSpeedDir_f());
//NormRadAngle(diff_course);
//h_ctl_roll_setpoint += coef_form_course * diff_course;
//h_ctl_roll_setpoint += coef_form_course * diff_heading;
@@ -249,8 +249,8 @@ int formation_flight(void) {
void formation_pre_call(void) {
if (leader_id == AC_ID) {
- estimator_x -= formation[the_acs_id[AC_ID]].east;
- estimator_y -= formation[the_acs_id[AC_ID]].north;
+ stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east;
+ stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north;
}
}
diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c
index fe82759935..fa0e09aa43 100644
--- a/sw/airborne/modules/multi/potential.c
+++ b/sw/airborne/modules/multi/potential.c
@@ -12,7 +12,7 @@
#include "dl_protocol.h"
#include "potential.h"
-#include "estimator.h"
+#include "state.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "autopilot.h"
@@ -59,8 +59,8 @@ int potential_task(void) {
uint8_t i;
- float ch = cosf(estimator_hspeed_dir);
- float sh = sinf(estimator_hspeed_dir);
+ float ch = cosf((*stateGetHorizontalSpeedDir_f()));
+ float sh = sinf((*stateGetHorizontalSpeedDir_f()));
potential_force.east = 0.;
potential_force.north = 0.;
potential_force.alt = 0.;
@@ -76,17 +76,17 @@ int potential_task(void) {
else {
float sha = sinf(ac->course);
float cha = cosf(ac->course);
- float de = ac->east + sha*delta_t - estimator_x;
+ float de = ac->east + sha*delta_t - stateGetPositionEnu_f()->x;
if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue;
- float dn = ac->north + cha*delta_t - estimator_y;
+ float dn = ac->north + cha*delta_t - stateGetPositionEnu_f()->y;
if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue;
- float da = ac->alt + ac->climb*delta_t - estimator_z;
+ float da = ac->alt + ac->climb*delta_t - stateGetPositionEnu_f()->z;
if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue;
float dist = sqrtf(de*de + dn*dn + da*da);
if (dist == 0.) continue;
- float dve = estimator_hspeed_mod * sh - ac->gspeed * sha;
- float dvn = estimator_hspeed_mod * ch - ac->gspeed * cha;
- float dva = estimator_z_dot - the_acs[i].climb;
+ float dve = (*stateGetHorizontalSpeedNorm_f()) * sh - ac->gspeed * sha;
+ float dvn = (*stateGetHorizontalSpeedNorm_f()) * ch - ac->gspeed * cha;
+ float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dve*de + dvn*dn + dva*da;
if (scal < 0.) continue; // No risk of collision
float d3 = dist * dist * dist;
diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c
index 518d1c4c02..48f834d458 100644
--- a/sw/airborne/modules/multi/tcas.c
+++ b/sw/airborne/modules/multi/tcas.c
@@ -29,7 +29,7 @@
#include "multi/tcas.h"
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "subsystems/gps.h"
#include "generated/flight_plan.h"
@@ -88,7 +88,7 @@ void tcas_init( void ) {
static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
struct ac_info_ * ac = get_ac_info(id);
- float dz = ac->alt - estimator_z;
+ float dz = ac->alt - stateGetPositionEnu_f()->z;
if (dz > tcas_alim/2) return RA_DESCEND;
else if (dz < -tcas_alim/2) return RA_CLIMB;
else // AC with the smallest ID descend
@@ -102,7 +102,7 @@ static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
/* conflicts detection and monitoring */
void tcas_periodic_task_1Hz( void ) {
// no TCAS under security_height
- if (estimator_z < GROUND_ALT + SECURITY_HEIGHT) {
+ if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) {
uint8_t i;
for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
return;
@@ -111,8 +111,8 @@ void tcas_periodic_task_1Hz( void ) {
float tau_min = tcas_tau_ta;
uint8_t ac_id_close = AC_ID;
uint8_t i;
- float vx = estimator_hspeed_mod * sinf(estimator_hspeed_dir);
- float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir);
+ float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
+ float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) continue; // no AC data
uint32_t dt = gps.tow - the_acs[i].itow;
@@ -121,12 +121,12 @@ void tcas_periodic_task_1Hz( void ) {
continue;
}
if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
- float dx = the_acs[i].east - estimator_x;
- float dy = the_acs[i].north - estimator_y;
- float dz = the_acs[i].alt - estimator_z;
+ float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
+ float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
+ float dz = the_acs[i].alt - stateGetPositionEnu_f()->z;
float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
- float dvz = estimator_z_dot - the_acs[i].climb;
+ float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dvx*dx + dvy*dy + dvz*dz;
float ddh = dx*dx + dy*dy;
float ddv = dz*dz;
@@ -219,7 +219,7 @@ void tcas_periodic_task_1Hz( void ) {
/* altitude control loop */
void tcas_periodic_task_4Hz( void ) {
// set alt setpoint
- if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
+ if (stateGetPositionEnu_f()->z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
switch (tcas_resolve) {
case RA_CLIMB :
diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c
index 21cf2f68a9..1819b81c84 100644
--- a/sw/airborne/modules/nav/nav_catapult.c
+++ b/sw/airborne/modules/nav/nav_catapult.c
@@ -37,7 +37,7 @@
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "ap_downlink.h"
#include "modules/nav/nav_catapult.h"
#include "subsystems/nav.h"
@@ -166,8 +166,8 @@ bool_t nav_catapult(uint8_t _to, uint8_t _climb)
WaypointY(_to) = GetPosY();
WaypointAlt(_to) = GetPosAlt();
- nav_catapult_x = estimator_x;
- nav_catapult_y = estimator_y;
+ nav_catapult_x = stateGetPositionEnu_f()->x;
+ nav_catapult_y = stateGetPositionEnu_f()->y;
}
// No Roll, Climb Pitch, Full Power
@@ -189,8 +189,8 @@ bool_t nav_catapult(uint8_t _to, uint8_t _climb)
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
- float dir_x = estimator_x - nav_catapult_x;
- float dir_y = estimator_y - nav_catapult_y;
+ float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
+ float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/AOA_adc.c
index 38b38e583f..be84620009 100644
--- a/sw/airborne/modules/sensors/AOA_adc.c
+++ b/sw/airborne/modules/sensors/AOA_adc.c
@@ -28,7 +28,7 @@
#include "mcu_periph/adc.h"
#include BOARD_CONFIG
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "std.h"
//Messages
#include "mcu_periph/uart.h"
@@ -78,6 +78,6 @@ void AOA_adc_update( void ) {
RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA));
#ifdef USE_AOA
- EstimatorSetAOA(AOA);
+ stateSetAngleOfAttack_f(AOA);
#endif
}
diff --git a/sw/airborne/modules/sensors/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c
index 776a15ae91..f461d310bf 100644
--- a/sw/airborne/modules/sensors/airspeed_adc.c
+++ b/sw/airborne/modules/sensors/airspeed_adc.c
@@ -24,7 +24,7 @@
#include "mcu_periph/adc.h"
#include BOARD_CONFIG
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
uint16_t adc_airspeed_val;
@@ -59,10 +59,10 @@ void airspeed_adc_update( void ) {
#else
float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS);
#endif
- EstimatorSetAirspeed(airspeed);
+ stateSetAirspeed_f(&airspeed);
#else // SITL
extern float sim_air_speed;
- EstimatorSetAirspeed(sim_air_speed);
+ stateSetAirspeed_f(&sim_air_speed);
adc_airspeed_val = 0;
#endif //SITL
}
diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c
index adb8f80a91..2c37dec03f 100644
--- a/sw/airborne/modules/sensors/airspeed_amsys.c
+++ b/sw/airborne/modules/sensors/airspeed_amsys.c
@@ -23,7 +23,7 @@
*/
#include "sensors/airspeed_amsys.h"
-#include "estimator.h"
+#include "state.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "messages.h"
@@ -106,7 +106,7 @@ void airspeed_amsys_read_periodic( void ) {
#else // SITL
extern float sim_air_speed;
- EstimatorSetAirspeed(sim_air_speed);
+ stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
@@ -146,7 +146,7 @@ void airspeed_amsys_read_event( void ) {
airspeed_old = airspeed_amsys;
#if USE_AIRSPEED
- EstimatorSetAirspeed(airspeed_amsys);
+ stateSetAirspeed_f(&airspeed_amsys);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c
index 227526ad9a..5d06b47d33 100644
--- a/sw/airborne/modules/sensors/airspeed_ets.c
+++ b/sw/airborne/modules/sensors/airspeed_ets.c
@@ -35,7 +35,7 @@
*
*/
#include "sensors/airspeed_ets.h"
-#include "estimator.h"
+#include "state.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "messages.h"
@@ -109,7 +109,7 @@ void airspeed_ets_read_periodic( void ) {
I2CReceive(AIRSPEED_ETS_I2C_DEV, airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#else // SITL
extern float sim_air_speed;
- EstimatorSetAirspeed(sim_air_speed);
+ stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
@@ -167,7 +167,7 @@ void airspeed_ets_read_event( void ) {
airspeed_ets += airspeed_ets_buffer[n];
airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
#if USE_AIRSPEED
- EstimatorSetAirspeed(airspeed_ets);
+ stateSetAirspeed_f(&airspeed_ets);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c
index e89f8519ea..d9e2a24cbb 100644
--- a/sw/airborne/modules/sensors/baro_MS5534A.c
+++ b/sw/airborne/modules/sensors/baro_MS5534A.c
@@ -35,7 +35,7 @@
#include "ap_downlink.h"
#endif
#include "subsystems/nav.h"
-#include "estimator.h"
+#include "state.h"
bool_t baro_MS5534A_do_reset;
uint32_t baro_MS5534A_pressure;
@@ -258,11 +258,11 @@ void baro_MS5534A_event( void ) {
spi_message_received = FALSE;
baro_MS5534A_event_task();
if (baro_MS5534A_available) {
- baro_MS5534A_available = FALSE;
+ // baro_MS5534A_available = FALSE; // Checked by INS
baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084;
- if (alt_baro_enabled) {
- EstimatorSetAlt(baro_MS5534A_z);
- }
+ // if (alt_baro_enabled) {
+ // EstimatorSetAlt(baro_MS5534A_z); // Updated by INS
+ // }
}
}
}
diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h
index d8e39137ba..f9c9ebef4e 100644
--- a/sw/airborne/modules/sensors/baro_MS5534A.h
+++ b/sw/airborne/modules/sensors/baro_MS5534A.h
@@ -55,6 +55,8 @@ void baro_MS5534A_event_task( void );
void baro_MS5534A_event( void );
+#define BaroMS5534AUpdate(_b) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; baro_MS5534A_available = FALSE; } }
+
#endif // USE_BARO_MS5534A
#endif // BARO_MS5534A_H
diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c
index 8e244f37e6..23f07b9ced 100644
--- a/sw/airborne/modules/sensors/baro_amsys.c
+++ b/sw/airborne/modules/sensors/baro_amsys.c
@@ -25,7 +25,7 @@
#include "sensors/baro_amsys.h"
#include "mcu_periph/i2c.h"
-#include "estimator.h"
+#include "state.h"
#include
#include "generated/flight_plan.h" // for ground alt
@@ -72,6 +72,7 @@
// Global variables
uint16_t pBaroRaw;
uint16_t tBaroRaw;
+uint16_t baro_amsys_adc;
float baro_amsys_offset;
bool_t baro_amsys_valid;
float baro_amsys_altitude;
@@ -126,8 +127,8 @@ void baro_amsys_read_periodic( void ) {
#else // SITL
pBaroRaw = 0;
baro_amsys_altitude = gps.hmsl / 1000.0;
+ baro_amsys_adc = baro_amsys_altitude / BARO_AMSYS_SCALE;
baro_amsys_valid = TRUE;
- EstimatorSetAlt(baro_amsys_altitude);
#endif
}
@@ -145,6 +146,7 @@ void baro_amsys_read_event( void ) {
else
baro_amsys_valid = TRUE;
+ baro_amsys_adc = pBaroRaw;
// Continue only if a new altimeter value was received
//if (baro_amsys_valid && GpsFixValid()) {
@@ -180,10 +182,7 @@ void baro_amsys_read_event( void ) {
// Lowpassfiltering and convert pressure to altitude
baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81);
baro_old = baro_amsys_altitude;
-
-
//New value available
- //EstimatorSetAlt(baro_amsys_abs_altitude);
}
baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init;
} /*else {
diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h
index 9e0884944d..7844695db6 100644
--- a/sw/airborne/modules/sensors/baro_amsys.h
+++ b/sw/airborne/modules/sensors/baro_amsys.h
@@ -30,9 +30,9 @@
#define BARO_AMSYS_DT 0.05
-// extern uint16_t baro_amsys_adc;
+extern uint16_t baro_amsys_adc;
// extern float baro_amsys_offset;
-// extern bool_t baro_amsys_valid;
+extern bool_t baro_amsys_valid;
// extern bool_t baro_amsys_updated;
// extern bool_t baro_amsys_enabled;
extern float baro_amsys_altitude;
@@ -48,4 +48,6 @@ extern void baro_amsys_read_event( void );
#define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); }
+#define BaroAmsysUpdate(_b) { if (baro_amsys_valid) { _b = baro_amsys_adc; baro_amsys_valid = FALSE; } }
+
#endif // BARO_AMSYS_H
diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c
index 4bb12a8b34..8935bc23bd 100644
--- a/sw/airborne/modules/sensors/baro_bmp.c
+++ b/sw/airborne/modules/sensors/baro_bmp.c
@@ -37,7 +37,7 @@
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#ifdef SITL
@@ -73,6 +73,7 @@ float baro_bmp_sigma2;
// Global variables
uint8_t baro_bmp_status;
+bool_t baro_bmp_valid;
uint32_t baro_bmp_pressure;
uint16_t baro_bmp_temperature;
int32_t baro_bmp_altitude, baro_bmp,baro_bmp_temp,baro_bmp_offset;
@@ -91,6 +92,7 @@ uint16_t baro_bmp_cnt;
void baro_bmp_init( void ) {
baro_bmp_status = BARO_BMP_UNINIT;
+ baro_bmp_valid = FALSE;
baro_bmp_r = BARO_BMP_R;
baro_bmp_sigma2 = BARO_BMP_SIGMA2;
baro_bmp_enabled = TRUE;
@@ -124,7 +126,8 @@ void baro_bmp_periodic( void ) {
}
#else // SITL
baro_bmp_altitude = gps.hmsl / 1000.0;
- EstimatorSetAlt(baro_bmp_altitude);
+ baro_bmp_pressure = baro_bmp_altitude; //FIXME do a proper scaling here
+ baro_bmp_valid = TRUE;
#endif
}
@@ -233,10 +236,7 @@ void baro_bmp_event( void ) {
if (baro_bmp_offset_init) {
baro_bmp_altitude = ground_alt + baro_bmp_temp;
// New value available
-#if USE_BARO_BMP
-#pragma message "USING BARO BMP"
- EstimatorSetAlt(baro_bmp_altitude);
-#endif
+ baro_bmp_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h
index 0d82a37b3c..53adeb184d 100644
--- a/sw/airborne/modules/sensors/baro_bmp.h
+++ b/sw/airborne/modules/sensors/baro_bmp.h
@@ -40,6 +40,7 @@ extern float baro_bmp_r;
extern float baro_bmp_sigma2;
extern uint8_t baro_bmp_status;
+extern bool_t baro_bmp_valid;
extern uint32_t baro_bmp_pressure;
extern uint16_t baro_bmp_temperature;
extern int32_t baro_bmp_altitude;
@@ -50,4 +51,6 @@ void baro_bmp_init(void);
void baro_bmp_periodic(void);
void baro_bmp_event(void);
+#define BaroBmpUpdate(_b) { if (baro_bmp_valid) { _b = baro_bmp_pressure; baro_bmp_valid = FALSE; } }
+
#endif
diff --git a/sw/airborne/modules/sensors/baro_board_module.h b/sw/airborne/modules/sensors/baro_board_module.h
index 5ba3d796b6..f8d88e5595 100644
--- a/sw/airborne/modules/sensors/baro_board_module.h
+++ b/sw/airborne/modules/sensors/baro_board_module.h
@@ -29,7 +29,35 @@
#include "subsystems/sensors/baro.h"
-static inline void baro_abs(void) {}
-static inline void baro_diff(void) {}
+/** Absolute baro macro mapping.
+ * Select the baro module you want to use to feed the common baro interface
+ * in your airframe file when configuring baro_board module
+ * ex:
+ * for module baro_ets
+ *
+ */
+#ifndef BARO_ABS_EVENT
+#define BARO_ABS_EVENT NoBaro
+#endif
+
+/** Differential baro macro mapping.
+ * TODO
+ */
+#ifndef BARO_DIFF_EVENT
+#define BARO_DIFF_EVENT NoBaro
+#endif
+
+#define NoBaro(_b) {}
+
+/** BaroEvent macro.
+ * Need to be maped to one the external baro running has a module
+ */
+#define BaroEvent(_b_abs_handler, _b_diff_handler) { \
+ BARO_ABS_EVENT(baro.absolute); \
+ BARO_DIFF_EVENT(baro.differential); \
+ _b_abs_handler(); \
+ _b_diff_handler(); \
+}
+
#endif
diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c
index 4f9a355c0c..f1bf697227 100644
--- a/sw/airborne/modules/sensors/baro_ets.c
+++ b/sw/airborne/modules/sensors/baro_ets.c
@@ -37,7 +37,7 @@
#include "sensors/baro_ets.h"
#include "mcu_periph/i2c.h"
-#include "estimator.h"
+#include "state.h"
#include
#include "subsystems/nav.h"
@@ -97,10 +97,9 @@ void baro_ets_read_periodic( void ) {
if (baro_ets_i2c_trans.status == I2CTransDone)
I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
#else // SITL
- baro_ets_adc = 0;
baro_ets_altitude = gps.hmsl / 1000.0;
+ baro_ets_adc = baro_ets_altitude / BARO_ETS_SCALE;
baro_ets_valid = TRUE;
- EstimatorSetAlt(baro_ets_altitude);
#endif
}
@@ -149,7 +148,6 @@ void baro_ets_read_event( void ) {
if (baro_ets_offset_init) {
baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
// New value available
- EstimatorSetAlt(baro_ets_altitude);
#ifdef BARO_ETS_TELEMETRY
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h
index bfdc568b6d..86e45a83ef 100644
--- a/sw/airborne/modules/sensors/baro_ets.h
+++ b/sw/airborne/modules/sensors/baro_ets.h
@@ -60,4 +60,6 @@ extern void baro_ets_read_event( void );
#define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); }
+#define BaroEtsUpdate(_b) { if (baro_ets_valid) { _b = baro_ets_adc; baro_ets_valid = FALSE; } }
+
#endif // BARO_ETS_H
diff --git a/sw/airborne/modules/sensors/baro_scp.h b/sw/airborne/modules/sensors/baro_scp.h
index c69534ec0f..2619adb96d 100644
--- a/sw/airborne/modules/sensors/baro_scp.h
+++ b/sw/airborne/modules/sensors/baro_scp.h
@@ -20,4 +20,6 @@ void baro_scp_init(void);
void baro_scp_periodic(void);
void baro_scp_event(void);
+#define BaroScpUpdate(_b) { if (baro_scp_available) { _b = baro_scp_pressure; baro_scp_available = FALSE; } }
+
#endif
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
index 07a7b8e5d7..d503cba8bd 100644
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -17,7 +17,6 @@
* Boston, MA 02111-1307, USA.
*
*/
-#include "estimator.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "messages.h"
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c
index 09fd922c2a..a407e14889 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.c
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.c
@@ -26,7 +26,7 @@
#include "pressure_board_navarro.h"
-#include "estimator.h"
+#include "state.h"
/* Default I2C device on tiny is i2c0
*/
@@ -140,7 +140,7 @@ void pbn_read_event( void ) {
pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.);
#if USE_AIRSPEED
- EstimatorSetAirspeed(pbn_airspeed);
+ stateSetAirspeed_f(&pbn_airspeed);
#endif
//alt_kalman(pbn_altitude);
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h
index f5e78f2a2f..3ba8b2d208 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.h
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.h
@@ -54,6 +54,8 @@ extern void pbn_read_event( void );
#define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); }
+#define BaroPbnUpdate(_b) { if (data_valid) { _b = altitude_adc; data_valid = FALSE; } }
+
#define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset);
#endif // PRESSURE_BOARD_NAVARRO_H
diff --git a/sw/airborne/rc_settings.c b/sw/airborne/rc_settings.c
index 15573d398a..494bf6c606 100644
--- a/sw/airborne/rc_settings.c
+++ b/sw/airborne/rc_settings.c
@@ -30,7 +30,6 @@
#include "autopilot.h"
#include "subsystems/nav.h"
#include "subsystems/sensors/infrared.h"
-#include "estimator.h"
#include "inter_mcu.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
diff --git a/sw/airborne/state.c b/sw/airborne/state.c
new file mode 100644
index 0000000000..401ce04c58
--- /dev/null
+++ b/sw/airborne/state.c
@@ -0,0 +1,1399 @@
+/*
+ * Copyright (C) 2011-2012 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, see
+ * .
+ */
+
+/**
+ * @file state.c
+ *
+ * General inteface for the main vehicle states.
+ *
+ * This is the more detailed description of this file.
+ *
+ * @author Felix Ruess
+ */
+
+#include "state.h"
+
+struct State state;
+
+/**
+ * @addtogroup StateGroup
+ * @{
+ */
+
+void stateInit(void) {
+ state.pos_status = 0;
+ state.speed_status = 0;
+ state.accel_status = 0;
+ state.att_status = 0;
+ state.rate_status = 0;
+ state.wind_air_status = 0;
+ state.ned_initialized_i = FALSE;
+ state.ned_initialized_f = FALSE;
+ state.utm_initialized_f = FALSE;
+}
+
+
+/*******************************************************************************
+ * *
+ * transformation functions for the POSITION representations *
+ * *
+ ******************************************************************************/
+/** @addtogroup PosGroup
+ * @{ */
+
+void stateCalcPositionEcef_i(void) {
+ if (bit_is_set(state.pos_status, POS_ECEF_I))
+ return;
+
+ if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ //TODO check if resolution is good enough
+ ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ /* transform ned_f to ecef_f, set status bit, then convert to int */
+ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f to ecef_f, set status bit, then convert to int */
+ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_i _ecef_zero = {0};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_ECEF_I);
+}
+
+void stateCalcPositionNed_i(void) {
+ if (bit_is_set(state.pos_status, POS_NED_I))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_i) {
+ if (bit_is_set(state.pos_status, POS_NED_F)) {
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ SetBit(state.pos_status, POS_ENU_I);
+ INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ /* transform ecef_f -> ned_f, set status bit, then convert to int */
+ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_NED_F);
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
+ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_NED_F);
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_NED_F)) {
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ SetBit(state.pos_status, POS_ENU_I);
+ INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ /* transform utm_f -> ned_f -> ned_i, set status bits */
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_NED_F);
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_NED_F);
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_NED_F);
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct NedCoor_i _ned_zero = {0};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_NED_I);
+}
+
+void stateCalcPositionEnu_i(void) {
+ if (bit_is_set(state.pos_status, POS_ENU_I))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_i) {
+ if (bit_is_set(state.pos_status, POS_NED_I)) {
+ INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ SetBit(state.pos_status, POS_NED_I);
+ INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ /* transform ecef_f -> enu_f, set status bit, then convert to int */
+ enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_ENU_F);
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
+ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_ENU_F);
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
+ SetBit(state.pos_status, POS_NED_I);
+ INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ /* transform utm_f -> enu_f -> enu_i , set status bits */
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_ENU_F);
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_ENU_F);
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ SetBit(state.pos_status, POS_ENU_F);
+ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct EnuCoor_i _enu_zero = {0};
+ //return _enu_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_ENU_I);
+}
+
+void stateCalcPositionLla_i(void) {
+ if (bit_is_set(state.pos_status, POS_LLA_I))
+ return;
+
+ if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ /* transform ecef_f -> lla_f, set status bit, then convert to int */
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_LLA_F);
+ LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
+ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ SetBit(state.pos_status, POS_LLA_F);
+ LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ /* transform ned_i -> ecef_i -> lla_i, set status bits */
+ //TODO check if resolution is enough
+ ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
+ SetBit(state.pos_status, POS_ECEF_I);
+ lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ /* transform utm_f -> lla_f -> lla_i, set status bits */
+ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
+ SetBit(state.pos_status, POS_LLA_F);
+ LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct LlaCoor_i _lla_zero = {0};
+ //return _lla_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_LLA_I);
+}
+
+void stateCalcPositionUtm_f(void) {
+ if (bit_is_set(state.pos_status, POS_UTM_F))
+ return;
+
+ if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> lla_f -> utm_f, set status bits */
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_f _ecef_zero = {0.0f};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_UTM_F);
+}
+
+void stateCalcPositionEcef_f(void) {
+ if (bit_is_set(state.pos_status, POS_ECEF_F))
+ return;
+
+ if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ /* transform ned_i -> ecef_i -> ecef_f, set status bits */
+ //TODO check if resolution is good enough
+ ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
+ SetBit(state.pos_status, POS_ECEF_F);
+ ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_f _ecef_zero = {0.0f};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_ECEF_F);
+}
+
+void stateCalcPositionNed_f(void) {
+ if (bit_is_set(state.pos_status, POS_NED_F))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_NED_I)) {
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ /* transform ecef_i -> ned_i -> ned_f, set status bits */
+ ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ SetBit(state.pos_status, POS_NED_I);
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
+ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
+ SetBit(state.pos_status, POS_ECEF_I);
+ ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ SetBit(state.pos_status, POS_NED_I);
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_NED_I)) {
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ SetBit(state.pos_status, POS_ENU_F);
+ VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> utm_f -> ned, set status bits */
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct NedCoor_f _ned_zero = {0.0f};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_NED_F);
+}
+
+void stateCalcPositionEnu_f(void) {
+ if (bit_is_set(state.pos_status, POS_ENU_F))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_NED_F)) {
+ VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ SetBit(state.pos_status, POS_NED_F);
+ VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ /* transform ecef_i -> enu_i -> enu_f, set status bits */
+ enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ SetBit(state.pos_status, POS_ENU_I);
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
+ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
+ SetBit(state.pos_status, POS_ECEF_I);
+ enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+ SetBit(state.pos_status, POS_ENU_I);
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ SetBit(state.pos_status, POS_NED_F);
+ VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_F)) {
+ /* transform lla_f -> utm_f -> enu, set status bits */
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
+ SetBit(state.pos_status, POS_LLA_F);
+ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
+ SetBit(state.pos_status, POS_UTM_F);
+ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct EnuCoor_f _enu_zero = {0.0f};
+ //return _enu_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_ENU_F);
+}
+
+void stateCalcPositionLla_f(void) {
+ if (bit_is_set(state.pos_status, POS_LLA_F))
+ return;
+
+ if (bit_is_set(state.pos_status, POS_LLA_I)) {
+ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
+ /* transform ecef_i -> ecef_f -> lla_f, set status bits */
+ ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
+ SetBit(state.pos_status, POS_ECEF_F);
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ /* transform ned_f -> ecef_f -> lla_f, set status bits */
+ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ SetBit(state.pos_status, POS_NED_F);
+ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
+ SetBit(state.pos_status, POS_ECEF_F);
+ lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_UTM_F)) {
+ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct LlaCoor_f _lla_zero = {0.0};
+ //return _lla_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.pos_status, POS_LLA_F);
+}
+/** @}*/
+
+
+
+
+
+/******************************************************************************
+ * *
+ * Transformation functions for the SPEED representations *
+ * *
+ *****************************************************************************/
+/** @addtogroup SpeedGroup
+ * @{ */
+/************************ Set functions ****************************/
+
+void stateCalcSpeedNed_i(void) {
+ if (bit_is_set(state.speed_status, SPEED_NED_I))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_i) {
+ if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
+ SetBit(state.speed_status, SPEED_ENU_I);
+ INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ /* transform ecef_f -> ecef_i -> ned_i , set status bits */
+ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
+ SetBit(state.speed_status, SPEED_ECEF_I);
+ ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
+ SetBit(state.speed_status, SPEED_ENU_I);
+ INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct NedCoor_i _ned_zero = {0};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_NED_I);
+}
+
+void stateCalcSpeedEnu_i(void) {
+ if (bit_is_set(state.speed_status, SPEED_ENU_I))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_i) {
+ if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
+ }
+ if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ SetBit(state.pos_status, SPEED_NED_I);
+ INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ /* transform ecef_f -> ecef_i -> enu_i , set status bits */
+ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
+ SetBit(state.speed_status, SPEED_ECEF_I);
+ enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
+ }
+ if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ SetBit(state.pos_status, SPEED_NED_I);
+ INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct EnuCoor_i _enu_zero = {0};
+ //return _enu_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_ENU_I);
+}
+
+void stateCalcSpeedEcef_i(void) {
+ if (bit_is_set(state.speed_status, SPEED_ECEF_I))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ /* transform ned_f -> ned_i -> ecef_i , set status bits */
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ SetBit(state.speed_status, SPEED_NED_I);
+ ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_i _ecef_zero = {0};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_ECEF_I);
+}
+
+void stateCalcHorizontalSpeedNorm_i(void) { //TODO
+ if (bit_is_set(state.speed_status, SPEED_HNORM_I))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_HNORM_F)){
+ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ //TODO consider INT32_SPEED_FRAC
+ INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+ SetBit(state.speed_status, SPEED_HNORM_F);
+ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ //TODO consider INT32_SPEED_FRAC
+ INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+ SetBit(state.speed_status, SPEED_HNORM_F);
+ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ /* transform ecef speed to ned, set status bit, then compute norm */
+ //foo
+ //TODO consider INT32_SPEED_FRAC
+ //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
+ SetBit(state.speed_status, SPEED_NED_F);
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+ SetBit(state.speed_status, SPEED_HNORM_F);
+ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
+ }
+ else {
+ //int32_t _norm_zero = 0;
+ //return _norm_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_HNORM_I);
+
+ //return state.h_speed_norm_i;
+}
+
+void stateCalcHorizontalSpeedDir_i(void) { //TODO
+ if (bit_is_set(state.speed_status, SPEED_HDIR_I))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_HDIR_F)){
+ state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
+ INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
+ INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
+ SetBit(state.speed_status, SPEED_NED_I);
+ INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
+ INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
+ SetBit(state.speed_status, SPEED_ENU_I);
+ INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
+ INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_HDIR_I);
+}
+
+void stateCalcSpeedNed_f(void) {
+ if (bit_is_set(state.speed_status, SPEED_NED_F))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ SetBit(state.speed_status, SPEED_ENU_F);
+ VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ /* transform ecef_i -> ecef_f -> ned_f , set status bits */
+ SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
+ SetBit(state.speed_status, SPEED_ECEF_F);
+ ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ SetBit(state.speed_status, SPEED_ENU_F);
+ VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct NedCoor_f _ned_zero = {0.0f};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_NED_F);
+}
+
+void stateCalcSpeedEnu_f(void) {
+ if (bit_is_set(state.speed_status, SPEED_ENU_F))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ SetBit(state.pos_status, SPEED_NED_F);
+ VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
+ enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ /* transform ecef_I -> ecef_f -> enu_f , set status bits */
+ SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
+ SetBit(state.speed_status, SPEED_ECEF_F);
+ enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ }
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ SetBit(state.pos_status, SPEED_NED_F);
+ VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 2;
+ }
+ }
+ else { /* ned coordinate system not initialized, set errno */
+ errno = 3;
+ }
+ if (errno) {
+ //struct EnuCoor_f _enu_zero = {0};
+ //return _enu_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_ENU_F);
+}
+
+void stateCalcSpeedEcef_f(void) {
+ if (bit_is_set(state.speed_status, SPEED_ECEF_F))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ /* transform ned_f -> ned_i -> ecef_i , set status bits */
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ SetBit(state.speed_status, SPEED_NED_F);
+ ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_f _ecef_zero = {0.0f};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_ECEF_F);
+}
+
+void stateCalcHorizontalSpeedNorm_f(void) { //TODO
+ if (bit_is_set(state.speed_status, SPEED_HNORM_F))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_HNORM_I)){
+ state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ SetBit(state.speed_status, SPEED_NED_F);
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ SetBit(state.speed_status, SPEED_ENU_F);
+ FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_HNORM_F);
+}
+
+void stateCalcHorizontalSpeedDir_f(void) { //TODO
+ if (bit_is_set(state.speed_status, SPEED_HDIR_F))
+ return;
+
+ if (bit_is_set(state.speed_status, SPEED_HDIR_I)){
+ state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
+ state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
+ state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
+ SetBit(state.speed_status, SPEED_NED_F);
+ state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
+ }
+ else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
+ SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
+ SetBit(state.speed_status, SPEED_ENU_F);
+ state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.speed_status, SPEED_HDIR_F);
+}
+/** @}*/
+
+
+
+/******************************************************************************
+ * *
+ * Transformation functions for the ACCELERATION representations *
+ * *
+ *****************************************************************************/
+/** @addtogroup AccelGroup
+ * @{ */
+
+void stateCalcAccelNed_i(void) {
+ if (bit_is_set(state.accel_status, ACCEL_NED_I))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_i) {
+ if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+ ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+ ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
+ /* transform ecef_f -> ecef_i -> ned_i , set status bits */
+ ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
+ SetBit(state.accel_status, ACCEL_ECEF_I);
+ ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ } else { /* ned coordinate system not initialized, set errno */
+ errno = 2;
+ }
+ if (errno) {
+ //struct NedCoor_i _ned_zero = {0};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.accel_status, ACCEL_NED_I);
+}
+
+void stateCalcAccelEcef_i(void) {
+ if (bit_is_set(state.accel_status, ACCEL_ECEF_I))
+ return;
+
+ if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
+ ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+ ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+ /* transform ned_f -> ned_i -> ecef_i , set status bits */
+ ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
+ SetBit(state.accel_status, ACCEL_NED_I);
+ ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_i _ecef_zero = {0};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.accel_status, ACCEL_ECEF_I);
+}
+
+void stateCalcAccelNed_f(void) {
+ if (bit_is_set(state.accel_status, ACCEL_NED_F))
+ return;
+
+ int errno = 0;
+ if (state.ned_initialized_f) {
+ if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+ ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
+ ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+ /* transform ecef_i -> ecef_f -> ned_f , set status bits */
+ ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
+ SetBit(state.accel_status, ACCEL_ECEF_F);
+ ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
+ }
+ else { /* could not get this representation, set errno */
+ errno = 1;
+ }
+ } else { /* ned coordinate system not initialized, set errno */
+ errno = 2;
+ }
+ if (errno) {
+ //struct NedCoor_f _ned_zero = {0.0f};
+ //return _ned_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.accel_status, ACCEL_NED_F);
+}
+
+void stateCalcAccelEcef_f(void) {
+ if (bit_is_set(state.accel_status, ACCEL_ECEF_F))
+ return;
+
+ if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+ ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+ ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
+ }
+ else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+ /* transform ned_f -> ned_i -> ecef_i , set status bits */
+ ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
+ SetBit(state.accel_status, ACCEL_NED_F);
+ ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
+ }
+ else {
+ /* could not get this representation, set errno */
+ //struct EcefCoor_f _ecef_zero = {0.0f};
+ //return _ecef_zero;
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.accel_status, ACCEL_ECEF_F);
+}
+/** @}*/
+
+
+
+/******************************************************************************
+ * *
+ * Transformation functions for the ATTITUDE representations *
+ * *
+ *****************************************************************************/
+/** @addtogroup AttGroup
+ * @{ */
+
+void stateCalcNedToBodyQuat_i(void) {
+ if (bit_is_set(state.att_status, ATT_QUAT_I))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_QUAT_F)) {
+ QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_I)) {
+ INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_I)) {
+ INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_F)) {
+ RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f);
+ SetBit(state.att_status, ATT_RMAT_I);
+ INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_F)) {
+ EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f);
+ SetBit(state.att_status, ATT_EULER_I);
+ INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_QUAT_I);
+}
+
+void stateCalcNedToBodyRMat_i(void) {
+ if (bit_is_set(state.att_status, ATT_RMAT_I))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_RMAT_F)) {
+ RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_I)) {
+ INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_I)) {
+ INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_F)) {
+ QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f);
+ SetBit(state.att_status, ATT_QUAT_I);
+ INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_F)) {
+ EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f);
+ SetBit(state.att_status, ATT_EULER_I);
+ INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_RMAT_I);
+}
+
+void stateCalcNedToBodyEulers_i(void) {
+ if (bit_is_set(state.att_status, ATT_EULER_I))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_EULER_F)) {
+ EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_I)) {
+ INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_I)) {
+ INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_F)) {
+ RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f);
+ SetBit(state.att_status, ATT_RMAT_I);
+ INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_F)) {
+ QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f);
+ SetBit(state.att_status, ATT_QUAT_I);
+ INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_EULER_I);
+}
+
+void stateCalcNedToBodyQuat_f(void) {
+ if (bit_is_set(state.att_status, ATT_QUAT_F))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_QUAT_I)) {
+ QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_F)) {
+ FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_F)) {
+ FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_I)) {
+ RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i);
+ SetBit(state.att_status, ATT_RMAT_F);
+ FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_I)) {
+ EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i);
+ SetBit(state.att_status, ATT_EULER_F);
+ FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_QUAT_F);
+}
+
+void stateCalcNedToBodyRMat_f(void) {
+ if (bit_is_set(state.att_status, ATT_RMAT_F))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_RMAT_I)) {
+ RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_F)) {
+ FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_F)) {
+ FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_I)) {
+ QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i);
+ SetBit(state.att_status, ATT_QUAT_F);
+ FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_f, state.ned_to_body_quat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_EULER_I)) {
+ EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i);
+ SetBit(state.att_status, ATT_EULER_F);
+ FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_f, state.ned_to_body_eulers_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_RMAT_F);
+}
+
+void stateCalcNedToBodyEulers_f(void) {
+ if (bit_is_set(state.att_status, ATT_EULER_F))
+ return;
+
+ if (bit_is_set(state.att_status, ATT_EULER_I)) {
+ EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_F)) {
+ FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_f, state.ned_to_body_rmat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_F)) {
+ FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f);
+ }
+ else if (bit_is_set(state.att_status, ATT_RMAT_I)) {
+ RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i);
+ SetBit(state.att_status, ATT_RMAT_F);
+ FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i);
+ }
+ else if (bit_is_set(state.att_status, ATT_QUAT_I)) {
+ QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i);
+ SetBit(state.att_status, ATT_QUAT_F);
+ FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.att_status, ATT_EULER_F);
+}
+/** @}*/
+
+
+/******************************************************************************
+ * *
+ * Transformation functions for the ANGULAR RATE representations *
+ * *
+ *****************************************************************************/
+/** @addtogroup RateGroup
+ * @{ */
+
+void stateCalcBodyRates_i(void) {
+ if (bit_is_set(state.rate_status, RATE_I))
+ return;
+
+ if (bit_is_set(state.rate_status, RATE_F)) {
+ RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.rate_status, RATE_I);
+}
+
+void stateCalcBodyRates_f(void) {
+ if (bit_is_set(state.rate_status, RATE_F))
+ return;
+
+ if (bit_is_set(state.rate_status, RATE_I)) {
+ RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.rate_status, RATE_F);
+}
+
+/** @}*/
+
+
+/******************************************************************************
+ * *
+ * Transformation functions for the WIND- AND AIRSPEED representations *
+ * *
+ *****************************************************************************/
+/** @addtogroup WindAirGroup
+ * @{ */
+
+void stateCalcHorizontalWindspeed_i(void) {
+ if (bit_is_set(state.wind_air_status, WINDSPEED_I))
+ return;
+
+ if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
+ state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x);
+ state.h_windspeed_i.y = SPEED_BFP_OF_REAL(state.h_windspeed_f.y);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.rate_status, WINDSPEED_I);
+}
+
+void stateCalcAirspeed_i(void) {
+ if (bit_is_set(state.wind_air_status, AIRSPEED_I))
+ return;
+
+ if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
+ state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.wind_air_status, AIRSPEED_I);
+}
+
+void stateCalcHorizontalWindspeed_f(void) {
+ if (bit_is_set(state.wind_air_status, WINDSPEED_F))
+ return;
+
+ if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
+ state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x);
+ state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.y);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.rate_status, WINDSPEED_F);
+}
+
+void stateCalcAirspeed_f(void) {
+ if (bit_is_set(state.wind_air_status, AIRSPEED_F))
+ return;
+
+ if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
+ state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i);
+ }
+ /* set bit to indicate this representation is computed */
+ SetBit(state.wind_air_status, AIRSPEED_F);
+}
+/** @}*/
+
+/** @}*/
diff --git a/sw/airborne/state.h b/sw/airborne/state.h
new file mode 100644
index 0000000000..a06296529f
--- /dev/null
+++ b/sw/airborne/state.h
@@ -0,0 +1,1334 @@
+/*
+ * Copyright (C) 2011-2012 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, see
+ * .
+ */
+
+/**
+ * @file state.h
+ *
+ * API to get/set the generic vehicle states.
+ *
+ * This general state interface holds all the most important vehicle states like
+ * position, velocity, attitude, etc. It handles coordinate system and
+ * fixed-/floating-point conversion on the fly when needed.
+ *
+ * You can set e.g. the position in any coordinate system you wish:
+ * stateSetPositionNed_i() to set the position in fixed-point NED coordinates.
+ * If you need to read the position somewhere else in a different representation,
+ * call: stateGetPositionLla_f() and only then the LLA float position representation
+ * is calculated on the fly and returned. It's also only calculated once,
+ * until a new position is set which invalidates all the other representations again.
+ *
+ * @author Felix Ruess
+ */
+
+#ifndef STATE_H
+#define STATE_H
+
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
+
+#include "std.h"
+#include
+
+/**
+ * @defgroup StateGroup State interface
+ * @{
+ */
+
+/**
+ * @defgroup PosGroup Position representations
+ * @{
+ */
+#define POS_ECEF_I 0
+#define POS_NED_I 1
+#define POS_ENU_I 2
+#define POS_LLA_I 3
+#define POS_UTM_I 4
+#define POS_ECEF_F 5
+#define POS_NED_F 6
+#define POS_ENU_F 7
+#define POS_LLA_F 8
+#define POS_UTM_F 9
+#define POS_LOCAL_COORD ((1<commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
+ float speed = *stateGetHorizontalSpeedNorm_f();
+ if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
- if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
+ if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
- if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
+ if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
@@ -515,7 +506,7 @@ void Drift_correction(void)
void Matrix_update(void)
{
- Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega[0], &ahrs_impl.imu_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)
@@ -530,13 +521,13 @@ void Matrix_update(void)
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
- Update_Matrix[0][1]=-G_Dt*ahrs_float.imu_rate.r;//-z
- Update_Matrix[0][2]=G_Dt*ahrs_float.imu_rate.q;//y
- Update_Matrix[1][0]=G_Dt*ahrs_float.imu_rate.r;//z
+ Update_Matrix[0][1]=-G_Dt*ahrs_impl.imu_rate.r;//-z
+ Update_Matrix[0][2]=G_Dt*ahrs_impl.imu_rate.q;//y
+ Update_Matrix[1][0]=G_Dt*ahrs_impl.imu_rate.r;//z
Update_Matrix[1][1]=0;
- Update_Matrix[1][2]=-G_Dt*ahrs_float.imu_rate.p;
- Update_Matrix[2][0]=-G_Dt*ahrs_float.imu_rate.q;
- Update_Matrix[2][1]=G_Dt*ahrs_float.imu_rate.p;
+ Update_Matrix[1][2]=-G_Dt*ahrs_impl.imu_rate.p;
+ Update_Matrix[2][0]=-G_Dt*ahrs_impl.imu_rate.q;
+ Update_Matrix[2][1]=G_Dt*ahrs_impl.imu_rate.p;
Update_Matrix[2][2]=0;
#endif
@@ -554,34 +545,41 @@ void Matrix_update(void)
/*
* Compute body orientation and rates from imu orientation and rates
*/
-static inline void compute_body_orientation_and_rates(void) {
+static inline void set_body_orientation_and_rates(void) {
- FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
- ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
- FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
- ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
- FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
- FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
+ struct FloatRates body_rate;
+ FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
+ stateSetBodyRates_f(&body_rate);
+
+ struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
+ FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
+ FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
+
+ // Some stupid lines of code for neutrals
+ struct FloatEulers ltp_to_body_euler;
+ FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
+ ltp_to_body_euler.phi -= ins_roll_neutral;
+ ltp_to_body_euler.theta -= ins_pitch_neutral;
+ stateSetNedToBodyEulers_f(<p_to_body_euler);
+
+ // should be replaced at the end by:
+ // stateSetNedToBodyRMat_f(<p_to_body_rmat);
}
static inline void compute_ahrs_representations(void) {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
- ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
- ahrs_float.ltp_to_imu_euler.psi = 0;
+ ahrs_impl.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
+ ahrs_impl.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
+ ahrs_impl.ltp_to_imu_euler.psi = 0;
#else
- ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
- ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
- ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
- ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
+ ahrs_impl.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ ahrs_impl.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
+ ahrs_impl.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+ ahrs_impl.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
- /* set quaternion and rotation matrix representations as well */
- FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
- FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
-
- compute_body_orientation_and_rates();
+ set_body_orientation_and_rates();
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
@@ -601,15 +599,3 @@ static inline void compute_ahrs_representations(void) {
*/
}
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-void ahrs_update_fw_estimator( void ) {
- // export results to estimator
- estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
- estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
- estimator_psi = ahrs_float.ltp_to_body_euler.psi;
-
- estimator_p = ahrs_float.body_rate.p;
- estimator_q = ahrs_float.body_rate.q;
- estimator_r = ahrs_float.body_rate.r;
-}
-#endif //AHRS_UPDATE_FW_ESTIMATOR
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index 31f82f60e6..80ae0b1b13 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -27,13 +27,10 @@
struct AhrsFloatDCM {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
- /*
- Holds float version of IMU alignement
- in order to be able to run against the fixed point
- version of the IMU
- */
- struct FloatQuat body_to_imu_quat;
+
+ struct FloatEulers ltp_to_imu_euler;
struct FloatRMat body_to_imu_rmat;
+ struct FloatRates imu_rate;
float gps_speed;
float gps_acceleration;
@@ -44,13 +41,10 @@ struct AhrsFloatDCM {
extern struct AhrsFloatDCM ahrs_impl;
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// FIXME neutrals should be a feature of state interface ?
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
-void ahrs_update_fw_estimator(void);
-#endif
-
// DCM Parameters
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
index 064cf7dcf7..e3c2a5f0b3 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c
@@ -25,24 +25,14 @@
#include "subsystems/imu.h"
#include "subsystems/gps.h"
-#include "estimator.h"
-
+#include "state.h"
+float heading;
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
- /* set ltp_to_body to zero */
- FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
- FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
- FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
- FLOAT_RATES_ZERO(ahrs_float.body_rate);
-
- /* set ltp_to_body to zero */
- FLOAT_QUAT_ZERO(ahrs_float.ltp_to_imu_quat);
- FLOAT_EULERS_ZERO(ahrs_float.ltp_to_imu_euler);
- FLOAT_RMAT_ZERO(ahrs_float.ltp_to_imu_rmat);
- FLOAT_RATES_ZERO(ahrs_float.imu_rate);
+ heading = 0.;
}
void ahrs_align(void) {
@@ -53,15 +43,17 @@ void ahrs_align(void) {
}
void ahrs_propagate(void) {
+ struct FloatRates body_rate = { 0., 0., 0. };
#ifdef ADC_CHANNEL_GYRO_P
- ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
+ body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
#endif
#ifdef ADC_CHANNEL_GYRO_Q
- ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
+ body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
#endif
#ifdef ADC_CHANNEL_GYRO_R
- ahrs_float.body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
+ body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
#endif
+ stateSetBodyRates_f(&body_rate);
}
void ahrs_update_accel(void) {
@@ -77,50 +69,29 @@ void ahrs_update_gps(void) {
// Heading estimator from wind-information, usually computed with -DWIND_INFO
// wind_north and wind_east initialized to 0, so still correct if not updated
- float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
- float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
- ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn);
- if (ahrs_float.ltp_to_body_euler.psi < 0.)
- ahrs_float.ltp_to_body_euler.psi += 2 * M_PI;
+ float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
+ float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
+ heading = atan2f(w_ve, w_vn);
+ if (heading < 0.)
+ heading += 2 * M_PI;
- ahrs_update_fw_estimator();
}
void ahrs_update_infrared(void) {
- ahrs_float.ltp_to_body_euler.phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
+ float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
+ float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
- ahrs_float.ltp_to_body_euler.theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
+ if (theta < -M_PI_2) theta += M_PI;
+ else if (theta > M_PI_2) theta -= M_PI;
- if (ahrs_float.ltp_to_body_euler.theta < -M_PI_2)
- ahrs_float.ltp_to_body_euler.theta += M_PI;
- else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2)
- ahrs_float.ltp_to_body_euler.theta -= M_PI;
+ if (phi >= 0) phi *= infrared.correction_right;
+ else phi *= infrared.correction_left;
- if (ahrs_float.ltp_to_body_euler.phi >= 0)
- ahrs_float.ltp_to_body_euler.phi *= infrared.correction_right;
- else
- ahrs_float.ltp_to_body_euler.phi *= infrared.correction_left;
+ if (theta >= 0) theta *= infrared.correction_up;
+ else theta *= infrared.correction_down;
- if (ahrs_float.ltp_to_body_euler.theta >= 0)
- ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up;
- else
- ahrs_float.ltp_to_body_euler.theta *= infrared.correction_down;
-
- ahrs_update_fw_estimator();
-}
-
-
-// TODO use ahrs result directly
-void ahrs_update_fw_estimator(void)
-{
- // export results to estimator
-
- estimator_phi = ahrs_float.ltp_to_body_euler.phi;
- estimator_theta = ahrs_float.ltp_to_body_euler.theta;
- estimator_psi = ahrs_float.ltp_to_body_euler.psi;
-
- estimator_p = ahrs_float.body_rate.p;
- estimator_q = ahrs_float.body_rate.q;
- estimator_r = ahrs_float.body_rate.r;
+ struct FloatEulers att = { phi, theta, heading };
+ stateSetNedToBodyEulers_f(&att);
}
+
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
index a4d2870756..02c677260f 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -33,7 +33,4 @@ extern float ins_pitch_neutral;
extern void ahrs_update_infrared(void);
-// TODO copy ahrs to state instead of estimator
-extern void ahrs_update_fw_estimator(void);
-
#endif /* AHRS_INFRARED_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
index 4718311da1..c5a56a3bf6 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -21,6 +21,7 @@
#include "ahrs_int_cmpl_euler.h"
+#include "state.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "math/pprz_trig_int.h"
@@ -32,13 +33,28 @@
#define FACE_REINJ_1 1024
#endif
+#ifndef AHRS_MAG_OFFSET
+#define AHRS_MAG_OFFSET 0.
+#endif
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// remotely settable (for FW)
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+#endif
+
struct AhrsIntCmplEuler ahrs_impl;
static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel);
static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag);
-static inline void compute_imu_quat_and_rmat_from_euler(void);
-static inline void compute_body_orientation(void);
+static inline void set_body_state_from_euler(void);
#define F_UPDATE 512
@@ -52,26 +68,14 @@ static inline void compute_body_orientation(void);
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
- /* set ltp_to_body to zero */
- INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
- INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
- INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
- INT_RATES_ZERO(ahrs.body_rate);
-
/* set ltp_to_imu so that body is zero */
- QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
- RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
- INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
- INT_RATES_ZERO(ahrs.imu_rate);
+ INT32_EULERS_OF_RMAT(ahrs_impl.ltp_to_imu_euler, imu.body_to_imu_rmat);
+ INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
-#ifdef IMU_MAG_OFFSET
- ahrs_mag_offset = IMU_MAG_OFFSET;
-#else
- ahrs_mag_offset = 0.;
-#endif
+ ahrs_impl.mag_offset = AHRS_MAG_OFFSET;
}
void ahrs_align(void) {
@@ -84,11 +88,9 @@ void ahrs_align(void) {
EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
/* Compute LTP to IMU eulers */
- EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
+ EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
- compute_imu_quat_and_rmat_from_euler();
-
- compute_body_orientation();
+ set_body_state_from_euler();
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING;
@@ -156,11 +158,11 @@ void ahrs_propagate(void) {
#endif
/* low pass rate */
#if USE_NOISE_FILTER
- RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
- RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
+ RATES_SUM_SCALED(ahrs_impl.imu_rate, ahrs_impl.imu_rate, uf_rate, NOISE_FILTER_GAIN);
+ RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, NOISE_FILTER_GAIN+1);
#else
- RATES_ADD(ahrs.imu_rate, uf_rate);
- RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
+ RATES_ADD(ahrs_impl.imu_rate, uf_rate);
+ RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 2);
#endif
#if USE_NOISE_CUT
}
@@ -169,7 +171,7 @@ void ahrs_propagate(void) {
/* integrate eulers */
struct Int32Eulers euler_dot;
- INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
+ INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs_impl.ltp_to_imu_euler, ahrs_impl.imu_rate);
EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
/* low pass measurement */
@@ -189,11 +191,9 @@ void ahrs_propagate(void) {
/* Compute LTP to IMU eulers */
- EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
+ EULERS_SDIV(ahrs_impl.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
- compute_imu_quat_and_rmat_from_euler();
-
- compute_body_orientation();
+ set_body_state_from_euler();
}
@@ -220,7 +220,7 @@ void ahrs_update_accel(void) {
void ahrs_update_mag(void) {
- get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs.ltp_to_imu_euler.phi, ahrs.ltp_to_imu_euler.theta, imu.mag);
+ get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs_impl.ltp_to_imu_euler.phi, ahrs_impl.ltp_to_imu_euler.theta, imu.mag);
}
@@ -265,62 +265,33 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(
// sphi_ctheta * imu.mag.y +
// cphi_ctheta * imu.mag.z;
float m_psi = -atan2(me, mn);
- *psi_meas = ((m_psi - ahrs_mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+ *psi_meas = ((m_psi - ahrs_impl.mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
}
-/* Compute ltp to imu rotation in quaternion and rotation matrice representation
- from the euler angle representation */
-__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
-
- /* Compute LTP to IMU quaternion */
- INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
+/* Rotate angles and rates from imu to body frame and set state */
+__attribute__ ((always_inline)) static inline void set_body_state_from_euler(void) {
+ struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat;
/* Compute LTP to IMU rotation matrix */
- INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
-
-}
-
-__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
-
- /* Compute LTP to BODY quaternion */
- INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
+ INT32_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
/* Compute LTP to BODY rotation matrix */
- INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
- /* compute LTP to BODY eulers */
- INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
- /* compute body rates */
- INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
-
-}
-
-
+ INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, imu.body_to_imu_rmat);
+ /* Set state */
#ifdef AHRS_UPDATE_FW_ESTIMATOR
-// TODO use ahrs result directly
-#include "estimator.h"
-// remotely settable
-#ifndef INS_ROLL_NEUTRAL_DEFAULT
-#define INS_ROLL_NEUTRAL_DEFAULT 0
+ struct Int32Eulers ltp_to_body_euler;
+ INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
+ ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral);
+ ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral);
+ stateSetNedToBodyEulers_i(<p_to_body_euler);
+#else
+ stateSetNedToBodyRMat_i(<p_to_body_rmat);
#endif
-#ifndef INS_PITCH_NEUTRAL_DEFAULT
-#define INS_PITCH_NEUTRAL_DEFAULT 0
-#endif
-float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
-float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-void ahrs_update_fw_estimator(void)
-{
- struct FloatEulers att;
- // export results to estimator
- EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler);
- estimator_phi = att.phi - ins_roll_neutral;
- estimator_theta = att.theta - ins_pitch_neutral;
- estimator_psi = att.psi;
-
- struct FloatRates rates;
- RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
- estimator_p = rates.p;
- estimator_q = rates.q;
- estimator_r = rates.r;
+ struct Int32Rates body_rate;
+ /* compute body rates */
+ INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate);
+ /* Set state */
+ stateSetBodyRates_i(&body_rate);
}
-#endif //AHRS_UPDATE_FW_ESTIMATOR
+
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
index b6ad467aa4..9210793a28 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
@@ -28,19 +28,20 @@
struct AhrsIntCmplEuler {
struct Int32Rates gyro_bias;
+ struct Int32Rates imu_rate;
struct Int32Eulers hi_res_euler;
struct Int32Eulers measure;
struct Int32Eulers residual;
struct Int32Eulers measurement;
+ struct Int32Eulers ltp_to_imu_euler;
int32_t reinj_1;
+ float mag_offset;
};
extern struct AhrsIntCmplEuler ahrs_impl;
#ifdef AHRS_UPDATE_FW_ESTIMATOR
-// TODO copy ahrs to state instead of estimator
-void ahrs_update_fw_estimator(void);
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
#endif
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index d2cafe0f25..940cb95f65 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -24,6 +24,8 @@
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
+#include "state.h"
+
#include "subsystems/imu.h"
#if USE_GPS
#include "subsystems/gps.h"
@@ -52,10 +54,19 @@ static inline void ahrs_update_mag_2d(void);
struct AhrsIntCmpl ahrs_impl;
-static inline void compute_imu_euler_and_rmat_from_quat(void);
-static inline void compute_body_euler_and_rmat_from_quat(void);
-static inline void compute_imu_orientation(void);
-static inline void compute_body_orientation(void);
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+#endif
+
+static inline void set_body_state_from_quat(void);
void ahrs_init(void) {
@@ -63,17 +74,9 @@ void ahrs_init(void) {
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
- /* set ltp_to_body to zero */
- INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
- INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
- INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
- INT_RATES_ZERO(ahrs.body_rate);
-
/* set ltp_to_imu so that body is zero */
- QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
- RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
- INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
- INT_RATES_ZERO(ahrs.imu_rate);
+ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
+ INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
@@ -97,18 +100,15 @@ void ahrs_align(void) {
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
- ahrs_int_get_quat_from_accel_mag(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+ ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
ahrs_impl.heading_aligned = TRUE;
#else
/* Compute an initial orientation from accel and just set heading to zero */
- ahrs_int_get_quat_from_accel(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
+ ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
ahrs_impl.heading_aligned = FALSE;
#endif
- /* Convert initial orientation from quat to euler and rotation matrix representations. */
- compute_imu_euler_and_rmat_from_quat();
-
- compute_body_orientation();
+ set_body_state_from_quat();
/* Use low passed gyro value as initial bias */
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
@@ -133,11 +133,11 @@ void ahrs_propagate(void) {
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
- RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
- RATES_ADD(ahrs.imu_rate, omega);
- RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
+ RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate,2);
+ RATES_ADD(ahrs_impl.imu_rate, omega);
+ RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 3);
#else
- RATES_COPY(ahrs.imu_rate, omega);
+ RATES_COPY(ahrs_impl.imu_rate, omega);
#endif
/* add correction */
@@ -146,12 +146,10 @@ void ahrs_propagate(void) {
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
- INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
- INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);
+ INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
+ INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
- compute_imu_euler_and_rmat_from_quat();
-
- compute_body_orientation();
+ set_body_state_from_quat();
}
@@ -161,9 +159,11 @@ void ahrs_propagate(void) {
void ahrs_update_accel(void) {
// c2 = ltp z-axis in imu-frame
- struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
- RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
- RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
+ struct Int32RMat ltp_to_imu_rmat;
+ INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.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 Int32Vect3 residual;
struct Int32Vect3 pseudo_gravity_measurement;
@@ -180,7 +180,7 @@ void ahrs_update_accel(void) {
// FIXME: check overflows !
const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0};
struct Int32Vect3 acc_c_body;
- VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
+ VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC);
/* convert centrifucal acceleration from body to imu frame */
@@ -251,11 +251,14 @@ void ahrs_update_mag(void) {
static inline void ahrs_update_mag_full(void) {
+
+ struct Int32RMat ltp_to_imu_rmat;
+ INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu;
- INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
+ INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, expected_ltp);
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
@@ -277,11 +280,12 @@ static inline void ahrs_update_mag_full(void) {
static inline void ahrs_update_mag_2d(void) {
+ struct Int32RMat ltp_to_imu_rmat;
+ INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y)};
-
struct Int32Vect3 measured_ltp;
- INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
+ INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag);
struct Int32Vect3 residual_ltp =
{ 0,
@@ -289,7 +293,7 @@ static inline void ahrs_update_mag_2d(void) {
(measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
struct Int32Vect3 residual_imu;
- INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
+ INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp);
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// rate_correction FRAC = RATE_FRAC = 12
@@ -360,9 +364,10 @@ void ahrs_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 Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i();
struct Int32Vect2 expected_ltp =
- { RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 0),
- RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 1) };
+ { RMAT_ELMT((*ltp_to_body_rmat), 0, 0),
+ RMAT_ELMT((*ltp_to_body_rmat), 0, 1) };
int32_t heading_x, heading_y;
PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
@@ -375,7 +380,9 @@ void ahrs_update_heading(int32_t heading) {
(expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<
-#endif
-
-
-#include "math/pprz_geodetic_int.h"
-
-#include "generated/flight_plan.h"
-
-/* gps transformed to LTP-NED */
-struct LtpDef_i ins_ltp_def;
- bool_t ins_ltp_initialised;
-struct NedCoor_i ins_gps_pos_cm_ned;
-struct NedCoor_i ins_gps_speed_cm_s_ned;
-#if USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-struct FloatVect2 ins_gps_pos_m_ned;
-struct FloatVect2 ins_gps_speed_m_s_ned;
-#endif
-bool_t ins_hf_realign;
-
-/* barometer */
-#if USE_VFF
-int32_t ins_qfe;
-bool_t ins_baro_initialised;
-int32_t ins_baro_alt;
-#if USE_SONAR
-bool_t ins_update_on_agl;
-int32_t ins_sonar_offset;
-#endif
-#endif
-bool_t ins_vf_realign;
-
-/* output */
-struct NedCoor_i ins_ltp_pos;
-struct NedCoor_i ins_ltp_speed;
-struct NedCoor_i ins_ltp_accel;
-struct EnuCoor_i ins_enu_pos;
-struct EnuCoor_i ins_enu_speed;
-struct EnuCoor_i ins_enu_accel;
-
-
-void ins_init() {
-#if USE_INS_NAV_INIT
- ins_ltp_initialised = TRUE;
-
- /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
- struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
- llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
- /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
- llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
-
- struct EcefCoor_i ecef_nav0;
- ecef_of_lla_i(&ecef_nav0, &llh_nav0);
-
- ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
- ins_ltp_def.hmsl = NAV_ALT0;
-#else
- ins_ltp_initialised = FALSE;
-#endif
-#if USE_VFF
- ins_baro_initialised = FALSE;
-#if USE_SONAR
- ins_update_on_agl = FALSE;
-#endif
- vff_init(0., 0., 0.);
-#endif
- ins_vf_realign = FALSE;
- ins_hf_realign = FALSE;
-#if USE_HFF
- b2_hff_init(0., 0., 0., 0.);
-#endif
- INT32_VECT3_ZERO(ins_ltp_pos);
- INT32_VECT3_ZERO(ins_ltp_speed);
- INT32_VECT3_ZERO(ins_ltp_accel);
- INT32_VECT3_ZERO(ins_enu_pos);
- INT32_VECT3_ZERO(ins_enu_speed);
- INT32_VECT3_ZERO(ins_enu_accel);
-}
-
-void ins_periodic( void ) {
-}
-
-#if USE_HFF
-void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
- b2_hff_realign(pos, speed);
-}
-#else
-void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
-#endif /* USE_HFF */
-
-
-void ins_realign_v(float z) {
-#if USE_VFF
- vff_realign(z);
-#endif
-}
-
-void ins_propagate() {
- /* untilt accels */
- struct Int32Vect3 accel_meas_body;
- INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
- struct Int32Vect3 accel_meas_ltp;
- INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
-
-#if USE_VFF
- float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
- if (baro.status == BS_RUNNING && ins_baro_initialised) {
- vff_propagate(z_accel_meas_float);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
- }
- else { // feed accel from the sensors
- // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
- ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
- }
-#else
- ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
-#endif /* USE_VFF */
-
-#if USE_HFF
- /* propagate horizontal filter */
- b2_hff_propagate();
-#else
- ins_ltp_accel.x = accel_meas_ltp.x;
- ins_ltp_accel.y = accel_meas_ltp.y;
-#endif /* USE_HFF */
-
- INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
-}
-
-void ins_update_baro() {
-#if USE_VFF
- if (baro.status == BS_RUNNING) {
- if (!ins_baro_initialised) {
- ins_qfe = baro.absolute;
- ins_baro_initialised = TRUE;
- }
- if (ins_vf_realign) {
- ins_vf_realign = FALSE;
- ins_qfe = baro.absolute;
-#if USE_SONAR
- ins_sonar_offset = sonar_meas;
-#endif
- vff_realign(0.);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
- ins_enu_pos.z = -ins_ltp_pos.z;
- ins_enu_speed.z = -ins_ltp_speed.z;
- ins_enu_accel.z = -ins_ltp_accel.z;
- }
- else { /* not realigning, so normal update with baro measurement */
- ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
- float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
- vff_update(alt_float);
- }
- }
-#endif
-}
-
-
-void ins_update_gps(void) {
-#if USE_GPS
- if (gps.fix == GPS_FIX_3D) {
- if (!ins_ltp_initialised) {
- ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
- ins_ltp_def.lla.alt = gps.lla_pos.alt;
- ins_ltp_def.hmsl = gps.hmsl;
- ins_ltp_initialised = TRUE;
- }
- ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
- ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
-#if USE_HFF
- VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
- VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
- VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
- if (ins_hf_realign) {
- ins_hf_realign = FALSE;
-#ifdef SITL
- struct FloatVect2 true_pos, true_speed;
- VECT2_COPY(true_pos, fdm.ltpprz_pos);
- VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
- b2_hff_realign(true_pos, true_speed);
-#else
- const struct FloatVect2 zero = {0.0, 0.0};
- b2_hff_realign(ins_gps_pos_m_ned, zero);
-#endif
- }
- b2_hff_update_gps();
-#if !USE_VFF /* vff not used */
- ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
- ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
-#endif /* vff not used */
-#endif /* hff used */
-
-
-#if !USE_HFF /* hff not used */
-#if !USE_VFF /* neither hf nor vf used */
- INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
-#else /* only vff used */
- INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
-#endif
-
-#if USE_GPS_LAG_HACK
- VECT2_COPY(d_pos, ins_ltp_speed);
- INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
- VECT2_ADD(ins_ltp_pos, d_pos);
-#endif
-#endif /* hff not used */
-
- INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
- }
-#endif /* USE_GPS */
-}
-
-void ins_update_sonar() {
-#if USE_SONAR && USE_VFF
- static int32_t sonar_filtered = 0;
- sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
- /* update baro_qfe assuming a flat ground */
- if (ins_update_on_agl && baro.status == BS_RUNNING) {
- int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
- ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
- }
-#endif
-}
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index a3f876031d..4ce715988e 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2008-2012 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -25,48 +25,74 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_algebra_float.h"
+#include "state.h"
-/* gps transformed to LTP-NED */
-extern struct LtpDef_i ins_ltp_def;
-extern bool_t ins_ltp_initialised;
-extern struct NedCoor_i ins_gps_pos_cm_ned;
-extern struct NedCoor_i ins_gps_speed_cm_s_ned;
+#define INS_UNINIT 0
+#define INS_RUNNING 1
-/* barometer */
-#if USE_VFF || USE_VFF_EXTENDED
-extern int32_t ins_baro_alt;
-extern int32_t ins_qfe;
-extern bool_t ins_baro_initialised;
-#if USE_SONAR
-extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
-extern int32_t ins_sonar_offset;
-#endif
+/* underlying includes (needed for parameters) */
+#ifdef INS_TYPE_H
+#include INS_TYPE_H
#endif
-/* output LTP NED */
-extern struct NedCoor_i ins_ltp_pos;
-extern struct NedCoor_i ins_ltp_speed;
-extern struct NedCoor_i ins_ltp_accel;
-/* output LTP ENU */
-extern struct EnuCoor_i ins_enu_pos;
-extern struct EnuCoor_i ins_enu_speed;
-extern struct EnuCoor_i ins_enu_accel;
-#if USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-extern struct FloatVect2 ins_gps_pos_m_ned;
-extern struct FloatVect2 ins_gps_speed_m_s_ned;
-#endif
+/** Inertial Navigation System state */
+struct Ins {
+ uint8_t status; ///< status of the INS
+};
+/** global INS state */
+extern struct Ins ins;
+
+// TODO add to struct
extern bool_t ins_hf_realign;
extern bool_t ins_vf_realign;
+
+/** INS initialization. Called at startup.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_init( void );
+
+/** INS periodic call.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_periodic( void );
+
+/** INS horizontal realign.
+ * @param pos new horizontal position to set
+ * @param speed new horizontal speed to set
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
+
+/** INS vertical realign.
+ * @param z new altitude to set
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_realign_v(float z);
+
+/** Propagation. Usually integrates the gyro rates to angles.
+ * Reads the global #imu data struct.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_propagate( void );
+
+/** Update INS state with barometer measurements.
+ * Reads the global #baro data struct.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_update_baro( void );
+
+/** Update INS state with GPS measurements.
+ * Reads the global #gps data struct.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_update_gps( void );
+
+/** Update INS state with sonar measurements.
+ * Reads the global #sonar data struct.
+ * Needs to be implemented by each INS algorithm.
+ */
extern void ins_update_sonar( void );
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index f9cdcc78bc..bc0adaaeaf 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -25,7 +25,7 @@
#include "subsystems/ins/hf_float.h"
#include "subsystems/ins.h"
#include "subsystems/imu.h"
-#include "subsystems/ahrs.h"
+#include "state.h"
#include "subsystems/gps.h"
#include
@@ -440,7 +440,8 @@ void b2_hff_propagate(void) {
/* compute float ltp mean acceleration */
b2_hff_compute_accel_body_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_ltp;
- INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
+ struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i();
+ INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, (*ltp_to_body_rmat), acc_body_mean);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h
index 8c7669cc3e..eb8c6aa1f3 100644
--- a/sw/airborne/subsystems/ins/hf_float.h
+++ b/sw/airborne/subsystems/ins/hf_float.h
@@ -26,6 +26,7 @@
#include "std.h"
#include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
#define HFF_STATE_SIZE 2
diff --git a/sw/airborne/estimator.c b/sw/airborne/subsystems/ins/ins_float.c
similarity index 62%
rename from sw/airborne/estimator.c
rename to sw/airborne/subsystems/ins/ins_float.c
index 30c3b76e23..bc9298fe38 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/subsystems/ins/ins_float.c
@@ -26,83 +26,114 @@
* \brief State estimate, fusioning sensors
*/
+#include "subsystems/ins/ins_float.h"
+
#include
#include
-#include "estimator.h"
+#include "state.h"
#include "mcu_periph/uart.h"
#include "ap_downlink.h"
#include "subsystems/gps.h"
#include "subsystems/nav.h"
-#ifdef EXTRA_DOWNLINK_DEVICE
-#include "core/extra_pprz_dl.h"
-#endif
-/* position in meters */
-float estimator_x;
-float estimator_y;
+/* vertical position and speed in meters */
float estimator_z;
-
float estimator_z_dot;
-/* attitude in radian */
-float estimator_phi;
-float estimator_psi;
-float estimator_theta;
-
-/* rates in radians per second */
-float estimator_p;
-float estimator_q;
-float estimator_r;
-
-/* flight time in seconds */
-uint16_t estimator_flight_time;
-/* flight time in seconds */
-float estimator_t;
-
-/* horizontal speed in module and dir */
-float estimator_hspeed_mod;
-float estimator_hspeed_dir;
-
-/* wind */
-float wind_east, wind_north;
-float estimator_airspeed;
-float estimator_AOA;
-
-#define NORM_RAD_ANGLE2(x) { \
- while (x > 2 * M_PI) x -= 2 * M_PI; \
- while (x < 0 ) x += 2 * M_PI; \
- }
-
-
-#define EstimatorSetSpeedCart(vx, vy, vz) { \
- estimator_vx = vx; \
- estimator_vy = vy; \
- estimator_vz = vz; \
-}
-
-
-void estimator_init( void ) {
-
- EstimatorSetPosXY(0., 0.);
- EstimatorSetAlt(0.);
-
- EstimatorSetAtt (0., 0., 0);
-
- EstimatorSetSpeedPol ( 0., 0., 0.);
-
- EstimatorSetRate(0., 0., 0.);
-
-#ifdef USE_AOA
- EstimatorSetAOA( 0. );
+#if USE_BAROMETER
+#include "subsystems/sensors/baro.h"
+int32_t ins_qfe;
+bool_t ins_baro_initialised;
+float ins_baro_alt;
#endif
- estimator_flight_time = 0;
+bool_t ins_hf_realign;
+bool_t ins_vf_realign;
+
+void ins_init() {
+
+ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
+ stateSetLocalUtmOrigin_f(&utm0);
+
+ stateSetPositionUtm_f(&utm0);
+
+#ifdef ALT_KALMAN
+ alt_kalman_init();
+#endif
+
+#if USE_BAROMETER
+ ins_qfe = 0;;
+ ins_baro_initialised = FALSE;
+ ins_baro_alt = 0.;
+#endif
+ ins_vf_realign = FALSE;
+
+ EstimatorSetAlt(0.);
- // FIXME? Set initial airspeed to zero if USE_AIRSPEED ?
- EstimatorSetAirspeed( NOMINAL_AIRSPEED );
}
+void ins_periodic( void ) {
+}
+
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
+}
+
+void ins_realign_v(float z __attribute__ ((unused))) {
+}
+
+void ins_propagate() {
+}
+
+void ins_update_baro() {
+#if USE_BAROMETER
+ // TODO update kalman filter with baro struct
+ if (baro.status == BS_RUNNING) {
+ if (!ins_baro_initialised) {
+ ins_qfe = baro.absolute;
+ ins_baro_initialised = TRUE;
+ }
+ if (ins_vf_realign) {
+ ins_vf_realign = FALSE;
+ ins_qfe = baro.absolute;
+ }
+ else { /* not realigning, so normal update with baro measurement */
+ ins_baro_alt = (baro.absolute - ins_qfe) * INS_BARO_SENS;
+ EstimatorSetAlt(ins_baro_alt);
+ }
+ }
+#endif
+}
+
+
+void ins_update_gps(void) {
+#if USE_GPS
+ struct UtmCoor_f utm;
+ utm.east = gps.utm_pos.east / 100.;
+ utm.north = gps.utm_pos.north / 100.;
+ utm.zone = nav_utm_zone0;
+
+#if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A
+ float falt = gps.hmsl / 1000.;
+ EstimatorSetAlt(falt);
+#endif
+ utm.alt = estimator_z;
+ // set position
+ stateSetPositionUtm_f(&utm);
+
+ struct NedCoor_f ned_vel = {
+ gps.ned_vel.x / 100.,
+ gps.ned_vel.y / 100.,
+ gps.ned_vel.z / 100.
+ };
+ // set velocity
+ stateSetSpeedNed_f(&ned_vel);
+
+#endif
+}
+
+void ins_update_sonar() {
+}
bool_t alt_kalman_enabled;
@@ -132,7 +163,7 @@ void alt_kalman_init( void ) {
alt_kalman_reset();
}
-void alt_kalman(float gps_z) {
+void alt_kalman(float z_meas) {
float DT;
float R;
float SIGMA2;
@@ -182,7 +213,7 @@ void alt_kalman(float gps_z) {
if (fabs(e) > 1e-5) {
float k_0 = p[0][0] / e;
float k_1 = p[1][0] / e;
- e = gps_z - estimator_z;
+ e = z_meas - estimator_z;
/* correction */
estimator_z += k_0 * e;
@@ -201,24 +232,3 @@ void alt_kalman(float gps_z) {
#endif // ALT_KALMAN
-void estimator_update_state_gps( void ) {
- float gps_east = gps.utm_pos.east / 100.;
- float gps_north = gps.utm_pos.north / 100.;
-
- /* Relative position to reference */
- gps_east -= nav_utm_east0;
- gps_north -= nav_utm_north0;
-
- EstimatorSetPosXY(gps_east, gps_north);
-#if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A
- float falt = gps.hmsl / 1000.;
- EstimatorSetAlt(falt);
-#endif
- float fspeed = gps.gspeed / 100.;
- float fclimb = -gps.ned_vel.z / 100.;
- float fcourse = gps.course / 1e7;
- EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
-
- // Heading estimation now in ahrs_infrared
-
-}
diff --git a/sw/airborne/subsystems/ins/ins_float.h b/sw/airborne/subsystems/ins/ins_float.h
new file mode 100644
index 0000000000..d2cab0b407
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_float.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * 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 INS_FLOAT_H
+#define INS_FLOAT_H
+
+#include "subsystems/ins.h"
+
+#include
+#include "std.h"
+#include "state.h"
+
+#ifdef BARO_MS5534A
+#include "baro_MS5534A.h"
+#endif
+
+#if USE_BARO_ETS
+#include "modules/sensors/baro_ets.h"
+#endif
+
+#if USE_BARO_BMP
+#include "modules/sensors/baro_bmp.h"
+#endif
+
+
+/* position in meters, ENU frame, relative to reference */
+extern float estimator_z; ///< altitude above MSL in meters
+
+/* speed in meters per second */
+extern float estimator_z_dot;
+
+extern bool_t alt_kalman_enabled;
+#ifdef ALT_KALMAN
+extern void alt_kalman_reset( void );
+extern void alt_kalman_init( void );
+extern void alt_kalman( float );
+#endif
+
+#ifdef ALT_KALMAN
+
+#if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP
+/* Kalman filter cannot be disabled in this mode (no z_dot) */
+#define EstimatorSetAlt(z) alt_kalman(z)
+#else /* USE_BARO_x */
+#define EstimatorSetAlt(z) { \
+ if (!alt_kalman_enabled) { \
+ estimator_z = z; \
+ } else { \
+ alt_kalman(z); \
+ } \
+}
+#endif /* ! USE_BARO_x */
+
+#else /* ALT_KALMAN */
+#define EstimatorSetAlt(z) { estimator_z = z; }
+#endif
+
+#endif /* INS_FLOAT_H */
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
new file mode 100644
index 0000000000..ef8efb4bf6
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (C) 2008-2010 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.
+ */
+
+#include "subsystems/ins/ins_int.h"
+
+#include "subsystems/imu.h"
+#include "subsystems/sensors/baro.h"
+#include "subsystems/gps.h"
+
+#include "generated/airframe.h"
+
+#if USE_VFF
+#include "subsystems/ins/vf_float.h"
+#endif
+
+#if USE_HFF
+#include "subsystems/ins/hf_float.h"
+#endif
+
+#ifdef SITL
+#include "nps_fdm.h"
+#include
+#endif
+
+
+#include "math/pprz_geodetic_int.h"
+
+#include "generated/flight_plan.h"
+
+/* gps transformed to LTP-NED */
+struct LtpDef_i ins_ltp_def;
+ bool_t ins_ltp_initialised;
+struct NedCoor_i ins_gps_pos_cm_ned;
+struct NedCoor_i ins_gps_speed_cm_s_ned;
+#if USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+struct FloatVect2 ins_gps_pos_m_ned;
+struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+bool_t ins_hf_realign;
+
+/* barometer */
+#if USE_VFF
+int32_t ins_qfe;
+bool_t ins_baro_initialised;
+int32_t ins_baro_alt;
+#if USE_SONAR
+bool_t ins_update_on_agl;
+int32_t ins_sonar_offset;
+#endif
+#endif
+bool_t ins_vf_realign;
+
+/* output */
+struct NedCoor_i ins_ltp_pos;
+struct NedCoor_i ins_ltp_speed;
+struct NedCoor_i ins_ltp_accel;
+
+
+void ins_init() {
+#if USE_INS_NAV_INIT
+ ins_ltp_initialised = TRUE;
+
+ /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
+ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
+ llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+ llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
+ llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
+
+ struct EcefCoor_i ecef_nav0;
+ ecef_of_lla_i(&ecef_nav0, &llh_nav0);
+
+ ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
+ ins_ltp_def.hmsl = NAV_ALT0;
+ stateSetLocalOrigin_i(&ins_ltp_def);
+#else
+ ins_ltp_initialised = FALSE;
+#endif
+#if USE_VFF
+ ins_baro_initialised = FALSE;
+#if USE_SONAR
+ ins_update_on_agl = FALSE;
+#endif
+ vff_init(0., 0., 0.);
+#endif
+ ins_vf_realign = FALSE;
+ ins_hf_realign = FALSE;
+#if USE_HFF
+ b2_hff_init(0., 0., 0., 0.);
+#endif
+ INT32_VECT3_ZERO(ins_ltp_pos);
+ INT32_VECT3_ZERO(ins_ltp_speed);
+ INT32_VECT3_ZERO(ins_ltp_accel);
+
+ // TODO correct init
+ ins.status = INS_RUNNING;
+
+}
+
+void ins_periodic( void ) {
+}
+
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
+#if USE_HFF
+ b2_hff_realign(pos, speed);
+#endif /* USE_HFF */
+}
+
+void ins_realign_v(float z __attribute__ ((unused))) {
+#if USE_VFF
+ vff_realign(z);
+#endif
+}
+
+void ins_propagate() {
+ /* untilt accels */
+ struct Int32Vect3 accel_meas_body;
+ INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
+ struct Int32Vect3 accel_meas_ltp;
+ INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
+
+#if USE_VFF
+ float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
+ if (baro.status == BS_RUNNING && ins_baro_initialised) {
+ vff_propagate(z_accel_meas_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
+ }
+ else { // feed accel from the sensors
+ // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
+ ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
+ }
+#else
+ ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
+#endif /* USE_VFF */
+
+#if USE_HFF
+ /* propagate horizontal filter */
+ b2_hff_propagate();
+#else
+ ins_ltp_accel.x = accel_meas_ltp.x;
+ ins_ltp_accel.y = accel_meas_ltp.y;
+#endif /* USE_HFF */
+
+ INS_NED_TO_STATE();
+}
+
+void ins_update_baro() {
+#if USE_VFF
+ if (baro.status == BS_RUNNING) {
+ if (!ins_baro_initialised) {
+ ins_qfe = baro.absolute;
+ ins_baro_initialised = TRUE;
+ }
+ if (ins_vf_realign) {
+ ins_vf_realign = FALSE;
+ ins_qfe = baro.absolute;
+#if USE_SONAR
+ ins_sonar_offset = sonar_meas;
+#endif
+ vff_realign(0.);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
+ }
+ else { /* not realigning, so normal update with baro measurement */
+ ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
+ float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
+ vff_update(alt_float);
+ }
+ }
+ INS_NED_TO_STATE();
+#endif
+}
+
+
+void ins_update_gps(void) {
+#if USE_GPS
+ if (gps.fix == GPS_FIX_3D) {
+ if (!ins_ltp_initialised) {
+ ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
+ ins_ltp_def.lla.alt = gps.lla_pos.alt;
+ ins_ltp_def.hmsl = gps.hmsl;
+ ins_ltp_initialised = TRUE;
+ stateSetLocalOrigin_i(&ins_ltp_def);
+ }
+ ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
+ ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
+#if USE_HFF
+ VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
+ VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
+ VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
+ VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
+ if (ins_hf_realign) {
+ ins_hf_realign = FALSE;
+#ifdef SITL
+ struct FloatVect2 true_pos, true_speed;
+ VECT2_COPY(true_pos, fdm.ltpprz_pos);
+ VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
+ b2_hff_realign(true_pos, true_speed);
+#else
+ const struct FloatVect2 zero = {0.0, 0.0};
+ b2_hff_realign(ins_gps_pos_m_ned, zero);
+#endif
+ }
+ b2_hff_update_gps();
+#if !USE_VFF /* vff not used */
+ ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
+ ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
+#endif /* vff not used */
+#endif /* hff used */
+
+
+#if !USE_HFF /* hff not used */
+#if !USE_VFF /* neither hf nor vf used */
+ INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+ INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#else /* only vff used */
+ INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+ INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#endif
+
+#if USE_GPS_LAG_HACK
+ VECT2_COPY(d_pos, ins_ltp_speed);
+ INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
+ VECT2_ADD(ins_ltp_pos, d_pos);
+#endif
+#endif /* hff not used */
+
+ INS_NED_TO_STATE();
+ }
+#endif /* USE_GPS */
+}
+
+void ins_update_sonar() {
+#if USE_SONAR && USE_VFF
+ static int32_t sonar_filtered = 0;
+ sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
+ /* update baro_qfe assuming a flat ground */
+ if (ins_update_on_agl && baro.status == BS_RUNNING) {
+ int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
+ ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
+ }
+#endif
+}
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
new file mode 100644
index 0000000000..6699d45f16
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_int.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2008-2012 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.
+ */
+
+#ifndef INS_INT_H
+#define INS_INT_H
+
+#include "subsystems/ins.h"
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
+
+// TODO integrate all internal state to the structure
+///** Ins implementation state (fixed point) */
+//struct InsInt {
+//};
+//
+///** global INS state */
+//extern struct InsInt ins_impl;
+
+/* gps transformed to LTP-NED */
+extern struct LtpDef_i ins_ltp_def;
+extern bool_t ins_ltp_initialised;
+extern struct NedCoor_i ins_gps_pos_cm_ned;
+extern struct NedCoor_i ins_gps_speed_cm_s_ned;
+
+/* barometer */
+#if USE_VFF || USE_VFF_EXTENDED
+extern int32_t ins_baro_alt;
+extern int32_t ins_qfe;
+extern bool_t ins_baro_initialised;
+#if USE_SONAR
+extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
+extern int32_t ins_sonar_offset;
+#endif
+#endif
+
+/* output LTP NED */
+extern struct NedCoor_i ins_ltp_pos;
+extern struct NedCoor_i ins_ltp_speed;
+extern struct NedCoor_i ins_ltp_accel;
+#if USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+extern struct FloatVect2 ins_gps_pos_m_ned;
+extern struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+
+/* copy position and speed to state interface */
+#define INS_NED_TO_STATE() { \
+ stateSetPositionNed_i(&ins_ltp_pos); \
+ stateSetSpeedNed_i(&ins_ltp_speed); \
+ stateSetAccelNed_i(&ins_ltp_accel); \
+}
+
+#endif /* INS_INT_H */
diff --git a/sw/airborne/subsystems/ins_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c
similarity index 99%
rename from sw/airborne/subsystems/ins_extended.c
rename to sw/airborne/subsystems/ins/ins_int_extended.c
index f1d6e3976f..fcc97e4990 100644
--- a/sw/airborne/subsystems/ins_extended.c
+++ b/sw/airborne/subsystems/ins/ins_int_extended.c
@@ -20,7 +20,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "subsystems/ins.h"
+#include "subsystems/ins/ins_int.h"
#include "subsystems/imu.h"
#include "subsystems/sensors/baro.h"
diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c
index b13a17d342..11ae9c195d 100644
--- a/sw/airborne/subsystems/nav.c
+++ b/sw/airborne/subsystems/nav.c
@@ -32,7 +32,6 @@
#include "subsystems/nav.h"
#include "subsystems/gps.h"
-#include "estimator.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/autopilot.h"
#include "inter_mcu.h"
@@ -89,7 +88,8 @@ bool_t nav_survey_active;
int nav_mode;
void nav_init_stage( void ) {
- last_x = estimator_x; last_y = estimator_y;
+ last_x = stateGetPositionEnu_f()->x;
+ last_y = stateGetPositionEnu_f()->y;
stage_time = 0;
nav_circle_radians = 0;
nav_in_circle = FALSE;
@@ -106,8 +106,9 @@ void nav_init_stage( void ) {
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius) {
+ struct EnuCoor_f* pos = stateGetPositionEnu_f();
float last_trigo_qdr = nav_circle_trigo_qdr;
- nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x);
+ nav_circle_trigo_qdr = atan2(pos->y - y, pos->x - x);
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
@@ -115,7 +116,7 @@ void nav_circle_XY(float x, float y, float radius) {
nav_circle_radians += trigo_diff;
}
- float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y);
+ float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
float dist_carrot = CARROT*NOMINAL_AIRSPEED;
float sign_radius = radius > 0 ? 1 : -1;
@@ -128,7 +129,7 @@ void nav_circle_XY(float x, float y, float radius) {
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
- atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius));
+ atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI/4);
@@ -151,7 +152,7 @@ void nav_circle_XY(float x, float y, float radius) {
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
- float pre_climb = estimator_hspeed_mod * diff_alt / nav_leg_length; \
+ float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
@@ -202,7 +203,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height);
static void nav_ground_speed_loop( void ) {
if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
&& nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
- float err = nav_ground_speed_setpoint - estimator_hspeed_mod;
+ float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f());
v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err;
Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
} else {
@@ -257,10 +258,11 @@ static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td,
Computes Top Of Descent waypoint from Touch Down and Approach Fix
waypoints, using glide airspeed, glide vertical speed and wind */
static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) {
+ struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
- float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind_east*wind_east + wind_north*wind_north));
+ float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind->x*wind->x + wind->y*wind->y));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
@@ -290,7 +292,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
float y = ac->north - _distance*sa;
fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
- float s = (estimator_x-x)*ca+(estimator_y-y)*sa;
+ float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa;
nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
nav_ground_speed_loop();
#endif
@@ -311,12 +313,12 @@ float fp_pitch; /* deg */
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
- float pw_x = x - estimator_x;
+ float pw_x = x - stateGetPositionEnu_f()->x;
/** distance to waypoint in y */
- float pw_y = y - estimator_y;
+ float pw_y = y - stateGetPositionEnu_f()->y;
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
- float min_dist = approaching_time * estimator_hspeed_mod;
+ float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist)
return TRUE;
@@ -331,19 +333,21 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
*/
//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
+ struct EnuCoor_f* pos = stateGetPositionEnu_f();
desired_x = x;
desired_y = y;
if (nav_mode == NAV_MODE_COURSE) {
- h_ctl_course_setpoint = atan2(x - estimator_x, y - estimator_y);
+ h_ctl_course_setpoint = atan2(x - pos->x, y - pos->y);
if (h_ctl_course_setpoint < 0.)
h_ctl_course_setpoint += 2 * M_PI;
lateral_mode = LATERAL_MODE_COURSE;
} else {
- float diff = atan2(x - estimator_x, y - estimator_y) - estimator_hspeed_dir;
+ float diff = atan2(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(diff);
BoundAbs(diff,M_PI/2.);
float s = sin(diff);
- h_ctl_roll_setpoint = atan(2 * estimator_hspeed_mod*estimator_hspeed_mod * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
+ float speed = *stateGetHorizontalSpeedNorm_f();
+ h_ctl_roll_setpoint = atan(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
@@ -356,7 +360,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_x = wp_x - last_wp_x;
float leg_y = wp_y - last_wp_y;
float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
- nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2;
+ nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2;
nav_leg_length = sqrt(leg2);
/** distance of carrot (in meter) */
diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h
index 42ef5d2b96..e719594fa7 100644
--- a/sw/airborne/subsystems/nav.h
+++ b/sw/airborne/subsystems/nav.h
@@ -34,6 +34,7 @@
#include "std.h"
#include "paparazzi.h"
+#include "state.h"
#ifdef CTRL_TYPE_H
#include CTRL_TYPE_H
#endif
@@ -131,7 +132,7 @@ extern void nav_circle_XY(float x, float y, float radius);
/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
#define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
-#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(estimator_hspeed_dir))
+#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f()))
/*********** Navigation along a line *************************************/
extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
@@ -193,4 +194,9 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
#define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }
#define NavKillThrottle() { kill_throttle = 1; }
+
+#define GetPosX() (stateGetPositionUtm_f()->north)
+#define GetPosY() (stateGetPositionUtm_f()->east)
+#define GetPosAlt() (stateGetPositionUtm_f()->alt)
+
#endif /* NAV_H */
diff --git a/sw/airborne/subsystems/navigation/OSAMNav.c b/sw/airborne/subsystems/navigation/OSAMNav.c
index 11a2f27a5c..abf28882d0 100644
--- a/sw/airborne/subsystems/navigation/OSAMNav.c
+++ b/sw/airborne/subsystems/navigation/OSAMNav.c
@@ -1,7 +1,7 @@
#include "subsystems/navigation/OSAMNav.h"
#include "subsystems/nav.h"
-#include "estimator.h"
+#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
@@ -47,15 +47,15 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP)
Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
- TransCurrentX = estimator_x - WaypointX(Center);
- TransCurrentY = estimator_y - WaypointY(Center);
+ TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
+ TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center);
- FlyFromX = estimator_x;
- FlyFromY = estimator_y;
+ FlyFromX = stateGetPositionEnu_f()->x;
+ FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Flowerradius)
CFlowerStatus = Outside;
@@ -69,8 +69,8 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP)
bool_t FlowerNav(void)
{
- TransCurrentX = estimator_x - WaypointX(Center);
- TransCurrentY = estimator_y - WaypointY(Center);
+ TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
+ TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE;
@@ -92,8 +92,8 @@ bool_t FlowerNav(void)
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center);
- FlyFromX = estimator_x;
- FlyFromY = estimator_y;
+ FlyFromX = stateGetPositionEnu_f()->x;
+ FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
@@ -122,8 +122,8 @@ bool_t FlowerNav(void)
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center);
- FlyFromX = estimator_x;
- FlyFromY = estimator_y;
+ FlyFromX = stateGetPositionEnu_f()->x;
+ FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
@@ -180,8 +180,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP)
{
float ThrottleB;
- initialx = estimator_x;
- initialy = estimator_y;
+ initialx = stateGetPositionEnu_f()->x;
+ initialy = stateGetPositionEnu_f()->y;
BungeeWaypoint = BungeeWP;
@@ -233,8 +233,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP)
bool_t BungeeTakeoff(void)
{
//Translate current position so Throttle point is (0,0)
- float Currentx = estimator_x-throttlePx;
- float Currenty = estimator_y-throttlePy;
+ float Currentx = stateGetPositionEnu_f()->x-throttlePx;
+ float Currenty = stateGetPositionEnu_f()->y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
@@ -249,10 +249,10 @@ bool_t BungeeTakeoff(void)
kill_throttle = 1;
//recalculate lines if below min speed
- if(estimator_hspeed_mod < Takeoff_MinSpeed)
+ if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed)
{
- initialx = estimator_x;
- initialy = estimator_y;
+ initialx = stateGetPositionEnu_f()->x;
+ initialy = stateGetPositionEnu_f()->y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(WaypointX(BungeeWaypoint));
@@ -294,7 +294,7 @@ bool_t BungeeTakeoff(void)
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
- if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed)
+ if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
@@ -308,7 +308,7 @@ bool_t BungeeTakeoff(void)
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
- if((estimator_z > BungeeAlt+Takeoff_Height-10) && (estimator_hspeed_mod > Takeoff_Speed))
+ if((stateGetPositionEnu_f()->z > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
@@ -573,7 +573,7 @@ bool_t PolygonSurvey(void)
//follow the circle
nav_circle_XY(C.x, C.y, SurveyRadius);
- if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && estimator_z > waypoints[SurveyEntryWP].a-10)
+ if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && stateGetPositionEnu_f()->z > waypoints[SurveyEntryWP].a-10)
{
CSurveyStatus = Sweep;
nav_init_stage();
@@ -806,7 +806,7 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
- if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) {
+ if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-10)) {
line_status = LQC22;
nav_init_stage();
}
@@ -835,7 +835,7 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
- if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) {
+ if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-5)) {
line_status = LQC11;
nav_init_stage();
}
@@ -893,7 +893,7 @@ bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius)
TDWaypoint = TDWP;
CLandingStatus = CircleDown;
LandRadius = radius;
- LandAppAlt = estimator_z;
+ LandAppAlt = stateGetPositionEnu_f()->z;
FinalLandAltitude = Landing_FinalHeight;
FinalLandCount = 1;
waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight;
@@ -942,7 +942,7 @@ bool_t SkidLanding(void)
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
- if(estimator_z < waypoints[AFWaypoint].a + 5)
+ if(stateGetPositionEnu_f()->z < waypoints[AFWaypoint].a + 5)
{
CLandingStatus = LandingWait;
nav_init_stage();
@@ -1017,8 +1017,8 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo
V.y = WaypointY(To_WP) - WaypointY(From_WP);
//Record Aircraft Position
- P.x = estimator_x;
- P.y = estimator_y;
+ P.x = stateGetPositionEnu_f()->x;
+ P.y = stateGetPositionEnu_f()->y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP));
diff --git a/sw/airborne/subsystems/navigation/bomb.c b/sw/airborne/subsystems/navigation/bomb.c
index 733109994c..4622f3767e 100644
--- a/sw/airborne/subsystems/navigation/bomb.c
+++ b/sw/airborne/subsystems/navigation/bomb.c
@@ -22,7 +22,7 @@
*
*/
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "subsystems/navigation/bomb.h"
#include "generated/flight_plan.h"
@@ -57,9 +57,9 @@ static void integrate( uint8_t wp_target ) {
/* Inspired from Arnold Schroeter's code */
int i = 0;
while (bomb_z > 0. && i < MAX_STEPS) {
- /* relative wind experienced by the ball */
- float airx = -bomb_vx + wind_east;
- float airy = -bomb_vy + wind_north;
+ /* relative wind experienced by the ball (wind in NED frame) */
+ float airx = -bomb_vx + stateGetHorizontalWindspeed_f()->y;
+ float airy = -bomb_vy + stateGetHorizontalWindspeed_f()->x;
float airz = -bomb_vz;
/* alpha / m * air */
@@ -93,12 +93,12 @@ static void integrate( uint8_t wp_target ) {
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t bomb_update_release( uint8_t wp_target ) {
- bomb_z = estimator_z - waypoints[wp_target].a;
+ bomb_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a;
bomb_x = 0.;
bomb_y = 0.;
- bomb_vx = estimator_hspeed_mod * sin(estimator_hspeed_dir);
- bomb_vy = estimator_hspeed_mod * cos(estimator_hspeed_dir);
+ bomb_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
+ bomb_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
bomb_vz = 0.;
integrate(wp_target);
@@ -132,8 +132,9 @@ unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_ra
if (bomb_radius < 0)
bomb_start_qdr += M_PI;
- bomb_vx = x1 * airspeed + wind_east;
- bomb_vy = y_1 * airspeed + wind_north;
+ // wind in NED frame
+ bomb_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
+ bomb_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
bomb_vz = 0.;
float vx0 = bomb_vx;
diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c
index c2609a9a1c..34127ee69b 100644
--- a/sw/airborne/subsystems/navigation/common_nav.c
+++ b/sw/airborne/subsystems/navigation/common_nav.c
@@ -23,7 +23,6 @@
*/
#include "subsystems/navigation/common_nav.h"
-#include "estimator.h"
#include "generated/flight_plan.h"
#include "subsystems/gps.h"
#include "math/pprz_geodetic_float.h"
@@ -47,12 +46,13 @@ float max_dist_from_home = MAX_DIST_FROM_HOME;
* \a too_far_from_home
*/
void compute_dist2_to_home(void) {
- float ph_x = waypoints[WP_HOME].x - estimator_x;
- float ph_y = waypoints[WP_HOME].y - estimator_y;
+ struct EnuCoor_f* pos = stateGetPositionEnu_f();
+ float ph_x = waypoints[WP_HOME].x - pos->x;
+ float ph_y = waypoints[WP_HOME].y - pos->y;
dist2_to_home = ph_x*ph_x + ph_y *ph_y;
too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME);
#if defined InAirspace
- too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y));
+ too_far_from_home = too_far_from_home || !(InAirspace(pos_x, pos_y));
#endif
}
@@ -80,6 +80,10 @@ unit_t nav_reset_reference( void ) {
nav_utm_north0 = gps.utm_pos.north/100;
#endif
+ // reset state UTM ref
+ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
+ stateSetLocalUtmOrigin_f(&utm0);
+
previous_ground_alt = ground_alt;
ground_alt = gps.hmsl/1000.;
return 0;
diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h
index d97e5c27bb..25bf528ba0 100644
--- a/sw/airborne/subsystems/navigation/common_nav.h
+++ b/sw/airborne/subsystems/navigation/common_nav.h
@@ -26,6 +26,7 @@
#define COMMON_NAV_H
#include "std.h"
+#include "state.h"
#include "subsystems/navigation/common_flight_plan.h"
extern float max_dist_from_home;
@@ -66,8 +67,8 @@ void common_nav_periodic_task_4Hz(void);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
#define NavSetWaypointHere(_wp) ({ \
- waypoints[_wp].x = estimator_x; \
- waypoints[_wp].y = estimator_y; \
+ waypoints[_wp].x = stateGetPositionEnu_f()->x; \
+ waypoints[_wp].y = stateGetPositionEnu_f()->y; \
FALSE; \
})
diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c
index 1a56f3b8f2..309451fa5c 100644
--- a/sw/airborne/subsystems/navigation/discsurvey.c
+++ b/sw/airborne/subsystems/navigation/discsurvey.c
@@ -1,7 +1,7 @@
#include "subsystems/navigation/discsurvey.h"
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "std.h"
#include "subsystems/nav.h"
#include "generated/flight_plan.h"
@@ -18,13 +18,14 @@ bool_t disc_survey_init( float grid ) {
nav_survey_shift = grid;
status = DOWNWIND;
sign = 1;
- c1.x = estimator_x;
- c1.y = estimator_y;
+ c1.x = stateGetPositionEnu_f()->x;
+ c1.y = stateGetPositionEnu_f()->y;
return FALSE;
}
bool_t disc_survey( uint8_t center, float radius) {
- float wind_dir = atan2(wind_north, wind_east) + M_PI;
+ struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+ float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
@@ -36,10 +37,10 @@ bool_t disc_survey( uint8_t center, float radius) {
case UTURN:
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
- c1.x = estimator_x;
- c1.y = estimator_y;
+ c1.x = stateGetPositionEnu_f()->x;
+ c1.y = stateGetPositionEnu_f()->y;
- float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
+ float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
diff --git a/sw/airborne/subsystems/navigation/gls.c b/sw/airborne/subsystems/navigation/gls.c
index 8f22c9548a..b7e7806d58 100644
--- a/sw/airborne/subsystems/navigation/gls.c
+++ b/sw/airborne/subsystems/navigation/gls.c
@@ -45,7 +45,7 @@
#include "generated/airframe.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/navigation/gls.h"
#include "subsystems/nav.h"
#include "generated/flight_plan.h"
@@ -101,7 +101,8 @@ bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) {
init = TRUE;
#if USE_AIRSPEED
- //float wind_additional = sqrt(wind_east*wind_east + wind_north*wind_north); // should be gusts only!
+ //struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+ //float wind_additional = sqrt(wind->x*wind->x + wind->y*wind->y); // should be gusts only!
//Bound(wind_additional, 0, 0.5);
//target_speed = FL_ENVE_V_S * 1.3 + wind_additional; FIXME
target_speed = APP_TARGET_SPEED; // ok for now!
@@ -137,11 +138,11 @@ bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
- float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
+ float nav_final_progress = ((stateGetPositionEnu_f()->x - WaypointX(_tod)) * final_x + (stateGetPositionEnu_f()->y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress,-1,1);
float nav_final_length = sqrt(final2);
- float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
+ float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / (*stateGetHorizontalSpeedNorm_f()));
Bound(pre_climb, -5, 0.);
float start_alt = WaypointAlt(_tod);
diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c
index 6779748ce7..8698f6d937 100644
--- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c
+++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c
@@ -1,5 +1,5 @@
#include "subsystems/navigation/nav_survey_rectangle.h"
-#include "estimator.h"
+#include "state.h"
static struct point survey_from;
static struct point survey_to;
@@ -29,8 +29,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
survey_orientation = so;
if (survey_orientation == NS) {
- survey_from.x = survey_to.x = Min(Max(estimator_x, nav_survey_west+grid/2.), nav_survey_east-grid/2.);
- if (estimator_y > nav_survey_north || (estimator_y > nav_survey_south && estimator_hspeed_dir > M_PI/2. && estimator_hspeed_dir < 3*M_PI/2)) {
+ survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west+grid/2.), nav_survey_east-grid/2.);
+ if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south && (*stateGetHorizontalSpeedDir_f()) > M_PI/2. && (*stateGetHorizontalSpeedDir_f()) < 3*M_PI/2)) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else {
@@ -38,8 +38,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
survey_to.y = nav_survey_north;
}
} else { /* survey_orientation == WE */
- survey_from.y = survey_to.y = Min(Max(estimator_y, nav_survey_south+grid/2.), nav_survey_north-grid/2.);
- if (estimator_x > nav_survey_east || (estimator_x > nav_survey_west && estimator_hspeed_dir > M_PI)) {
+ survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south+grid/2.), nav_survey_north-grid/2.);
+ if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west && (*stateGetHorizontalSpeedDir_f()) > M_PI)) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
} else {
@@ -79,10 +79,10 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
}
if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
- if ((estimator_y < nav_survey_north && SurveyGoingNorth()) ||
- (estimator_y > nav_survey_south && SurveyGoingSouth()) ||
- (estimator_x < nav_survey_east && SurveyGoingEast()) ||
- (estimator_x > nav_survey_west && SurveyGoingWest())) {
+ if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
+ (stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
+ (stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
+ (stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
/* Continue ... */
nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y);
} else {
diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c
index 73f6e74852..de25066e87 100644
--- a/sw/airborne/subsystems/navigation/poly_survey_adv.c
+++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c
@@ -1,7 +1,7 @@
#include "poly_survey_adv.h"
#include "subsystems/nav.h"
-#include "estimator.h"
+#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
@@ -276,7 +276,7 @@ bool_t poly_survey_adv(void)
nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
if (NavCourseCloseTo(segment_angle)
&& nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
- && fabs(estimator_z - psa_altitude) <= 20) {
+ && fabs(stateGetPositionEnu_f()->z - psa_altitude) <= 20) {
psa_stage = SEG;
NavVerticalAutoThrottleMode(0.0);
nav_init_stage();
diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c
index b4a659a0e7..10e4903188 100644
--- a/sw/airborne/subsystems/navigation/snav.c
+++ b/sw/airborne/subsystems/navigation/snav.c
@@ -4,7 +4,7 @@
#include
#include "generated/airframe.h"
#include "subsystems/navigation/snav.h"
-#include "estimator.h"
+#include "state.h"
#include "subsystems/nav.h"
#include "subsystems/gps.h"
@@ -25,15 +25,15 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) {
wp_a = a;
radius = fabs(radius);
- float da_x = WaypointX(wp_a) - estimator_x;
- float da_y = WaypointY(wp_a) - estimator_y;
+ float da_x = WaypointX(wp_a) - stateGetPositionEnu_f()->x;
+ float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y;
/* D_CD orthogonal to current course, CD on the side of A */
- float u_x = cos(M_PI_2 - estimator_hspeed_dir);
- float u_y = sin(M_PI_2 - estimator_hspeed_dir);
+ float u_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
+ float u_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
d_radius = - Sign(u_x*da_y - u_y*da_x) * radius;
- wp_cd.x = estimator_x + d_radius * u_y;
- wp_cd.y = estimator_y - d_radius * u_x;
+ wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y;
+ wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x;
wp_cd.a = WaypointAlt(wp_a);
/* A_CA orthogonal to desired course, CA on the side of D */
@@ -148,12 +148,12 @@ static void compute_ground_speed(float airspeed,
bool_t snav_on_time(float nominal_radius) {
nominal_radius = fabs(nominal_radius);
- float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x);
+ float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y-wp_ca.y, stateGetPositionEnu_f()->x-wp_ca.x);
float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr));
float remaining_time = snav_desired_tow - gps.tow / 1000.;
/* Use the nominal airspeed if the estimated one is not realistic */
- float airspeed = estimator_airspeed;
+ float airspeed = *stateGetAirspeed_f();
if (airspeed < NOMINAL_AIRSPEED / 2. ||
airspeed > 2.* NOMINAL_AIRSPEED)
airspeed = NOMINAL_AIRSPEED;
@@ -161,7 +161,7 @@ bool_t snav_on_time(float nominal_radius) {
/* Recompute ground speeds every 10 s */
if (ground_speed_timer == 0) {
ground_speed_timer = 40; /* every 10s, called at 40Hz */
- compute_ground_speed(airspeed, wind_east, wind_north);
+ compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
}
ground_speed_timer--;
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index 7d47c57d41..d08cb2576d 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/sw/airborne/subsystems/navigation/spiral.c
@@ -9,7 +9,7 @@
#include "subsystems/navigation/spiral.h"
#include "subsystems/nav.h"
-#include "estimator.h"
+#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
@@ -64,9 +64,9 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float
Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
- TransCurrentX = estimator_x - WaypointX(Center);
- TransCurrentY = estimator_y - WaypointY(Center);
- TransCurrentZ = estimator_z - ZPoint;
+ TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
+ TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
+ TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
@@ -76,8 +76,8 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float
// Alphalimit denotes angle, where the radius will be increased
Alphalimit = 2*M_PI / Segments;
//current position
- FlyFromX = estimator_x;
- FlyFromY = estimator_y;
+ FlyFromX = stateGetPositionEnu_f()->x;
+ FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Spiralradius)
CSpiralStatus = Outside;
@@ -86,8 +86,8 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float
bool_t SpiralNav(void)
{
- TransCurrentX = estimator_x - WaypointX(Center);
- TransCurrentY = estimator_y - WaypointY(Center);
+ TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
+ TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
float DistanceStartEstim;
@@ -113,8 +113,8 @@ bool_t SpiralNav(void)
// calculation needed, State switch to Circle
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
if(DistanceFromCenter >= SRad){
- LastCircleX = estimator_x;
- LastCircleY = estimator_y;
+ LastCircleX = stateGetPositionEnu_f()->x;
+ LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = Circle;
// Start helix
#ifdef DIGITAL_CAM
@@ -130,12 +130,12 @@ bool_t SpiralNav(void)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
- DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
- + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y)));
+ DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
+ + ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
- LastCircleX = estimator_x;
- LastCircleY = estimator_y;
+ LastCircleX = stateGetPositionEnu_f()->x;
+ LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = IncSpiral;
}
break;
@@ -148,7 +148,7 @@ bool_t SpiralNav(void)
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
- TransCurrentZ = estimator_z - ZPoint;
+ TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
}
#endif
diff --git a/sw/airborne/subsystems/sensors/infrared_i2c.c b/sw/airborne/subsystems/sensors/infrared_i2c.c
index 2067fd0a2e..206816cf56 100644
--- a/sw/airborne/subsystems/sensors/infrared_i2c.c
+++ b/sw/airborne/subsystems/sensors/infrared_i2c.c
@@ -21,7 +21,6 @@
*/
#include "sensors/infrared_i2c.h"
-#include "estimator.h"
// IR I2C definitions
#define IR_HOR_I2C_ADDR (0x6C << 1)
diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3
index a2c5b6391d..7011d47c70 160000
--- a/sw/ext/libopencm3
+++ b/sw/ext/libopencm3
@@ -1 +1 @@
-Subproject commit a2c5b6391d82684838e923c6d7437367110f5480
+Subproject commit 7011d47c70076f995e8470c59eeaa8f3efcc9f05
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 42dddbcfd4..f089089b23 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -697,7 +697,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
connect "kill_throttle" ac.strip#connect_kill;
connect "nav_shift" ac.strip#connect_shift_lateral;
connect "pprz_mode" ac.strip#connect_mode;
- connect "estimator_flight_time" ac.strip#connect_flight_time;
+ connect "autopilot_flight_time" ac.strip#connect_flight_time;
let get_ac_unix_time = fun () -> ac.last_unix_time in
connect ~warning:false "snav_desired_tow" (ac.strip#connect_apt get_ac_unix_time);
begin (* Periodically update the appointment *)
diff --git a/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c b/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c
index 7bb3f9dc55..af8faf2717 100644
--- a/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c
+++ b/sw/ground_segment/tmtc/GSM/SMS_Ground_UDtest_final.c
@@ -99,7 +99,7 @@ pile *MaPile = NULL;
typedef enum {INIT, AT, CMGF, SMSMODE, CNMI, CPMS, ERREUR} Etat_Liste;
// Informations extraites du SMS recu de l'avion
-char extr_gps_utm_east[15], extr_gps_utm_north[15], extr_gps_course[15], extr_gps_alt[15], extr_gps_gspeed[15], extr_gps_climb[15], extr_vsupply[15], extr_estimator_flight_time[15], extr_qualite_signal_GSM[10];
+char extr_gps_utm_east[15], extr_gps_utm_north[15], extr_gps_course[15], extr_gps_alt[15], extr_gps_gspeed[15], extr_gps_climb[15], extr_vsupply[15], extr_autopilot_flight_time[15], extr_qualite_signal_GSM[10];
char reponse_attendue[20];
int prompt_recu = 0;
@@ -472,10 +472,10 @@ void decoupage( char message_complet[])
Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_gps_gspeed);
Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_gps_climb);
Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_vsupply);
- Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_estimator_flight_time);
+ Extraction(data_to_cut, ' ', 1, 1, ' ', 1, 0, extr_autopilot_flight_time);
Extraction(data_to_cut, ' ', 1, 1, '\r', 1, 0, extr_qualite_signal_GSM);
- printf("Message :\n utm_east %s\n utm_north %s\n course %s\n alt %s\n speed %s\n climb %s\n bat %s\n flight_time %s\n signal %s\n", extr_gps_utm_east, extr_gps_utm_north, extr_gps_course, extr_gps_alt, extr_gps_gspeed, extr_gps_climb, extr_vsupply, extr_estimator_flight_time, extr_qualite_signal_GSM);
+ printf("Message :\n utm_east %s\n utm_north %s\n course %s\n alt %s\n speed %s\n climb %s\n bat %s\n flight_time %s\n signal %s\n", extr_gps_utm_east, extr_gps_utm_north, extr_gps_course, extr_gps_alt, extr_gps_gspeed, extr_gps_climb, extr_vsupply, extr_autopilot_flight_time, extr_qualite_signal_GSM);
fflush(stdout);
}
diff --git a/sw/in_progress/satcom/tcp2ivy.c b/sw/in_progress/satcom/tcp2ivy.c
index e6e65712c1..a569130387 100644
--- a/sw/in_progress/satcom/tcp2ivy.c
+++ b/sw/in_progress/satcom/tcp2ivy.c
@@ -75,7 +75,7 @@ unsigned char electrical_vsupply;
unsigned char nav_block;
unsigned char energy;
unsigned char throttle;
-unsigned short estimator_flight_time;
+unsigned short autopilot_flight_time;
unsigned char nav_utm_zone0;
float latlong_utm_x, latlong_utm_y;
unsigned char pprz_mode;
@@ -196,8 +196,8 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
pprz_mode = buf[19];
// com_trans.buf[20] = nav_block;
nav_block = buf[20];
-// FillBufWith16bit(com_trans.buf, 21, estimator_flight_time);
- estimator_flight_time = buf2ushort(&buf[21]);
+// FillBufWith16bit(com_trans.buf, 21, autopilot_flight_time);
+ autopilot_flight_time = buf2ushort(&buf[21]);
//gps_lat = 52.2648312 * 1e7;
//gps_lon = 9.9939456 * 1e7;
@@ -211,7 +211,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
//throttle = 51;
//pprz_mode = 2;
//nav_block = 1;
-//estimator_flight_time = 123;
+//autopilot_flight_time = 123;
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
@@ -230,7 +230,7 @@ printf("energy %d\n", energy);
printf("throttle %d\n", throttle);
printf("pprz_mode %d\n", pprz_mode);
printf("nav_block %d\n", nav_block);
-printf("estimator_flight_time %d\n", estimator_flight_time);
+printf("autopilot_flight_time %d\n", autopilot_flight_time);
printf("gps_utm_east %d\n", gps_utm_east);
printf("gps_utm_north %d\n", gps_utm_north);
@@ -316,7 +316,7 @@ printf("gps_utm_zone %d\n", gps_utm_zone);
throttle * MAX_PPRZ / 100,
electrical_vsupply,
0, // amps
- estimator_flight_time,
+ autopilot_flight_time,
0, // kill_auto_throttle
0, // block_time
0, // stage_time
diff --git a/sw/in_progress/satcom/tcp2ivy_generic.c b/sw/in_progress/satcom/tcp2ivy_generic.c
index e40ce3665f..caa9f88138 100644
--- a/sw/in_progress/satcom/tcp2ivy_generic.c
+++ b/sw/in_progress/satcom/tcp2ivy_generic.c
@@ -78,7 +78,7 @@ unsigned char electrical_vsupply;
unsigned char nav_block;
unsigned short energy;
unsigned char throttle;
-unsigned short estimator_flight_time;
+unsigned short autopilot_flight_time;
unsigned char nav_utm_zone0;
float latlong_utm_x, latlong_utm_y;
unsigned char pprz_mode;
@@ -133,8 +133,8 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
pprz_mode = buf[19];
// com_trans.buf[21] = nav_block;
nav_block = buf[20];
- // FillBufWith16bit(com_trans.buf, 22, estimator_flight_time);
- estimator_flight_time = buf2ushort(&buf[21]);
+ // FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
+ autopilot_flight_time = buf2ushort(&buf[21]);
#if 0
gps_lat = 52.2648312 * 1e7;
@@ -148,7 +148,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
throttle = 51;
pprz_mode = 2;
nav_block = 1;
- estimator_flight_time = 123;
+ autopilot_flight_time = 123;
#endif
printf("**** message received from iridium module ****\n");
@@ -163,7 +163,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
printf("throttle %d\n", throttle);
printf("pprz_mode %d\n", pprz_mode);
printf("nav_block %d\n", nav_block);
- printf("estimator_flight_time %d\n", estimator_flight_time);
+ printf("autopilot_flight_time %d\n", autopilot_flight_time);
fflush(stdout);
IvySendMsg("%d GENERIC_COM %d %d %d %d %d %d %d %d %d %d %d %d",
@@ -179,7 +179,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
throttle,
pprz_mode,
nav_block,
- estimator_flight_time);
+ autopilot_flight_time);
}
else {
diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml
index e553008ba1..fd9d5ad2c0 100644
--- a/sw/lib/ocaml/expr_syntax.ml
+++ b/sw/lib/ocaml/expr_syntax.ml
@@ -72,7 +72,7 @@ let functions = [
let variables = [
"launch";
"estimator_z";
- "estimator_flight_time";
+ "autopilot_flight_time";
"estimator_hspeed_mod";
"estimator_theta";
"circle_count";
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 3796fddaf7..08d5d19dd2 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -30,6 +30,7 @@
#include "baro_board.h"
#include "subsystems/electrical.h"
#include "mcu_periph/sys_time.h"
+#include "state.h"
#include "actuators/supervision.h"
@@ -109,12 +110,12 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
#include "math/pprz_algebra.h"
void sim_overwrite_ahrs(void) {
- EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);
+ struct Int32Quat quat;
+ QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat);
+ stateSetNedToBodyQuat_i(&quat);
- QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);
-
- RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);
-
- INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);
+ struct Int32Rates rates;
+ RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel);
+ stateSetBodyRates_i(&rates);
}