mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
Merge pull request #2320 from enacuavlab/mekf_wind-integration
Mekf Wind INS
This commit is contained in:
Gautier Hattenberger
GitHub
@@ -42,3 +42,6 @@
|
||||
path = sw/ext/tudelft_gazebo_models
|
||||
url = https://github.com/tudelft/gazebo_models.git
|
||||
shallow = true
|
||||
[submodule "sw/ext/eigen"]
|
||||
path = sw/ext/eigen
|
||||
url = https://github.com/eigenteam/eigen-git-mirror.git
|
||||
|
||||
@@ -100,7 +100,7 @@ ifeq (,$(findstring $(RTOS_DEBUG),0 FALSE))
|
||||
else
|
||||
CH_OPT ?= 2 -ggdb
|
||||
endif
|
||||
USE_OPT = -std=gnu99 -O$(CH_OPT) \
|
||||
USE_OPT = -O$(CH_OPT) \
|
||||
-falign-functions=16 -fomit-frame-pointer \
|
||||
-W -Wall \
|
||||
$(PPRZ_DEFINITION)
|
||||
@@ -108,7 +108,7 @@ endif
|
||||
|
||||
# C specific options here (added to USE_OPT).
|
||||
ifeq ($(USE_COPT),)
|
||||
USE_COPT =
|
||||
USE_COPT = -std=gnu99
|
||||
endif
|
||||
|
||||
# C++ specific options here (added to USE_OPT).
|
||||
@@ -224,7 +224,8 @@ CSRC = $(STARTUPSRC) \
|
||||
$(FATFSSRC) \
|
||||
$(CHIBIOS_BOARD_MAIN)
|
||||
|
||||
ECSRC = $($(TARGET).srcs)
|
||||
ECSRC = $(filter %.c, $($(TARGET).srcs))
|
||||
ECPPSRC = $(filter %.cpp, $($(TARGET).srcs))
|
||||
|
||||
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
|
||||
# setting.
|
||||
@@ -275,8 +276,11 @@ CPPC = $(TRGT)g++
|
||||
# Enable loading with g++ only if you need C++ runtime support.
|
||||
# NOTE: You can use C++ even without C++ support if you are careful. C++
|
||||
# runtime support makes code size explode.
|
||||
ifeq ($(ECPPSRC),)
|
||||
LD = $(TRGT)gcc
|
||||
#LD = $(TRGT)g++
|
||||
else
|
||||
LD = $(TRGT)g++
|
||||
endif
|
||||
CP = $(TRGT)objcopy
|
||||
AS = $(TRGT)gcc -x assembler-with-cpp
|
||||
AR = $(TRGT)ar
|
||||
|
||||
+7
-1
@@ -48,6 +48,7 @@ DEBUG_FLAGS ?= -ggdb3
|
||||
|
||||
CSTANDARD ?= -std=gnu99
|
||||
CXXSTANDARD ?= -std=c++98
|
||||
CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include
|
||||
|
||||
# input files
|
||||
SRCS = $($(TARGET).srcs)
|
||||
@@ -107,6 +108,9 @@ CXXFLAGS += $(FLOAT_ABI)
|
||||
CXXFLAGS += $(ARCH_FLAGS)
|
||||
CXXFLAGS += $(USER_CFLAGS)
|
||||
CXXFLAGS += $(WARN_FLAGS)
|
||||
CXXFLAGS += $(CINCS)
|
||||
CXXFLAGS += $($(TARGET).CFLAGS)
|
||||
CXXFLAGS += $(BOARD_CFLAGS)
|
||||
|
||||
|
||||
CFLAGS += -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES)
|
||||
@@ -128,9 +132,11 @@ CFLAGS += $(BOARD_CFLAGS)
|
||||
ifneq ($(ARCH_L), )
|
||||
ifeq ($(ARCH_L),f4)
|
||||
CFLAGS += -DSTM32F4
|
||||
CXXFLAGS += -DSTM32F4
|
||||
endif
|
||||
else
|
||||
CFLAGS += -DSTM32F1
|
||||
CXXFLAGS += -DSTM32F1
|
||||
endif
|
||||
|
||||
# with cortex-m3 and m4 unaligned data can still be read
|
||||
@@ -227,7 +233,7 @@ sym: $(OBJDIR)/$(TARGET).sym
|
||||
.PRECIOUS : $(OBJS) $(AOBJ)
|
||||
%.elf: $(OBJS) $(AOBJ) | $(OBJDIR)
|
||||
@echo LD $@
|
||||
$(Q)$(LD) $(LDFLAGS) $($(TARGET).LDFLAGS) -o $@ $(COBJ) $(AOBJ) $(LDLIBS)
|
||||
$(Q)$(LD) $(LDFLAGS) $($(TARGET).LDFLAGS) -o $@ $(OBJS) $(AOBJ) $(LDLIBS)
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
|
||||
|
||||
@@ -166,6 +166,12 @@
|
||||
<field name="vz" type="float" unit="m/s"/>
|
||||
</message>
|
||||
|
||||
<message name="INCIDENCE" id="25">
|
||||
<field name="flag" type="uint8_t">bit 1: AoA is updated, bit 2: sideslip is updated</field>
|
||||
<field name="aoa" type="float" unit="rad">Angle of attack</field>
|
||||
<field name="sideslip" type="float" unit="rad">Sideslip angle</field>
|
||||
</message>
|
||||
|
||||
</msg_class>
|
||||
|
||||
</protocol>
|
||||
|
||||
@@ -0,0 +1,249 @@
|
||||
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||
|
||||
<airframe name="Zagi MEKF Wind">
|
||||
|
||||
<description>
|
||||
Zagi with ENAC Aero Probe
|
||||
- Propeller: 6 x 4
|
||||
- Motor: T-Motor 2200 rpm/v
|
||||
- Motor controller:
|
||||
- Radio modem: Xbee 2.4 Ghz
|
||||
- Radio control: Futaba R6303SB (s-bus)
|
||||
- GPS: Ublox M8
|
||||
- Autopilot; Chimera
|
||||
</description>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
|
||||
<target name="ap" board="chimera_1.0">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
|
||||
<module name="radio_control" type="sbus"/>
|
||||
<module name="ins" type="mekf_wind"/>
|
||||
<define name="LOG_MEKF_WIND" value="TRUE"/>
|
||||
<define name="AP_THREAD_STACK_SIZE" value="40*1024"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<configure name="PERIODIC_FREQUENCY" value="100"/>
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="ins" type="mekf_wind">
|
||||
<define name="BARO_BOARD_CHIMERA_FREQ" value="50"/>
|
||||
</module>
|
||||
|
||||
|
||||
<define name="USE_NPS_AIRSPEED"/>
|
||||
<define name="USE_NPS_AOA"/>
|
||||
<define name="USE_NPS_SIDESLIP"/>
|
||||
<define name="NPS_SYNC_INCIDENCE"/>
|
||||
<define name="WE_LOG_PATH" value="/tmp"/>
|
||||
<define name="LOG_MEKF_WIND" value="FALSE"/>
|
||||
</target>
|
||||
|
||||
<target name="sim" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<module name="ahrs" type="float_dcm"/>
|
||||
<module name="ins" type="alt_float"/>
|
||||
</target>
|
||||
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<module name="imu" type="chimera"/>
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B38400"/>
|
||||
</module>
|
||||
<!--DGPS-->
|
||||
<!--define name="USE_GPS_UBX_RTCM" value="TRUE"/-->
|
||||
|
||||
<module name="control" type="new"/>
|
||||
<module name="navigation"/>
|
||||
<module name="takeoff_detect"/>
|
||||
|
||||
<module name="tlsf"/>
|
||||
<module name="pprzlog"/>
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<module name="flight_recorder"/>
|
||||
|
||||
<module name="sys_mon"/>
|
||||
|
||||
<module name="pwm_meas"/>
|
||||
<module name="AOA_pwm">
|
||||
<configure name="AOA_PWM_CHANNEL" value="PWM_INPUT1"/>
|
||||
<configure name="SSA_PWM_CHANNEL" value="PWM_INPUT2"/>
|
||||
<define name="USE_AOA"/>
|
||||
<define name="USE_SIDESLIP"/>
|
||||
</module>
|
||||
<module name="air_data">
|
||||
<!--define name="AIR_DATA_BARO_ABS_ID" value="BARO_BOARD_SENDER_ID"/-->
|
||||
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
|
||||
<!--define name="AIR_DATA_BARO_DIFF_ID" value="BARODIFF_BOARD_SENDER_ID"/-->
|
||||
<!--define name="USE_AIRSPEED_AIR_DATA" value="FALSE"/-->
|
||||
</module>
|
||||
|
||||
<module name="airspeed" type="ms45xx_i2c">
|
||||
<define name="MS45XX_I2C_DEV" value="i2c1"/>
|
||||
<!--define name="USE_AIRSPEED_MS45XX" value="TRUE"/-->
|
||||
</module>
|
||||
|
||||
</firmware>
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" min="1040" neutral="1040" max="2000"/>
|
||||
<servo name="AILEVON_RIGHT" no="1" min="1100" neutral="1500" max="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="2" min="2000" neutral="1500" max="1100"/>
|
||||
</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>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="45." unit="deg"/>
|
||||
<define name="MAX_PITCH" value="30." unit="deg"/>
|
||||
</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="AOA">
|
||||
<define name="AOA_REVERSE" value="FALSE"/>
|
||||
<define name="AOA_SENS" value="(0.745104507767*2*M_PI/4096)"/>
|
||||
<define name="AOA_OFFSET" value="-125.2253" unit="deg"/>
|
||||
<define name="LOG_AOA" value="FALSE"/>
|
||||
<define name="SSA_REVERSE" value="TRUE"/>
|
||||
<define name="SSA_SENS" value="(1.0*2.0*M_PI/4096.)"/>
|
||||
<define name="SSA_OFFSET" value="42.0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="45." unit="deg"/>
|
||||
<define name="MAX_PITCH" value="30." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="GYRO_P_SIGN" value="-1"/>
|
||||
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||
<define name="GYRO_R_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="26"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="-28"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="253"/>
|
||||
<define name="ACCEL_X_SENS" value="2.44377554254" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="2.45827623443" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="2.44550194945" integer="16"/>
|
||||
|
||||
<define name="MAG_X_SIGN" value="-1"/>
|
||||
<define name="MAG_Y_SIGN" value="-1"/>
|
||||
<define name="MAG_Z_SIGN" value="1"/>
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="185"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-413"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-379"/>
|
||||
<define name="MAG_X_SENS" value="9.22861684868" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="10.1508941535" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="10.0108725363" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="4" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<!--muret-->
|
||||
<define name="H_X" value="0.5180"/>
|
||||
<define name="H_Y" value="-0.0071"/>
|
||||
<define name="H_Z" value="0.8554"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.5" unit="V"/>
|
||||
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-620)*4.536"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.06"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="3."/>
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.35"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.6"/>
|
||||
<!-- Climb loop (throttle) -->
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.08" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.007"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.008"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.09"/>
|
||||
<!-- Climb loop (pitch) -->
|
||||
<define name="AUTO_PITCH_PGAIN" value="0.024"/>
|
||||
<define name="AUTO_PITCH_DGAIN" value="0.013"/>
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_PITCH_MAX_PITCH" value="20" unit="deg"/>
|
||||
<define name="AUTO_PITCH_MIN_PITCH" value="-20" unit="deg"/>
|
||||
<!-- is it necessary? -->
|
||||
<define name="PITCH_TRIM" value="0." unit="deg"/>
|
||||
<define name="THROTTLE_SLEW" value="0.1"/>
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="0.4"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30." unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30." unit="deg"/>
|
||||
<define name="PITCH_PGAIN" value="10210"/>
|
||||
<define name="PITCH_IGAIN" value="322"/>
|
||||
<define name="PITCH_DGAIN" value="1485"/>
|
||||
|
||||
<define name="ROLL_MAX_SETPOINT" value="41.0000004297" unit="deg"/>
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="10041"/>
|
||||
</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="15." unit="deg"/>
|
||||
<define name="DEFAULT_PITCH" value="5." unit="deg"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="JSBSIM_LAUNCHSPEED" value="15"/>
|
||||
<define name="JSBSIM_MODEL" value="easystar" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_wind_estimator.h" type="string"/>
|
||||
</section>
|
||||
|
||||
|
||||
</airframe>
|
||||
@@ -19,6 +19,21 @@ else
|
||||
@$(CC) -c $(CFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
|
||||
endif
|
||||
|
||||
OBJS += $(ECOBJS)
|
||||
ECPPOBJS = $(sort $(addprefix $(OBJDIR)/, $(ECPPSRC:.cpp=.o)))
|
||||
|
||||
$(ECPPOBJS) : $(OBJDIR)/%.o : %.cpp Makefile
|
||||
ifeq ($(USE_VERBOSE_COMPILE),yes)
|
||||
@echo
|
||||
VPATH=
|
||||
test -d $(dir $@) || mkdir -p $(dir $@)
|
||||
$(CPPC) -c $(CPPFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
|
||||
else
|
||||
@echo Compiling $(<F)
|
||||
@VPATH=
|
||||
@test -d $(dir $@) || mkdir -p $(dir $@)
|
||||
@$(CPPC) -c $(CPPFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
|
||||
endif
|
||||
|
||||
OBJS += $(ECOBJS) $(ECPPOBJS)
|
||||
|
||||
# *** EOF ***
|
||||
|
||||
@@ -545,3 +545,13 @@ test_module.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
|
||||
test_module.srcs += $(COMMON_TELEMETRY_SRCS)
|
||||
test_module.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
|
||||
test_module.srcs += test/test_module.c
|
||||
|
||||
|
||||
test_eigen.ARCHDIR = $(ARCH)
|
||||
test_eigen.CFLAGS += $(COMMON_TEST_CFLAGS)
|
||||
test_eigen.CXXFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/eigen -Wno-shadow
|
||||
test_eigen.srcs += $(COMMON_TEST_SRCS)
|
||||
test_eigen.srcs += test/test_eigen.cpp
|
||||
test_eigen.srcs += pprz_syscalls.c
|
||||
test_eigen.LDFLAGS += -lstdc++
|
||||
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
<dl_settings>
|
||||
<dl_settings NAME="MS45XX">
|
||||
<dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="ms45xx.sync_send" type="bool" shortname="sync_send" module="modules/sensors/airspeed_ms45xx_i2c" param="MS45XX_SYNC_SEND" persistent="true"/>
|
||||
<dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="ms45xx.autoset_offset" type="bool" shortname="autoset offset" module="modules/sensors/airspeed_ms45xx_i2c" param="MS45XX_SYNC_SEND" persistent="true"/>
|
||||
<dl_setting min="0.9" max="1.2" step="0.01" var="ms45xx.pressure_scale" type="float" shortname="PressScale" module="modules/sensors/airspeed_ms45xx_i2c" param="MS45XX_PRESSURE_SCALE" persistent="true"/>
|
||||
<dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="ms45xx.pressure_type" type="bool" shortname="Type G" module="modules/sensors/airspeed_ms45xx_i2c" param="MS45XX_PRESSURE_TYPE" persistent="true"/>
|
||||
<dl_setting min="8300.0" max="8900.0" step="0.1" var="ms45xx.pressure_offset" type="float" shortname="PressOffset" module="modules/sensors/airspeed_ms45xx_i2c" param="MS45XX_PRESSURE_OFFSET" persistent="true"/>
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ins_mekf_wind" dir="ins">
|
||||
<doc>
|
||||
<description>
|
||||
MEKF-Wind INS estimator.
|
||||
Estimates attitude, velocity, position, (gyro, accel, baro) biases and wind velocity.
|
||||
Using Eigen for math operations.
|
||||
Only for fixed-wing firmware.
|
||||
</description>
|
||||
<configure name="USE_MAGNETOMETER" value="TRUE|FALSE" description="use magnetometer"/>
|
||||
<configure name="AHRS_ALIGNER_LED" value="2" description="LED number to indicate if AHRS/INS is aligned"/>
|
||||
<define name="LOG_MEKF_WIND" value="FALSE|TRUE" description="enable logging on SD card (default: FALSE)"/>
|
||||
<section name="MEKF_WIND" prefix="INS_MEKF_WIND_">
|
||||
<define name="DISABLE_WIND" value="FALSE|TRUE" description="Disable wind estimation (true by default)"/>
|
||||
<define name="P0_QUAT" value="0.007615" description="Initial covariance on quaternion"/>
|
||||
<define name="P0_SPEED" value="1E+2" description="Initial covariance on speed"/>
|
||||
<define name="P0_POS" value="1E+1" description="Initial covariance on position"/>
|
||||
<define name="P0_RATES_BIAS" value="1E-5" description="Initial covariance on gyrometers bias"/>
|
||||
<define name="P0_ACCEL_BIAS" value="1E-5" description="Initial covariance on accelerometers bias"/>
|
||||
<define name="P0_WIND" value="1." description="Initial covariance on wind estimate"/>
|
||||
<define name="Q_GYRO" value="1E-2" description="Process noise on gyrometers"/>
|
||||
<define name="Q_ACCEL" value="1E-2" description="Process noise on accelerometers"/>
|
||||
<define name="Q_RATES_BIAS" value="1E-6" description="Process noise on gyrometers bias"/>
|
||||
<define name="Q_ACCEL_BIAS" value="1E-6" description="Process noise on accelerometers bias"/>
|
||||
<define name="Q_BARO_BIAS" value="1E-3" description="Process noise on baro bias"/>
|
||||
<define name="Q_WIND" value="1." description="Process nois on wind estimate"/>
|
||||
<define name="R_SPEED" value="0.1" description="Measurement noise on speed"/>
|
||||
<define name="R_SPEED_Z" value="0.2" description="Measurement noise on vertical speed"/>
|
||||
<define name="R_POS" value="2." description="Measurement noise on position"/>
|
||||
<define name="R_POS_Z" value="4." description="Measurement noise on vertical position"/>
|
||||
<define name="R_MAG" value="1." description="Measurement noise on magnetometers"/>
|
||||
<define name="R_BARO" value="2." description="Measurement noise on barometer"/>
|
||||
<define name="R_AIRSPEED" value="0.1" description="Measurement noise en airspeed"/>
|
||||
<define name="R_AOA" value="0.1" description="Measurement noise on angle of attack"/>
|
||||
<define name="R_AOS" value="0.1" description="Measurement noise on sideslip angle"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="MEKF_Wind">
|
||||
<dl_setting min="0" max="1" step="1" var="ins_mekf_wind_params.disable_wind" module="ins/ins_mekf_wind" shortname="wind estimation" values="ENABLED|DISABLED"/>
|
||||
<dl_setting min="1E-3" max="1E-1" step="0.001" var="ins_mekf_wind_params.Q_gyro" module="ins/ins_mekf_wind" shortname="Q gyro" handler="update_Q_gyro"/>
|
||||
<dl_setting min="1E-3" max="1E-1" step="0.001" var="ins_mekf_wind_params.Q_accel" module="ins/ins_mekf_wind" shortname="Q accel" handler="update_Q_accel"/>
|
||||
<dl_setting min="1E-6" max="1E-5" step="0.00000001" var="ins_mekf_wind_params.Q_rates_bias" module="ins/ins_mekf_wind" shortname="Q rates bias" handler="update_Q_rates_bias"/>
|
||||
<dl_setting min="1E-6" max="1E-5" step="0.00000001" var="ins_mekf_wind_params.Q_accel_bias" module="ins/ins_mekf_wind" shortname="Q accel bias" handler="update_Q_accel_bias"/>
|
||||
<dl_setting min="1E-7" max="1E-5" step="0.00000001" var="ins_mekf_wind_params.Q_baro_bias" module="ins/ins_mekf_wind" shortname="Q baro bias" handler="update_Q_baro_bias"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.Q_wind" module="ins/ins_mekf_wind" shortname="Q wind" handler="update_Q_wind"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_speed" module="ins/ins_mekf_wind" shortname="R speed" handler="update_R_speed"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_speed_z" module="ins/ins_mekf_wind" shortname="R speed_z" handler="update_R_speed_z"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_pos" module="ins/ins_mekf_wind" shortname="R pos" handler="update_R_pos"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_pos_z" module="ins/ins_mekf_wind" shortname="R pos_z" handler="update_R_pos_z"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_mag" module="ins/ins_mekf_wind" shortname="R mag" handler="update_R_mag"/>
|
||||
<dl_setting min="1E-1" max="1E+2" step="0.01" var="ins_mekf_wind_params.R_baro" module="ins/ins_mekf_wind" shortname="R baro" handler="update_R_baro"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_airspeed" module="ins/ins_mekf_wind" shortname="R airpeed" handler="update_R_airspeed"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_aoa" module="ins/ins_mekf_wind" shortname="R aoa" handler="update_R_aoa"/>
|
||||
<dl_setting min="1E-1" max="1E+1" step="0.01" var="ins_mekf_wind_params.R_aos" module="ins/ins_mekf_wind" shortname="R aos" handler="update_R_aos"/>
|
||||
<dl_setting min="0" max="1" step="1" var="ins_mekf_wind.reset" module="ins/ins_mekf_wind_wrapper" shortname="reset" handler="Reset"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="ins_mekf_wind_wrapper.h"/>
|
||||
</header>
|
||||
<init fun="ins_mekf_wind_wrapper_init()"/>
|
||||
<makefile target="ap|nps" firmware="fixedwing">
|
||||
<configure name="CXXSTANDARD" value="-std=c++14"/>
|
||||
<configure name="USE_MAGNETOMETER" default="TRUE"/>
|
||||
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
|
||||
<include name="$(PAPARAZZI_SRC)/sw/ext/eigen"/>
|
||||
<file name="ahrs_aligner.c" dir="subsystems/ahrs"/>
|
||||
<file name="ins.c" dir="subsystems"/>
|
||||
<file name="ins_mekf_wind.cpp"/>
|
||||
<file name="ins_mekf_wind_wrapper.c"/>
|
||||
<flag name="LDFLAGS" value="lstdc++" />
|
||||
<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"/>
|
||||
</makefile>
|
||||
<makefile target="ap" firmware="fixedwing">
|
||||
<define name="EIGEN_NO_DEBUG"/>
|
||||
<flag name="CXXFLAGS" value="Wno-bool-compare"/>
|
||||
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
|
||||
<file name="pprz_syscalls.c" dir="."/>
|
||||
</makefile>
|
||||
<makefile target="sim" firmware="fixedwing">
|
||||
<define name="AHRS_TYPE_H" value="subsystems/ahrs/ahrs_sim.h" type="string"/>
|
||||
<define name="USE_AHRS"/>
|
||||
<file name="ahrs.c" dir="subsystems"/>
|
||||
<file name="ahrs_sim.c" dir="subsystems/ahrs"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -36,6 +36,7 @@
|
||||
<message name="AIR_DATA" period="1.3"/>
|
||||
<message name="ESC" period="0.9"/>
|
||||
<message name="LOGGER_STATUS" period="5.1"/>
|
||||
<message name="WIND_INFO_RET" period="0.1"/>
|
||||
</mode>
|
||||
<mode name="minimal">
|
||||
<message name="ALIVE" period="5"/>
|
||||
|
||||
@@ -103,7 +103,11 @@ void autopilot_init(void)
|
||||
autopilot.power_switch = false;
|
||||
#ifdef POWER_SWITCH_GPIO
|
||||
gpio_setup_output(POWER_SWITCH_GPIO);
|
||||
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
|
||||
#ifdef POWER_SWITCH_ENABLE
|
||||
autopilot_set_power_switch(POWER_SWITCH_ENABLE); // set initial status
|
||||
#else
|
||||
gpio_clear(POWER_SWITCH_GPIO); // by default POWER OFF
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// call firmware specific init
|
||||
|
||||
@@ -53,18 +53,32 @@
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Default autopilot thread stack size
|
||||
*/
|
||||
#ifndef AP_THREAD_STACK_SIZE
|
||||
#define AP_THREAD_STACK_SIZE 8192
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PPRZ/AP thread
|
||||
*/
|
||||
static void thd_ap(void *arg);
|
||||
static THD_WORKING_AREA(wa_thd_ap, 8192);
|
||||
static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE);
|
||||
static thread_t *apThdPtr = NULL;
|
||||
|
||||
/*
|
||||
* Default FBW thread stack size
|
||||
*/
|
||||
#ifndef FBW_THREAD_STACK_SIZE
|
||||
#define FBW_THREAD_STACK_SIZE 1024
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PPRZ/FBW thread
|
||||
*/
|
||||
static void thd_fbw(void *arg);
|
||||
static THD_WORKING_AREA(wa_thd_fbw, 1024);
|
||||
static THD_WORKING_AREA(wa_thd_fbw, FBW_THREAD_STACK_SIZE);
|
||||
static thread_t *fbwThdPtr = NULL;
|
||||
|
||||
/**
|
||||
|
||||
@@ -65,6 +65,11 @@ extern "C" {
|
||||
|
||||
static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT *PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY);
|
||||
|
||||
/** Convert temperature from Kelvin to Celsius */
|
||||
#define CelsiusOfKelvin(_t) (_t - 274.15f)
|
||||
/** Convert temperature from Celsius to Kelvin */
|
||||
#define KelvinOfCelsius(_t) (_t + 274.15f)
|
||||
|
||||
/**
|
||||
* Get absolute altitude from pressure (using simplified equation).
|
||||
* Referrence pressure is standard pressure at sea level
|
||||
@@ -165,6 +170,17 @@ static inline float pprz_isa_ref_pressure_of_height_full(float pressure, float h
|
||||
return pressure / pow(Trel, expo);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get ISA temperature from a MSL altitude
|
||||
*
|
||||
* @param alt AMSL altitude
|
||||
* @return temperature in ISA condition
|
||||
*/
|
||||
static inline float pprz_isa_temperature_of_altitude(float alt)
|
||||
{
|
||||
return PPRZ_ISA_SEA_LEVEL_TEMP - PPRZ_ISA_TEMP_LAPS_RATE * alt;
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
@@ -66,6 +66,13 @@ static abi_event temperature_ev;
|
||||
#endif
|
||||
static abi_event airspeed_ev;
|
||||
|
||||
/** ABI binding for incidence angles
|
||||
*/
|
||||
#ifndef AIR_DATA_INCIDENCE_ID
|
||||
#define AIR_DATA_INCIDENCE_ID ABI_BROADCAST
|
||||
#endif
|
||||
static abi_event incidence_ev;
|
||||
|
||||
/** Default factor to convert estimated airspeed (EAS) to true airspeed (TAS) */
|
||||
#ifndef AIR_DATA_TAS_FACTOR
|
||||
#define AIR_DATA_TAS_FACTOR 1.0
|
||||
@@ -162,6 +169,20 @@ static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float eas)
|
||||
}
|
||||
}
|
||||
|
||||
static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
|
||||
{
|
||||
if (bit_is_set(flag, 0)) {
|
||||
// update angle of attack
|
||||
air_data.aoa = aoa;
|
||||
stateSetAngleOfAttack_f(aoa);
|
||||
}
|
||||
if (bit_is_set(flag, 1)) {
|
||||
// update sideslip angle
|
||||
air_data.sideslip = sideslip;
|
||||
stateSetSideslip_f(sideslip);
|
||||
}
|
||||
}
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
@@ -224,6 +245,7 @@ void air_data_init(void)
|
||||
AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
|
||||
AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
|
||||
AbiBindMsgAIRSPEED(AIR_DATA_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
|
||||
AbiBindMsgINCIDENCE(AIR_DATA_INCIDENCE_ID, &incidence_ev, incidence_cb);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BARO_RAW, send_baro_raw);
|
||||
@@ -300,7 +322,7 @@ float get_tas_factor(float p, float t)
|
||||
* sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
|
||||
* convert input temp to Kelvin
|
||||
*/
|
||||
return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * (t + 274.15)) /
|
||||
return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * KelvinOfCelsius(t)) /
|
||||
(p * PPRZ_ISA_SEA_LEVEL_TEMP));
|
||||
}
|
||||
|
||||
@@ -315,6 +337,20 @@ float get_tas_factor(float p, float t)
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
{
|
||||
// update tas factor if requested
|
||||
if (air_data.calc_tas_factor) {
|
||||
if (air_data.pressure > 0.f && air_data.temperature > -900.f) {
|
||||
// compute air density from pressure and temperature
|
||||
air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature);
|
||||
}
|
||||
else {
|
||||
// compute air density from altitude in ISA condition
|
||||
const float z = air_data_get_amsl();
|
||||
const float p = pprz_isa_pressure_of_altitude(z);
|
||||
const float t = pprz_isa_temperature_of_altitude(z);
|
||||
air_data.tas_factor = get_tas_factor(p, CelsiusOfKelvin(t));
|
||||
}
|
||||
}
|
||||
return air_data.tas_factor * eas;
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Marton Brossard <martin.brossard@mines-paristech.fr>
|
||||
* Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/ins_mekf_wind.h
|
||||
*
|
||||
* Multiplicative Extended Kalman Filter in rotation matrix formulation.
|
||||
*
|
||||
* Estimate attitude, ground speed, position, gyro bias, accelerometer bias and wind speed.
|
||||
*/
|
||||
|
||||
#ifndef INS_MEKF_WIND_H
|
||||
#define INS_MEKF_WIND_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
// Settings
|
||||
struct ins_mekf_wind_parameters {
|
||||
float Q_gyro; ///< gyro process noise
|
||||
float Q_accel; ///< accel process noise
|
||||
float Q_rates_bias; ///< rates bias process noise
|
||||
float Q_accel_bias; ///< accel bias process noise
|
||||
float Q_baro_bias; ///< baro bias process noise
|
||||
float Q_wind; ///< wind process noise
|
||||
float R_speed; ///< speed measurement noise
|
||||
float R_pos; ///< pos measurement noise
|
||||
float R_speed_z; ///< vertical speed measurement noise
|
||||
float R_pos_z; ///< vertical pos measurement noise
|
||||
float R_mag; ///< mag measurement noise
|
||||
float R_baro; ///< baro measurement noise
|
||||
float R_airspeed; ///< airspeed measurement noise
|
||||
float R_aoa; ///< angle of attack measurement noise
|
||||
float R_aos; ///< sideslip angle measurement noise
|
||||
bool disable_wind; ///< disable wind estimation
|
||||
};
|
||||
|
||||
extern struct ins_mekf_wind_parameters ins_mekf_wind_params;
|
||||
|
||||
// Init functions
|
||||
extern void ins_mekf_wind_init(void);
|
||||
extern void ins_mekf_wind_align(struct FloatRates *gyro_bias,
|
||||
struct FloatQuat *quat);
|
||||
extern void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h);
|
||||
extern void ins_mekf_wind_reset(void);
|
||||
|
||||
// Filtering functions
|
||||
extern void ins_mekf_wind_propagate(struct FloatRates* gyro,
|
||||
struct FloatVect3* accel, float dt);
|
||||
extern void ins_mekf_wind_propagate_ahrs(struct FloatRates* gyro,
|
||||
struct FloatVect3* accel, float dt);
|
||||
extern void ins_mekf_wind_update_mag(struct FloatVect3* mag, bool attitude_only);
|
||||
extern void ins_mekf_wind_update_baro(float baro_alt);
|
||||
extern void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos,
|
||||
struct FloatVect3 *speed);
|
||||
extern void ins_mekf_wind_update_airspeed(float airspeed);
|
||||
extern void ins_mekf_wind_update_incidence(float aoa, float aos);
|
||||
|
||||
// Getter/Setter functions
|
||||
extern struct NedCoor_f ins_mekf_wind_get_pos_ned(void);
|
||||
extern void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p);
|
||||
extern struct NedCoor_f ins_mekf_wind_get_speed_ned(void);
|
||||
extern void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s);
|
||||
extern struct NedCoor_f ins_mekf_wind_get_accel_ned(void);
|
||||
extern struct FloatQuat ins_mekf_wind_get_quat(void);
|
||||
extern void ins_mekf_wind_set_quat(struct FloatQuat *quat);
|
||||
extern struct FloatRates ins_mekf_wind_get_body_rates(void);
|
||||
extern struct NedCoor_f ins_mekf_wind_get_wind_ned(void);
|
||||
extern struct NedCoor_f ins_mekf_wind_get_airspeed_body(void);
|
||||
extern float ins_mekf_wind_get_airspeed_norm(void);
|
||||
extern struct FloatVect3 ins_mekf_wind_get_accel_bias(void);
|
||||
extern struct FloatRates ins_mekf_wind_get_rates_bias(void);
|
||||
extern float ins_mekf_wind_get_baro_bias(void);
|
||||
|
||||
// Settings handlers
|
||||
extern void ins_mekf_wind_update_params(void);
|
||||
|
||||
#define ins_mekf_wind_update_Q_gyro(_v) { \
|
||||
ins_mekf_wind_params.Q_gyro = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_Q_accel(_v) { \
|
||||
ins_mekf_wind_params.Q_accel = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_Q_rates_bias(_v) { \
|
||||
ins_mekf_wind_params.Q_rates_bias = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_Q_accel_bias(_v) { \
|
||||
ins_mekf_wind_params.Q_accel_bias = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_Q_baro_bias(_v) { \
|
||||
ins_mekf_wind_params.Q_baro_bias = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_Q_wind(_v) { \
|
||||
ins_mekf_wind_params.Q_wind = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_speed(_v) { \
|
||||
ins_mekf_wind_params.R_speed = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_pos(_v) { \
|
||||
ins_mekf_wind_params.R_pos = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_speed_z(_v) { \
|
||||
ins_mekf_wind_params.R_speed_z = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_pos_z(_v) { \
|
||||
ins_mekf_wind_params.R_pos_z = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_mag(_v) { \
|
||||
ins_mekf_wind_params.R_mag = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_baro(_v) { \
|
||||
ins_mekf_wind_params.R_baro = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_airspeed(_v) { \
|
||||
ins_mekf_wind_params.R_airspeed = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_aoa(_v) { \
|
||||
ins_mekf_wind_params.R_aoa = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
#define ins_mekf_wind_update_R_aos(_v) { \
|
||||
ins_mekf_wind_params.R_aos = _v; \
|
||||
ins_mekf_wind_update_params(); \
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* INS_MEKF_WIND_H */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/ins_mekf_wind_wrapper.h
|
||||
*
|
||||
* Paparazzi specific wrapper to run MEKF-Wind INS filter.
|
||||
*/
|
||||
|
||||
#ifndef INS_MEKF_WIND_WRAPPER_H
|
||||
#define INS_MEKF_WIND_WRAPPER_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_orientation_conversion.h"
|
||||
|
||||
/** filter structure
|
||||
*/
|
||||
struct InsMekfWind {
|
||||
struct OrientationReps body_to_imu;
|
||||
bool is_aligned;
|
||||
bool baro_initialized;
|
||||
bool gps_initialized;
|
||||
bool reset;
|
||||
};
|
||||
|
||||
extern struct InsMekfWind ins_mekf_wind;
|
||||
|
||||
extern void ins_mekf_wind_wrapper_init(void);
|
||||
|
||||
#define ins_mekf_wind_wrapper_Reset(_v) { \
|
||||
ins_mekf_wind.reset = false; \
|
||||
ins_mekf_wind.baro_initialized = false; \
|
||||
ins_mekf_wind.gps_initialized = false; \
|
||||
ins_mekf_wind_reset(); \
|
||||
}
|
||||
|
||||
#endif /* INS_MEKF_WIND_WRAPPER_H */
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
#include BOARD_CONFIG
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h"
|
||||
#include "subsystems/abi.h"
|
||||
|
||||
#ifndef USE_AIRSPEED_ADC
|
||||
#define USE_AIRSPEED_ADC TRUE
|
||||
@@ -85,6 +86,8 @@ void airspeed_adc_update(void)
|
||||
airspeed_adc.airspeed = sim_air_speed;
|
||||
#endif //SITL
|
||||
|
||||
AbiSendMsgAIRSPEED(AIRSPEED_ADC_ID, airspeed_adc.airspeed);
|
||||
|
||||
#if USE_AIRSPEED_ADC
|
||||
stateSetAirspeed_f(airspeed_adc.airspeed);
|
||||
#endif
|
||||
|
||||
@@ -66,7 +66,7 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED
|
||||
* if not defined to be MS45XX_PRESSURE_OUTPUT_TYPE_InH2O then PSI is used
|
||||
* */
|
||||
#ifndef MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
|
||||
#define MS45XX_PRESSURE_OUTPUT_TYPE_InH2O 1
|
||||
#define MS45XX_PRESSURE_OUTPUT_TYPE_InH2O 0
|
||||
#endif
|
||||
|
||||
/** MS45xx pressure range in PSI or InH2O
|
||||
@@ -77,11 +77,18 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED
|
||||
#define MS45XX_PRESSURE_RANGE 1
|
||||
#endif
|
||||
|
||||
/* Pressure Type 0 = Differential, 1 = Gauge , note there are theoretical more types than 2, e.g. Absolute not implemented */
|
||||
/** Pressure Type 0 = Differential, 1 = Gauge
|
||||
* note there are theoretical more types than 2, e.g. Absolute not implemented */
|
||||
#ifndef MS45XX_PRESSURE_TYPE
|
||||
#define MS45XX_PRESSURE_TYPE 0
|
||||
#endif
|
||||
|
||||
/** Use low pass filter on pressure values
|
||||
*/
|
||||
#ifndef USE_AIRSPEED_LOWPASS_FILTER
|
||||
#define USE_AIRSPEED_LOWPASS_FILTER TRUE
|
||||
#endif
|
||||
|
||||
/** MS45xx output Type.
|
||||
* 0 = Output Type A with 10% to 90%
|
||||
* 1 = Output Type B with 5% to 95%
|
||||
@@ -96,7 +103,7 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED
|
||||
/** Conversion factor from psi to Pa */
|
||||
#define PSI_TO_PA 6894.75729
|
||||
|
||||
#ifdef MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
|
||||
#if MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
|
||||
#define OutputPressureToPa InH2O_TO_PA
|
||||
#else
|
||||
#define OutputPressureToPa PSI_TO_PA
|
||||
@@ -131,6 +138,12 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED
|
||||
#endif
|
||||
#endif
|
||||
|
||||
PRINT_CONFIG_VAR(MS45XX_OUTPUT_TYPE)
|
||||
PRINT_CONFIG_VAR(MS45XX_PRESSURE_TYPE)
|
||||
PRINT_CONFIG_VAR(MS45XX_PRESSURE_RANGE)
|
||||
PRINT_CONFIG_VAR(MS45XX_PRESSURE_SCALE)
|
||||
PRINT_CONFIG_VAR(MS45XX_PRESSURE_OFFSET)
|
||||
|
||||
/** Send a AIRSPEED_MS45XX message with every new measurement.
|
||||
* Mainly for debugging, use with caution, sends message at ~100Hz.
|
||||
*/
|
||||
@@ -156,7 +169,9 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED
|
||||
|
||||
struct AirspeedMs45xx ms45xx;
|
||||
static struct i2c_transaction ms45xx_trans;
|
||||
#ifdef USE_AIRSPEED_LOWPASS_FILTER
|
||||
static Butterworth2LowPass ms45xx_filter;
|
||||
#endif
|
||||
|
||||
static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
@@ -179,10 +194,10 @@ void ms45xx_i2c_init(void)
|
||||
|
||||
ms45xx_trans.status = I2CTransDone;
|
||||
// setup low pass filter with time constant and 100Hz sampling freq
|
||||
#ifdef USE_AIRSPEED_LOWPASS_FILTER
|
||||
#ifdef USE_AIRSPEED_LOWPASS_FILTER
|
||||
init_butterworth_2_low_pass(&ms45xx_filter, MS45XX_LOWPASS_TAU,
|
||||
MS45XX_I2C_PERIODIC_PERIOD, 0);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, ms45xx_downlink);
|
||||
@@ -197,8 +212,13 @@ void ms45xx_i2c_periodic(void)
|
||||
}
|
||||
}
|
||||
|
||||
#define AUTOSET_NB_MAX 20
|
||||
|
||||
void ms45xx_i2c_event(void)
|
||||
{
|
||||
static int autoset_nb = 0;
|
||||
static float autoset_offset = 0.f;
|
||||
|
||||
/* Check if transaction is succesfull */
|
||||
if (ms45xx_trans.status == I2CTransSuccess) {
|
||||
|
||||
@@ -218,11 +238,23 @@ void ms45xx_i2c_event(void)
|
||||
*/
|
||||
|
||||
float p_out = (p_raw * ms45xx.pressure_scale) - ms45xx.pressure_offset;
|
||||
#ifdef USE_AIRSPEED_LOWPASS_FILTER
|
||||
#ifdef USE_AIRSPEED_LOWPASS_FILTER
|
||||
ms45xx.pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_out);
|
||||
#else
|
||||
#else
|
||||
ms45xx.pressure = p_out;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if (ms45xx.autoset_offset) {
|
||||
if (autoset_nb < AUTOSET_NB_MAX) {
|
||||
autoset_offset += p_raw * ms45xx.pressure_scale;
|
||||
autoset_nb++;
|
||||
} else {
|
||||
ms45xx.pressure_offset = autoset_offset / (float)autoset_nb;
|
||||
autoset_offset = 0.f;
|
||||
autoset_nb = 0;
|
||||
ms45xx.autoset_offset = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* 11bit raw temperature, 5 LSB bits not used */
|
||||
uint16_t temp_raw = 0xFFE0 & (((uint16_t)(ms45xx_trans.buf[2]) << 8) |
|
||||
|
||||
@@ -36,6 +36,7 @@ struct AirspeedMs45xx {
|
||||
float airspeed_scale; ///< Quadratic scale factor to convert (differential) pressure to airspeed
|
||||
float pressure_scale; ///< Scaling factor from raw measurement to Pascal
|
||||
float pressure_offset; ///< Offset in Pascal
|
||||
bool autoset_offset; ///< Set offset value from current filtered value
|
||||
bool sync_send; ///< Flag to enable sending every new measurement via telemetry for debugging purpose
|
||||
};
|
||||
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
|
||||
#include "modules/sensors/aoa_adc.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "state.h"
|
||||
|
||||
// Messages
|
||||
@@ -84,6 +85,9 @@ void aoa_adc_update(void)
|
||||
prev_aoa = aoa_adc.angle;
|
||||
|
||||
#ifdef USE_AOA
|
||||
uint8_t flag = 1;
|
||||
float foo = 0.f;
|
||||
AbiSendMsgINCIDENCE(AOA_ADC_ID, flag, aoa_adc.angle, foo);
|
||||
stateSetAngleOfAttack_f(aoa_adc.angle);
|
||||
#endif
|
||||
|
||||
|
||||
@@ -36,6 +36,7 @@
|
||||
#include "mcu_periph/pwm_input.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#if LOG_AOA
|
||||
@@ -164,6 +165,9 @@ void aoa_pwm_init(void)
|
||||
|
||||
/* update, log and send */
|
||||
void aoa_pwm_update(void) {
|
||||
#if USE_AOA || USE_SIDESLIP
|
||||
uint8_t flag = 0;
|
||||
#endif
|
||||
static float prev_aoa = 0.0f;
|
||||
// raw duty cycle in usec
|
||||
uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
|
||||
@@ -178,7 +182,7 @@ void aoa_pwm_update(void) {
|
||||
prev_aoa = aoa_pwm.angle;
|
||||
|
||||
#if USE_AOA
|
||||
stateSetAngleOfAttack_f(aoa_pwm.angle);
|
||||
SetBit(flag, 0);
|
||||
#endif
|
||||
|
||||
#if USE_SIDESLIP
|
||||
@@ -195,7 +199,11 @@ void aoa_pwm_update(void) {
|
||||
ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle;
|
||||
prev_ssa = ssa_pwm.angle;
|
||||
|
||||
stateSetSideslip_f(ssa_pwm.angle);
|
||||
SetBit(flag, 1);
|
||||
#endif
|
||||
|
||||
#if USE_AOA || USE_SIDESLIP
|
||||
AbiSendMsgINCIDENCE(AOA_PWM_ID, flag, aoa_pwm.angle, ssa_pwm.angle);
|
||||
#endif
|
||||
|
||||
#if SEND_SYNC_AOA
|
||||
|
||||
@@ -0,0 +1,174 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Fake system calls
|
||||
*
|
||||
* based on:
|
||||
* ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
*/
|
||||
|
||||
/*
|
||||
* **** This file incorporates work covered by the following copyright and ****
|
||||
* **** permission notice: ****
|
||||
*
|
||||
* Copyright (c) 2009 by Michael Fischer. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* 3. Neither the name of the author nor the names of its contributors may
|
||||
* be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
|
||||
* THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
|
||||
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <signal.h>
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
#pragma GCC diagnostic ignored "-Wmissing-prototypes"
|
||||
|
||||
#ifndef USE_CHIBIOS_RTOS // already defined by chibios syscalls
|
||||
|
||||
__attribute__((used))
|
||||
int _read_r(struct _reent *r, int file, char * ptr, int len)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
(void)len;
|
||||
return -1;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
int _lseek_r(struct _reent *r, int file, int ptr, int dir)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
(void)dir;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
int _write_r(struct _reent *r, int file, char * ptr, int len)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
return len;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
int _close_r(struct _reent *r, int file)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
caddr_t _sbrk_r(struct _reent *r, int incr)
|
||||
{
|
||||
(void)r;
|
||||
(void)incr;
|
||||
return (caddr_t)-1;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
int _fstat_r(struct _reent *r, int file, struct stat * st)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)st;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
int _isatty_r(struct _reent *r, int fd)
|
||||
{
|
||||
(void)r;
|
||||
(void)fd;
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif // USE_CHIBIOS_RTOS
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((used))
|
||||
pid_t _getpid(void)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
__attribute__((noreturn))
|
||||
void _exit(int i) {
|
||||
(void)i;
|
||||
while(1);
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
|
||||
void _kill(void) {}
|
||||
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
/*** EOF ***/
|
||||
@@ -88,6 +88,32 @@
|
||||
#define MS45XX_SENDER_ID 40
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of airspeed sensors (message 14)
|
||||
*/
|
||||
#ifndef AIRSPEED_NPS_ID
|
||||
#define AIRSPEED_NPS_ID 1
|
||||
#endif
|
||||
|
||||
#ifndef AIRSPEED_ADC_ID
|
||||
#define AIRSPEED_ADC_ID 2
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of Incidence angles (message 24)
|
||||
*/
|
||||
#ifndef AOA_ADC_ID
|
||||
#define AOA_ADC_ID 1
|
||||
#endif
|
||||
|
||||
#ifndef AOA_PWM_ID
|
||||
#define AOA_PWM_ID 2
|
||||
#endif
|
||||
|
||||
#ifndef INCIDENCE_NPS_ID
|
||||
#define INCIDENCE_NPS_ID 20
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of AGL measurment modules that can be loaded (sonars,...) (message 2)
|
||||
*/
|
||||
@@ -130,6 +156,7 @@
|
||||
#ifndef AGL_RAY_SENSOR_GAZEBO_ID
|
||||
#define AGL_RAY_SENSOR_GAZEBO_ID 10
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of magnetometer sensors (including IMUs with mag)
|
||||
*/
|
||||
@@ -281,7 +308,7 @@
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of OPTICFLOW estimates (message 12)
|
||||
* IDs of OPTICFLOW estimates (message 11)
|
||||
*/
|
||||
#ifndef FLOW_OPTICFLOW_ID
|
||||
#define FLOW_OPTICFLOW_ID 1
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
#define EIGEN_NO_MALLOC 1
|
||||
#define EIGEN_NO_DEBUG 1
|
||||
#define EIGEN_NO_STATIC_ASSERT 1
|
||||
#define eigen_assert(_c) { }
|
||||
#define assert(ignore) ((void)0)
|
||||
|
||||
#include <Eigen/Dense>
|
||||
using namespace Eigen;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f m = Matrix3f::Random(3,3);
|
||||
Vector3f v = Vector3f::Zero();
|
||||
Quaternionf q = Quaternionf::Identity();
|
||||
|
||||
while(1) {
|
||||
v(0) += m(1,2);
|
||||
if (v(0) < -1000 || v(0) > 1000) { v(0) = 0.; }
|
||||
m(0,0) = v(0);
|
||||
q.normalize();
|
||||
|
||||
Matrix3f m2 = Matrix3f::Random(3,3);
|
||||
Matrix3f m3 = m * m2;
|
||||
volatile Matrix3f m4 = m3.inverse();
|
||||
}
|
||||
}
|
||||
|
||||
Submodule
+1
Submodule sw/ext/eigen added at dde02fceed
@@ -140,7 +140,7 @@ void nps_autopilot_run_step(double time)
|
||||
|
||||
#if USE_AIRSPEED || USE_NPS_AIRSPEED
|
||||
if (nps_sensors_airspeed_available()) {
|
||||
stateSetAirspeed_f((float)sensors.airspeed.value);
|
||||
AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
|
||||
Fbw(event_task);
|
||||
Ap(event_task);
|
||||
}
|
||||
@@ -167,22 +167,44 @@ void nps_autopilot_run_step(double time)
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_NPS_AOA
|
||||
// standalone angle of attack
|
||||
#if USE_NPS_AOA && !NPS_SYNC_INCIDENCE
|
||||
if (nps_sensors_aoa_available()) {
|
||||
stateSetAngleOfAttack_f((float)sensors.aoa.value);
|
||||
AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 1, (float)sensors.aoa.value, 0.f);
|
||||
Fbw(event_task);
|
||||
Ap(event_task);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_NPS_SIDESLIP
|
||||
// standalone sideslip
|
||||
#if USE_NPS_SIDESLIP && !NPS_SYNC_INCIDENCE
|
||||
if (nps_sensors_sideslip_available()) {
|
||||
stateSetSideslip_f((float)sensors.sideslip.value);
|
||||
AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 2, 0.f, (float)sensors.sideslip.value);
|
||||
Fbw(event_task);
|
||||
Ap(event_task);
|
||||
}
|
||||
#endif
|
||||
|
||||
// synchronized angle of attack and sideslip
|
||||
#if NPS_SYNC_INCIDENCE && USE_NPS_AOA && USE_NPS_SIDESLIP
|
||||
static uint8_t flag = 0;
|
||||
if (nps_sensors_aoa_available()) {
|
||||
SetBit(flag, 0);
|
||||
}
|
||||
if (nps_sensors_sideslip_available()) {
|
||||
SetBit(flag, 1);
|
||||
}
|
||||
if (flag == 3) {
|
||||
// both sensors are updated
|
||||
AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 3, (float)sensors.aoa.value, (float)sensors.sideslip.value);
|
||||
Fbw(event_task);
|
||||
Ap(event_task);
|
||||
flag = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
if (nps_bypass_ahrs) {
|
||||
sim_overwrite_ahrs();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user