Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 28 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 *"/>
</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>
</protocol>
conf/airframes/bebop.xml
+5 -1
View File
@@ -21,11 +21,15 @@
<subsystem name="imu" type="bebop"/>
<subsystem name="gps" type="furuno"/>
<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"/>
</firmware>
<modules main_freq="512">
<load name="geo_mag.xml"/>
<load name="send_imu_mag_current.xml"/>
<!--load name="logger_file.xml">
<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"
telemetry="telemetry/default_rotorcraft.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=""
gui_color="red"
/>
conf/conf_tests.xml
+1 -1
View File
@@ -358,7 +358,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.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=""
gui_color="red"
/>
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
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
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
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
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS
INS_CFLAGS += -DINS_UPDATE_FW_ESTIMATOR
INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
INS_CFLAGS += -DINS_FINV_USE_UTM
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.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)
conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile
+1
View File
@@ -1,5 +1,6 @@
# 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/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)/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 += -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)))
$(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/ahrs_sim.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 += -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 -*-
ins_CFLAGS = -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough.h\"
ins_srcs += $(SRC_SUBSYSTEMS)/ins.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
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_FC_CFLAGS = -DUSE_AHRS
AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER
AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
endif
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
# this is the secondary AHRS
AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
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)
ap.srcs += $(AHRS_SRCS)
AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_QUAT
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)
nps.srcs += $(AHRS_SRCS)
ap.CFLAGS += $(AHRS_FC_CFLAGS)
ap.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS)
nps.CFLAGS += $(AHRS_FC_CFLAGS)
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
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_FC_CFLAGS = -DUSE_AHRS
AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER
AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
endif
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), fcr float_cmpl_rmat))
# this is the secondary AHRS
AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
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)
ap.srcs += $(AHRS_SRCS)
AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_RMAT
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)
nps.srcs += $(AHRS_SRCS)
ap.CFLAGS += $(AHRS_FC_CFLAGS)
ap.srcs += $(AHRS_FC_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS)
nps.CFLAGS += $(AHRS_FC_CFLAGS)
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
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_MLKF_CFLAGS = -DUSE_AHRS
AHRS_MLKF_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER
AHRS_MLKF_CFLAGS += -DUSE_MAGNETOMETER
else
$(error ahrs_float_mlkf needs a magnetometer)
endif
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
AHRS_MLKF_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), mlkf))
# this is the secondary AHRS
AHRS_MLKF_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
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)
ap.srcs += $(AHRS_SRCS)
AHRS_MLKF_SRCS += subsystems/ahrs.c
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)
nps.srcs += $(AHRS_SRCS)
ap.CFLAGS += $(AHRS_MLKF_CFLAGS)
ap.srcs += $(AHRS_MLKF_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS)
nps.CFLAGS += $(AHRS_MLKF_CFLAGS)
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
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_ICE_CFLAGS = -DUSE_AHRS
AHRS_ICE_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER
AHRS_ICE_CFLAGS += -DUSE_MAGNETOMETER
endif
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
AHRS_ICE_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), ice int_cmpl_euler))
# this is the secondary AHRS
AHRS_ICE_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
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)
ap.srcs += $(AHRS_SRCS)
AHRS_ICE_SRCS += subsystems/ahrs.c
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)
nps.srcs += $(AHRS_SRCS)
ap.CFLAGS += $(AHRS_ICE_CFLAGS)
ap.srcs += $(AHRS_ICE_SRCS)
nps.CFLAGS += $(AHRS_ICE_CFLAGS)
nps.srcs += $(AHRS_ICE_SRCS)
#
# Simple simulation of the AHRS result
@@ -41,5 +55,5 @@ ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
sim.CFLAGS += $(ahrssim_CFLAGS)
sim.srcs += $(ahrssim_srcs)
test_ahrs.CFLAGS += $(AHRS_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS)
test_ahrs.CFLAGS += $(AHRS_ICE_CFLAGS)
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
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_ICQ_CFLAGS = -DUSE_AHRS
AHRS_ICQ_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
AHRS_CFLAGS += -DUSE_MAGNETOMETER
AHRS_ICQ_CFLAGS += -DUSE_MAGNETOMETER
endif
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
AHRS_ICQ_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS),ahrs_icq int_cmpl_quat))
# this is the secondary AHRS
AHRS_ICQ_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
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)
ap.srcs += $(AHRS_SRCS)
AHRS_ICQ_SRCS += subsystems/ahrs.c
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)
nps.srcs += $(AHRS_SRCS)
ap.CFLAGS += $(AHRS_ICQ_CFLAGS)
ap.srcs += $(AHRS_ICQ_SRCS)
test_ahrs.CFLAGS += $(AHRS_CFLAGS)
test_ahrs.srcs += $(AHRS_SRCS)
nps.CFLAGS += $(AHRS_ICQ_CFLAGS)
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 -*-
# attitude and speed estimation for fixedwings via invariant filter
# attitude and speed estimation via invariant filter
USE_MAGNETOMETER ?= 1
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS
# for geo mag
INS_CFLAGS += -DAHRS_FLOAT
INS_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
INS_CFLAGS += -DUSE_MAGNETOMETER
endif
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.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)
INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
@@ -32,5 +28,3 @@ ap.srcs += $(INS_SRCS)
#
nps.CFLAGS += $(INS_CFLAGS)
nps.srcs += $(INS_SRCS)
conf/messages.xml
+2 -1
View File
@@ -2029,8 +2029,9 @@
</message>
<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="value" type="uint16" />
<field name="value" type="uint16"/>
</message>
<!--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>
<dl_settings>
<dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_impl.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_impl.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.01" VAR="ins_impl.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_impl.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_impl.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_impl.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_impl.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_impl.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="1" STEP="1" VAR="ins_float_inv.reset" shortname="reset"/>
<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_float_inv.gains.lb" shortname="lb"/>
<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_float_inv.gains.mh" shortname="mh"/>
<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_float_inv.gains.nxz" shortname="nxz"/>
<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_float_inv.gains.nh" shortname="nh"/>
<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_float_inv.gains.ob" shortname="ob"/>
<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_float_inv.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.sh" shortname="sh"/>
</dl_settings>
</dl_settings>
</settings>