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

State interface input filter (#3409)

Browse Source 
* [generator] generate unique ID and names table for modules
* [state] add accessors for the state origin
* [state] don't access directly to state origin, use getters
* [state] filter inputs with module ID
* [ahrs] convert AHRS modules to state input filter
- selection of ahrs with settings from each module
- init functions for each ahrs modules
- remove old chimu
* update unit test
* [ins] decoupled INS implementation
To allow multiple INS at the same time:
- remove weak functions from ins.c
- remove the INS_TYPE_H define
- use ABI message to trigger INS reset
* [state] protect for c++
This commit is contained in:
Gautier Hattenberger
2024-12-03 22:34:27 +01:00
committed by GitHub
parent 32b0dc6944
commit dfd8e93927
109 changed files with 1168 additions and 2239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+4
View File
@@ -257,6 +257,10 @@
<field name="data" type="uint8_t*">pointer to the data of the matrix</field>
</message>
<message name="INS_RESET" id="38">
<field name="flag" type="uint8_t">flag to reset REF|VECTICAL_REF|VERTICAL_POS</field>
</message>
</msg_class>
</protocol>
conf/airframes/examples/lisa_l_chimu.xml
-147
View File
@@ -1,147 +0,0 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="LisaL Chimu">
<firmware name="fixedwing">
<target name="ap" board="lisa_l_1.0"/>
<target name="sim" board="pc">
<module name="ahrs" type="int_cmpl_quat"/>
</target>
<define name="AGR_CLIMB" />
<define name="LOITER_TRIM" />
<module name="radio_control" type="ppm"/>
<!-- Communication -->
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B9600"/>
</module>
<module name="control"/>
<!-- Sensors -->
<module name="gps" type="ublox"/>
<module name="navigation"/>
<module name="ins" type="alt_float"/>
<module name="ahrs_chimu_uart">
<configure name="CHIMU_UART_NR" value="3"/>
</module>
</firmware>
<servos>
<servo name="MOTOR" no="0" min="1290" neutral="1290" max="1810"/>
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1510" max="1000"/>
<servo name="AILEVON_RIGHT" no="2" min="1000" neutral="1535" max="2000"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.75"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="ATPL2\rATRN5\rATTT80\r" type="string"/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="0.03"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="1.4"/>
<define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
<define name="PITCH_PGAIN" value="12000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1250"/>
<define name="ROLL_SLEW" value="0.1"/>
<define name="ROLL_ATTITUDE_GAIN" value="7500"/>
<define name="ROLL_RATE_GAIN" value="1500"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
</airframe>
conf/modules/ahrs_chimu_spi.xml
-29
View File
@@ -1,29 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ahrs_chimu_spi" dir="ahrs" task="estimation">
<doc>
<description>CHimu (SPI)</description>
<define name="CHIMU_BIG_ENDIAN" value="TRUE" description="For older CHIMU v1.0 you should define CHIMU_BIG_ENDIAN"/>
</doc>
<dep>
<depends>spi_slave_hs</depends>
<provides>ahrs,imu</provides>
</dep>
<header>
<file name="ins_module.h" dir="modules/ins"/>
</header>
<init fun="ahrs_init()"/>
<event fun="parse_ins_msg()"/>
<makefile target="ap">
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<define name="USE_AHRS"/>
<define name="USE_INS_MODULE"/>
<define name="USE_SPI"/>
<define name="INS_LINK" value="spi_slave_hs"/>
<define name="USE_USB_HIGH_PCLK" />
<file name="ahrs_chimu_spi.c"/>
<file name="imu_chimu.c" dir="modules/ins"/>
<file name="ahrs.c"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_chimu.h" type="string"/>
</makefile>
</module>
conf/modules/ahrs_chimu_uart.xml
-30
View File
@@ -1,30 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ahrs_chimu_uart" dir="ahrs" task="estimation">
<doc>
<description>CHimu (UART)</description>
<configure name="CHIMU_UART_NR" value="3" description="UART"/>
<define name="CHIMU_BIG_ENDIAN" value="TRUE" description="For older CHIMU v1.0 you should define CHIMU_BIG_ENDIAN"/>
</doc>
<dep>
<depends>uart</depends>
<provides>ahrs,imu</provides>
</dep>
<header>
<file name="ins_module.h" dir="modules/ins"/>
</header>
<init fun="ahrs_init()"/>
<event fun="parse_ins_msg()"/>
<makefile target="ap">
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<define name="USE_AHRS"/>
<define name="USE_INS_MODULE"/>
<define name="USE_UART$(CHIMU_UART_NR)" value="1"/>
<define name="INS_LINK" value="uart$(CHIMU_UART_NR)"/>
<define name="UART$(CHIMU_UART_NR)_BAUD" value="B115200"/>
<file name="ahrs_chimu_uart.c"/>
<file name="imu_chimu.c" dir="modules/ins"/>
<file name="ahrs.c"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_chimu.h" type="string"/>
</makefile>
</module>
conf/modules/ahrs_common.xml
+1 -4
View File
@@ -18,10 +18,7 @@
<define name="USE_AHRS_ALIGNER"/>
<file name="ahrs.c"/>
<file name="ahrs_aligner.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_dcm"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_dcm_wrapper.h" type="string"/>
</test>
<test/>
</makefile>
<makefile target="sim">
<define name="USE_AHRS"/>
conf/modules/ahrs_float_cmpl_quat.xml
+10 -19
View File
@@ -9,6 +9,7 @@
By default uses magnetometer for heading for rotorcrafts.
For fixedwing firmware AHRS_GRAVITY_UPDATE_COORDINATED_TURN is enabled by default.
</description>
<configure name="AHRS_FC_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="USE_MAGNETOMETER" value="TRUE" description="set to FALSE to disable magnetometer"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE|TRUE" description="Use magnetometer to update all axes and not only yaw"/>
@@ -25,7 +26,8 @@
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_settings NAME="AHRS FC">
<dl_setting var="ahrs_fc_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_float_cmpl_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting var="ahrs_fc.gravity_heuristic_factor" min="0" step="1" max="50" module="modules/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR" type="uint8" persistent="true"/>
<dl_setting var="ahrs_fc.accel_omega" min="0.02" step="0.02" max="0.2" module="modules/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" type="float" persistent="true"/>
<dl_setting var="ahrs_fc.accel_zeta" min="0.7" step="0.05" max="1.5" module="modules/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" type="float" persistent="true"/>
@@ -39,37 +41,26 @@
<depends>ahrs_common,@imu,@gps|@mag</depends>
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs_float_cmpl_wrapper.h"/>
</header>
<init fun="ahrs_fc_wrapper_init()"/>
<makefile target="!sim|fbw" firmware="fixedwing">
<configure name="USE_MAGNETOMETER" default="0"/>
<define name="AHRS_USE_GPS_HEADING" cond="ifneq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
</makefile>
<makefile target="!sim|fbw">
<configure name="AHRS_FC_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_FC_TYPE" value="$(AHRS_FC_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="1"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<define name="AHRS_PROPAGATE_QUAT"/>
<file name="ahrs_float_cmpl.c"/>
<file name="ahrs_float_cmpl_wrapper.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_fc"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_cmpl_wrapper.h" type="string"/>
<define name="AHRS_FC_TYPE" value="AHRS_PRIMARY"/>
<define name="AHRS_PROPAGATE_QUAT"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_fc
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_fc
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_float_cmpl_rmat.xml
+11 -20
View File
@@ -6,6 +6,7 @@
AHRS using complementary filter in floating point.
Propagation is done in rotation matrix representation.
</description>
<configure name="AHRS_FC_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="USE_MAGNETOMETER" value="TRUE" description="set to FALSE to disable magnetometer"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE|TRUE" description="Use magnetometer to update all axes and not only yaw"/>
@@ -21,7 +22,8 @@
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_settings NAME="AHRS FC">
<dl_setting var="ahrs_fc_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_float_cmpl_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting var="ahrs_fc.gravity_heuristic_factor" min="0" step="1" max="50" module="modules/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR" type="uint8" persistent="true"/>
<dl_setting var="ahrs_fc.accel_omega" min="0.02" step="0.02" max="0.2" module="modules/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" type="float" persistent="true"/>
<dl_setting var="ahrs_fc.accel_zeta" min="0.7" step="0.05" max="1.5" module="modules/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" type="float" persistent="true"/>
@@ -35,37 +37,26 @@
<depends>ahrs_common,@imu,@gps|@mag</depends>
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs_float_cmpl_wrapper.h"/>
</header>
<init fun="ahrs_fc_wrapper_init()"/>
<makefile target="!sim|fbw" firmware="fixedwing">
<configure name="USE_MAGNETOMETER" default="0"/>
<define name="AHRS_USE_GPS_HEADING" cond="ifneq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
</makefile>
<makefile target="!sim|fbw">
<configure name="AHRS_FC_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_FC_TYPE" value="$(AHRS_FC_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="1"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<define name="AHRS_PROPAGATE_RMAT"/>
<file name="ahrs_float_cmpl.c"/>
<file name="ahrs_float_cmpl_wrapper.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_fc"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_cmpl_wrapper.h" type="string"/>
<define name="AHRS_PROPAGATE_RMAT"/>
<define name="AHRS_FC_TYPE" value="AHRS_PRIMARY"/>
<define name="AHRS_PROPAGATE_RMAT"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), fcr float_cmpl_rmat))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_fc
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_fc
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_cmpl_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_float_dcm.xml
+12 -21
View File
@@ -14,6 +14,7 @@
The name DCM for the algorithm is really a misnomer, as that just means that the orientation is represented as a DirectionCosineMatrix (rotation matrix).
But since people already know it under that name, we kept it.
</description>
<configure name="AHRS_DCM_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="USE_MAGNETOMETER" value="FALSE" description="set to FALSE to disable magnetometer"/>
<define name="USE_MAGNETOMETER_ONGROUND" description="use magnetic compensation before takeoff only while GPS course not good"/>
<define name="USE_AHRS_GPS_ACCELERATIONS" description="enable forward acceleration compensation from GPS speed"/>
@@ -23,9 +24,10 @@
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="modules/ahrs/ahrs" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="modules/ahrs/ahrs" />
<dl_settings NAME="AHRS DCM">
<dl_setting var="ahrs_dcm_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_float_dcm_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="modules/ahrs/ahrs_float_dcm" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="modules/ahrs/ahrs_float_dcm" />
</dl_settings>
</dl_settings>
</settings>
@@ -35,30 +37,19 @@
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs_float_dcm_wrapper.h"/>
</header>
<init fun="ahrs_dcm_wrapper_init()"/>
<makefile target="!sim|fbw">
<configure name="AHRS_DCM_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_DCM_TYPE" value="$(AHRS_DCM_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="0"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<file name="ahrs_float_dcm.c"/>
<file name="ahrs_float_dcm_wrapper.c"/>
<test firmware="fixedwing">
<define name="PRIMARY_AHRS" value="ahrs_dcm"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_dcm_wrapper.h" type="string"/>
<define name="AHRS_DCM_TYPE" value="AHRS_PRIMARY"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), dcm float_dcm))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_float_dcm_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_dcm
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_dcm_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_dcm
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_dcm_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_float_invariant.xml
+8 -19
View File
@@ -5,6 +5,7 @@
<description>
AHRS using invariant filter.
</description>
<configure name="AHRS_FINV_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="USE_MAGNETOMETER" value="TRUE" description="set to FALSE to disable magnetometer"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="SEND_INVARIANT_FILTER" description="send INV_FILTER message for debugging"/>
@@ -14,7 +15,8 @@
<settings>
<dl_settings>
<dl_settings NAME="invariant">
<dl_settings NAME="AHRS INV">
<dl_setting var="ahrs_finv_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_float_invariant_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ahrs_float_inv.reset" shortname="reset"/>
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.lx" shortname="lx" module="modules/ahrs/ahrs_float_invariant" param="AHRS_INV_LX"/>
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.ly" shortname="ly" param="AHRS_INV_LY"/>
@@ -32,30 +34,17 @@
<depends>ahrs_common,@imu,@mag</depends>
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs_float_invariant_wrapper.h"/>
</header>
<init fun="ahrs_finv_wrapper_init()"/>
<makefile target="!sim|fbw">
<configure name="USE_MAGNETOMETER" default="1"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<file name="ahrs_float_invariant.c"/>
<file name="ahrs_float_invariant_wrapper.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_float_invariant"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_invariant_wrapper.h" type="string"/>
<define name="AHRS_FINV_TYPE" value="AHRS_PRIMARY"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), invariant float_invariant))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_float_invariant_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_float_invariant
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_invariant_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_float_invariant
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_invariant_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_float_mlkf.xml
+6 -19
View File
@@ -9,6 +9,7 @@
Not suitable for fixedwings!
Suitable for rotorcraft. The magnetometer is used and needs to be well calibrated. Estimates attitude and heading. Does not use GPS.
</description>
<configure name="AHRS_MLKF_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE|TRUE" description="Use magnetometer to update all axes and not only yaw"/>
<define name="AHRS_MLKF_IMU_ID" value="ABI_BROADCAST" description="ABI sender id of IMU to use"/>
@@ -17,7 +18,8 @@
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_settings NAME="AHRS MLKF">
<dl_setting var="ahrs_mlkf_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_float_mlkf_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting var="ahrs_mlkf.mag_noise.x" min="0.0" step="0.02" max="1.0" module="modules/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X" type="float" persistent="true"/>
<dl_setting var="ahrs_mlkf.mag_noise.y" min="0.0" step="0.02" max="1.0" module="modules/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y" type="float" persistent="true"/>
<dl_setting var="ahrs_mlkf.mag_noise.z" min="0.0" step="0.02" max="1.0" module="modules/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z" type="float" persistent="true"/>
@@ -30,29 +32,14 @@
<provides>ahrs</provides>
</dep>
<makefile target="!sim|fbw">
<configure name="AHRS_MLKF_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_MLKF_TYPE" value="$(AHRS_MLKF_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="1"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<file name="ahrs_float_mlkf.c"/>
<file name="ahrs_float_mlkf_wrapper.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_mlkf"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_float_mlkf_wrapper.h" type="string"/>
<define name="AHRS_MLKF_TYPE" value="AHRS_PRIMARY"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), mlkf float_mlkf))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_float_mlkf_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_mlkf
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_mlkf_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_float_mlkf_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_int_cmpl_quat.xml
+14 -24
View File
@@ -20,6 +20,7 @@
- The arithmetic is fixed point and is thus suitable if the processor (on your board) has no FPU.
- Estimates the gyro bias.
</description>
<configure name="AHRS_ICQ_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="USE_MAGNETOMETER" value="TRUE" description="set to FALSE to disable magnetometer"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE|TRUE" description="Use magnetometer to update all axes and not only yaw"/>
@@ -38,7 +39,8 @@
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_settings NAME="AHRS ICQ">
<dl_setting var="ahrs_icq_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_int_cmpl_quat_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting var="ahrs_icq.gravity_heuristic_factor" min="0" step="1" max="50" module="modules/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR" type="uint8" persistent="true"/>
<dl_setting var="ahrs_icq.accel_omega" min="0.02" step="0.02" max="0.2" module="modules/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega" type="float" persistent="true"/>
<dl_setting var="ahrs_icq.accel_zeta" min="0.7" step="0.05" max="1.5" module="modules/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta" type="float" persistent="true"/>
@@ -52,6 +54,10 @@
<depends>ahrs_common,@imu,@mag|@gps</depends>
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs_int_cmpl_quat_wrapper.h"/>
</header>
<init fun="ahrs_icq_wrapper_init()"/>
<makefile target="!sim|fbw" firmware="fixedwing">
<configure name="USE_MAGNETOMETER" default="0"/>
@@ -59,36 +65,20 @@
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
</makefile>
<makefile target="!sim|fbw">
<configure name="AHRS_ICQ_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_ICQ_TYPE" value="$(AHRS_ICQ_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="1"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<file name="ahrs_int_cmpl_quat.c"/>
<file name="ahrs_int_cmpl_quat_wrapper.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_icq"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_int_cmpl_quat_wrapper.h" type="string"/>
<define name="USE_MAGNETOMETER"/>
<define name="AHRS_ICQ_TYPE" value="AHRS_PRIMARY"/>
<define name="USE_MAGNETOMETER"/>
</test>
<test>
<define name="PRIMARY_AHRS" value="ahrs_icq"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_int_cmpl_quat_wrapper.h" type="string"/>
<define name="AHRS_USE_GPS_HEADING"/>
<define name="USE_GPS"/>
<define name="AHRS_ICQ_TYPE" value="AHRS_PRIMARY"/>
<define name="AHRS_USE_GPS_HEADING"/>
<define name="USE_GPS"/>
</test>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), icq int_cmpl_quat))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_icq
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/ahrs_madgwick.xml
+8 -20
View File
@@ -5,13 +5,15 @@
<description>
AHRS using IMU (accel, gyro) only with Madwick implementation.
</description>
<configure name="AHRS_MADGWICK_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MADWICK_IMU_ID" value="ABI_BROADCAST" description="ABI sender id of IMU to use"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="madwick">
<dl_settings NAME="AHRS madgwick">
<dl_setting var="ahrs_madgwick_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_madgwick_wrapper" values="DISABLE|ENABLE" handler="enable"/>
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ahrs_madgwick.reset" shortname="reset"/>
</dl_settings>
</dl_settings>
@@ -22,33 +24,19 @@
<provides>ahrs</provides>
</dep>
<header>
<file name="ahrs.h"/>
<file name="ahrs_madgwick_wrapper.h"/>
</header>
<init fun="ahrs_madgwick_wrapper_init()"/>
<makefile target="!sim|fbw">
<configure name="AHRS_MADGWICK_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_MADGWICK_TYPE" value="$(AHRS_MADGWICK_TYPE)"/>
<configure name="USE_MAGNETOMETER" default="0"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<file name="ahrs_madgwick.c"/>
<file name="ahrs_madgwick_wrapper.c"/>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), madwick))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_madgwick
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_madgwick
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
endif
</raw>
<test>
<define name="PRIMARY_AHRS" value="ahrs_madgwick"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_madgwick_wrapper.h" type="string"/>
<define name="AHRS_MADGWICK_TYPE" value="AHRS_PRIMARY"/>
</test>
</makefile>
</module>
conf/modules/ahrs_sim.xml
+1 -5
View File
@@ -16,11 +16,7 @@
</header>
<periodic fun="update_ahrs_from_sim()"/>
<makefile target="sim">
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
<file name="ahrs_sim.c"/>
<test>
<define name="PRIMARY_AHRS" value="ahrs_sim"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
</test>
<test/>
</makefile>
</module>
conf/modules/ahrs_vectornav.xml
+13 -21
View File
@@ -5,19 +5,29 @@
<description>
Vectornav VN200 over uart used as AHRS.
</description>
<configure name="AHRS_VECTORNAV_TYPE" value="AHRS_PRIMARY|AHRS_SECONDARY" description="set if the AHRS is the primary source (default) or one of the secondary sources"/>
<configure name="VN_PORT" value="uart3" description="UART to use"/>
<configure name="VN_BAUD" value="B921600" description="UART baudrate"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="AHRS vectornav">
<dl_setting var="ahrs_vectornav_enable" min="0" step="1" max="1" module="modules/ahrs/ahrs_vectornav_wrapper" values="DISABLE|ENABLE" handler="enable"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>uart</depends>
<provides>ahrs,imu</provides>
</dep>
<header>
<file name="ahrs.h"/>
<file name="ahrs_vectornav_wrapper.h"/>
</header>
<init fun="ahrs_init()"/>
<init fun="ahrs_vectornav_wrapper_init()"/>
<event fun="ahrs_vectornav_event()"/>
<makefile target="ap">
<configure name="AHRS_VECTORNAV_TYPE" default="AHRS_PRIMARY"/>
<define name="AHRS_VECTORNAV_TYPE" value="$(AHRS_VECTORNAV_TYPE)"/>
<configure name="VN_PORT" default="uart3" case="upper|lower"/>
<configure name="VN_BAUD" default="B921600"/>
<define name="USE_$(VN_PORT_UPPER)"/>
@@ -30,26 +40,8 @@
<file name="ahrs_vectornav_wrapper.c"/>
<define name="USE_AHRS"/>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), vectornav))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_vectornav_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_vectornav
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_vectornav_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_vectornav
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_vectornav_wrapper.h\"
endif
</raw>
<test>
<define name="PRIMARY_AHRS" value="ahrs_vectornav"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_vectornav_wrapper.h" type="string"/>
<define name="AHRS_VECTORNAV_TYPE" value="AHRS_PRIMARY"/>
<define name="VN_PORT" value="uart0"/>
<define name="USE_UART0"/>
</test>
conf/modules/ins.xml
+1 -13
View File
@@ -5,24 +5,12 @@
<description>
simple INS with vertical filter.
</description>
<define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
<define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
<define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
<define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
<define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
</doc>
<dep>
<depends>@imu,@gps|@position|@velocity</depends>
<depends>ins_int_common</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_int.h" dir="modules/ins"/>
</header>
<init fun="ins_int_init()"/>
<makefile target="ap|nps">
<define name="INS_TYPE_H" value="modules/ins/ins_int.h" type="string"/>
<file name="ins.c"/>
<file name="ins_int.c"/>
<file name="vf_float.c"/>
<test firmware="rotorcraft"/>
</makefile>
conf/modules/ins_alt_float.xml
-1
View File
@@ -21,7 +21,6 @@
</header>
<init fun="ins_alt_float_init()"/>
<makefile target="ap|nps|sim" firmware="fixedwing">
<define name="INS_TYPE_H" value="modules/ins/ins_alt_float.h" type="string"/>
<file name="ins.c"/>
<file name="ins_alt_float.c"/>
<test firmware="fixedwing"/>
conf/modules/ins_ekf2.xml
-1
View File
@@ -70,7 +70,6 @@
<flag name="CXXFLAGS" value="Wno-unused-variable"/>
<!-- EKF2 files -->
<define name="INS_TYPE_H" value="modules/ins/ins_ekf2.h" type="string"/>
<file name="ins.c"/>
<file name="ins_ekf2.cpp"/>
conf/modules/ins_ext_pose.xml
-1
View File
@@ -26,7 +26,6 @@
<datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>
<makefile target="ap">
<define name="INS_TYPE_H" value="modules/ins/ins_ext_pose.h" type="string"/>
<define name="INS_EXT_POSE" value="TRUE"/>
<file name="ins_ext_pose.c"/>
<file name="ins.c"/>
conf/modules/ins_extended.xml
+1 -16
View File
@@ -5,12 +5,6 @@
<description>
extended INS with vertical filter using sonar.
</description>
<define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
<define name="INS_INT_AGL_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
<define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
<define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
<define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
<define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
<define name="INS_SONAR_MIN_RANGE" value="0.001" description="min sonar range in meters"/>
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="max sonar range in meters"/>
<define name="INS_SONAR_UPDATE_ON_AGL" value="FALSE" description="assume flat ground and use sonar for height"/>
@@ -21,7 +15,6 @@
<define name="VFF_EXTENDED_R_BARO" value="2." description="Barometer noise setting"/>
<define name="VFF_EXTENDED_NON_FLAT_GROUND" value="FALSE" description="VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height of the obstacles under the vehicle."/>
</doc>
<settings>
<dl_settings NAME="Ins Extended">
<dl_settings NAME="INS">
@@ -33,18 +26,10 @@
</dl_settings>
</settings>
<dep>
<depends>@imu,@gps</depends>
<depends>ins_int_common</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_int.h"/>
</header>
<init fun="ins_int_init()"/>
<makefile target="ap|nps">
<define name="INS_TYPE_H" value="modules/ins/ins_int.h" type="string"/>
<file name="ins.c"/>
<file name="ins_int.c"/>
<file name="vf_extended_float.c"/>
<define name="USE_VFF_EXTENDED"/>
<test firmware="rotorcraft"/>
conf/modules/ins_float_invariant.xml
-2
View File
@@ -48,14 +48,12 @@
<file name="ins_float_invariant_wrapper.c"/>
<define name="USE_AHRS_ALIGNER"/>
<define name="INS_TYPE_H" value="modules/ins/ins_float_invariant_wrapper.h" type="string"/>
<test firmware="rotorcraft"/>
<test firmware="fixedwing">
<define name="FIXEDWING_FIRMWARE"/>
</test>
</makefile>
<makefile target="sim">
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
<define name="USE_AHRS"/>
<file name="ahrs.c" dir="modules/ahrs"/>
<file name="ahrs_sim.c" dir="modules/ahrs"/>
conf/modules/ins_flow.xml
-5
View File
@@ -31,17 +31,12 @@
<!-- FLOW files -->
<!-- <define name="USE_AHRS"/> -->
<define name="USE_AHRS_ALIGNER"/>
<define name="INS_TYPE_H" value="modules/ins/ins_flow.h" type="string"/>
<file name="ins.c" dir="modules/ins/"/>
<file name="ins_flow.c" dir="modules/ins"/>
<file name="ahrs_int_cmpl_quat.c" dir="modules/ahrs"/>
<!-- <file name="ahrs.c" dir="subsystems"/> -->
<file name="ahrs_aligner.c" dir="modules/ahrs"/>
<!-- <file name="pprz_algebra_float.c" dir="math"/> -->
<raw>
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_flow.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
</raw>
<test firmware="rotorcraft">
<define name="MOTOR_MIXING_NB_MOTOR" value="4"/>
<define name="USE_MAGNETOMETER" value="true"/>
conf/modules/ins_gps_passthrough.xml
-3
View File
@@ -16,18 +16,15 @@
</header>
<init fun="ins_gps_passthrough_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough.c"/>
<test firmware="rotorcraft"/>
</makefile>
<makefile target="ap|sim|nps" firmware="fixedwing">
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough_utm.c"/>
</makefile>
<makefile target="ap|nps" firmware="rover">
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough.c"/>
</makefile>
conf/modules/ins_hff.xml
+1 -13
View File
@@ -5,24 +5,12 @@
<description>
INS with float vertical and horizontal filters.
</description>
<define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
<define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
<define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
<define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
<define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
</doc>
<dep>
<depends>@imu,@gps|@position|@velocity</depends>
<depends>ins_int_common</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_int.h"/>
</header>
<init fun="ins_int_init()"/>
<makefile target="ap|nps">
<define name="INS_TYPE_H" value="modules/ins/ins_int.h" type="string"/>
<file name="ins.c"/>
<file name="ins_int.c"/>
<file name="vf_float.c"/>
<file name="hf_float.c"/>
<define name="USE_HFF"/>
conf/modules/ins_hff_extended.xml
+1 -15
View File
@@ -5,12 +5,6 @@
<description>
extended INS with vertical filter using sonar.
</description>
<define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
<define name="INS_INT_AGL_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
<define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
<define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
<define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
<define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
<define name="INS_SONAR_MIN_RANGE" value="0.001" description="min sonar range in meters"/>
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="max sonar range in meters"/>
<define name="INS_SONAR_UPDATE_ON_AGL" value="FALSE" description="assume flat ground and use sonar for height"/>
@@ -27,18 +21,10 @@
</dl_settings>
</settings>
<dep>
<depends>@imu,@gps|@position|@velocity</depends>
<depends>ins_int_common</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_int.h"/>
</header>
<init fun="ins_int_init()"/>
<makefile target="ap|nps">
<define name="INS_TYPE_H" value="modules/ins/ins_int.h" type="string"/>
<file name="ins.c"/>
<file name="ins_int.c"/>
<file name="vf_extended_float.c"/>
<define name="USE_VFF_EXTENDED"/>
<file name="hf_float.c"/>
conf/modules/ins_int_common.xml
+27
View File
@@ -0,0 +1,27 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins_int_common" dir="ins" task="estimation">
<doc>
<description>
Common base module for INS_INT
</description>
<define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
<define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
<define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
<define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
<define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
</doc>
<dep>
<depends>@imu,@gps|@position|@velocity</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_int.h" dir="modules/ins"/>
</header>
<init fun="ins_int_init()"/>
<makefile target="ap|nps">
<file name="ins.c"/>
<file name="ins_int.c"/>
<test firmware="rotorcraft"/>
</makefile>
</module>
conf/modules/ins_mekf_wind.xml
-4
View File
@@ -80,15 +80,12 @@
<define name="EIGEN_NO_MALLOC"/>
<define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
<define name="USE_AHRS_ALIGNER"/>
<define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
<test firmware="fixedwing">
<define name="FIXEDWING_FIRMWARE"/>
<define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
<include name="../ext/eigen"/>
</test>
<test firmware="rotorcraft">
<define name="ROTORCRAFT_FIRMWARE"/>
<define name="INS_TYPE_H" value="modules/ins/ins_mekf_wind_wrapper.h" type="string"/>
<include name="../ext/eigen"/>
</test>
</makefile>
@@ -98,7 +95,6 @@
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
</makefile>
<makefile target="sim" firmware="fixedwing">
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
<define name="USE_AHRS"/>
<file name="ahrs.c" dir="modules/ahrs"/>
<file name="ahrs_sim.c" dir="modules/ahrs"/>
conf/modules/ins_nps.xml
+1 -14
View File
@@ -7,12 +7,9 @@
</description>
</doc>
<dep>
<depends>@imu,@gps</depends>
<depends>@imu,@gps,ins_gps_passthrough</depends>
<provides>ins,ahrs</provides>
</dep>
<header>
<file name="ins_gps_passthrough.h"/>
</header>
<init fun="ins_gps_passthrough_init()"/>
<makefile target="sim|nps">
<file name="imu.c" dir="modules/imu"/>
@@ -29,14 +26,4 @@
</test>
</makefile>
<makefile target="nps" firmware="rotorcraft">
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough.c"/>
</makefile>
<makefile target="sim|nps" firmware="fixedwing">
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough_utm.c"/>
</makefile>
</module>
conf/modules/ins_sim.xml
+1 -14
View File
@@ -7,20 +7,7 @@
</description>
</doc>
<dep>
<depends>@imu,@gps</depends>
<depends>@imu,@gps,ins_gps_passthrough</depends>
<provides>ins</provides>
</dep>
<header>
<file name="ins_gps_passthrough.h"/>
</header>
<init fun="ins_gps_passthrough_init()"/>
<makefile target="sim">
<file name="ins.c"/>
<file name="ins_gps_passthrough_utm.c"/>
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
<test firmware="rotorcraft">
<define name="PRIMARY_AHRS" value="ahrs_sim"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
</test>
</makefile>
</module>
conf/modules/ins_skeleton.xml
-1
View File
@@ -27,7 +27,6 @@
<file name="ins_skeleton.h"/>
</header>
<makefile target="ap|nps" firmware="rotorcraft">
<define name="INS_TYPE_H" value="modules/ins/ins_skeleton.h" type="string"/>
<file name="ins.c"/>
<file name="ins_skeleton.c"/>
<test firmware="rotorcraft"/>
conf/modules/ins_vectornav.xml
-1
View File
@@ -32,7 +32,6 @@
<file name="ins.c"/>
<file name="ins_vectornav.c"/>
<file name="ins_vectornav_wrapper.c"/>
<define name="INS_TYPE_H" value="modules/ins/ins_vectornav_wrapper.h" type="string"/>
<file name="gps.c" dir="modules/gps"/>
<define name="USE_GPS"/>
sw/airborne/arch/sim/modules/ins/ins_arduimu.c
+1 -1
View File
@@ -31,7 +31,7 @@ void ArduIMU_periodic(void)
sim_theta - ins_pitch_neutral,
0.
};
stateSetNedToBodyEulers_f(&att);
stateSetNedToBodyEulers_f(MODULE_INS_ARDUIMU_ID, &att);
}
void ArduIMU_periodicGPS(void) {}
void IMU_Daten_verarbeiten(void) {}
sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
+2 -2
View File
@@ -33,9 +33,9 @@ void ArduIMU_periodic(void)
sim_theta - ins_pitch_neutral,
0.
};
stateSetNedToBodyEulers_f(&att);
stateSetNedToBodyEulers_f(MODULE_INS_ARDUIMU_BASIC_ID, &att);
struct FloatRates rates = { sim_p, sim_q, sim_r };
stateSetBodyRates_f(&rates);
stateSetBodyRates_f(MODULE_INS_ARDUIMU_BASIC_ID, &rates);
}
void ArduIMU_periodicGPS(void) {}
void ArduIMU_event(void) {}
sw/airborne/firmwares/rotorcraft/navigation.c
+5 -5
View File
@@ -130,8 +130,8 @@ void nav_parse_MOVE_WP(uint8_t *buf)
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
stateGetLlaOrigin_i().alt;
waypoint_move_lla(wp_id, &lla);
}
}
@@ -224,7 +224,7 @@ static inline void nav_set_altitude(void)
// altitude mode
if (fabsf(nav.fp_altitude - last_alt) > 0.2f) {
nav.nav_altitude = nav.fp_altitude;
flight_altitude = nav.nav_altitude + state.ned_origin_f.hmsl;
flight_altitude = nav.nav_altitude + stateGetHmslOrigin_f();
last_alt = nav.fp_altitude;
}
}
@@ -233,14 +233,14 @@ static inline void nav_set_altitude(void)
/** Reset the geographic reference to the current GPS fix */
void nav_reset_reference(void)
{
ins_reset_local_origin();
AbiSendMsgINS_RESET(0, INS_RESET_REF);
/* update local ENU coordinates of global waypoints */
waypoints_localize_all();
}
void nav_reset_alt(void)
{
ins_reset_altitude_ref();
AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
waypoints_localize_all();
}
sw/airborne/firmwares/rotorcraft/navigation.h
+3 -3
View File
@@ -180,7 +180,7 @@ extern float flight_altitude; // hmsl flight altitude in meters
/// Get current y (north) position in local coordinates
#define GetPosY() (stateGetPositionEnu_f()->y)
/// Get current altitude above MSL
#define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
#define GetPosAlt() (stateGetPositionEnu_f()->z+stateGetHmslOrigin_f())
/// Get current height above reference
#define GetPosHeight() (stateGetPositionEnu_f()->z)
/**
@@ -189,7 +189,7 @@ extern float flight_altitude; // hmsl flight altitude in meters
* but might be updated later through a call to NavSetGroundReferenceHere() or
* NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
*/
#define GetAltRef() (state.ned_origin_f.hmsl)
#define GetAltRef() (stateGetHmslOrigin_f())
extern void nav_init(void);
@@ -381,7 +381,7 @@ static inline void NavGlide(uint8_t wp_start, uint8_t wp_end)
#define nav_SetNavRadius(x) {}
#define navigation_SetFlightAltitude(x) { \
flight_altitude = x; \
nav.nav_altitude = flight_altitude - state.ned_origin_f.hmsl; \
nav.nav_altitude = flight_altitude - stateGetHmslOrigin_f(); \
}
/** Unused compat macros
sw/airborne/firmwares/rover/navigation.c
+4 -4
View File
@@ -126,8 +126,8 @@ void nav_parse_MOVE_WP(uint8_t *buf)
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
lla.alt = DL_MOVE_WP_alt(buf) - stateGetHmslOrigin_i() +
stateGetLlaOrigin_i().alt;
waypoint_move_lla(wp_id, &lla);
}
}
@@ -175,14 +175,14 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
/** Reset the geographic reference to the current GPS fix */
void nav_reset_reference(void)
{
ins_reset_local_origin();
AbiSendMsgINS_RESET(0, INS_RESET_REF);
/* update local ENU coordinates of global waypoints */
waypoints_localize_all();
}
void nav_reset_alt(void)
{
ins_reset_altitude_ref();
AbiSendMsgINS_RESET(0, INS_RESET_VERTICAL_REF);
waypoints_localize_all();
}
sw/airborne/firmwares/rover/navigation.h
+2 -2
View File
@@ -135,7 +135,7 @@ extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval
/// Get current y (north) position in local coordinates
#define GetPosY() (stateGetPositionEnu_f()->y)
/// Get current altitude above MSL
#define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
#define GetPosAlt() (stateGetPositionEnu_f()->z+stateGetHmslOrigin_f())
/// Get current height above reference
#define GetPosHeight() (stateGetPositionEnu_f()->z)
/**
@@ -144,7 +144,7 @@ extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval
* but might be updated later through a call to NavSetGroundReferenceHere() or
* NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
*/
#define GetAltRef() (state.ned_origin_f.hmsl)
#define GetAltRef() (stateGetHmslOrigin_f())
/** Normalize a degree angle between 0 and 359 */
sw/airborne/modules/ahrs/ahrs.c
-70
View File
@@ -21,88 +21,18 @@
/**
* @file modules/ahrs/ahrs.c
* Dispatcher to register actual AHRS implementations.
*/
#include "modules/ahrs/ahrs.h"
#if USE_AHRS_ALIGNER
#include "modules/ahrs/ahrs_aligner.h"
#endif
#ifndef PRIMARY_AHRS
#error "PRIMARY_AHRS not set!"
#else
PRINT_CONFIG_VAR(PRIMARY_AHRS)
#endif
#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)
{
int i;
for (i=0; i < AHRS_NB_IMPL; i++) {
if (ahrs_impls[i].enable == NULL) {
ahrs_impls[i].enable = enable;
break;
}
}
}
void ahrs_init(void)
{
int i;
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);
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
}
int ahrs_switch(uint8_t idx)
{
if (idx >= AHRS_NB_IMPL) { return -1; }
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/modules/ahrs/ahrs.h
+6 -32
View File
@@ -21,7 +21,6 @@
/**
* @file modules/ahrs/ahrs.h
* Dispatcher to register actual AHRS implementations.
*/
#ifndef AHRS_H
@@ -29,6 +28,10 @@
#include "std.h"
#define AHRS_DISABLE 0
#define AHRS_PRIMARY 1
#define AHRS_SECONDARY 2
#define AHRS_COMP_ID_NONE 0
#define AHRS_COMP_ID_GENERIC 1
#define AHRS_COMP_ID_IR 2
@@ -39,43 +42,14 @@
#define AHRS_COMP_ID_FINV 7
#define AHRS_COMP_ID_MLKF 8
#define AHRS_COMP_ID_GX3 9
#define AHRS_COMP_ID_CHIMU 10
// CHIMU (10) is not supported anymore
#define AHRS_COMP_ID_VECTORNAV 11
#define AHRS_COMP_ID_EKF2 12
#define AHRS_COMP_ID_MADGWICK 13
#define AHRS_COMP_ID_FLOW 14
/* include actual (primary) implementation header */
#ifdef AHRS_TYPE_H
#include AHRS_TYPE_H
#endif
/* include secondary implementation header */
#ifdef AHRS_SECONDARY_TYPE_H
#include AHRS_SECONDARY_TYPE_H
#endif
typedef bool (*AhrsEnableOutput)(bool);
/* for settings when using secondary AHRS */
extern uint8_t ahrs_output_idx;
/**
* 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.
* Registers/initializes the default AHRS.
/** AHRS initialization.
*/
extern void ahrs_init(void);
/**
* Switch to the output of another AHRS impl.
* @param idx index of the AHRS impl (0 = PRIMARY_AHRS, 1 = SECONDARY_AHRS).
*/
extern int ahrs_switch(uint8_t idx);
#endif /* AHRS_H */
sw/airborne/modules/ahrs/ahrs_chimu.h
-46
View File
@@ -1,46 +0,0 @@
/*
* Copyright (C) 2014 Felix Ruess
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/ahrs/ahrs_chimu.h
*/
#ifndef AHRS_CHIMU_H
#define AHRS_CHIMU_H
#include "modules/ins/ins_module.h"
#include "modules/ahrs/ahrs.h"
struct AhrsChimu {
bool is_enabled;
bool is_aligned;
};
extern struct AhrsChimu ahrs_chimu;
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_chimu
#endif
extern void ahrs_chimu_register(void);
extern void ahrs_chimu_init(void);
#endif
sw/airborne/modules/ahrs/ahrs_chimu_spi.c
-170
View File
@@ -1,170 +0,0 @@
/*
C code to connect a CHIMU using uart
*/
#include <stdbool.h>
// SPI
#include "mcu_periph/spi.h"
#include "mcu_periph/spi_slave_hs_arch.h"
// Output
#include "state.h"
// For centripedal corrections
#include "modules/gps/gps.h"
#include "modules/ahrs/ahrs.h"
#include "generated/airframe.h"
#if CHIMU_DOWNLINK_IMMEDIATE
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#endif
#include "modules/ins/ins_module.h"
#include "modules/ins/imu_chimu.h"
#include "modules/ahrs/ahrs_chimu.h"
#include "led.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
struct AhrsChimu ahrs_chimu;
void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d);
#include "modules/core/abi.h"
/** ABI binding for gps data.
* Used for GPS ABI messages.
*/
#ifndef AHRS_CHIMU_GPS_ID
#define AHRS_CHIMU_GPS_ID GPS_MULTI_ID
#endif
PRINT_CONFIG_VAR(AHRS_CHIMU_GPS_ID)
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);
}
static bool ahrs_chimu_enable_output(bool enable)
{
ahrs_chimu.is_enabled = enable;
return ahrs_chimu.is_enabled;
}
void ahrs_chimu_register(void)
{
ahrs_chimu_init();
ahrs_register_impl(ahrs_chimu_enable_output);
AbiBindMsgGPS(AHRS_CHIMU_GPS_ID, &gps_ev, gps_cb);
}
void ahrs_chimu_init(void)
{
ahrs_chimu.is_enabled = true;
ahrs_chimu.is_aligned = false;
// uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
// Init
CHIMU_Init(&CHIMU_DATA);
// Quat Filter
CHIMU_Checksum(quaternions, 7);
InsSend(quaternions, 7);
// Wait a bit (SPI send zero)
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
// 50Hz data: attitude only
CHIMU_Checksum(rate, 12);
InsSend(rate, 12);
}
void parse_ins_msg(void)
{
struct link_device *dev = InsLinkDevice;
while (dev->char_available(dev->periph)) {
uint8_t ch = dev->get_byte(dev->periph);
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) {
// TODO: remove this flag as it doesn't work with multiple AHRS
new_ins_attitude = 1;
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
//FIXME
ahrs_chimu.is_aligned = true;
if (ahrs_chimu.is_enabled) {
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
struct FloatRates rates = {
CHIMU_DATA.m_sensor.rate[0],
CHIMU_DATA.m_attrates.euler.theta,
0.
}; // FIXME rate r
stateSetBodyRates_f(&rates);
}
} else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
&CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
#endif
}
}
}
}
void ahrs_chimu_update_gps(uint8_t gps_fix __attribute__((unused)), uint16_t gps_speed_3d)
{
// 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 };
float gps_speed = 0;
if (GpsFixValid()) {
gps_speed = gps_speed_3d / 100.;
}
gps_speed = FloatSwap(gps_speed);
memmove(&centripedal[6], &gps_speed, 4);
// Fill X-speed
CHIMU_Checksum(centripedal, 19);
InsSend(centripedal, 19);
// Downlink Send
}
sw/airborne/modules/ahrs/ahrs_chimu_uart.c
-118
View File
@@ -1,118 +0,0 @@
/*
C code to connect a CHIMU using uart
*/
#include <stdbool.h>
// Output
#include "state.h"
// For centripedal corrections
#include "modules/gps/gps.h"
#include "modules/ahrs/ahrs.h"
#include "generated/airframe.h"
#if CHIMU_DOWNLINK_IMMEDIATE
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#endif
#include "modules/ins/ins_module.h"
#include "modules/ins/imu_chimu.h"
#include "led.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
struct AhrsChimu ahrs_chimu;
static bool ahrs_chimu_enable_output(bool enable)
{
ahrs_chimu.is_enabled = enable;
return ahrs_chimu.is_enabled;
}
void ahrs_chimu_register(void)
{
ahrs_chimu_init();
ahrs_register_impl(ahrs_chimu_enable_output);
}
void ahrs_chimu_init(void)
{
ahrs_chimu.is_enabled = true;
ahrs_chimu.is_aligned = false;
uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
CHIMU_Init(&CHIMU_DATA);
// Request Software version
for (int i = 0; i < 7; i++) {
InsUartSend1(ping[i]);
}
// Quat Filter
for (int i = 0; i < 7; i++) {
InsUartSend1(quaternions[i]);
}
// 50Hz
CHIMU_Checksum(rate, 12);
InsSend(rate, 12);
}
void parse_ins_msg(void)
{
struct link_device *dev = InsLinkDevice;
while (dev->char_available(dev->periph)) {
uint8_t ch = dev->get_byte(dev->periph);
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
if (CHIMU_DATA.m_MsgID == 0x03) {
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
ahrs_chimu.is_aligned = true;
if (ahrs_chimu.is_enabled) {
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
}
#if CHIMU_DOWNLINK_IMMEDIATE
static uint8_t ahrs_chimu_id = AHRS_COMP_ID_CHIMU;
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice,
&CHIMU_DATA.m_attitude.euler.phi,
&CHIMU_DATA.m_attitude.euler.theta,
&CHIMU_DATA.m_attitude.euler.psi,
&ahrs_chimu_id);
#endif
}
}
}
}
sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
+28 -19
View File
@@ -29,13 +29,11 @@
#include "modules/core/abi.h"
#include "state.h"
#ifndef AHRS_FC_OUTPUT_ENABLED
#define AHRS_FC_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_FC_OUTPUT_ENABLED)
PRINT_CONFIG_VAR(AHRS_FC_TYPE)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_fc_output_enabled;
uint8_t ahrs_fc_enable;
static uint32_t ahrs_fc_last_stamp;
static uint8_t ahrs_fc_id = AHRS_COMP_ID_FC;
@@ -243,29 +241,30 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
compute_body_orientation_and_rates();
}
static bool ahrs_fc_enable_output(bool enable)
{
ahrs_fc_output_enabled = enable;
return ahrs_fc_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
#if (defined MODULE_AHRS_FLOAT_CMPL_QUAT_ID)
#define MODULE_AHRS_FLOAT_CMPL_ID MODULE_AHRS_FLOAT_CMPL_QUAT_ID
#elif (defined MODULE_AHRS_FLOAT_CMPL_RMAT_ID)
#define MODULE_AHRS_FLOAT_CMPL_ID MODULE_AHRS_FLOAT_CMPL_RMAT_ID
#else
#error "wrong ahrs_cmpl module type"
#endif
static void compute_body_orientation_and_rates(void)
{
if (ahrs_fc_output_enabled) {
/* Set state */
stateSetNedToBodyQuat_f(&ahrs_fc.ltp_to_body_quat);
stateSetBodyRates_f(&ahrs_fc.body_rate);
}
stateSetNedToBodyQuat_f(MODULE_AHRS_FLOAT_CMPL_ID, &ahrs_fc.ltp_to_body_quat);
stateSetBodyRates_f(MODULE_AHRS_FLOAT_CMPL_ID, &ahrs_fc.body_rate);
}
void ahrs_fc_register(void)
void ahrs_fc_wrapper_init(void)
{
ahrs_fc_output_enabled = AHRS_FC_OUTPUT_ENABLED;
ahrs_fc_init();
ahrs_register_impl(ahrs_fc_enable_output);
if (AHRS_FC_TYPE == AHRS_PRIMARY) {
ahrs_float_cmp_quat_wrapper_enable(1);
} else {
ahrs_float_cmp_quat_wrapper_enable(0);
}
/*
* Subscribe to scaled IMU measurements and attach callbacks
@@ -285,3 +284,13 @@ void ahrs_fc_register(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
#endif
}
void ahrs_float_cmp_quat_wrapper_enable(uint8_t enable)
{
if (enable) {
stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_CMPL_ID);
stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_CMPL_ID);
}
ahrs_fc_enable = enable;
}
sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.h
+4 -4
View File
@@ -29,10 +29,10 @@
#include "modules/ahrs/ahrs_float_cmpl.h"
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_fc
#endif
extern void ahrs_fc_wrapper_init(void);
extern void ahrs_fc_register(void);
// enable setting
extern uint8_t ahrs_fc_enable;
extern void ahrs_float_cmp_quat_wrapper_enable(uint8_t enable);
#endif /* AHRS_FLOAT_CMPL_WRAPPER_H */
sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
+24 -23
View File
@@ -29,13 +29,9 @@
#include "modules/core/abi.h"
#include "state.h"
#ifndef AHRS_DCM_OUTPUT_ENABLED
#define AHRS_DCM_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_DCM_OUTPUT_ENABLED)
PRINT_CONFIG_VAR(AHRS_DCM_TYPE)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_dcm_output_enabled;
uint8_t ahrs_dcm_enable;
static uint32_t ahrs_dcm_last_stamp;
static uint8_t ahrs_dcm_id = AHRS_COMP_ID_DCM;
@@ -162,33 +158,28 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ahrs_dcm_update_gps(gps_s);
}
static bool ahrs_dcm_enable_output(bool 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) {
/* Set the state */
stateSetBodyRates_f(&ahrs_dcm.body_rate);
/* Set the state */
stateSetBodyRates_f(MODULE_AHRS_FLOAT_DCM_ID, &ahrs_dcm.body_rate);
/* Convert eulers to RMaat and set state */
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_eulers(&ltp_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
}
/* Convert eulers to RMaat and set state */
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_eulers(&ltp_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
stateSetNedToBodyRMat_f(MODULE_AHRS_FLOAT_DCM_ID, &ltp_to_body_rmat);
}
void ahrs_dcm_register(void)
void ahrs_dcm_wrapper_init(void)
{
ahrs_dcm_output_enabled = AHRS_DCM_OUTPUT_ENABLED;
ahrs_dcm_init();
ahrs_register_impl(ahrs_dcm_enable_output);
if (AHRS_DCM_TYPE == AHRS_PRIMARY) {
ahrs_float_dcm_wrapper_enable(1);
} else {
ahrs_float_dcm_wrapper_enable(0);
}
/*
* Subscribe to scaled IMU measurements and attach callbacks
@@ -203,3 +194,13 @@ void ahrs_dcm_register(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
#endif
}
void ahrs_float_dcm_wrapper_enable(uint8_t enable)
{
if (enable) {
stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_DCM_ID);
stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_DCM_ID);
}
ahrs_dcm_enable = enable;
}
sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.h
+4 -4
View File
@@ -29,10 +29,10 @@
#include "modules/ahrs/ahrs_float_dcm.h"
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_dcm
#endif
extern void ahrs_dcm_wrapper_init(void);
extern void ahrs_dcm_register(void);
// enable setting
extern uint8_t ahrs_dcm_enable;
extern void ahrs_float_dcm_wrapper_enable(uint8_t enable);
#endif /* AHRS_FLOAT_DCM_WRAPPER_H */
sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
+24 -23
View File
@@ -32,13 +32,9 @@
#include "message_pragmas.h"
#include "state.h"
#ifndef AHRS_FINV_OUTPUT_ENABLED
#define AHRS_FINV_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_FINV_OUTPUT_ENABLED)
PRINT_CONFIG_VAR(AHRS_FINV_TYPE)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_finv_output_enabled;
uint8_t ahrs_finv_enable;
/** last gyro msg timestamp */
static uint32_t ahrs_finv_last_stamp = 0;
static uint8_t ahrs_finv_id = AHRS_COMP_ID_FINV;
@@ -188,34 +184,29 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe
ahrs_float_inv.mag_h = *h;
}
static bool ahrs_float_invariant_enable_output(bool enable)
{
ahrs_finv_output_enabled = enable;
return ahrs_finv_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static void compute_body_orientation_and_rates(void)
{
if (ahrs_finv_output_enabled) {
/* Set state */
stateSetNedToBodyQuat_f(&ahrs_float_inv.state.quat);
/* Set state */
stateSetNedToBodyQuat_f(MODULE_AHRS_FLOAT_INVARIANT_ID, &ahrs_float_inv.state.quat);
/* compute body rates */
struct FloatRates body_rate;
RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias);
stateSetBodyRates_f(&body_rate);
}
/* compute body rates */
struct FloatRates body_rate;
RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias);
stateSetBodyRates_f(MODULE_AHRS_FLOAT_INVARIANT_ID, &body_rate);
}
void ahrs_float_invariant_register(void)
void ahrs_finv_wrapper_init(void)
{
ahrs_finv_output_enabled = AHRS_FINV_OUTPUT_ENABLED;
ahrs_float_invariant_init();
ahrs_register_impl(ahrs_float_invariant_enable_output);
if (AHRS_FINV_TYPE == AHRS_PRIMARY) {
ahrs_float_invariant_wrapper_enable(1);
} else {
ahrs_float_invariant_wrapper_enable(0);
}
/*
* Subscribe to scaled IMU measurements and attach callbacks
@@ -232,3 +223,13 @@ void ahrs_float_invariant_register(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
#endif
}
void ahrs_float_invariant_wrapper_enable(uint8_t enable)
{
if (enable) {
stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_INVARIANT_ID);
stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_INVARIANT_ID);
}
ahrs_finv_enable = enable;
}
sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.h
+4 -4
View File
@@ -29,10 +29,10 @@
#include "modules/ahrs/ahrs_float_invariant.h"
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_float_invariant
#endif
extern void ahrs_finv_wrapper_init(void);
extern void ahrs_float_invariant_register(void);
// enable setting
extern uint8_t ahrs_finv_enable;
extern void ahrs_float_invariant_wrapper_enable(uint8_t enable);
#endif /* AHRS_FLOAT_INVARIANT_WRAPPER_H */
sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
+20 -20
View File
@@ -30,13 +30,9 @@
#include "modules/core/abi.h"
#include "state.h"
#ifndef AHRS_MLKF_OUTPUT_ENABLED
#define AHRS_MLKF_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)
PRINT_CONFIG_VAR(AHRS_MLKF_TYPE)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_mlkf_output_enabled;
uint8_t ahrs_mlkf_enable;
static uint32_t ahrs_mlkf_last_stamp;
static uint8_t ahrs_mlkf_id = AHRS_COMP_ID_MLKF;
@@ -183,29 +179,24 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe
ahrs_mlkf.mag_h = *h;
}
static bool ahrs_mlkf_enable_output(bool 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) {
/* Set in state interface */
stateSetNedToBodyQuat_f(&ahrs_mlkf.ltp_to_body_quat);
stateSetBodyRates_f(&ahrs_mlkf.body_rate);
}
/* Set in state interface */
stateSetNedToBodyQuat_f(MODULE_AHRS_FLOAT_MLKF_ID, &ahrs_mlkf.ltp_to_body_quat);
stateSetBodyRates_f(MODULE_AHRS_FLOAT_MLKF_ID, &ahrs_mlkf.body_rate);
}
void ahrs_mlkf_register(void)
void ahrs_mlkf_wrapper_init(void)
{
ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
ahrs_mlkf_init();
ahrs_register_impl(ahrs_mlkf_enable_output);
if (AHRS_MLKF_TYPE == AHRS_PRIMARY) {
ahrs_float_mlkf_wrapper_enable(1);
} else {
ahrs_float_mlkf_wrapper_enable(0);
}
/*
* Subscribe to scaled IMU measurements and attach callbacks
@@ -224,3 +215,12 @@ void ahrs_mlkf_register(void)
#endif
}
void ahrs_float_mlkf_wrapper_enable(uint8_t enable)
{
if (enable) {
stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_MLKF_ID);
stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_MLKF_ID);
}
ahrs_mlkf_enable = enable;
}
sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.h
+4 -4
View File
@@ -29,10 +29,10 @@
#include "modules/ahrs/ahrs_float_mlkf.h"
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_mlkf
#endif
extern void ahrs_mlkf_wrapper_init(void);
extern void ahrs_mlkf_register(void);
// enable setting
extern uint8_t ahrs_mlkf_enable;
extern void ahrs_float_mlkf_wrapper_enable(uint8_t enable);
#endif /* AHRS_FLOAT_MLKF_WRAPPER_H */
sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c
+21 -20
View File
@@ -29,13 +29,9 @@
#include "modules/core/abi.h"
#include "state.h"
#ifndef AHRS_ICQ_OUTPUT_ENABLED
#define AHRS_ICQ_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED)
PRINT_CONFIG_VAR(AHRS_ICQ_TYPE)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_icq_output_enabled;
uint8_t ahrs_icq_enable;
static uint32_t ahrs_icq_last_stamp;
static uint8_t ahrs_icq_id = AHRS_COMP_ID_ICQ;
@@ -235,27 +231,22 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ahrs_icq_update_gps(gps_s);
}
static bool ahrs_icq_enable_output(bool enable)
{
ahrs_icq_output_enabled = enable;
return ahrs_icq_output_enabled;
}
/** Rotate angles and rates from imu to body frame and set state */
static void set_body_state_from_quat(void)
{
if (ahrs_icq_output_enabled) {
/* Set state */
stateSetNedToBodyQuat_i(&ahrs_icq.ltp_to_body_quat);
stateSetBodyRates_i(&ahrs_icq.body_rate);
}
/* Set state */
stateSetNedToBodyQuat_i(MODULE_AHRS_INT_CMPL_QUAT_ID, &ahrs_icq.ltp_to_body_quat);
stateSetBodyRates_i(MODULE_AHRS_INT_CMPL_QUAT_ID, &ahrs_icq.body_rate);
}
void ahrs_icq_register(void)
void ahrs_icq_wrapper_init(void)
{
ahrs_icq_output_enabled = AHRS_ICQ_OUTPUT_ENABLED;
ahrs_icq_init();
ahrs_register_impl(ahrs_icq_enable_output);
if (AHRS_ICQ_TYPE == AHRS_PRIMARY) {
ahrs_int_cmpl_quat_wrapper_enable(1);
} else {
ahrs_int_cmpl_quat_wrapper_enable(0);
}
/*
* Subscribe to scaled IMU measurements and attach callbacks
@@ -275,3 +266,13 @@ void ahrs_icq_register(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
#endif
}
void ahrs_int_cmpl_quat_wrapper_enable(uint8_t enable)
{
if (enable) {
stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_INT_CMPL_QUAT_ID);
stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_INT_CMPL_QUAT_ID);
}
ahrs_icq_enable = enable;
}

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