Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 12:23:23 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #1130 from paparazzi/refactor_estimators

Browse Source 
Continue refactoring of AHRS and INS:
- add GEO_MAG ABI message
- add GPS ABI message
- remove explicit calling of update_gps functions (replaced by GPS ABI callbacks)
- each ins has it's own unique struct/function names
- send STATE_FILTER_STATUS from each implementation (with added id if you run multiple ones)
- use stateIsAttitudeValid() instead of AHRS.is_aligned
  - The state interface is initialized with invalid attitude.
    It becomes valid as soon as an attitude is set (via one of the stateSetNedToBodyX functions).
    This should be only done by an AHRS/INS after it is aligned.
- add possibility to run multiple (currently two) AHRS implementations and switch which one should push the output to the state interface during runtime

`ahrs_init()` and `ins_init()` are now sort of "dispatcher" functions, that init/register the actually used implementation.

As an example using `float_mlkf` as the `PRIMARY_AHRS` and `int_cmpl_quat` as `SECONDARY_AHRS`, in your firmware section of the airframe file:
```
  <subsystem name="ahrs"          type="float_mlkf"/>
  <subsystem name="ahrs"          type="int_cmpl_quat">
    <configure name="SECONDARY_AHRS" value="int_cmpl_quat"/>
  </subsystem>
```

In `ahrs_init()` it calls the ahrs_x_register functions for both. Each of those calls their own ahrs_foo_init and then `ahrs_register_impl(enable_output_function)`, then do the ABI binding and register periodic telemetry functions.

The "enable_output_function" pointer is used to keep a reference to the implementation functions to switch on/off publishing of the output to the state interface.
Hence you can call `ahrs_switch(idx)` at runtime to switch the output of the AHRS implementations.
E.g. with idx=1 switch to the output of the secondary AHRS (with 0 being the primary)
Add the `conf/settings/estimators/ahrs_secondary.xml` settings file to switch them via settings.
This commit is contained in:
Felix Ruess
2015-03-24 14:19:03 +01:00
parent 90f54ecbbc fc383fd34a
commit b95a1aff03
77 changed files with 1737 additions and 1024 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+9 -1
View File
@@ -46,7 +46,15 @@
<field name="q_b2i_f" type="struct FloatQuat *"/> <field name="q_b2i_f" type="struct FloatQuat *"/>
</message> </message>
<message name="GEO_MAG" id="9">
<field name="h" type="struct FloatVect3 *" unit="1.0"/>
</message>
<message name="GPS" id="10">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gps_s" type="struct GpsState *"/>
</message>
</msg_class> </msg_class>
</protocol> </protocol>
conf/airframes/bebop.xml
+5 -1
View File
@@ -21,11 +21,15 @@
<subsystem name="imu" type="bebop"/> <subsystem name="imu" type="bebop"/>
<subsystem name="gps" type="furuno"/> <subsystem name="gps" type="furuno"/>
<subsystem name="stabilization" type="int_quat"/> <subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/> <subsystem name="ahrs" type="float_mlkf"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="SECONDARY_AHRS" value="int_cmpl_quat"/>
</subsystem>
<subsystem name="ins" type="extended"/> <subsystem name="ins" type="extended"/>
</firmware> </firmware>
<modules main_freq="512"> <modules main_freq="512">
<load name="geo_mag.xml"/>
<load name="send_imu_mag_current.xml"/> <load name="send_imu_mag_current.xml"/>
<!--load name="logger_file.xml"> <!--load name="logger_file.xml">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/> <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
conf/conf_example.xml
+1 -1
View File
@@ -226,7 +226,7 @@
radio="radios/cockpitSX.xml" radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml"
settings_modules="" settings_modules=""
gui_color="red" gui_color="red"
/> />
conf/conf_tests.xml
+1 -1
View File
@@ -358,7 +358,7 @@
radio="radios/cockpitSX.xml" radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml"
settings_modules="" settings_modules=""
gui_color="red" gui_color="red"
/> />
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 0 USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 0 USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile
+3 -4
View File
@@ -2,15 +2,14 @@
# attitude and speed estimation for fixedwings via invariant filter # attitude and speed estimation for fixedwings via invariant filter
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
INS_CFLAGS += -DINS_UPDATE_FW_ESTIMATOR INS_CFLAGS += -DINS_FINV_USE_UTM
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile
+1
View File
@@ -1,5 +1,6 @@
# Hey Emacs, this is a -*- makefile -*- # Hey Emacs, this is a -*- makefile -*-
ins_CFLAGS = -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
ins_srcs += $(SRC_SUBSYSTEMS)/ins.c ins_srcs += $(SRC_SUBSYSTEMS)/ins.c
ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+2 -1
View File
@@ -58,7 +58,8 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c $(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG $(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile
+3 -2
View File
@@ -46,13 +46,14 @@ SIM_TARGETS = sim nps
ifneq (,$(findstring $(TARGET),$(SIM_TARGETS))) ifneq (,$(findstring $(TARGET),$(SIM_TARGETS)))
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR $(TARGET).CFLAGS += -DUSE_AHRS
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c $(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG $(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile
+1
View File
@@ -1,5 +1,6 @@
# Hey Emacs, this is a -*- makefile -*- # Hey Emacs, this is a -*- makefile -*-
ins_CFLAGS = -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough.h\"
ins_srcs += $(SRC_SUBSYSTEMS)/ins.c ins_srcs += $(SRC_SUBSYSTEMS)/ins.c
ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c
conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
+30 -16
View File
@@ -8,29 +8,43 @@
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_FC_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
endif endif
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" ifdef SECONDARY_AHRS
AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
AHRS_SRCS += subsystems/ahrs.c # this is the secondary AHRS
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c else
# this is the primary AHRS
AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_FC_CFLAGS += -DPRIMARY_AHRS=ahrs_fc
endif
else
# plain old single AHRS usage
AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
endif
ap.CFLAGS += $(AHRS_CFLAGS) AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_QUAT
ap.srcs += $(AHRS_SRCS) AHRS_FC_SRCS += subsystems/ahrs.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_aligner.c
nps.CFLAGS += $(AHRS_CFLAGS) ap.CFLAGS += $(AHRS_FC_CFLAGS)
nps.srcs += $(AHRS_SRCS) ap.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS) nps.CFLAGS += $(AHRS_FC_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS) nps.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_FC_CFLAGS)
test_ahrs.srcs += $(AHRS_FC_SRCS)
conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
+30 -16
View File
@@ -7,29 +7,43 @@
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_FC_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
endif endif
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" ifdef SECONDARY_AHRS
AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT ifneq (,$(findstring $(SECONDARY_AHRS), fcr float_cmpl_rmat))
AHRS_SRCS += subsystems/ahrs.c # this is the secondary AHRS
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c else
# this is the primary AHRS
AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_FC_CFLAGS += -DPRIMARY_AHRS=ahrs_fc
endif
else
# plain old single AHRS usage
AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
endif
ap.CFLAGS += $(AHRS_CFLAGS) AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_RMAT
ap.srcs += $(AHRS_SRCS) AHRS_FC_SRCS += subsystems/ahrs.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_FC_SRCS += subsystems/ahrs/ahrs_aligner.c
nps.CFLAGS += $(AHRS_CFLAGS) ap.CFLAGS += $(AHRS_FC_CFLAGS)
nps.srcs += $(AHRS_SRCS) ap.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS) nps.CFLAGS += $(AHRS_FC_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS) nps.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_FC_CFLAGS)
test_ahrs.srcs += $(AHRS_FC_SRCS)
conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
+29 -15
View File
@@ -5,30 +5,44 @@
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_MLKF_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_MLKF_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER AHRS_MLKF_CFLAGS += -DUSE_MAGNETOMETER
else else
$(error ahrs_float_mlkf needs a magnetometer) $(error ahrs_float_mlkf needs a magnetometer)
endif endif
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) AHRS_MLKF_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\" ifdef SECONDARY_AHRS
AHRS_SRCS += subsystems/ahrs.c ifneq (,$(findstring $(SECONDARY_AHRS), mlkf))
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf.c # this is the secondary AHRS
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c AHRS_MLKF_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c AHRS_MLKF_CFLAGS += -DSECONDARY_AHRS=ahrs_mlkf
else
# this is the primary AHRS
AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
AHRS_MLKF_CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
endif
else
# plain old single AHRS usage
AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
endif
ap.CFLAGS += $(AHRS_CFLAGS) AHRS_MLKF_SRCS += subsystems/ahrs.c
ap.srcs += $(AHRS_SRCS) AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_float_mlkf.c
AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_aligner.c
nps.CFLAGS += $(AHRS_CFLAGS) ap.CFLAGS += $(AHRS_MLKF_CFLAGS)
nps.srcs += $(AHRS_SRCS) ap.srcs += $(AHRS_MLKF_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS) nps.CFLAGS += $(AHRS_MLKF_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS) nps.srcs += $(AHRS_MLKF_SRCS)
test_ahrs.CFLAGS += $(AHRS_MLKF_CFLAGS)
test_ahrs.srcs += $(AHRS_MLKF_SRCS)
conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile
+29 -15
View File
@@ -6,28 +6,42 @@
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS AHRS_ICE_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_ICE_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER AHRS_ICE_CFLAGS += -DUSE_MAGNETOMETER
endif endif
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) AHRS_ICE_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\" ifdef SECONDARY_AHRS
AHRS_SRCS += subsystems/ahrs.c ifneq (,$(findstring $(SECONDARY_AHRS), ice int_cmpl_euler))
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c # this is the secondary AHRS
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c AHRS_ICE_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c AHRS_ICE_CFLAGS += -DSECONDARY_AHRS=ahrs_ice
else
# this is the primary AHRS
AHRS_ICE_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
AHRS_ICE_CFLAGS += -DPRIMARY_AHRS=ahrs_ice
endif
else
# plain old single AHRS usage
AHRS_ICE_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
endif
ap.CFLAGS += $(AHRS_CFLAGS) AHRS_ICE_SRCS += subsystems/ahrs.c
ap.srcs += $(AHRS_SRCS) AHRS_ICE_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c
AHRS_ICE_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
AHRS_ICE_SRCS += subsystems/ahrs/ahrs_aligner.c
nps.CFLAGS += $(AHRS_CFLAGS) ap.CFLAGS += $(AHRS_ICE_CFLAGS)
nps.srcs += $(AHRS_SRCS) ap.srcs += $(AHRS_ICE_SRCS)
nps.CFLAGS += $(AHRS_ICE_CFLAGS)
nps.srcs += $(AHRS_ICE_SRCS)
# #
# Simple simulation of the AHRS result # Simple simulation of the AHRS result
@@ -41,5 +55,5 @@ ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
sim.CFLAGS += $(ahrssim_CFLAGS) sim.CFLAGS += $(ahrssim_CFLAGS)
sim.srcs += $(ahrssim_srcs) sim.srcs += $(ahrssim_srcs)
test_ahrs.CFLAGS += $(AHRS_CFLAGS) test_ahrs.CFLAGS += $(AHRS_ICE_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS) test_ahrs.srcs += $(AHRS_ICE_SRCS)
conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile
+29 -15
View File
@@ -8,28 +8,42 @@
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS AHRS_ICQ_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER AHRS_ICQ_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER AHRS_ICQ_CFLAGS += -DUSE_MAGNETOMETER
endif endif
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) AHRS_ICQ_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\" ifdef SECONDARY_AHRS
AHRS_SRCS += subsystems/ahrs.c ifneq (,$(findstring $(SECONDARY_AHRS),ahrs_icq int_cmpl_quat))
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c # this is the secondary AHRS
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c AHRS_ICQ_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c AHRS_ICQ_CFLAGS += -DSECONDARY_AHRS=ahrs_icq
else
# this is the primary AHRS
AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
AHRS_ICQ_CFLAGS += -DPRIMARY_AHRS=ahrs_icq
endif
else
# plain old single AHRS usage
AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
endif
ap.CFLAGS += $(AHRS_CFLAGS) AHRS_ICQ_SRCS += subsystems/ahrs.c
ap.srcs += $(AHRS_SRCS) AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c
AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_aligner.c
nps.CFLAGS += $(AHRS_CFLAGS) ap.CFLAGS += $(AHRS_ICQ_CFLAGS)
nps.srcs += $(AHRS_SRCS) ap.srcs += $(AHRS_ICQ_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS) nps.CFLAGS += $(AHRS_ICQ_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS) nps.srcs += $(AHRS_ICQ_SRCS)
test_ahrs.CFLAGS += $(AHRS_ICQ_CFLAGS)
test_ahrs.srcs += $(AHRS_ICQ_SRCS)
conf/firmwares/subsystems/rotorcraft/ins_float_invariant.makefile → conf/firmwares/subsystems/shared/ins_float_invariant.makefile
+3 -9
View File
@@ -1,24 +1,20 @@
# Hey Emacs, this is a -*- makefile -*- # Hey Emacs, this is a -*- makefile -*-
# attitude and speed estimation for fixedwings via invariant filter # attitude and speed estimation via invariant filter
USE_MAGNETOMETER ?= 1 USE_MAGNETOMETER ?= 1
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
# for geo mag
INS_CFLAGS += -DAHRS_FLOAT
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
INS_CFLAGS += -DUSE_MAGNETOMETER INS_CFLAGS += -DUSE_MAGNETOMETER
endif endif
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant_wrapper.c
ifneq ($(AHRS_ALIGNER_LED),none) ifneq ($(AHRS_ALIGNER_LED),none)
INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
@@ -32,5 +28,3 @@ ap.srcs += $(INS_SRCS)
# #
nps.CFLAGS += $(INS_CFLAGS) nps.CFLAGS += $(INS_CFLAGS)
nps.srcs += $(INS_SRCS) nps.srcs += $(INS_SRCS)
conf/messages.xml
+2 -1
View File
@@ -2029,8 +2029,9 @@
</message> </message>
<message name="STATE_FILTER_STATUS" id="232"> <message name="STATE_FILTER_STATUS" id="232">
<field name="id" type="uint8"/>
<field name="state_filter_mode" type="uint8" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/> <field name="state_filter_mode" type="uint8" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
<field name="value" type="uint16" /> <field name="value" type="uint16"/>
</message> </message>
<!--233 is free --> <!--233 is free -->
conf/settings/estimation/ahrs_secondary.xml
+11
View File
@@ -0,0 +1,11 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_output_idx" min="0" step="1" max="1" values="PRIMARY|SECONDARY" module="subsystems/ahrs" shortname="ahrs output" handler="switch"/>
</dl_settings>
</dl_settings>
</settings>
conf/settings/estimation/ins_float_invariant.xml
+14 -14
View File
@@ -3,20 +3,20 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="invariant"> <dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_impl.reset" shortname="reset"/> <dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_float_inv.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/> <dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lb" shortname="lb"/> <dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.mv" shortname="mv"/> <dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.mv" shortname="mv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.mh" shortname="mh"/> <dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_impl.gains.nx" shortname="nx"/> <dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.nxz" shortname="nxz"/> <dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_impl.gains.mvz" shortname="mvz"/> <dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mvz" shortname="mvz"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.nh" shortname="nh"/> <dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nh" shortname="nh"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.ov" shortname="ov"/> <dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_impl.gains.ob" shortname="ob"/> <dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ob" shortname="ob"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rv" shortname="rv"/> <dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rv" shortname="rv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rh" shortname="rh"/> <dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_impl.gains.sh" shortname="sh"/> <dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.sh" shortname="sh"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
sw/airborne/boards/ardrone/navdata.c
+3 -11
View File
@@ -65,6 +65,9 @@ static bool_t navdata_available = FALSE;
static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER; static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER;
#ifndef NAVDATA_FILTER_ID
#define NAVDATA_FILTER_ID 2
#endif
/** Sonar offset. /** Sonar offset.
* Offset value in ADC * Offset value in ADC
@@ -158,16 +161,6 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev)
&navdata.measure.chksum, &navdata.measure.chksum,
&navdata.checksum_errors); &navdata.checksum_errors);
} }
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
if (navdata.imu_lost) { mde = 5; }
uint16_t val = navdata.lost_imu_frames;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
}
#endif #endif
/** /**
@@ -245,7 +238,6 @@ bool_t navdata_init()
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif #endif
// Set to initialized // Set to initialized
sw/airborne/firmwares/fixedwing/main_ap.c
+12 -57
View File
@@ -127,11 +127,7 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#endif #endif
#define __DefaultAhrsRegister(_x) _x ## _register() #if USE_IMU
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
#if USE_AHRS && USE_IMU
#ifdef AHRS_PROPAGATE_FREQUENCY #ifdef AHRS_PROPAGATE_FREQUENCY
#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY) #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
@@ -143,19 +139,7 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
static inline void on_gyro_event(void); static inline void on_gyro_event(void);
static inline void on_accel_event(void); static inline void on_accel_event(void);
static inline void on_mag_event(void); static inline void on_mag_event(void);
volatile uint8_t ahrs_timeout_counter = 0; #endif // USE_IMU
//FIXME not the correct place
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
if (ahrs_timeout_counter > 10) { mde = 5; }
uint16_t val = 0;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
}
#endif // USE_AHRS && USE_IMU
#if USE_GPS #if USE_GPS
static inline void on_gps_solution(void); static inline void on_gps_solution(void);
@@ -200,20 +184,11 @@ void init_ap(void)
#endif #endif
#if USE_AHRS #if USE_AHRS
#if defined SITL && !USE_NPS
ahrs_sim_init();
#else
ahrs_init(); ahrs_init();
DefaultAhrsRegister();
#endif
#endif #endif
ins_init(); ins_init();
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif
#if USE_BARO_BOARD #if USE_BARO_BOARD
baro_init(); baro_init();
#endif #endif
@@ -617,12 +592,6 @@ void sensors_task(void)
{ {
#if USE_IMU #if USE_IMU
imu_periodic(); imu_periodic();
#if USE_AHRS
if (ahrs_timeout_counter < 255) {
ahrs_timeout_counter ++;
}
#endif // USE_AHRS
#endif // USE_IMU #endif // USE_IMU
//FIXME: this is just a kludge //FIXME: this is just a kludge
@@ -634,7 +603,10 @@ void sensors_task(void)
gps_periodic_check(); gps_periodic_check();
#endif #endif
ins_periodic(); //FIXME: temporary hack, remove me
#ifdef InsPeriodic
InsPeriodic();
#endif
} }
@@ -697,7 +669,7 @@ void event_task_ap(void)
mcu_event(); mcu_event();
#endif /* SINGLE_MCU */ #endif /* SINGLE_MCU */
#if USE_AHRS && USE_IMU #if USE_IMU
ImuEvent(on_gyro_event, on_accel_event, on_mag_event); ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif #endif
@@ -742,10 +714,11 @@ void event_task_ap(void)
#if USE_GPS #if USE_GPS
static inline void on_gps_solution(void) static inline void on_gps_solution(void)
{ {
#if USE_AHRS // current timestamp
ahrs_update_gps(); uint32_t now_ts = get_sys_time_usec();
#endif
ins_update_gps(); AbiSendMsgGPS(1, now_ts, &gps);
#ifdef GPS_TRIGGERED_FUNCTION #ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION(); GPS_TRIGGERED_FUNCTION();
#endif #endif
@@ -769,10 +742,6 @@ static inline void on_gyro_event(void)
// current timestamp // current timestamp
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
#if USE_AHRS
ahrs_timeout_counter = 0;
#endif
imu_scale_gyro(&imu); imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev); AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
@@ -788,20 +757,6 @@ static inline void on_gyro_event(void)
if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
#endif #endif
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ins_propagate(dt);
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1; new_ins_attitude = 1;
#endif #endif
sw/airborne/firmwares/rotorcraft/autopilot.c
+1 -2
View File
@@ -89,10 +89,9 @@ bool_t autopilot_detect_ground_once;
#endif #endif
#ifndef AUTOPILOT_DISABLE_AHRS_KILL #ifndef AUTOPILOT_DISABLE_AHRS_KILL
#include "subsystems/ahrs.h"
static inline int ahrs_is_aligned(void) static inline int ahrs_is_aligned(void)
{ {
return DefaultAhrsImpl.is_aligned; return stateIsAttitudeValid();
} }
#else #else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
sw/airborne/firmwares/rotorcraft/main.c
+15 -23
View File
@@ -105,10 +105,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
#endif #endif
#endif #endif
#define __DefaultAhrsRegister(_x) _x ## _register()
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
static inline void on_gyro_event(void); static inline void on_gyro_event(void);
static inline void on_accel_event(void); static inline void on_accel_event(void);
static inline void on_gps_event(void); static inline void on_gps_event(void);
@@ -160,10 +156,12 @@ STATIC_INLINE void main_init(void)
#if USE_AHRS_ALIGNER #if USE_AHRS_ALIGNER
ahrs_aligner_init(); ahrs_aligner_init();
#endif #endif
ahrs_init();
ins_init();
DefaultAhrsRegister(); #if USE_AHRS
ahrs_init();
#endif
ins_init();
#if USE_GPS #if USE_GPS
gps_init(); gps_init();
@@ -228,6 +226,11 @@ STATIC_INLINE void main_periodic(void)
imu_periodic(); imu_periodic();
//FIXME: temporary hack, remove me
#ifdef InsPeriodic
InsPeriodic();
#endif
/* run control loops */ /* run control loops */
autopilot_periodic(); autopilot_periodic();
/* set actuators */ /* set actuators */
@@ -363,20 +366,6 @@ static inline void on_gyro_event( void ) {
if (nps_bypass_ahrs) sim_overwrite_ahrs(); if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif #endif
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ins_propagate(dt);
#ifdef USE_VEHICLE_INTERFACE #ifdef USE_VEHICLE_INTERFACE
vi_notify_imu_available(); vi_notify_imu_available();
#endif #endif
@@ -384,8 +373,11 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
static inline void on_gps_event(void) static inline void on_gps_event(void)
{ {
ahrs_update_gps(); // current timestamp
ins_update_gps(); uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(1, now_ts, &gps);
#ifdef USE_VEHICLE_INTERFACE #ifdef USE_VEHICLE_INTERFACE
if (gps.fix == GPS_FIX_3D) { if (gps.fix == GPS_FIX_3D) {
vi_notify_gps_available(); vi_notify_gps_available();
sw/airborne/modules/ahrs/ahrs_infrared.c
+6 -1
View File
@@ -38,6 +38,10 @@
#include "state.h" #include "state.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
#ifndef INFRARED_FILTER_ID
#define INFRARED_FILTER_ID 2
#endif
static float heading; static float heading;
/** ABI binding for gyro data. /** ABI binding for gyro data.
@@ -69,8 +73,9 @@ static void send_status(struct transport_tx *trans, struct link_device *dev)
{ {
uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
uint8_t mde = 3; uint8_t mde = 3;
uint8_t id = INFRARED_FILTER_ID;
if (contrast < 50) { mde = 7; } if (contrast < 50) { mde = 7; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &contrast); pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &id, &contrast);
} }
#endif #endif
sw/airborne/modules/datalink/mavlink.c
+1 -2
View File
@@ -38,7 +38,6 @@
#include "mcu_periph/sys_time.h" #include "mcu_periph/sys_time.h"
#include "subsystems/electrical.h" #include "subsystems/electrical.h"
#include "subsystems/ahrs.h"
#include "state.h" #include "state.h"
#include "pprz_version.h" #include "pprz_version.h"
@@ -296,7 +295,7 @@ static inline void mavlink_send_heartbeat(void)
#else #else
uint8_t mav_type = MAV_TYPE_QUADROTOR; uint8_t mav_type = MAV_TYPE_QUADROTOR;
#endif #endif
if (DefaultAhrsImpl.is_aligned) { if (stateIsAttitudeValid()) {
if (kill_throttle) { if (kill_throttle) {
mav_state = MAV_STATE_STANDBY; mav_state = MAV_STATE_STANDBY;
} }
sw/airborne/modules/geo_mag/geo_mag.c
+12 -10
View File
@@ -29,9 +29,14 @@
#include "math/pprz_geodetic_wmm2010.h" #include "math/pprz_geodetic_wmm2010.h"
#include "math/pprz_algebra_double.h" #include "math/pprz_algebra_double.h"
#include "subsystems/gps.h" #include "subsystems/gps.h"
#include "subsystems/abi.h"
// for kill_throttle check
#include "autopilot.h" #include "autopilot.h"
#include "subsystems/ahrs.h" #ifndef GEO_MAG_SENDER_ID
#define GEO_MAG_SENDER_ID 1
#endif
bool_t geo_mag_calc_flag; bool_t geo_mag_calc_flag;
struct GeoMag geo_mag; struct GeoMag geo_mag;
@@ -72,16 +77,13 @@ void geo_mag_event(void)
mag_calc(1, latitude, longitude, alt, nmax, gha, mag_calc(1, latitude, longitude, alt, nmax, gha,
&geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z, &geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3); IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
double_vect3_normalize(&geo_mag.vect);
// copy to ahrs // send as normalized float vector via ABI
#ifdef AHRS_FLOAT struct FloatVect3 h = { .x = geo_mag.vect.x,
VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect); .y = geo_mag.vect.y,
#else .z = geo_mag.vect.z };
// convert to MAG_BFP and copy to ahrs float_vect3_normalize(&h);
VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), AbiSendMsgGEO_MAG(GEO_MAG_SENDER_ID, &h);
MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
geo_mag.ready = TRUE; geo_mag.ready = TRUE;
} }
sw/airborne/modules/ins/ahrs_chimu.h
+3 -1
View File
@@ -35,7 +35,9 @@ struct AhrsChimu {
extern struct AhrsChimu ahrs_chimu; extern struct AhrsChimu ahrs_chimu;
#define DefaultAhrsImpl ahrs_chimu #ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_chimu
#endif
extern void ahrs_chimu_register(void); extern void ahrs_chimu_register(void);
extern void ahrs_chimu_init(void); extern void ahrs_chimu_init(void);
sw/airborne/modules/ins/ahrs_chimu_spi.c
+16 -5
View File
@@ -37,11 +37,22 @@ INS_FORMAT ins_pitch_neutral;
struct AhrsChimu ahrs_chimu; struct AhrsChimu ahrs_chimu;
void ahrs_chimu_update_gps(void); void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d);
#include "subsystems/abi.h"
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
ahrs_chimu_update_gps(gps_s->fix, gps_s->speed_3d);
}
void ahrs_chimu_register(void) void ahrs_chimu_register(void)
{ {
ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps); ahrs_chimu_init();
/// @TODO: provide enable function
ahrs_register_impl(NULL);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
} }
void ahrs_chimu_init(void) void ahrs_chimu_init(void)
@@ -118,15 +129,15 @@ void parse_ins_msg(void)
} }
void ahrs_chimu_update_gps(void) void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d)
{ {
// Send SW Centripetal Corrections // Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
float gps_speed = 0; float gps_speed = 0;
if (gps.fix == GPS_FIX_3D) { if (gps_fix == GPS_FIX_3D) {
gps_speed = gps.speed_3d / 100.; gps_speed = gps_speed_3d / 100.;
} }
gps_speed = FloatSwap(gps_speed); gps_speed = FloatSwap(gps_speed);
sw/airborne/modules/ins/ahrs_chimu_uart.c
+3 -1
View File
@@ -34,7 +34,9 @@ struct AhrsChimu ahrs_chimu;
void ahrs_chimu_register(void) void ahrs_chimu_register(void)
{ {
ahrs_register_impl(ahrs_chimu_init, NULL); ahrs_chimu_init();
/// @TODO: provide enable function
ahrs_register_impl(NULL);
} }
void ahrs_chimu_init(void) void ahrs_chimu_init(void)
sw/airborne/modules/ins/ins_xsens.c
+24 -13
View File
@@ -210,7 +210,6 @@ uint8_t send_ck;
volatile int xsens_configured = 0; volatile int xsens_configured = 0;
void xsens_init(void); void xsens_init(void);
void xsens_periodic(void);
void xsens_init(void) void xsens_init(void)
{ {
@@ -247,7 +246,9 @@ void imu_periodic(void)
#endif /* USE_IMU */ #endif /* USE_IMU */
#if USE_INS_MODULE #if USE_INS_MODULE
void ins_init(void) void ins_xsens_update_gps(struct GpsState *gps_s);
void ins_xsens_init(void)
{ {
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0); stateSetLocalUtmOrigin_f(&utm0);
@@ -256,26 +257,36 @@ void ins_init(void)
xsens_init(); xsens_init();
} }
void ins_periodic(void) #include "subsystems/abi.h"
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{ {
xsens_periodic(); ins_xsens_update_gps(gps_s);
} }
void ins_update_gps(void) void ins_xsens_register(void)
{
ins_register_impl(ins_xsens_init);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
void ins_xsens_update_gps(struct GpsState *gps_s)
{ {
struct UtmCoor_f utm; struct UtmCoor_f utm;
utm.east = gps.utm_pos.east / 100.; utm.east = gps_s->utm_pos.east / 100.;
utm.north = gps.utm_pos.north / 100.; utm.north = gps_s->utm_pos.north / 100.;
utm.zone = nav_utm_zone0; utm.zone = nav_utm_zone0;
utm.alt = gps.hmsl / 1000.; utm.alt = gps_s->hmsl / 1000.;
// set position // set position
stateSetPositionUtm_f(&utm); stateSetPositionUtm_f(&utm);
struct NedCoor_f ned_vel = { struct NedCoor_f ned_vel = {
gps.ned_vel.x / 100., gps_s->ned_vel.x / 100.,
gps.ned_vel.y / 100., gps_s->ned_vel.y / 100.,
gps.ned_vel.z / 100. gps_s->ned_vel.z / 100.
}; };
// set velocity // set velocity
stateSetSpeedNed_f(&ned_vel); stateSetSpeedNed_f(&ned_vel);
@@ -352,7 +363,7 @@ void xsens_periodic(void)
#if USE_INS_MODULE #if USE_INS_MODULE
#include "state.h" #include "state.h"
static inline void update_fw_estimator(void) static inline void update_state_interface(void)
{ {
// Send to Estimator (Control) // Send to Estimator (Control)
#ifdef XSENS_BACKWARDS #ifdef XSENS_BACKWARDS
@@ -387,7 +398,7 @@ void handle_ins_msg(void)
{ {
#if USE_INS_MODULE #if USE_INS_MODULE
update_fw_estimator(); update_state_interface();
#endif #endif
#if USE_IMU #if USE_IMU
sw/airborne/modules/ins/ins_xsens.h
+5
View File
@@ -46,6 +46,7 @@ extern struct XsensTime xsens_time;
extern uint8_t xsens_msg_status; extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp; extern uint16_t xsens_time_stamp;
extern void xsens_periodic(void);
/* To use Xsens to just provide IMU measurements /* To use Xsens to just provide IMU measurements
* for use with an external AHRS algorithm * for use with an external AHRS algorithm
@@ -82,6 +83,10 @@ extern struct ImuXsens imu_xsens;
#define InsEvent(_ins_handler) { \ #define InsEvent(_ins_handler) { \
ins_event_check_and_handle(handle_ins_msg); \ ins_event_check_and_handle(handle_ins_msg); \
} }
#define DefaultInsImpl ins_xsens
#define InsPeriodic xsens_periodic
extern void ins_xsens_init(void);
extern void ins_xsens_register(void);
#endif #endif
sw/airborne/modules/ins/ins_xsens700.c
+2 -7
View File
@@ -188,11 +188,6 @@ void ins_init(void)
xsens_init(); xsens_init();
} }
void ins_periodic(void)
{
xsens_periodic();
}
void ins_update_gps(void) void ins_update_gps(void)
{ {
struct UtmCoor_f utm; struct UtmCoor_f utm;
@@ -304,7 +299,7 @@ void xsens_periodic(void)
#if USE_INS_MODULE #if USE_INS_MODULE
#include "state.h" #include "state.h"
static inline void update_fw_estimator(void) static inline void update_state_interface(void)
{ {
// Send to Estimator (Control) // Send to Estimator (Control)
#if XSENS_BACKWARDS #if XSENS_BACKWARDS
@@ -339,7 +334,7 @@ void handle_ins_msg(void)
{ {
#if USE_INS_MODULE #if USE_INS_MODULE
update_fw_estimator(); update_state_interface();
#endif #endif
#if USE_GPS_XSENS #if USE_GPS_XSENS
sw/airborne/subsystems/abi_common.h
+1
View File
@@ -32,6 +32,7 @@
#include "std.h" #include "std.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
#include "subsystems/gps.h"
/* Include here headers with structure definition you may want to use with ABI /* Include here headers with structure definition you may want to use with ABI
* Ex: '#include "subsystems/gps.h"' in order to use the GpsState structure * Ex: '#include "subsystems/gps.h"' in order to use the GpsState structure
*/ */
sw/airborne/subsystems/ahrs.c
+60 -12
View File
@@ -21,31 +21,79 @@
/** /**
* @file subsystems/ahrs.c * @file subsystems/ahrs.c
* Attitude and Heading Reference System interface. * Dispatcher to register actual AHRS implementations.
*/ */
#include "subsystems/ahrs.h" #include "subsystems/ahrs.h"
struct Ahrs ahrs; #ifndef PRIMARY_AHRS
#error "PRIMARY_AHRS not set!"
#else
PRINT_CONFIG_VAR(PRIMARY_AHRS)
#endif
void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps) #ifdef SECONDARY_AHRS
PRINT_CONFIG_VAR(SECONDARY_AHRS)
#endif
#define __RegisterAhrs(_x) _x ## _register()
#define _RegisterAhrs(_x) __RegisterAhrs(_x)
#define RegisterAhrs(_x) _RegisterAhrs(_x)
/** maximum number of AHRS implementations that can register */
#ifndef AHRS_NB_IMPL
#define AHRS_NB_IMPL 2
#endif
/** references a registered AHRS implementation */
struct AhrsImpl {
AhrsEnableOutput enable;
};
struct AhrsImpl ahrs_impls[AHRS_NB_IMPL];
uint8_t ahrs_output_idx;
void ahrs_register_impl(AhrsEnableOutput enable)
{ {
ahrs.init = init; int i;
ahrs.update_gps = update_gps; for (i=0; i < AHRS_NB_IMPL; i++) {
if (ahrs_impls[i].enable == NULL) {
ahrs.init(); ahrs_impls[i].enable = enable;
break;
}
}
} }
void ahrs_init(void) void ahrs_init(void)
{ {
ahrs.init = NULL; int i;
ahrs.update_gps = NULL; for (i=0; i < AHRS_NB_IMPL; i++) {
ahrs_impls[i].enable = NULL;
}
RegisterAhrs(PRIMARY_AHRS);
#ifdef SECONDARY_AHRS
RegisterAhrs(SECONDARY_AHRS);
#endif
// enable primary AHRS by default
ahrs_switch(0);
} }
void ahrs_update_gps(void) int ahrs_switch(uint8_t idx)
{ {
if (ahrs.update_gps != NULL) { if (idx >= AHRS_NB_IMPL) { return -1; }
ahrs.update_gps(); if (ahrs_impls[idx].enable == NULL) { return -1; }
/* first disable other AHRS output */
int i;
for (i=0; i < AHRS_NB_IMPL; i++) {
if (ahrs_impls[i].enable != NULL) {
ahrs_impls[i].enable(FALSE);
}
} }
/* enable requested AHRS */
ahrs_impls[idx].enable(TRUE);
ahrs_output_idx = idx;
return ahrs_output_idx;
} }
sw/airborne/subsystems/ahrs.h
+20 -18
View File
@@ -21,7 +21,7 @@
/** /**
* @file subsystems/ahrs.h * @file subsystems/ahrs.h
* Attitude and Heading Reference System interface. * Dispatcher to register actual AHRS implementations.
*/ */
#ifndef AHRS_H #ifndef AHRS_H
@@ -29,35 +29,37 @@
#include "std.h" #include "std.h"
/* underlying includes (needed for parameters) */ /* include actual (primary) implementation header */
#ifdef AHRS_TYPE_H #ifdef AHRS_TYPE_H
#include AHRS_TYPE_H #include AHRS_TYPE_H
#endif #endif
typedef void (*AhrsInit)(void); /* include secondary implementation header */
typedef void (*AhrsUpdateGps)(void); #ifdef AHRS_SECONDARY_TYPE_H
#include AHRS_SECONDARY_TYPE_H
#endif
/** Attitude and Heading Reference System state */ typedef bool_t (*AhrsEnableOutput)(bool_t);
struct Ahrs {
/* function pointers to actual implementation, set by ahrs_register_impl */
AhrsInit init;
AhrsUpdateGps update_gps;
};
/** global AHRS state */ /* for settings when using secondary AHRS */
extern struct Ahrs ahrs; extern uint8_t ahrs_output_idx;
extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps); /**
* Register an AHRS implementation.
* Adds it to an internal list.
* @param enable pointer to function to enable/disable the output of registering AHRS
*/
extern void ahrs_register_impl(AhrsEnableOutput enable);
/** AHRS initialization. Called at startup. /** AHRS initialization. Called at startup.
* Initialized the global AHRS struct. * Registers/initializes the default AHRS.
*/ */
extern void ahrs_init(void); extern void ahrs_init(void);
/** Update AHRS state with GPS measurements. /**
* Calls implementation if registered. * Switch to the output of another AHRS impl.
* Reads the global #gps data struct. * @param idx index of the AHRS impl (0 = PRIMARY_AHRS, 1 = SECONDARY_AHRS).
*/ */
extern void ahrs_update_gps(void); extern int ahrs_switch(uint8_t idx);
#endif /* AHRS_H */ #endif /* AHRS_H */
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+3 -1
View File
@@ -70,7 +70,9 @@ static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev)
void ahrs_ardrone2_register(void) void ahrs_ardrone2_register(void)
{ {
ahrs_register_impl(ahrs_ardrone2_init, NULL); ahrs_ardrone2_init();
/// @TODO: provide enable function
ahrs_register_impl(NULL);
} }
void ahrs_ardrone2_init(void) void ahrs_ardrone2_init(void)
sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
+3 -1
View File
@@ -47,7 +47,9 @@ struct AhrsARDrone {
}; };
extern struct AhrsARDrone ahrs_ardrone2; extern struct AhrsARDrone ahrs_ardrone2;
#define DefaultAhrsImpl ahrs_ardrone2 #ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_ardrone2
#endif
extern void ahrs_ardrone2_register(void); extern void ahrs_ardrone2_register(void);
extern void ahrs_ardrone2_init(void); extern void ahrs_ardrone2_init(void);
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+23 -49
View File
@@ -36,7 +36,6 @@
#if USE_GPS #if USE_GPS
#include "subsystems/gps.h" #include "subsystems/gps.h"
#endif #endif
#include "state.h"
//#include "../../test/pprz_algebra_print.h" //#include "../../test/pprz_algebra_print.h"
@@ -55,7 +54,6 @@
#warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course." #warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
#endif #endif
/* /*
* default gains for correcting attitude and bias from accel/mag * default gains for correcting attitude and bias from accel/mag
*/ */
@@ -83,9 +81,6 @@ void ahrs_fc_update_mag_full(struct Int32Vect3 *mag, float dt);
void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt); void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt);
void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag); void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag);
static inline void compute_body_orientation_and_rates(void);
struct AhrsFloatCmpl ahrs_fc; struct AhrsFloatCmpl ahrs_fc;
void ahrs_fc_init(void) void ahrs_fc_init(void)
@@ -138,9 +133,6 @@ bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
/* Convert initial orientation from quat to rotation matrix representations. */ /* Convert initial orientation from quat to rotation matrix representations. */
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */ /* used averaged gyro as initial value for bias */
struct Int32Rates bias0; struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro); RATES_COPY(bias0, *lp_gyro);
@@ -185,7 +177,6 @@ void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif #endif
compute_body_orientation_and_rates();
// increase accel and mag propagation counters // increase accel and mag propagation counters
ahrs_fc.accel_cnt++; ahrs_fc.accel_cnt++;
@@ -221,12 +212,14 @@ void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
* assumption: tangential velocity only along body x-axis * assumption: tangential velocity only along body x-axis
*/ */
const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0}; const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body; struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */ /* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu; struct FloatVect3 acc_c_imu;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
@@ -403,11 +396,11 @@ void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag)
} }
void ahrs_fc_update_gps(void) void ahrs_fc_update_gps(struct GpsState *gps_s)
{ {
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
if (gps.fix == GPS_FIX_3D) { if (gps_s->fix == GPS_FIX_3D) {
ahrs_fc.ltp_vel_norm = gps.speed_3d / 100.; ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.;
ahrs_fc.ltp_vel_norm_valid = TRUE; ahrs_fc.ltp_vel_norm_valid = TRUE;
} else { } else {
ahrs_fc.ltp_vel_norm_valid = FALSE; ahrs_fc.ltp_vel_norm_valid = FALSE;
@@ -416,12 +409,11 @@ void ahrs_fc_update_gps(void)
#if AHRS_USE_GPS_HEADING && USE_GPS #if AHRS_USE_GPS_HEADING && USE_GPS
//got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
if (gps.fix == GPS_FIX_3D && if (3dfix && gps_s->gspeed >= 500 &&
gps.gspeed >= 500 && gps_s->cacc <= RadOfDeg(10 * 1e7)) {
gps.cacc <= RadOfDeg(10 * 1e7)) {
// gps.course is in rad * 1e7, we need it in rad // gps_s->course is in rad * 1e7, we need it in rad
float course = gps.course / 1e7; float course = gps_s->course / 1e7;
if (ahrs_fc.heading_aligned) { if (ahrs_fc.heading_aligned) {
/* the assumption here is that there is no side-slip, so heading=course */ /* the assumption here is that there is no side-slip, so heading=course */
@@ -440,12 +432,16 @@ void ahrs_fc_update_heading(float heading)
FLOAT_ANGLE_NORMALIZE(heading); FLOAT_ANGLE_NORMALIZE(heading);
struct FloatRMat *ltp_to_body_rmat = stateGetNedToBodyRMat_f(); struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_quat(&ltp_to_body_rmat, &ltp_to_body_quat);
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y // we only consider x and y
struct FloatVect2 expected_ltp = { struct FloatVect2 expected_ltp = {
RMAT_ELMT(*ltp_to_body_rmat, 0, 0), RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(*ltp_to_body_rmat, 0, 1) RMAT_ELMT(ltp_to_body_rmat, 0, 1)
}; };
// expected_heading cross measured_heading // expected_heading cross measured_heading
@@ -488,10 +484,12 @@ void ahrs_fc_realign_heading(float heading)
q_h_new.qz = sinf(heading / 2.0); q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0); q_h_new.qi = cosf(heading / 2.0);
struct FloatQuat *ltp_to_body_quat = stateGetNedToBodyQuat_f(); struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* quaternion representing current heading only */ /* quaternion representing current heading only */
struct FloatQuat q_h; struct FloatQuat q_h;
QUAT_COPY(q_h, *ltp_to_body_quat); QUAT_COPY(q_h, ltp_to_body_quat);
q_h.qx = 0.; q_h.qx = 0.;
q_h.qy = 0.; q_h.qy = 0.;
float_quat_normalize(&q_h); float_quat_normalize(&q_h);
@@ -502,39 +500,15 @@ void ahrs_fc_realign_heading(float heading)
/* correct current heading in body frame */ /* correct current heading in body frame */
struct FloatQuat q; struct FloatQuat q;
float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); float_quat_comp_norm_shortest(&q, &q_c, &ltp_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */ /* compute ltp to imu rotation quaternion and matrix */
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
/* set state */
stateSetNedToBodyQuat_f(&q);
ahrs_fc.heading_aligned = TRUE; ahrs_fc.heading_aligned = TRUE;
} }
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void compute_body_orientation_and_rates(void)
{
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* Set state */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
stateSetBodyRates_f(&body_rate);
}
void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu) void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
{ {
ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+2 -1
View File
@@ -33,6 +33,7 @@
#include "std.h" #include "std.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h" #include "math/pprz_orientation_conversion.h"
#include "subsystems/gps.h"
enum AhrsFCStatus { enum AhrsFCStatus {
AHRS_FC_UNINIT, AHRS_FC_UNINIT,
@@ -84,7 +85,7 @@ extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_ac
extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt); extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt);
extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt); extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt);
extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt); extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt);
extern void ahrs_fc_update_gps(void); extern void ahrs_fc_update_gps(struct GpsState *gps_s);
/** Update yaw based on a heading measurement. /** Update yaw based on a heading measurement.
* e.g. from GPS course * e.g. from GPS course
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+84 -4
View File
@@ -29,8 +29,21 @@
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "state.h" #include "state.h"
#ifndef AHRS_FC_OUTPUT_ENABLED
#define AHRS_FC_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_FC_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool_t ahrs_fc_output_enabled;
static uint32_t ahrs_fc_last_stamp;
static void compute_body_orientation_and_rates(void);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
static void send_att(struct transport_tx *trans, struct link_device *dev) static void send_att(struct transport_tx *trans, struct link_device *dev)
{ {
@@ -53,6 +66,22 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
pprz_msg_send_GEO_MAG(trans, dev, AC_ID, pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z);
} }
#ifndef AHRS_FC_FILTER_ID
#define AHRS_FC_FILTER_ID 5
#endif
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t id = AHRS_FC_FILTER_ID;
uint8_t mde = 3;
uint16_t val = 0;
if (!ahrs_fc.is_aligned) { mde = 2; }
uint32_t t_diff = get_sys_time_usec() - ahrs_fc_last_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) { mde = 5; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
}
#endif #endif
@@ -67,12 +96,14 @@ static abi_event accel_ev;
static abi_event mag_ev; static abi_event mag_ev;
static abi_event aligner_ev; static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static abi_event gps_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp, uint32_t stamp, struct Int32Rates *gyro)
struct Int32Rates *gyro)
{ {
ahrs_fc_last_stamp = stamp;
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
/* timestamp in usec when last callback was received */ /* timestamp in usec when last callback was received */
@@ -81,6 +112,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (last_stamp > 0 && ahrs_fc.is_aligned) { if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_propagate(gyro, dt); ahrs_fc_propagate(gyro, dt);
compute_body_orientation_and_rates();
} }
last_stamp = stamp; last_stamp = stamp;
#else #else
@@ -89,6 +121,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (ahrs_fc.status == AHRS_FC_RUNNING) { if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_fc_propagate(gyro, dt); ahrs_fc_propagate(gyro, dt);
compute_body_orientation_and_rates();
} }
#endif #endif
} }
@@ -143,7 +176,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_fc.is_aligned) { if (!ahrs_fc.is_aligned) {
ahrs_fc_align(lp_gyro, lp_accel, lp_mag); if (ahrs_fc_align(lp_gyro, lp_accel, lp_mag)) {
compute_body_orientation_and_rates();
}
} }
} }
@@ -153,9 +188,51 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_fc_set_body_to_imu_quat(q_b2i_f); ahrs_fc_set_body_to_imu_quat(q_b2i_f);
} }
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
memcpy(&ahrs_fc.mag_h, h, sizeof(struct FloatVect3));
}
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
ahrs_fc_update_gps(gps_s);
compute_body_orientation_and_rates();
}
static bool_t ahrs_fc_enable_output(bool_t enable)
{
ahrs_fc_output_enabled = enable;
return ahrs_fc_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static void compute_body_orientation_and_rates(void)
{
if (ahrs_fc_output_enabled) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* Set state */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
stateSetBodyRates_f(&body_rate);
}
}
void ahrs_fc_register(void) void ahrs_fc_register(void)
{ {
ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps); ahrs_fc_output_enabled = AHRS_FC_OUTPUT_ENABLED;
ahrs_fc_init();
ahrs_register_impl(ahrs_fc_enable_output);
/* /*
* Subscribe to scaled IMU measurements and attach callbacks * Subscribe to scaled IMU measurements and attach callbacks
@@ -165,9 +242,12 @@ void ahrs_fc_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif #endif
} }
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h
+3 -1
View File
@@ -29,7 +29,9 @@
#include "subsystems/ahrs/ahrs_float_cmpl.h" #include "subsystems/ahrs/ahrs_float_cmpl.h"
#define DefaultAhrsImpl ahrs_fc #ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_fc
#endif
extern void ahrs_fc_register(void); extern void ahrs_fc_register(void);
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+7 -53
View File
@@ -34,8 +34,6 @@
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
#include "state.h"
#if USE_GPS #if USE_GPS
#include "subsystems/gps.h" #include "subsystems/gps.h"
#endif #endif
@@ -51,7 +49,6 @@
#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/downlink.h"
#endif #endif
struct AhrsFloatDCM ahrs_dcm; struct AhrsFloatDCM ahrs_dcm;
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
@@ -76,8 +73,7 @@ float MAG_Heading_X = 1;
float MAG_Heading_Y = 0; float MAG_Heading_Y = 0;
#endif #endif
static inline void compute_ahrs_representations(void); static void compute_ahrs_representations(void);
static inline void set_body_orientation_and_rates(void);
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat); static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
void Normalize(void); void Normalize(void);
@@ -133,9 +129,6 @@ bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
/* set filter dcm */ /* set filter dcm */
set_dcm_matrix_from_rmat(&ltp_to_imu_rmat); set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
/* Set initial body orientation */
set_body_orientation_and_rates();
/* use averaged gyro as initial value for bias */ /* use averaged gyro as initial value for bias */
struct Int32Rates bias0; struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro); RATES_COPY(bias0, *lp_gyro);
@@ -179,17 +172,17 @@ void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
compute_ahrs_representations(); compute_ahrs_representations();
} }
void ahrs_dcm_update_gps(void) void ahrs_dcm_update_gps(struct GpsState *gps_s)
{ {
static float last_gps_speed_3d = 0; static float last_gps_speed_3d = 0;
#if USE_GPS #if USE_GPS
if (gps.fix == GPS_FIX_3D) { if (gps_s->fix == GPS_FIX_3D) {
ahrs_dcm.gps_age = 0; ahrs_dcm.gps_age = 0;
ahrs_dcm.gps_speed = gps.speed_3d / 100.; ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
if (gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s if (gps_s->gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s
ahrs_dcm.gps_course = ((float)gps.course) / 1.e7; ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
ahrs_dcm.gps_course_valid = TRUE; ahrs_dcm.gps_course_valid = TRUE;
} else { } else {
ahrs_dcm.gps_course_valid = FALSE; ahrs_dcm.gps_course_valid = FALSE;
@@ -512,27 +505,7 @@ void Matrix_update(float dt)
} }
} }
/* static void compute_ahrs_representations(void)
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_orientation_and_rates(void)
{
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
stateSetBodyRates_f(&body_rate);
struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
float_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat);
stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
}
static inline void compute_ahrs_representations(void)
{ {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z) ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z)
@@ -544,25 +517,6 @@ static inline void compute_ahrs_representations(void)
ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif #endif
set_body_orientation_and_rates();
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
} }
void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu) void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+2 -1
View File
@@ -27,6 +27,7 @@
#include <inttypes.h> #include <inttypes.h>
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h" #include "math/pprz_orientation_conversion.h"
#include "subsystems/gps.h"
enum AhrsDCMStatus { enum AhrsDCMStatus {
AHRS_DCM_UNINIT, AHRS_DCM_UNINIT,
@@ -87,6 +88,6 @@ extern bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_a
extern void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt); extern void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt);
extern void ahrs_dcm_update_accel(struct Int32Vect3 *accel); extern void ahrs_dcm_update_accel(struct Int32Vect3 *accel);
extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag); extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag);
extern void ahrs_dcm_update_gps(void); extern void ahrs_dcm_update_gps(struct GpsState *gps_s);
#endif // AHRS_FLOAT_DCM_H #endif // AHRS_FLOAT_DCM_H
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+81 -4
View File
@@ -29,6 +29,38 @@
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "state.h" #include "state.h"
#ifndef AHRS_DCM_OUTPUT_ENABLED
#define AHRS_DCM_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_DCM_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool_t ahrs_dcm_output_enabled;
static uint32_t ahrs_dcm_last_stamp;
static void set_body_orientation_and_rates(void);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
#ifndef AHRS_DCM_FILTER_ID
#define AHRS_DCM_FILTER_ID 6
#endif
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t id = AHRS_DCM_FILTER_ID;
uint8_t mde = 3;
uint16_t val = 0;
if (!ahrs_dcm.is_aligned) { mde = 2; }
uint32_t t_diff = get_sys_time_usec() - ahrs_dcm_last_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) { mde = 5; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
}
#endif
/** ABI binding for IMU data. /** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages. * Used for gyro, accel and mag ABI messages.
*/ */
@@ -40,12 +72,13 @@ static abi_event accel_ev;
static abi_event mag_ev; static abi_event mag_ev;
static abi_event aligner_ev; static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static abi_event gps_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp, uint32_t stamp, struct Int32Rates *gyro)
struct Int32Rates *gyro)
{ {
ahrs_dcm_last_stamp = stamp;
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
/* timestamp in usec when last callback was received */ /* timestamp in usec when last callback was received */
@@ -53,6 +86,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (last_stamp > 0 && ahrs_dcm.is_aligned) { if (last_stamp > 0 && ahrs_dcm.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_dcm_propagate(gyro, dt); ahrs_dcm_propagate(gyro, dt);
set_body_orientation_and_rates();
} }
last_stamp = stamp; last_stamp = stamp;
#else #else
@@ -61,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (ahrs_dcm.is_aligned) { if (ahrs_dcm.is_aligned) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_dcm_propagate(gyro, dt); ahrs_dcm_propagate(gyro, dt);
set_body_orientation_and_rates();
} }
#endif #endif
} }
@@ -89,7 +124,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_dcm.is_aligned) { if (!ahrs_dcm.is_aligned) {
ahrs_dcm_align(lp_gyro, lp_accel, lp_mag); if (ahrs_dcm_align(lp_gyro, lp_accel, lp_mag)) {
set_body_orientation_and_rates();
}
} }
} }
@@ -99,9 +136,44 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_dcm_set_body_to_imu_quat(q_b2i_f); ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
} }
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
ahrs_dcm_update_gps(gps_s);
}
static bool_t ahrs_dcm_enable_output(bool_t enable)
{
ahrs_dcm_output_enabled = enable;
return ahrs_dcm_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static void set_body_orientation_and_rates(void)
{
if (ahrs_dcm_output_enabled) {
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
stateSetBodyRates_f(&body_rate);
struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
float_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat);
stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
}
}
void ahrs_dcm_register(void) void ahrs_dcm_register(void)
{ {
ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps); ahrs_dcm_output_enabled = AHRS_DCM_OUTPUT_ENABLED;
ahrs_dcm_init();
ahrs_register_impl(ahrs_dcm_enable_output);
/* /*
* Subscribe to scaled IMU measurements and attach callbacks * Subscribe to scaled IMU measurements and attach callbacks
@@ -111,4 +183,9 @@ void ahrs_dcm_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif
} }
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h
+3 -1
View File
@@ -29,7 +29,9 @@
#include "subsystems/ahrs/ahrs_float_dcm.h" #include "subsystems/ahrs/ahrs_float_dcm.h"
#define DefaultAhrsImpl ahrs_dcm #ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_dcm
#endif
extern void ahrs_dcm_register(void); extern void ahrs_dcm_register(void);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
-29
View File
@@ -33,8 +33,6 @@
#include <string.h> /* for memcpy */ #include <string.h> /* for memcpy */
#include "state.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h" #include "math/pprz_simple_matrix.h"
@@ -57,7 +55,6 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected,
struct FloatVect3 *b_measured, struct FloatVect3 *b_measured,
struct FloatVect3 *noise); struct FloatVect3 *noise);
static inline void reset_state(void); static inline void reset_state(void);
static inline void set_body_state_from_quat(void);
struct AhrsMlkf ahrs_mlkf; struct AhrsMlkf ahrs_mlkf;
@@ -115,9 +112,6 @@ bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
/* Compute an initial orientation from accel and mag directly as quaternion */ /* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag); ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
/* set initial body orientation */
set_body_state_from_quat();
/* used averaged gyro as initial value for bias */ /* used averaged gyro as initial value for bias */
struct Int32Rates bias0; struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro); RATES_COPY(bias0, *lp_gyro);
@@ -132,7 +126,6 @@ void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt)
{ {
propagate_ref(gyro, dt); propagate_ref(gyro, dt);
propagate_state(dt); propagate_state(dt);
set_body_state_from_quat();
} }
void ahrs_mlkf_update_accel(struct Int32Vect3 *accel) void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
@@ -382,25 +375,3 @@ static inline void reset_state(void)
float_quat_identity(&ahrs_mlkf.gibbs_cor); float_quat_identity(&ahrs_mlkf.gibbs_cor);
} }
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_state_from_quat(void)
{
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
}
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+80 -4
View File
@@ -28,15 +28,44 @@
#include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h" #include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h"
#include "subsystems/ahrs.h" #include "subsystems/ahrs.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "state.h"
#ifndef AHRS_MLKF_OUTPUT_ENABLED
#define AHRS_MLKF_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool_t ahrs_mlkf_output_enabled;
static uint32_t ahrs_mlkf_last_stamp;
static void set_body_state_from_quat(void);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
{ {
pprz_msg_send_GEO_MAG(trans, dev, AC_ID, pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z);
} }
#ifndef AHRS_MLKF_FILTER_ID
#define AHRS_MLKF_FILTER_ID 6
#endif
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t id = AHRS_MLKF_FILTER_ID;
uint8_t mde = 3;
uint16_t val = 0;
if (!ahrs_mlkf.is_aligned) { mde = 2; }
uint32_t t_diff = get_sys_time_usec() - ahrs_mlkf_last_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) { mde = 5; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
}
#endif #endif
@@ -51,12 +80,13 @@ static abi_event accel_ev;
static abi_event mag_ev; static abi_event mag_ev;
static abi_event aligner_ev; static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp, uint32_t stamp, struct Int32Rates *gyro)
struct Int32Rates *gyro)
{ {
ahrs_mlkf_last_stamp = stamp;
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
/* timestamp in usec when last callback was received */ /* timestamp in usec when last callback was received */
@@ -65,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (last_stamp > 0 && ahrs_mlkf.is_aligned) { if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_mlkf_propagate(gyro, dt); ahrs_mlkf_propagate(gyro, dt);
set_body_state_from_quat();
} }
last_stamp = stamp; last_stamp = stamp;
#else #else
@@ -73,6 +104,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_mlkf_propagate(gyro, dt); ahrs_mlkf_propagate(gyro, dt);
set_body_state_from_quat();
} }
#endif #endif
} }
@@ -83,6 +115,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
{ {
if (ahrs_mlkf.is_aligned) { if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_accel(accel); ahrs_mlkf_update_accel(accel);
set_body_state_from_quat();
} }
} }
@@ -92,6 +125,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
{ {
if (ahrs_mlkf.is_aligned) { if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_mag(mag); ahrs_mlkf_update_mag(mag);
set_body_state_from_quat();
} }
} }
@@ -101,7 +135,10 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_mlkf.is_aligned) { if (!ahrs_mlkf.is_aligned) {
ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag); /* set initial body orientation in state interface if alignment was successful */
if (ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag)) {
set_body_state_from_quat();
}
} }
} }
@@ -111,9 +148,45 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_mlkf_set_body_to_imu_quat(q_b2i_f); ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
} }
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
memcpy(&ahrs_mlkf.mag_h, h, sizeof(struct FloatVect3));
}
static bool_t ahrs_mlkf_enable_output(bool_t enable)
{
ahrs_mlkf_output_enabled = enable;
return ahrs_mlkf_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static void set_body_state_from_quat(void)
{
if (ahrs_mlkf_output_enabled) {
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
}
}
void ahrs_mlkf_register(void) void ahrs_mlkf_register(void)
{ {
ahrs_register_impl(ahrs_mlkf_init, NULL); ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
ahrs_mlkf_init();
ahrs_register_impl(ahrs_mlkf_enable_output);
/* /*
* Subscribe to scaled IMU measurements and attach callbacks * Subscribe to scaled IMU measurements and attach callbacks
@@ -123,8 +196,11 @@ void ahrs_mlkf_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif #endif
} }
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h
+3 -1
View File
@@ -29,7 +29,9 @@
#include "subsystems/ahrs/ahrs_float_mlkf.h" #include "subsystems/ahrs/ahrs_float_mlkf.h"
#define DefaultAhrsImpl ahrs_mlkf #ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_mlkf
#endif
extern void ahrs_mlkf_register(void); extern void ahrs_mlkf_register(void);
sw/airborne/subsystems/ahrs/ahrs_gx3.c
+3 -1
View File
@@ -339,7 +339,9 @@ void ahrs_gx3_init(void)
void ahrs_gx3_register(void) void ahrs_gx3_register(void)
{ {
ahrs_register_impl(ahrs_gx3_init, NULL); ahrs_gx3_init();
/// @TODO: provide enable function
ahrs_register_impl(NULL);
} }

Some files were not shown because too many files have changed in this diff Show More