Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ins] Add EKF2 from the PX4 ECL library (#2402)

Browse Source 
* [abi] Add timestamp to AGL and BARO_ABS

* [make] Add support for c++0x for all architectures

* [math] Add air density calculation

* [gps] Add horizontal and vertical position accuracy

* [boards] PX4FMU change default baudrate and add SBUS

* [ins] Add EKF2

* [tests] Add Bebop2 with EKF2

* [nps] Fix jsbsim simulator initialisation and GPS accuracy
This commit is contained in:
Freek van Tienen
2019-04-12 23:12:31 +02:00
committed by Gautier Hattenberger
parent f328eb14fa
commit 009b121cfd
89 changed files with 981 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+6
View File
@@ -44,3 +44,9 @@
[submodule "sw/ext/eigen"] [submodule "sw/ext/eigen"]
path = sw/ext/eigen path = sw/ext/eigen
url = https://github.com/eigenteam/eigen-git-mirror.git url = https://github.com/eigenteam/eigen-git-mirror.git
[submodule "sw/ext/ecl"]
path = sw/ext/ecl
url = https://github.com/PX4/ecl.git
[submodule "sw/ext/matrix"]
path = sw/ext/matrix
url = https://github.com/PX4/Matrix.git
conf/Makefile.chibios
+9
View File
@@ -74,6 +74,12 @@ else
$(TARGET).CFLAGS += -DCH_DBG_THREADS_PROFILING=TRUE $(TARGET).CFLAGS += -DCH_DBG_THREADS_PROFILING=TRUE
endif endif
#
# Add syscalls and c++ new/delete operators
#
$(TARGET).srcs += c++.cpp pprz_syscalls.c
$(TARGET).CXXFLAGS += -fno-sized-deallocation
# #
# General rules # General rules
# #
@@ -312,6 +318,9 @@ CPPWARN = -Wall -Wextra
# List all user C define here, like -D_DEBUG=1 # List all user C define here, like -D_DEBUG=1
UDEFS = $($(TARGET).CFLAGS) $(USER_CFLAGS) $(BOARD_CFLAGS) UDEFS = $($(TARGET).CFLAGS) $(USER_CFLAGS) $(BOARD_CFLAGS)
# List all extra user CPP define here
UPDEFS = $($(TARGET).CXXFLAGS)
# Define ASM defines here # Define ASM defines here
UADEFS = $($(TARGET).CFLAGS) $(USER_CFLAGS) $(BOARD_CFLAGS) UADEFS = $($(TARGET).CFLAGS) $(USER_CFLAGS) $(BOARD_CFLAGS)
conf/Makefile.linux
+3 -1
View File
@@ -37,6 +37,7 @@ OPT ?= 3
# Slightly bigger .elf files but gains the ability to decode macros # Slightly bigger .elf files but gains the ability to decode macros
DEBUG_FLAGS ?= -ggdb3 DEBUG_FLAGS ?= -ggdb3
CSTANDARD ?= -std=gnu99 CSTANDARD ?= -std=gnu99
CXXSTANDARD ?= -std=c++0x
CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include
# #
@@ -65,10 +66,11 @@ CFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
CXXFLAGS += $(CINCS) CXXFLAGS += $(CINCS)
CXXFLAGS += -O$(OPT) -fPIC CXXFLAGS += -O$(OPT) -fPIC
CXXFLAGS += $(DEBUG_FLAGS) CXXFLAGS += $(DEBUG_FLAGS)
CXXFLAGS += $(CXXSTANDARD)
CXXFLAGS += -pipe -fshow-column -ffast-math CXXFLAGS += -pipe -fshow-column -ffast-math
CXXFLAGS += -g -ffunction-sections -fdata-sections CXXFLAGS += -g -ffunction-sections -fdata-sections
CXXFLAGS += -Wall -Wextra CXXFLAGS += -Wall -Wextra
#CXXFLAGS += $($(TARGET).CFLAGS) CXXFLAGS += $($(TARGET).CFLAGS)
CXXFLAGS += $($(TARGET).CXXFLAGS) CXXFLAGS += $($(TARGET).CXXFLAGS)
CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS) CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
conf/Makefile.nps
+1
View File
@@ -56,6 +56,7 @@ CXXFLAGS += $($(TARGET).CXXFLAGS)
CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS) CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
CXXFLAGS += -O$(OPT) CXXFLAGS += -O$(OPT)
CXXFLAGS += $(DEBUG_FLAGS) CXXFLAGS += $(DEBUG_FLAGS)
CXXFLAGS += -std=c++0x
CXXFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) CXXFLAGS += $(shell pkg-config --cflags-only-I ivy-glib)
CXXFLAGS += -D_GNU_SOURCE CXXFLAGS += -D_GNU_SOURCE
conf/Makefile.stm32
+6 -2
View File
@@ -36,7 +36,6 @@ Q=@
# #
include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain
ifeq ($(ARCH_L),f4) ifeq ($(ARCH_L),f4)
MCU = cortex-m4 MCU = cortex-m4
else else
@@ -47,9 +46,13 @@ OPT ?= s
DEBUG_FLAGS ?= -ggdb3 DEBUG_FLAGS ?= -ggdb3
CSTANDARD ?= -std=gnu99 CSTANDARD ?= -std=gnu99
CXXSTANDARD ?= -std=c++98 CXXSTANDARD ?= -std=c++0x
CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include
# add syscalls and c++ new/delete operators
$(TARGET).srcs += c++.cpp pprz_syscalls.c
$(TARGET).CXXFLAGS += -fno-sized-deallocation
# input files # input files
SRCS = $($(TARGET).srcs) SRCS = $($(TARGET).srcs)
#ASRC = #ASRC =
@@ -145,6 +148,7 @@ CFLAGS += -DPPRZLINK_UNALIGNED_ACCESS=1
# C++ only flags # C++ only flags
CXXFLAGS += $(CXXSTANDARD) CXXFLAGS += $(CXXSTANDARD)
CXXFLAGS += -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES)
CXXFLAGS += -pipe -fshow-column -ffast-math CXXFLAGS += -pipe -fshow-column -ffast-math
CXXFLAGS += -ffunction-sections -fdata-sections CXXFLAGS += -ffunction-sections -fdata-sections
CXXFLAGS += $($(TARGET).CXXFLAGS) CXXFLAGS += $($(TARGET).CXXFLAGS)
conf/abi.xml
+2
View File
@@ -5,6 +5,7 @@
<msg_class name="airborne"> <msg_class name="airborne">
<message name="BARO_ABS" id="0"> <message name="BARO_ABS" id="0">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="pressure" type="float" unit="Pa"/> <field name="pressure" type="float" unit="Pa"/>
</message> </message>
@@ -13,6 +14,7 @@
</message> </message>
<message name="AGL" id="2"> <message name="AGL" id="2">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="distance" type="float" unit="m"/> <field name="distance" type="float" unit="m"/>
</message> </message>
conf/airframes/examples/bebop2_indi.xml
+24 -31
View File
@@ -4,7 +4,7 @@
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="bebop2"> <target name="ap" board="bebop2">
<define name="STABILIZATION_INDI_G2_R" value="0.20"/> <define name="STABILIZATION_INDI_G2_R" value="0.2784"/>
</target> </target>
<target name="nps" board="pc"> <target name="nps" board="pc">
@@ -19,16 +19,16 @@
<module name="radio_control" type="datalink"/> <module name="radio_control" type="datalink"/>
<module name="motor_mixing"/> <module name="motor_mixing"/>
<module name="actuators" type="bebop"/> <module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/> <module name="imu" type="bebop">
<define name="BEBOP_LOWPASS_FILTER" value="MPU60X0_DLPF_42HZ"/>
<define name="BEBOP_SMPLRT_DIV" value="0"/>
<define name="BEBOP_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_2000"/>
<define name="BEBOP_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_16G"/>
</module>
<module name="gps" type="ublox"/> <module name="gps" type="ublox"/>
<module name="stabilization" type="indi_simple"/> <module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat"> <module name="ins" type="ekf2"/>
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
</module>
<module name="ins" type="extended"/>
<module name="geo_mag"/>
<module name="air_data"/> <module name="air_data"/>
<module name="send_imu_mag_current"/> <module name="send_imu_mag_current"/>
<module name="gps" type="ubx_ucenter"/> <module name="gps" type="ubx_ucenter"/>
@@ -75,31 +75,24 @@
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<!-- Magneto calibration --> <!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="18"/> <define name="MAG_X_NEUTRAL" value="286"/>
<define name="MAG_Y_NEUTRAL" value="46"/> <define name="MAG_Y_NEUTRAL" value="-156"/>
<define name="MAG_Z_NEUTRAL" value="202"/> <define name="MAG_Z_NEUTRAL" value="-275"/>
<define name="MAG_X_SENS" value="8.32441255621" integer="16"/> <define name="MAG_X_SENS" value="7.074334106912319" integer="16"/>
<define name="MAG_Y_SENS" value="8.25720085664" integer="16"/> <define name="MAG_Y_SENS" value="7.069181442728008" integer="16"/>
<define name="MAG_Z_SENS" value="8.60009819293" integer="16"/> <define name="MAG_Z_SENS" value="7.411815536475566" integer="16"/>
</section>
<!-- local magnetic field --> <!--define name="ACCEL_X_NEUTRAL" value="11"/>
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field --> <define name="ACCEL_Y_NEUTRAL" value="-43"/>
<section name="AHRS" prefix="AHRS_"> <define name="ACCEL_Z_NEUTRAL" value="-124"/>
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module --> <define name="ACCEL_X_SENS" value="2.4523434144387464" integer="16"/>
<!-- Toulouse --> <define name="ACCEL_Y_SENS" value="2.428429001527193" integer="16"/>
<!--define name="H_X" value="0.513081"/> <define name="ACCEL_Z_SENS" value="2.423519834802844" integer="16"/-->
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section> </section>
<section name="INS" prefix="INS_"> <section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/> <define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> <!--define name="SONAR_UPDATE_ON_AGL" value="TRUE"/-->
</section> </section>
@@ -133,9 +126,9 @@
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_"> <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness --> <!-- control effectiveness -->
<define name="G1_P" value="0.05"/> <define name="G1_P" value="0.0390"/>
<define name="G1_Q" value="0.025"/> <define name="G1_Q" value="0.0473"/>
<define name="G1_R" value="0.0022"/> <define name="G1_R" value="0.00122"/>
<!-- For the bebop2 we need to filter the roll rate due to the dampers --> <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
<define name="FILTER_ROLL_RATE" value="FALSE"/><!-- Set to TRUE when flying without locked dampers --> <define name="FILTER_ROLL_RATE" value="FALSE"/><!-- Set to TRUE when flying without locked dampers -->
conf/airframes/tudelft/splash3.xml
+27 -40
View File
@@ -12,14 +12,15 @@
<description>Splash</description> <description>Splash</description>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<target name="ap" board="px4fmu_4.0_chibios"> <target name="ap" board="px4fmu_4.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<module name="radio_control" type="sbus"> <module name="radio_control" type="sbus">
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
</module> </module>
</target> </target>
<target name="nps" board="pc"> <target name="nps" board="pc">
<module name="radio_control" type="spektrum"/>
<module name="fdm" type="jsbsim"/> <module name="fdm" type="jsbsim"/>
</target> </target>
@@ -27,10 +28,7 @@
<module name="actuators" type="pwm"> <module name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/> <define name="SERVO_HZ" value="400"/>
</module> </module>
<module name="telemetry" type="transparent"> <module name="telemetry" type="transparent"/>
<configure name="MODEM_PORT" value="UART2"/> <!-- Telem1 on pixracer -->
<configure name="MODEM_BAUD" value="B57600"/>
</module>
<module name="imu" type="mpu9250_spi"> <module name="imu" type="mpu9250_spi">
<configure name="IMU_MPU9250_SPI_DEV" value="spi1"/> <configure name="IMU_MPU9250_SPI_DEV" value="spi1"/>
<configure name="IMU_MPU9250_SPI_SLAVE_IDX" value="SPI_SLAVE2"/> <configure name="IMU_MPU9250_SPI_SLAVE_IDX" value="SPI_SLAVE2"/>
@@ -42,12 +40,7 @@
<module name="gps" type="ubx_ucenter"/> <module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="int_quat"/> <module name="stabilization" type="int_quat"/>
<module name="stabilization" type="rate"/> <module name="stabilization" type="rate"/>
<module name="ahrs" type="float_cmpl_quat"> <module name="ins" type="ekf2"/>
<!--define name="AHRS_ICQ_MAG_ID" value="MAG_LIS3MDL_SENDER_ID" /-->
<configure name="USE_MAGNETOMETER" value="TRUE"/>
</module>
<module name="ins" type="hff"/>
<!--module name="ins" type="ekf2"/-->
<!--module name="mag_lis3mdl"> <!--module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/> <define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="i2c1"/> <configure name="MAG_LIS3MDL_I2C_DEV" value="i2c1"/>
@@ -115,24 +108,17 @@
<define name="ACCEL_Y_SENS" value="4.85140885386" integer="16"/> <define name="ACCEL_Y_SENS" value="4.85140885386" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.89977338537" integer="16"/--> <define name="ACCEL_Z_SENS" value="4.89977338537" integer="16"/-->
<!--define name="MAG_X_CURRENT_COEF" value="9.037571651280377"/> <define name= "MAG_X_CURRENT_COEF" value="-1.8609825961051705"/>
<define name="MAG_Y_CURRENT_COEF" value="-4.490670308897883"/> <define name= "MAG_Y_CURRENT_COEF" value="-1.6568817208655613"/>
<define name="MAG_Z_CURRENT_COEF" value="30.7029351308597"/--> <define name= "MAG_Z_CURRENT_COEF" value="16.329866025311297"/>
<!-- replace this with your own calibration --> <!-- replace this with your own calibration -->
<!--define name="MAG_X_NEUTRAL" value="2197"/> <define name="MAG_X_NEUTRAL" value="-48"/>
<define name="MAG_Y_NEUTRAL" value="1460"/> <define name="MAG_Y_NEUTRAL" value="69"/>
<define name="MAG_Z_NEUTRAL" value="288"/> <define name="MAG_Z_NEUTRAL" value="155"/>
<define name="MAG_X_SENS" value="0.621654140011012" integer="16"/> <define name="MAG_X_SENS" value="7.646781508055661" integer="16"/>
<define name="MAG_Y_SENS" value="0.5993861893464758" integer="16"/> <define name="MAG_Y_SENS" value="7.71865134635356" integer="16"/>
<define name="MAG_Z_SENS" value="0.6238423840038542" integer="16"/--> <define name="MAG_Z_SENS" value="7.691679165490501" integer="16"/>
<define name="MAG_X_NEUTRAL" value="26"/>
<define name="MAG_Y_NEUTRAL" value="32"/>
<define name="MAG_Z_NEUTRAL" value="143"/>
<define name="MAG_X_SENS" value="7.891481623127505" integer="16"/>
<define name="MAG_Y_SENS" value="8.144089259820777" integer="16"/>
<define name="MAG_Z_SENS" value="8.186555447435524" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0" unit="deg"/> <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/> <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -166,13 +152,6 @@
<define name="IGAIN_R" value="50"/> <define name="IGAIN_R" value="50"/>
</section> </section>
<section name="INS" prefix="INS_">
<!-- Use GPS altitude measurments and set the R gain -->
<!-- define name="USE_GPS_ALT" value="1"/ -->
<!-- define name="VFF_R_GPS" value="0.01"/ -->
<!--<define name="VFF_VZ_R_GPS" value="0.01"/-->
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_"> <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints --> <!-- setpoints -->
<define name="SP_MAX_PHI" value="35." unit="deg"/> <define name="SP_MAX_PHI" value="35." unit="deg"/>
@@ -212,9 +191,9 @@
</section> </section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_"> <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/> <define name="HOVER_KP" value="250"/>
<define name="HOVER_KD" value="80"/> <define name="HOVER_KD" value="90"/>
<define name="HOVER_KI" value="20"/> <define name="HOVER_KI" value="5"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/> <define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/> <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section> </section>
@@ -222,15 +201,23 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="35" unit="deg"/> <define name="MAX_BANK" value="35" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/> <define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/> <define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/> <define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/> <define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/> <define name="IGAIN" value="20"/>
</section> </section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT"> <section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/> <define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section> </section>
conf/boards/px4fmu_4.0.makefile
+2 -1
View File
@@ -39,6 +39,7 @@ SYS_TIME_LED ?= 1
# #
# default UART configuration (RC receiver, telemetry modem, GPS) # default UART configuration (RC receiver, telemetry modem, GPS)
# #
SBUS_PORT ?= UART6
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART6 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART6
#RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART7 #RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART7
@@ -46,7 +47,7 @@ MODEM_PORT ?= UART1
MODEM_BAUD ?= B57600 MODEM_BAUD ?= B57600
GPS_PORT ?= UART4 GPS_PORT ?= UART4
GPS_BAUD ?= B38400 GPS_BAUD ?= B57600
# #
# default actuator configuration # default actuator configuration
conf/chibios/chibios_rules.mk
+1 -1
View File
@@ -118,7 +118,7 @@ ODFLAGS = -x --syms
ASFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.s=.lst)) $(ADEFS) ASFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.s=.lst)) $(ADEFS)
ASXFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.S=.lst)) $(ADEFS) ASXFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.S=.lst)) $(ADEFS)
CFLAGS = $(MCFLAGS) $(OPT) $(COPT) $(CWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.c=.lst)) $(DEFS) CFLAGS = $(MCFLAGS) $(OPT) $(COPT) $(CWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.c=.lst)) $(DEFS)
CPPFLAGS = $(MCFLAGS) $(OPT) $(CPPOPT) $(CPPWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.cpp=.lst)) $(DEFS) CPPFLAGS = $(MCFLAGS) $(OPT) $(CPPOPT) $(CPPWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.cpp=.lst)) $(DEFS) $(UPDEFS)
LDFLAGS = $(MCFLAGS) $(OPT) -nostartfiles $(LLIBDIR) -Wl,-Map=$(BUILDDIR)/$(PROJECT).map,--cref,--no-warn-mismatch,--library-path=$(RULESPATH)/ld,--script=$(LDSCRIPT)$(LDOPT) LDFLAGS = $(MCFLAGS) $(OPT) -nostartfiles $(LLIBDIR) -Wl,-Map=$(BUILDDIR)/$(PROJECT).map,--cref,--no-warn-mismatch,--library-path=$(RULESPATH)/ld,--script=$(LDSCRIPT)$(LDOPT)
# Thumb interwork enabled only if needed because it kills performance. # Thumb interwork enabled only if needed because it kills performance.
conf/conf_example.xml
+1 -1
View File
@@ -161,7 +161,7 @@
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml [modules/geo_mag.xml] modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red" gui_color="red"
/> />
<aircraft <aircraft
conf/conf_tests.xml
+11
View File
@@ -340,6 +340,17 @@
settings_modules="modules/geo_mag.xml modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" settings_modules="modules/geo_mag.xml modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue" gui_color="blue"
/> />
<aircraft
name="bebop2"
ac_id="35"
airframe="airframes/examples/bebop2_indi.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red"
/>
<aircraft <aircraft
name="quad_mavlink" name="quad_mavlink"
ac_id="36" ac_id="36"
conf/firmwares/fixedwing.makefile
+1
View File
@@ -21,6 +21,7 @@ SRC_MODULES=modules
FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING) -I$(SRC_BOARD) FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING) -I$(SRC_BOARD)
VPATH += $(PAPARAZZI_HOME)/var/share VPATH += $(PAPARAZZI_HOME)/var/share
VPATH += $(PAPARAZZI_HOME)/sw/ext
# Standard Fixed Wing Code # Standard Fixed Wing Code
include $(CFG_FIXEDWING)/autopilot.makefile include $(CFG_FIXEDWING)/autopilot.makefile
conf/firmwares/rotorcraft.makefile
+1
View File
@@ -37,6 +37,7 @@ ap.ARCHDIR = $(ARCH)
VPATH += $(PAPARAZZI_HOME)/var/share VPATH += $(PAPARAZZI_HOME)/var/share
VPATH += $(PAPARAZZI_HOME)/sw/ext
###################################################################### ######################################################################
## ##
conf/modules/ins_ekf2.xml
+46
View File
@@ -0,0 +1,46 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ekf2" dir="ins">
<doc>
<description>
simple INS and AHRS using EKF2 from PX4
</description>
</doc>
<header>
<file name="ins_ekf2.h" dir="subsystems/ins"/>
</header>
<init fun="ins_ekf2_init()"/>
<periodic fun="ins_ekf2_update()" autorun="TRUE"/>
<makefile target="ap|nps">
<!-- EKF2 files -->
<define name="INS_TYPE_H" value="subsystems/ins/ins_ekf2.h" type="string"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_ekf2.cpp" dir="subsystems/ins"/>
<!-- Include the ecl and matrix libraries from ext -->
<include name="$(PAPARAZZI_SRC)/sw/ext/ecl/"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
<define name="__PAPARAZZI" value="true"/>
<define name="ECL_STANDALONE" value="true"/>
<define name="USE_MAGNETOMETER" value="true"/> <!-- Needed for IMU to get scaled version -->
<!-- Compile needed ecl files -->
<file name="mathlib.cpp" dir="ecl/mathlib"/>
<file name="geo.cpp" dir="ecl/geo"/>
<file name="geo_mag_declination.cpp" dir="ecl/geo_lookup"/>
<file name="airspeed_fusion.cpp" dir="ecl/EKF"/>
<file name="control.cpp" dir="ecl/EKF"/>
<file name="covariance.cpp" dir="ecl/EKF"/>
<file name="drag_fusion.cpp" dir="ecl/EKF"/>
<file name="ekf.cpp" dir="ecl/EKF"/>
<file name="ekf_helper.cpp" dir="ecl/EKF"/>
<file name="estimator_interface.cpp" dir="ecl/EKF"/>
<file name="gps_checks.cpp" dir="ecl/EKF"/>
<file name="mag_fusion.cpp" dir="ecl/EKF"/>
<file name="optflow_fusion.cpp" dir="ecl/EKF"/>
<file name="sideslip_fusion.cpp" dir="ecl/EKF"/>
<file name="terrain_estimator.cpp" dir="ecl/EKF"/>
<file name="vel_pos_fusion.cpp" dir="ecl/EKF"/>
<file name="gps_yaw_fusion.cpp" dir="ecl/EKF"/>
</makefile>
</module>
conf/modules/ins_mekf_wind.xml
-1
View File
@@ -82,7 +82,6 @@
<define name="EIGEN_NO_DEBUG"/> <define name="EIGEN_NO_DEBUG"/>
<flag name="CXXFLAGS" value="Wno-bool-compare"/> <flag name="CXXFLAGS" value="Wno-bool-compare"/>
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/> <flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
<file name="pprz_syscalls.c" dir="."/>
</makefile> </makefile>
<makefile target="sim" firmware="fixedwing"> <makefile target="sim" firmware="fixedwing">
<define name="AHRS_TYPE_H" value="subsystems/ahrs/ahrs_sim.h" type="string"/> <define name="AHRS_TYPE_H" value="subsystems/ahrs/ahrs_sim.h" type="string"/>
conf/modules/rust_demo_module.xml
-7
View File
@@ -26,13 +26,6 @@
<init fun="rust_function()"/> <init fun="rust_function()"/>
<periodic fun="rust_periodic()" freq="1." autorun="TRUE"/> <periodic fun="rust_periodic()" freq="1." autorun="TRUE"/>
<makefile target="ap">
<!-->
Additional defines needed by Rust's `alloc` crate
-->
<file name="pprz_syscalls.c" dir="."/>
</makefile>
<makefile target="ap|nps|hitl"> <makefile target="ap|nps|hitl">
<!-- <!--
MODULE_PATH is where the module lives. MODULE_PATH is where the module lives.
conf/telemetry/default_rotorcraft.xml
+1
View File
@@ -34,6 +34,7 @@
<message name="DRAGSPEED" period="0.02"/> <message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/> <message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/> <message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
conf/userconf/tudelft/conf.xml
+1 -1
View File
@@ -339,7 +339,7 @@
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml" flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/lidar_tfmini.xml modules/ahrs_float_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml" settings_modules="modules/air_data.xml modules/lidar_tfmini.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue" gui_color="blue"
/> />
<aircraft <aircraft
sw/airborne/autopilot.h
+8
View File
@@ -32,6 +32,10 @@
#ifndef AUTOPILOT_H #ifndef AUTOPILOT_H
#define AUTOPILOT_H #define AUTOPILOT_H
#ifdef __cplusplus
extern "C" {
#endif
#include "std.h" #include "std.h"
#include "paparazzi.h" #include "paparazzi.h"
#include "generated/airframe.h" #include "generated/airframe.h"
@@ -196,5 +200,9 @@ extern void autopilot_send_version(void);
*/ */
extern void autopilot_send_mode(void); extern void autopilot_send_mode(void);
#ifdef __cplusplus
}
#endif
#endif /* AUTOPILOT_H */ #endif /* AUTOPILOT_H */
sw/airborne/boards/apogee/baro_board.c
+2 -1
View File
@@ -113,8 +113,9 @@ void apogee_baro_event(void)
{ {
mpl3115_event(&apogee_baro); mpl3115_event(&apogee_baro);
if (apogee_baro.data_available && startup_cnt == 0) { if (apogee_baro.data_available && startup_cnt == 0) {
uint32_t now_ts = get_sys_time_usec();
float pressure = ((float)apogee_baro.pressure / (1 << 2)); float pressure = ((float)apogee_baro.pressure / (1 << 2));
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = apogee_baro.temperature / 16.0f; float temp = apogee_baro.temperature / 16.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
apogee_baro.data_available = false; apogee_baro.data_available = false;
sw/airborne/boards/ardrone/baro_board.c
+2 -1
View File
@@ -94,6 +94,7 @@ void ardrone_baro_event(void)
if (navdata.baro_available) { if (navdata.baro_available) {
if (navdata.baro_calibrated) { if (navdata.baro_calibrated) {
// first read temperature because pressure calibration depends on temperature // first read temperature because pressure calibration depends on temperature
uint32_t now_ts = get_sys_time_usec();
float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.measure.temperature_pressure); float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.measure.temperature_pressure);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg);
int32_t press_pascal = baro_apply_calibration(navdata.measure.pressure); int32_t press_pascal = baro_apply_calibration(navdata.measure.pressure);
@@ -101,7 +102,7 @@ void ardrone_baro_event(void)
press_pascal = update_median_filter_i(&baro_median, press_pascal); press_pascal = update_median_filter_i(&baro_median, press_pascal);
#endif #endif
float pressure = (float)press_pascal; float pressure = (float)press_pascal;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
} }
navdata.baro_available = false; navdata.baro_available = false;
} }
sw/airborne/boards/ardrone/navdata.c
+2 -1
View File
@@ -388,8 +388,9 @@ void navdata_update()
#ifdef USE_SONAR #ifdef USE_SONAR
/* Check if there is a new sonar measurement and update the sonar */ /* Check if there is a new sonar measurement and update the sonar */
if (navdata.measure.ultrasound >> 15) { if (navdata.measure.ultrasound >> 15) {
uint32_t now_ts = get_sys_time_usec();
float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas); AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, now_ts, sonar_meas);
} }
#endif #endif
sw/airborne/boards/baro_board_ms5611_i2c.c
+2 -1
View File
@@ -106,8 +106,9 @@ void baro_event(void)
ms5611_i2c_event(&bb_ms5611); ms5611_i2c_event(&bb_ms5611);
if (bb_ms5611.data_available) { if (bb_ms5611.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure); float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = bb_ms5611.data.temperature / 100.0f; float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = false; bb_ms5611.data_available = false;
sw/airborne/boards/baro_board_ms5611_spi.c
+2 -1
View File
@@ -95,8 +95,9 @@ void baro_event(void)
ms5611_spi_event(&bb_ms5611); ms5611_spi_event(&bb_ms5611);
if (bb_ms5611.data_available) { if (bb_ms5611.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure); float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = bb_ms5611.data.temperature / 100.0f; float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = false; bb_ms5611.data_available = false;
sw/airborne/boards/booz/baro_board.c
+2 -1
View File
@@ -85,8 +85,9 @@ void baro_periodic(void)
if (baro_board.status == BB_UNINITIALIZED) { if (baro_board.status == BB_UNINITIALIZED) {
RunOnceEvery(10, { baro_board_calibrate();}); RunOnceEvery(10, { baro_board_calibrate();});
} else { } else {
uint32_t now_ts = get_sys_time_usec();
float pressure = 101325.0 - BOOZ_BARO_SENS * (BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); float pressure = 101325.0 - BOOZ_BARO_SENS * (BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
} }
} }
sw/airborne/boards/hbmini/baro_board.c
+2 -1
View File
@@ -56,8 +56,9 @@ void bmp_baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
#ifdef BARO_LED #ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED)); RunOnceEvery(10, LED_TOGGLE(BARO_LED));
sw/airborne/boards/lia/baro_board.c
+2 -1
View File
@@ -74,8 +74,9 @@ void baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = baro_bmp085.temperature / 10.0f; float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
sw/airborne/boards/lisa_l/baro_board.c
+2 -1
View File
@@ -138,9 +138,10 @@ void lisa_l_baro_event(void)
baro_trans.status != I2CTransPending) { baro_trans.status != I2CTransPending) {
baro_board.status = LBS_READ_ABS; baro_board.status = LBS_READ_ABS;
if (baro_trans.status == I2CTransSuccess) { if (baro_trans.status == I2CTransSuccess) {
uint32_t now_ts = get_sys_time_usec();
int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1]; int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1];
float pressure = LISA_L_BARO_SENS * (float)tmp; float pressure = LISA_L_BARO_SENS * (float)tmp;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
} }
} else if (baro_board.status == LBS_READING_DIFF && } else if (baro_board.status == LBS_READING_DIFF &&
baro_trans.status != I2CTransPending) { baro_trans.status != I2CTransPending) {
sw/airborne/boards/lisa_m/baro_board.c
+2 -1
View File
@@ -74,8 +74,9 @@ void baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = baro_bmp085.temperature / 10.0f; float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
sw/airborne/boards/lisa_mx/baro_board.c
+2 -1
View File
@@ -73,8 +73,9 @@ void baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = baro_bmp085.temperature / 10.0f; float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
sw/airborne/boards/lisa_mxs/baro_board.c
+2 -1
View File
@@ -73,8 +73,9 @@ void baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = baro_bmp085.temperature / 10.0f; float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
sw/airborne/boards/navgo/baro_board.c
+2 -1
View File
@@ -85,8 +85,9 @@ void navgo_baro_event(void)
if (mcp355x_data_available) { if (mcp355x_data_available) {
if (startup_cnt == 0) { if (startup_cnt == 0) {
// Send data when init phase is done // Send data when init phase is done
uint32_t now_ts = get_sys_time_usec();
float pressure = NAVGO_BARO_SENS * (mcp355x_data + NAVGO_BARO_OFFSET); float pressure = NAVGO_BARO_SENS * (mcp355x_data + NAVGO_BARO_OFFSET);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
} }
mcp355x_data_available = false; mcp355x_data_available = false;
} }
sw/airborne/boards/navstik/baro_board.c
+2 -1
View File
@@ -59,8 +59,9 @@ void baro_event(void)
bmp085_event(&baro_bmp085); bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) { if (baro_bmp085.data_available) {
uint32_t now_ts = get_sys_time_usec();
float pressure = (float)baro_bmp085.pressure; float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = baro_bmp085.temperature / 10.0f; float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = false; baro_bmp085.data_available = false;
sw/airborne/boards/swing/baro_board.c
+2 -1
View File
@@ -94,8 +94,9 @@ void baro_event(void)
if (baro_swing_available) { if (baro_swing_available) {
// From datasheet: raw_pressure / 4096 -> pressure in hPa // From datasheet: raw_pressure / 4096 -> pressure in hPa
// send data in Pa // send data in Pa
uint32_t now_ts = get_sys_time_usec();
float pressure = 100.f * ((float)baro_swing_raw) / 4096.f; float pressure = 100.f * ((float)baro_swing_raw) / 4096.f;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
baro_swing_available = false; baro_swing_available = false;
} }
pthread_mutex_unlock(&baro_swing_mutex); pthread_mutex_unlock(&baro_swing_mutex);
sw/airborne/boards/umarim/baro_board.c
+2 -1
View File
@@ -77,8 +77,9 @@ void umarim_baro_event(void)
Ads1114Event(); Ads1114Event();
if (BARO_ABS_ADS.data_available) { if (BARO_ABS_ADS.data_available) {
if (startup_cnt == 0) { if (startup_cnt == 0) {
uint32_t now_ts = get_sys_time_usec();
float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS); float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
} }
BARO_ABS_ADS.data_available = false; BARO_ABS_ADS.data_available = false;
} }
sw/airborne/c++.cpp
+50
View File
@@ -0,0 +1,50 @@
/*
* Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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/>.
*/
#include <stdlib.h>
/**
* Replace new and delete operators of C++
*/
void * operator new(size_t size)
{
if (size < 1) {
size = 1;
}
return(calloc(size, 1));
}
void operator delete(void *p)
{
if (p) free(p);
}
void * operator new[](size_t size)
{
if (size < 1) {
size = 1;
}
return(calloc(size, 1));
}
void operator delete[](void * ptr)
{
if (ptr) free(ptr);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+7
View File
@@ -27,6 +27,9 @@
#ifndef GUIDANCE_H_H #ifndef GUIDANCE_H_H
#define GUIDANCE_H_H #define GUIDANCE_H_H
#ifdef __cplusplus
extern "C" {
#endif
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
@@ -191,4 +194,8 @@ static inline void guidance_h_SetTau(float tau)
gh_set_tau(tau); gh_set_tau(tau);
} }
#ifdef __cplusplus
}
#endif
#endif /* GUIDANCE_H_H */ #endif /* GUIDANCE_H_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+7
View File
@@ -27,6 +27,10 @@
#ifndef STABILIZATION_ATTITUDE_H #ifndef STABILIZATION_ATTITUDE_H
#define STABILIZATION_ATTITUDE_H #define STABILIZATION_ATTITUDE_H
#ifdef __cplusplus
extern "C" {
#endif
#include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include STABILIZATION_ATTITUDE_TYPE_H #include STABILIZATION_ATTITUDE_TYPE_H
@@ -39,5 +43,8 @@ extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_attitude_run(bool in_flight); extern void stabilization_attitude_run(bool in_flight);
#ifdef __cplusplus
}
#endif
#endif /* STABILIZATION_ATTITUDE_H */ #endif /* STABILIZATION_ATTITUDE_H */
sw/airborne/math/pprz_isa.h
+14
View File
@@ -62,6 +62,8 @@ extern "C" {
#define PPRZ_ISA_AIR_GAS_CONSTANT (PPRZ_ISA_GAS_CONSTANT/PPRZ_ISA_MOLAR_MASS) #define PPRZ_ISA_AIR_GAS_CONSTANT (PPRZ_ISA_GAS_CONSTANT/PPRZ_ISA_MOLAR_MASS)
/** standard air density in kg/m^3 */ /** standard air density in kg/m^3 */
#define PPRZ_ISA_AIR_DENSITY 1.225 #define PPRZ_ISA_AIR_DENSITY 1.225
/** absolute null in celcius */
#define PPRZ_ISA_ABS_NULL -273.15
static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT *PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY); static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT *PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY);
@@ -181,6 +183,18 @@ static inline float pprz_isa_temperature_of_altitude(float alt)
return PPRZ_ISA_SEA_LEVEL_TEMP - PPRZ_ISA_TEMP_LAPS_RATE * alt; return PPRZ_ISA_SEA_LEVEL_TEMP - PPRZ_ISA_TEMP_LAPS_RATE * alt;
} }
/**
* Get the air density (rho) from a given pressure and temperature
*
* @param pressure current pressure in Pascal (Pa)
* @param temp temperature in celcius
* @return air density rho
*/
static inline float pprz_isa_density_of_pressure(float pressure, float temp)
{
return pressure * (1.0f / (PPRZ_ISA_AIR_GAS_CONSTANT * (temp - PPRZ_ISA_ABS_NULL)));
}
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */
#endif #endif
sw/airborne/modules/air_data/air_data.c
+1 -1
View File
@@ -109,7 +109,7 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
static uint8_t baro_health_counter; static uint8_t baro_health_counter;
static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure) static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float pressure)
{ {
air_data.pressure = pressure; air_data.pressure = pressure;
sw/airborne/modules/ctrl/optical_flow_landing.c
+2 -2
View File
@@ -166,7 +166,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev)
/// Function definitions /// Function definitions
/// Callback function of the ground altitude /// Callback function of the ground altitude
void vertical_ctrl_agl_cb(uint8_t sender_id, float distance); void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
// Callback function of the optical flow estimate: // Callback function of the optical flow estimate:
void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int16_t flow_x, void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int16_t flow_x,
int16_t flow_y, int16_t flow_der_x, int16_t flow_der_y, float quality, float size_divergence); int16_t flow_y, int16_t flow_der_x, int16_t flow_der_y, float quality, float size_divergence);
@@ -592,7 +592,7 @@ void update_errors(float err, float dt)
} }
// Reading from "sensors": // Reading from "sensors":
void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, float distance) void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint32_t stamp, float distance)
{ {
of_landing_ctrl.agl = distance; of_landing_ctrl.agl = distance;
} }
sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
+2 -2
View File
@@ -48,7 +48,7 @@ PRINT_CONFIG_VAR(VERTICAL_CTRL_MODULE_AGL_ID)
static abi_event agl_ev; ///< The altitude ABI event static abi_event agl_ev; ///< The altitude ABI event
/// Callback function of the ground altitude /// Callback function of the ground altitude
static void vertical_ctrl_agl_cb(uint8_t sender_id __attribute__((unused)), float distance); static void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
struct VerticalCtrlDemo v_ctrl; struct VerticalCtrlDemo v_ctrl;
@@ -85,7 +85,7 @@ void vertical_ctrl_module_run(bool in_flight)
} }
} }
static void vertical_ctrl_agl_cb(uint8_t sender_id, float distance) static void vertical_ctrl_agl_cb(__attribute__((unused)) uint8_t sender_id, __attribute__((unused)) uint32_t stamp, float distance)
{ {
v_ctrl.agl = distance; v_ctrl.agl = distance;
} }
sw/airborne/modules/ins/ins_mekf_wind_wrapper.c
+1 -1
View File
@@ -210,7 +210,7 @@ static abi_event geo_mag_ev;
static abi_event gps_ev; static abi_event gps_ev;
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
{ {
static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE; static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
static float alpha = 10.0f; static float alpha = 10.0f;
sw/airborne/modules/ins/ins_skeleton.c
+1 -1
View File
@@ -190,7 +190,7 @@ static void ins_ned_to_state(void)
* ABI callback functions * ABI callback functions
**********************************************************/ **********************************************************/
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
{ {
/* call module implementation */ /* call module implementation */
ins_module_update_baro(pressure); ins_module_update_baro(pressure);
sw/airborne/modules/lidar/lidar_lite.c
+4 -2
View File
@@ -137,8 +137,9 @@ void lidar_lite_periodic(void)
} }
} }
break; break;
case LIDAR_LITE_PARSE: case LIDAR_LITE_PARSE: {
// filter data // filter data
uint32_t now_ts = get_sys_time_usec();
lidar_lite.distance_raw = update_median_filter_i( lidar_lite.distance_raw = update_median_filter_i(
&lidar_lite_filter, &lidar_lite_filter,
(uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1])); (uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1]));
@@ -154,12 +155,13 @@ void lidar_lite_periodic(void)
// send message (if requested) // send message (if requested)
if (lidar_lite.update_agl) { if (lidar_lite.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar_lite.distance); AbiSendMsgAGL(AGL_LIDAR_LITE_ID, now_ts, lidar_lite.distance);
} }
// increment status // increment status
lidar_lite.status = LIDAR_LITE_INIT_RANGING; lidar_lite.status = LIDAR_LITE_INIT_RANGING;
break; break;
}
default: default:
break; break;
} }
sw/airborne/modules/lidar/lidar_sf11.c
+4 -2
View File
@@ -97,9 +97,10 @@ void lidar_sf11_periodic(void)
lidar_sf11.trans.buf[0] = 0; // set tx to zero lidar_sf11.trans.buf[0] = 0; // set tx to zero
i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2); i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2);
break; break;
case LIDAR_SF11_READ_OK: case LIDAR_SF11_READ_OK: {
// process results // process results
// filter data // filter data
uint32_t now_ts = get_sys_time_usec();
lidar_sf11.distance_raw = update_median_filter_i( lidar_sf11.distance_raw = update_median_filter_i(
&lidar_sf11_filter, &lidar_sf11_filter,
(uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1])); (uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1]));
@@ -115,12 +116,13 @@ void lidar_sf11_periodic(void)
// send message (if requested) // send message (if requested)
if (lidar_sf11.update_agl) { if (lidar_sf11.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_SF11_ID, lidar_sf11.distance); AbiSendMsgAGL(AGL_LIDAR_SF11_ID, now_ts, lidar_sf11.distance);
} }
// reset status // reset status
lidar_sf11.status = LIDAR_SF11_REQ_READ; lidar_sf11.status = LIDAR_SF11_REQ_READ;
break; break;
}
default: default:
break; break;
} }
sw/airborne/modules/lidar/tfmini.c
+2 -1
View File
@@ -145,6 +145,7 @@ static void tfmini_parse(uint8_t byte)
case TFMINI_PARSE_CHECKSUM: case TFMINI_PARSE_CHECKSUM:
// When the CRC matches // When the CRC matches
if (tfmini.parse_crc == byte) { if (tfmini.parse_crc == byte) {
uint32_t now_ts = get_sys_time_usec();
tfmini.distance = tfmini.raw_dist / 100.f; tfmini.distance = tfmini.raw_dist / 100.f;
tfmini.strength = tfmini.raw_strength; tfmini.strength = tfmini.raw_strength;
tfmini.mode = tfmini.raw_mode; tfmini.mode = tfmini.raw_mode;
@@ -161,7 +162,7 @@ static void tfmini_parse(uint8_t byte)
// send message (if requested) // send message (if requested)
if (tfmini.update_agl) { if (tfmini.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, tfmini.distance); AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, now_ts, tfmini.distance);
} }
} }
} }
sw/airborne/modules/meteo/meteo_stick.c
+2 -1
View File
@@ -396,9 +396,10 @@ void meteo_stick_event(void)
#ifdef MS_PRESSURE_SLAVE_IDX #ifdef MS_PRESSURE_SLAVE_IDX
// send absolute pressure data over ABI as soon as available // send absolute pressure data over ABI as soon as available
if (meteo_stick.pressure.data_available) { if (meteo_stick.pressure.data_available) {
uint32_t now_ts = get_sys_time_usec();
meteo_stick.current_pressure = get_pressure(meteo_stick.pressure.data); meteo_stick.current_pressure = get_pressure(meteo_stick.pressure.data);
#if USE_MS_PRESSURE #if USE_MS_PRESSURE
AbiSendMsgBARO_ABS(METEO_STICK_SENDER_ID, meteo_stick.current_pressure); AbiSendMsgBARO_ABS(METEO_STICK_SENDER_ID, now_ts, meteo_stick.current_pressure);
#endif #endif
meteo_stick.pressure.data_available = false; meteo_stick.pressure.data_available = false;
} }
sw/airborne/modules/optical_flow/px4flow.c
+2 -1
View File
@@ -79,6 +79,7 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
{ {
static float quality = 0; static float quality = 0;
static float noise = 0; static float noise = 0;
uint32_t now_ts = get_sys_time_usec();
quality = ((float)optical_flow.quality) / 255.0; quality = ((float)optical_flow.quality) / 255.0;
noise = px4flow_stddev + (1 - quality) * px4flow_stddev * 10; noise = px4flow_stddev + (1 - quality) * px4flow_stddev * 10;
noise = noise * noise; // square the noise to get variance of the measurement noise = noise * noise; // square the noise to get variance of the measurement
@@ -107,7 +108,7 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
if (px4flow_update_agl) { if (px4flow_update_agl) {
// positive distance means it's known/valid // positive distance means it's known/valid
if (optical_flow.ground_distance > 0) { if (optical_flow.ground_distance > 0) {
AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, optical_flow.ground_distance); AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, now_ts, optical_flow.ground_distance);
} }
} }
} }

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