mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 05:17:03 +08:00
[ins] EKF2 update to latest version (#2829)
* [conf] Add splash4 airframe * [ins] Update EKF2 * [ekf2] Add support for optitrack and update gps datalink module * [modules] Fix gps datalink module heading calculations * [ground] Remove old natnet2ivy
This commit is contained in:
Freek van Tienen
GitHub
+5
-3
@@ -34,21 +34,23 @@ Q=@
|
||||
|
||||
# flags for warnings (C and C++)
|
||||
WARN_FLAGS += -W -Wall -Wextra
|
||||
WARN_FLAGS += -Wunused -Wcast-align -Wpointer-arith -Wswitch-default -Wmissing-declarations
|
||||
WARN_FLAGS += -Wunused -Wcast-align -Wpointer-arith -Wmissing-declarations
|
||||
#WARN_FLAGS += -Wcast-qual
|
||||
#WARN_FLAGS += -Wredundant-decls
|
||||
#WARN_FLAGS += -Wshadow
|
||||
|
||||
CSTANDARD ?= -std=gnu99
|
||||
CFLAGS = $(WARN_FLAGS)
|
||||
CFLAGS += $(INCLUDES)
|
||||
CFLAGS += $($(TARGET).CFLAGS)
|
||||
CFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
|
||||
CFLAGS += -O$(OPT) -fPIC
|
||||
CFLAGS += $(DEBUG_FLAGS)
|
||||
CFLAGS += -std=gnu99
|
||||
CFLAGS += $(CSTANDARD)
|
||||
CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib)
|
||||
CFLAGS += -D_GNU_SOURCE
|
||||
|
||||
CXXSTANDARD ?= -std=c++0x
|
||||
CXXFLAGS = $(WARN_FLAGS)
|
||||
CXXFLAGS += $(INCLUDES)
|
||||
CXXFLAGS += $($(TARGET).CFLAGS)
|
||||
@@ -56,7 +58,7 @@ CXXFLAGS += $($(TARGET).CXXFLAGS)
|
||||
CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
|
||||
CXXFLAGS += -O$(OPT)
|
||||
CXXFLAGS += $(DEBUG_FLAGS)
|
||||
CXXFLAGS += -std=c++0x
|
||||
CXXFLAGS += $(CXXSTANDARD)
|
||||
CXXFLAGS += $(shell pkg-config --cflags-only-I ivy-glib)
|
||||
CXXFLAGS += -D_GNU_SOURCE
|
||||
|
||||
|
||||
@@ -66,7 +66,6 @@
|
||||
</program>
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="--id" constant="1"/>
|
||||
|
||||
@@ -55,7 +55,6 @@
|
||||
</program>
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--ivy_bus" variable="ivy_bus"/>
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
|
||||
@@ -19,15 +19,12 @@
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" 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="imu" type="bebop"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="ins" type="ekf2"/>
|
||||
<module name="ins" type="ekf2">
|
||||
<define name="INS_EKF2_OPTITRACK" value="true"/>
|
||||
</module>
|
||||
|
||||
<module name="air_data"/>
|
||||
<module name="send_imu_mag_current"/>
|
||||
@@ -81,13 +78,6 @@
|
||||
<define name="MAG_X_SENS" value="7.21201776785" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="7.20541442846" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="7.55306977197" integer="16"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="-1"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="-16"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-102"/>
|
||||
<define name="ACCEL_X_SENS" value="4.90615997669" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="4.8601827035" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="4.85967415403" integer="16"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
|
||||
@@ -0,0 +1,347 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- This is a Splash 4
|
||||
* Airframe: TUD00???
|
||||
* Autopilot: Pixhawk 4
|
||||
* Actuators: 4x motor and esc
|
||||
* Datalink: Herelink
|
||||
* GPS: UBlox F9P
|
||||
* RC: SBUS Crossfire
|
||||
-->
|
||||
|
||||
<airframe name="Splash4">
|
||||
<description>Splash4</description>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="px4fmu_5.0_chibios">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
<configure name="FLASH_MODE" value="SWD"/>
|
||||
|
||||
<module name="radio_control" type="sbus">
|
||||
<configure name="SBUS_PORT" value="UART3"/>
|
||||
</module>
|
||||
|
||||
<!-- Logger -->
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<module name="flight_recorder"/>
|
||||
|
||||
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
|
||||
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
|
||||
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
|
||||
|
||||
<!-- <module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
|
||||
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
</module> -->
|
||||
|
||||
<!--module name="ins" type="extended">
|
||||
<define name="INS_USE_GPS_ALT" value="1"/>
|
||||
<define name="INS_USE_GPS_ALT_SPEED" value="1"/>
|
||||
<define name="INS_VFF_R_GPS" value="0.01"/>
|
||||
</module-->
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<module name="logger_file">
|
||||
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
|
||||
</module>
|
||||
|
||||
<!--Not dealing with these in the simulation-->
|
||||
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
|
||||
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
|
||||
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
|
||||
<define name="RADIO_KILL_SWITCH" value="0"/>
|
||||
</target>
|
||||
|
||||
<!--module name="follow_me">
|
||||
<define name="FOLLOW_ME_DISTANCE" value="40"/>
|
||||
<define name="FOLLOW_ME_HEIGHT" value="15"/>
|
||||
</module>
|
||||
|
||||
<module name="approach_moving_target">
|
||||
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
|
||||
</module-->
|
||||
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B115200"/>
|
||||
</module>
|
||||
|
||||
<module name="ahrs" type="int_cmpl_quat" >
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
|
||||
</module>
|
||||
<module name="ins" type="extended" />
|
||||
|
||||
<module name="actuators" type="pwm">
|
||||
<define name="SERVO_HZ" value="400" />
|
||||
</module>
|
||||
<module name="imu" type="mpu6000"/>
|
||||
<module name="gps" type="datalink"/>
|
||||
<!-- <module name="gps" type="ublox">
|
||||
<configure name="UBX_GPS_BAUD" value="B460800"/>
|
||||
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||
</module> -->
|
||||
<module name="stabilization" type="indi">
|
||||
<!--define name="TILT_TWIST_CTRL" value="TRUE"/-->
|
||||
</module>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<!--module name="stabilization" type="int_quat"/-->
|
||||
|
||||
<module name="guidance" type="indi">
|
||||
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
|
||||
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.0354"/>
|
||||
</module>
|
||||
|
||||
<module name="air_data"/>
|
||||
|
||||
<module name="send_imu_mag_current"/>
|
||||
|
||||
<!-- Internal MAG -->
|
||||
<!--module name="mag_ist8310">
|
||||
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
|
||||
</module-->
|
||||
<!-- External MAG on GPS -->
|
||||
<module name="mag_lis3mdl">
|
||||
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
|
||||
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
|
||||
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
|
||||
</module>
|
||||
<!--module name="lidar" type="tfmini">
|
||||
<configure name="TFMINI_PORT" value="UART4"/>
|
||||
<configure name="USE_TFMINI_AGL" value="FALSE"/>
|
||||
</module-->
|
||||
|
||||
<module name="motor_mixing"/>
|
||||
</firmware>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="MOTOR_1" no="1" min="1000" neutral="1150" max="2000"/>
|
||||
<servo name="MOTOR_2" no="0" min="1000" neutral="1150" max="2000"/>
|
||||
<servo name="MOTOR_3" no="3" min="1000" neutral="1150" max="2000"/>
|
||||
<servo name="MOTOR_4" no="2" min="1000" neutral="1150" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THRUST" value="@THROTTLE" />
|
||||
<set command="ROLL" value="@ROLL" />
|
||||
<set command="PITCH" value="@PITCH" />
|
||||
<set command="YAW" value="@YAW" />
|
||||
</rc_commands>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
|
||||
|
||||
<!--set servo="MOTOR_1" value="($th_hold? -9600 : motor_mixing.commands[0])"/>
|
||||
<set servo="MOTOR_2" value="($th_hold? -9600 : motor_mixing.commands[1])"/>
|
||||
<set servo="MOTOR_3" value="($th_hold? -9600 : motor_mixing.commands[2])"/>
|
||||
<set servo="MOTOR_4" value="($th_hold? -9600 : motor_mixing.commands[3])"/-->
|
||||
|
||||
<set servo="MOTOR_1" value="(Or($th_hold, !autopilot_get_motors_on())? -9600 : actuators_pprz[0])"/>
|
||||
<set servo="MOTOR_2" value="(Or($th_hold, !autopilot_get_motors_on())? -9600 : actuators_pprz[1])"/>
|
||||
<set servo="MOTOR_3" value="(Or($th_hold, !autopilot_get_motors_on())? -9600 : actuators_pprz[2])"/>
|
||||
<set servo="MOTOR_4" value="(Or($th_hold, !autopilot_get_motors_on())? -9600 : actuators_pprz[3])"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 10.1 * adc)"/><!-- TODO: verify/calibrate -->
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
|
||||
<!-- Basic navigation settings -->
|
||||
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
|
||||
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
|
||||
<!-- Avoid GPS loss behavior when having RC or datalink -->
|
||||
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
|
||||
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503" />
|
||||
<define name="H_Y" value="0.0017972" />
|
||||
<define name="H_Z" value="0.9211303" />
|
||||
<!-- For vibrating airfames -->
|
||||
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Rotate the IMU -->
|
||||
<define name="MPU_CHAN_X" value="0"/>
|
||||
<define name="MPU_CHAN_Y" value="1"/>
|
||||
<define name="MPU_CHAN_Z" value="2"/>
|
||||
<define name="MPU_X_SIGN" value="-1"/>
|
||||
<define name="MPU_Y_SIGN" value="1"/>
|
||||
<define name="MPU_Z_SIGN" value="-1"/>
|
||||
|
||||
<!-- Caliobrated in the lab 10-12-2021 -->
|
||||
<define name="ACCEL_X_NEUTRAL" value="-8"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="201"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-76"/>
|
||||
<define name="ACCEL_X_SENS" value="4.892418146669337" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="4.749315335111977" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="4.811925698324739" integer="16"/>
|
||||
|
||||
<!-- Calibrated in the lab -->
|
||||
<define name="MAG_X_NEUTRAL" value="206"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="2602"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="3105"/>
|
||||
<define name="MAG_X_SENS" value="0.5891380148035075" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="0.6237738207876278" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="0.6195484141242792" integer="16"/>
|
||||
|
||||
<!-- Define axis in hover frame -->
|
||||
<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_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="35." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="35." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
||||
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="180." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="500"/>
|
||||
<define name="PHI_DGAIN" value="230"/>
|
||||
<define name="PHI_IGAIN" value="10"/>
|
||||
<define name="THETA_PGAIN" value="500"/>
|
||||
<define name="THETA_DGAIN" value="230"/>
|
||||
<define name="THETA_IGAIN" value="10"/>
|
||||
<define name="PSI_PGAIN" value="700"/>
|
||||
<define name="PSI_DGAIN" value="200"/>
|
||||
<define name="PSI_IGAIN" value="10"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<define name="G1_ROLL" value="{8., -8., -8., 8.}"/>
|
||||
<define name="G1_PITCH" value="{8., 8., -8., -8.}"/>
|
||||
<define name="G1_YAW" value="{-1.0, 1.0, -1.0, 1.0}"/>
|
||||
<define name="G1_THRUST" value="{-0.63, -0.63, -0.63, -0.63}"/>
|
||||
<define name="G2" value="{ -0.155, 0.155, -0.155, 0.155}"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="160.0"/>
|
||||
<define name="REF_ERR_Q" value="160.0"/>
|
||||
<define name="REF_ERR_R" value="20.0"/>
|
||||
<define name="REF_RATE_P" value="14.0"/>
|
||||
<define name="REF_RATE_Q" value="14.0"/>
|
||||
<define name="REF_RATE_R" value="5.0"/>
|
||||
|
||||
<!--Maxium yaw rate, to avoid instability-->
|
||||
<define name="MAX_R" value="50.0" unit="deg/s"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_CUTOFF" value="2.5"/>
|
||||
<define name="FILT_CUTOFF_RDOT" value="2.5"/>
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="2.5"/>
|
||||
|
||||
<define name="FILTER_YAW_RATE" value="TRUE"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354}"/>
|
||||
|
||||
<define name="WLS_PRIORITIES" value="{1000.f, 1000.f, 1.f, 100.f}"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="310"/>
|
||||
<define name="HOVER_KD" value="130"/>
|
||||
<define name="HOVER_KI" value="10"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="REF_MAX_SPEED" value="10"/>
|
||||
|
||||
<define name="MAX_BANK" value="30" unit="deg"/>
|
||||
<define name="USE_SPEED_REF" value="TRUE"/>
|
||||
<define name="PGAIN" value="60"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="AGAIN" value="0"/>
|
||||
<define name="IGAIN" value="20"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- Needed to use optitrack heading at any speed -->
|
||||
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="USE_GPS_ALT" value="1"/>
|
||||
<define name="USE_GPS_ALT_SPEED" value="1"/>
|
||||
<define name="VFF_R_GPS" value="0.01"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="nederdrone" 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"/>
|
||||
<define name="DEBUG_SPEED_SP" value="false"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="12.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="12.8" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
|
||||
<define name="BAT_NB_CELLS" value="4"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -19,7 +19,7 @@ MCU=cortex-m7
|
||||
|
||||
# FPU on F7
|
||||
USE_FPU=hard
|
||||
#USE_FPU_OPT= -mfpu=fpv5-d16
|
||||
USE_FPU_OPT= -mfpu=fpv5-d16
|
||||
|
||||
#USE_LTO=yes
|
||||
|
||||
|
||||
@@ -4,9 +4,8 @@
|
||||
<doc>
|
||||
<description>
|
||||
Remote GPS via datalink.
|
||||
Parses the REMOTE_GPS and REMOTE_GPS_SMALL datalink messages and publishes it onboard via ABI.
|
||||
Parses the EXTERNAL_POSE, EXTERNAL_POSE_SMALL and REMOTE_GPS_LOCAL datalink messages and publishes it as GPS onboard via ABI.
|
||||
</description>
|
||||
<define name="GPS_DATALINK_USE_MAG" value="1" description="Choose to receive also GPS extracted magnetometer messages"/>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>gps,@datalink</depends>
|
||||
@@ -19,8 +18,8 @@
|
||||
</header>
|
||||
<init fun="gps_datalink_init()"/>
|
||||
<periodic fun="gps_datalink_periodic_check()" freq="1." autorun="TRUE"/>
|
||||
<datalink message="REMOTE_GPS" fun="gps_datalink_parse_REMOTE_GPS(buf)"/>
|
||||
<datalink message="REMOTE_GPS_SMALL" fun="gps_datalink_parse_REMOTE_GPS_SMALL(buf)"/>
|
||||
<datalink message="EXTERNAL_POSE" fun="gps_datalink_parse_EXTERNAL_POSE(buf)"/>
|
||||
<datalink message="EXTERNAL_POSE_SMALL" fun="gps_datalink_parse_EXTERNAL_POSE_SMALL(buf)"/>
|
||||
<datalink message="REMOTE_GPS_LOCAL" fun="gps_datalink_parse_REMOTE_GPS_LOCAL(buf)"/>
|
||||
<makefile target="ap|fbw">
|
||||
<file name="gps_datalink.c"/>
|
||||
|
||||
+35
-10
@@ -5,21 +5,36 @@
|
||||
<description>
|
||||
simple INS and AHRS using EKF2 from PX4
|
||||
</description>
|
||||
<define name="INS_EKF2_OPTITRACK" value="false" description="Easy configuration of full optitrack (position- and yaw fusion, height, init origin)"/>
|
||||
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/>
|
||||
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/>
|
||||
<define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/>
|
||||
<define name="INS_EKF2_EVP_NOISE" value="0.1" description="External vision position noise [m]"/>
|
||||
<define name="INS_EKF2_EVV_NOISE" value="1.0" description="External vision velocity noise [m/s]"/>
|
||||
<define name="INS_EKF2_EVA_NOISE" value="0.05" description="External vision angle noise [rad]"/>
|
||||
<define name="INS_EKF2_GPS_V_NOISE" value="0.3" description="GPS measurement noise for horizontal velocity [m/s]"/>
|
||||
<define name="INS_EKF2_GPS_P_NOISE" value="0.5" description="GPS measurement position noise [m]"/>
|
||||
<define name="INS_EKF2_BARO_NOISE" value="3.5" description="Barometric measurement noise for altitude [m]"/>
|
||||
|
||||
<define name="INS_EKF2_IMU_POS_X" value="0" description="IMU X offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_IMU_POS_Y" value="0" description="IMU Y offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_IMU_POS_Z" value="0" description="IMU Z offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_GPS_POS_X" value="0" description="GPS X offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_GPS_POS_Y" value="0" description="GPS Y offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_GPS_POS_Z" value="0" description="GPS Z offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_FLOW_POS_X" value="0" description="Flow sensor X offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_FLOW_POS_Y" value="0" description="Flow sensor Y offset from CoG position [m]"/>
|
||||
<define name="INS_EKF2_FLOW_POS_Z" value="0" description="Flow sensor Z offset from CoG position [m]"/>
|
||||
|
||||
<define name="INS_EKF2_SONAR_MIN_RANGE" value="0.05" description="AGL sensor minimum range [m]"/>
|
||||
<define name="INS_EKF2_SONAR_MAX_RANGE" value="3" description="AGL sensor maximum range [m]"/>
|
||||
<define name="INS_EKF2_RANGE_MAIN_AGL" value="1" description="If enabled uses radar sensor as primary AGL source, if possible"/>
|
||||
<define name="INS_EKF2_FLOW_SENSOR_DELAY" value="15" description="flow/radar message delay [ms]"/>
|
||||
<define name="INS_EKF2_MIN_FLOW_QUALITY" value="100" description="Minimum quality of the optical flow message accepted [0-255]"/>
|
||||
<define name="INS_EKF2_MAX_FLOW_RATE" value="20" description="Maximum flow rate the sensor can perceive [rad/sec]]"/>
|
||||
<define name="INS_EKF2_FLOW_OFFSET_X" value="-130" description="Flow sensor X offset from IMU position [mm]"/>
|
||||
<define name="INS_EKF2_FLOW_OFFSET_Y" value="-150" description="Flow sensor Y offset from IMU position [mm]"/>
|
||||
<define name="INS_EKF2_FLOW_OFFSET_Z" value="170" description="Flow sensor Z offset from IMU position [mm]"/>
|
||||
<define name="INS_EKF2_FLOW_NOISE" value="0.01" description="Flow sensor noise [rad/sec]"/>
|
||||
<define name="INS_EKF2_FLOW_NOISE_QMIN" value="0.03" description="Flow sensor noise when quality is minimum [rad/sec]"/>
|
||||
<define name="INS_EKF2_FLOW_INNOV_GATE" value="3" description="Flow sensor innovation gate [STD]"/>
|
||||
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/>
|
||||
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/>
|
||||
<define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/>
|
||||
<define name="INS_EKF2_AGL_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for AGL measurements"/>
|
||||
<define name="INS_EKF2_BARO_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for Barometric measurements"/>
|
||||
<define name="INS_EKF2_GYRO_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for gyro measurements"/>
|
||||
@@ -31,8 +46,10 @@
|
||||
<dl_settings NAME="INS">
|
||||
<!-- EKF2 Configuration parameters -->
|
||||
<dl_settings name="ekf2">
|
||||
<dl_setting var="ekf2_params.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="modules/ins/ins_ekf2" handler="change_param"/>
|
||||
<dl_setting var="ekf2_params.fusion_mode" min="0" max="1" step="1" shortname="remove_gps" values="FALSE|TRUE" module="modules/ins/ins_ekf2" handler="remove_gps" type="bool" persistent="true"/>
|
||||
<dl_setting var="ekf2.qnh" min="500" step="0.1" max="1500" shortname="QNH"/>
|
||||
<dl_setting var="ekf2.temp" min="-50" step="0.1" max="80" shortname="temperature"/>
|
||||
<dl_setting var="ekf2.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="modules/ins/ins_ekf2" handler="change_param"/>
|
||||
<dl_setting var="ekf2.fusion_mode" min="0" max="1" step="1" shortname="remove_gps" values="FALSE|TRUE" module="modules/ins/ins_ekf2" handler="remove_gps" type="bool"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -45,8 +62,12 @@
|
||||
</header>
|
||||
<init fun="ins_ekf2_init()"/>
|
||||
<periodic fun="ins_ekf2_update()" autorun="TRUE"/>
|
||||
<datalink message="EXTERNAL_POSE" fun="ins_ekf2_parse_EXTERNAL_POSE(buf)"/>
|
||||
<datalink message="EXTERNAL_POSE_SMALL" fun="ins_ekf2_parse_EXTERNAL_POSE_SMALL(buf)"/>
|
||||
<makefile target="ap|nps">
|
||||
<configure name="CXXSTANDARD" value="-std=c++11"/>
|
||||
<configure name="CXXSTANDARD" value="-std=c++14"/>
|
||||
<flag name="CXXFLAGS" value="Wno-unused-but-set-variable"/>
|
||||
<flag name="CXXFLAGS" value="Wno-unused-variable"/>
|
||||
|
||||
<!-- EKF2 files -->
|
||||
<define name="INS_TYPE_H" value="modules/ins/ins_ekf2.h" type="string"/>
|
||||
@@ -61,11 +82,11 @@
|
||||
<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="mag_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"/>
|
||||
@@ -78,6 +99,10 @@
|
||||
<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"/>
|
||||
<file name="imu_down_sampler.cpp" dir="ecl/EKF"/>
|
||||
<file name="EKFGSF_yaw.cpp" dir="ecl/EKF"/>
|
||||
<file name="sensor_range_finder.cpp" dir="ecl/EKF"/>
|
||||
<file name="utils.cpp" dir="ecl/EKF"/>
|
||||
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
<message name="ESC" period="0.5"/>
|
||||
<!--message name="WINDTUNNEL_MEAS" period="0.1"/-->
|
||||
<message name="AHRS_REF_QUAT" period="0.1"/>
|
||||
<message name="GPS_RTK" period="0.1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="ppm">
|
||||
@@ -193,7 +194,7 @@
|
||||
<message name="GPS" period="11.1"/>
|
||||
<message name="DL_VALUE" period="2.5"/>
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="ROTORCRAFT_FP" period="0.02"/>
|
||||
<message name="ROTORCRAFT_FP" period="0.002"/>
|
||||
<message name="ROTORCRAFT_CMD" period=".002"/>
|
||||
<message name="COMMANDS" period=".02"/>
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
@@ -219,12 +220,14 @@
|
||||
<message name="AHRS_BIAS" period="0.5"/>
|
||||
<message name="AHRS_REF_QUAT" period="0.01"/>
|
||||
<message name="GUIDANCE_H_REF_INT" period="0.02"/>
|
||||
<message name="GUIDANCE_INDI_HYBRID" period="0.02"/>
|
||||
<message name="GUIDANCE_INDI_HYBRID" period="0.002"/>
|
||||
<message name="HYBRID_GUIDANCE" period="0.02"/>
|
||||
<message name="ESC" period="0.02"/>
|
||||
<message name="STAB_ATTITUDE_INDI" period="0.002"/>
|
||||
<message name="PPM" period="0.05"/>
|
||||
<message name="INDI_G" period="0.1"/>
|
||||
<message name="ACTUATORS" period="0.002"/>
|
||||
<message name="GPS_RTK" period="0.1"/>
|
||||
<!--<message name="WINDTUNNEL_MEAS" period="0.005"/>-->
|
||||
</mode>
|
||||
</process>
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
<program command="sw/ground_segment/misc/natnet2ivy" name="NatNet" icon="natnet.svg"/>
|
||||
|
||||
@@ -40,7 +40,6 @@
|
||||
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="--id" constant="1"/>
|
||||
|
||||
@@ -83,9 +83,9 @@
|
||||
airframe="airframes/tudelft/bebop_indi_actuators.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/delft_basic.xml"
|
||||
flight_plan="flight_plans/tudelft/rotorcraft_optitrack_path.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
|
||||
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
|
||||
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
|
||||
gui_color="#ffffcccaccca"
|
||||
/>
|
||||
<aircraft
|
||||
@@ -380,6 +380,17 @@
|
||||
gui_color="blue"
|
||||
release="009b121cfd12be8d3e236e0cbd5b564f2639d54f"
|
||||
/>
|
||||
<aircraft
|
||||
name="Splash4"
|
||||
ac_id="1"
|
||||
airframe="airframes/tudelft/splash4.xml"
|
||||
radio="radios/crossfire_sbus.xml"
|
||||
telemetry="telemetry/highspeed_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/rotorcraft_optitrack_path.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="TwoSeasTwenty"
|
||||
ac_id="200"
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
</program>
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="NatNet3" command="sw/ground_segment/python/natnet3.x/natnet2ivy.py"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="--id" constant="1"/>
|
||||
@@ -188,11 +188,13 @@
|
||||
<program name="GCS">
|
||||
<arg flag="-speech"/>
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac" constant="1"/>
|
||||
<arg flag="12"/>
|
||||
<arg flag="-small"/>
|
||||
<arg flag="-tf" constant="5"/>
|
||||
<arg flag="-sm"/>
|
||||
<arg flag="-f" constant="5"/>
|
||||
<arg flag="-o"/>
|
||||
<arg flag="-zf"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-d" constant="2"/>
|
||||
@@ -303,8 +305,10 @@
|
||||
<arg flag="hobbyking.xml"/>
|
||||
<arg flag="-d 0"/>
|
||||
</program>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac 9999" constant="@AC_ID"/>
|
||||
<arg flag="-o"/>
|
||||
<arg flag="-zf"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight ACM0 Transparent @57600">
|
||||
@@ -338,11 +342,13 @@
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac" constant="7"/>
|
||||
<arg flag="114"/>
|
||||
<arg flag="-small"/>
|
||||
<arg flag="-tf" constant="5"/>
|
||||
<arg flag="-sm"/>
|
||||
<arg flag="-f" constant="5"/>
|
||||
<arg flag="-o"/>
|
||||
<arg flag="-zf"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-d" constant="2"/>
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
<arg flag="sm600.xml"/>
|
||||
</program>
|
||||
<program name="Environment Simulator" command="sw/simulator/gaia"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="NatNet3" command="sw/ground_segment/python/natnet3.x/natnet2ivy.py"/>
|
||||
<program name="Gazebo" command="sw/tools/gzclient_launcher.sh"/>
|
||||
<program name="Real-time Distance Counter" command="sw/ground_segment/python/distance_counter/dist.py"/>
|
||||
</section>
|
||||
@@ -95,8 +95,10 @@
|
||||
<arg flag="sm600.xml"/>
|
||||
<arg flag="-d 0"/>
|
||||
</program>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac 9999" constant="@AC_ID"/>
|
||||
<arg flag="-o"/>
|
||||
<arg flag="-zf"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Distance Counter">
|
||||
|
||||
@@ -38,7 +38,7 @@
|
||||
</program>
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="NatNet3" command="sw/ground_segment/python/natnet3.x/natnet2ivy.py"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="--id" constant="1"/>
|
||||
@@ -67,11 +67,13 @@
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac" constant="1"/>
|
||||
<arg flag="12"/>
|
||||
<arg flag="-small"/>
|
||||
<arg flag="-tf" constant="5"/>
|
||||
<arg flag="-sm"/>
|
||||
<arg flag="-f" constant="5"/>
|
||||
<arg flag="-zf"/>
|
||||
<arg flag="-o"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-d" constant="2"/>
|
||||
@@ -92,9 +94,11 @@
|
||||
<arg flag="hobbyking.xml"/>
|
||||
<arg flag="-d" constant="1"/>
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<program name="NatNet3">
|
||||
<arg flag="-ac" constant="2"/>
|
||||
<arg flag="11"/>
|
||||
<arg flag="-zf"/>
|
||||
<arg flag="-o"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight USB0 @57600">
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
#include "inttypes.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
|
||||
@@ -30,6 +30,10 @@
|
||||
#ifndef SYS_TIME_H
|
||||
#define SYS_TIME_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdlib.h>
|
||||
#include "std.h"
|
||||
@@ -224,4 +228,8 @@ extern void sys_time_arch_init(void);
|
||||
#define SysTimeTimer(_t) ( get_sys_time_usec() - (_t))
|
||||
#define SysTimeTimerStop(_t) { _t = ( get_sys_time_usec() - (_t)); }
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* SYS_TIME_H */
|
||||
|
||||
@@ -36,15 +36,16 @@
|
||||
#include "modules/datalink/datalink.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
/** Set to 1 to receive also magnetometer ABI messages */
|
||||
#ifndef GPS_DATALINK_USE_MAG
|
||||
#define GPS_DATALINK_USE_MAG 1
|
||||
#ifndef EXTERNAL_POSE_SMALL_POS_RES
|
||||
#define EXTERNAL_POSE_SMALL_POS_RES 1.0
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(GPS_DATALINK_USE_MAG)
|
||||
|
||||
struct LtpDef_i ltp_def;
|
||||
#ifndef EXTERNAL_POSE_SMALL_SPEED_RES
|
||||
#define EXTERNAL_POSE_SMALL_SPEED_RES 1.0
|
||||
#endif
|
||||
|
||||
struct GpsState gps_datalink;
|
||||
static struct LtpDef_i ltp_def;
|
||||
|
||||
/** GPS initialization */
|
||||
void gps_datalink_init(void)
|
||||
@@ -66,185 +67,39 @@ void gps_datalink_init(void)
|
||||
ltp_def_from_lla_i(<p_def, &llh_nav0);
|
||||
}
|
||||
|
||||
// Send GPS heading info as magnetometer messages
|
||||
static void send_magnetometer(int32_t course, uint32_t now_ts)
|
||||
/** Publish the GPS data */
|
||||
static void gps_datalink_publish(uint32_t tow, struct EnuCoor_f *enu_pos, struct EnuCoor_f *enu_speed, float course)
|
||||
{
|
||||
struct Int32Vect3 mag;
|
||||
struct FloatVect3 mag_real;
|
||||
// course from gps in [0, 2*Pi]*1e7 (CW/north)
|
||||
float heading = course/1e7;
|
||||
mag_real.x = cos(heading);
|
||||
mag_real.y = -sin(heading);
|
||||
mag_real.z = 0;
|
||||
MAGS_BFP_OF_REAL(mag, mag_real);
|
||||
struct EnuCoor_i enu_pos_i, enu_speed_i;
|
||||
|
||||
// update IMU information
|
||||
VECT3_COPY(imu.mag_unscaled, mag);
|
||||
imu_scale_mag(&imu);
|
||||
|
||||
// Send fake ABI for GPS, Magnetometer and Optical Flow for GPS fusion
|
||||
AbiSendMsgIMU_MAG_INT32(MAG_DATALINK_SENDER_ID, now_ts, &mag);
|
||||
}
|
||||
|
||||
// Parse the REMOTE_GPS_SMALL datalink packet
|
||||
static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
|
||||
{
|
||||
struct EnuCoor_i enu_pos, enu_speed;
|
||||
|
||||
// Position in ENU coordinates
|
||||
enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
|
||||
if (enu_pos.x & 0x400) {
|
||||
enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
|
||||
if (enu_pos.y & 0x400) {
|
||||
enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
|
||||
enu_pos_i.x = (int32_t)CM_OF_M(enu_pos->x);
|
||||
enu_pos_i.y = (int32_t)CM_OF_M(enu_pos->y);
|
||||
enu_pos_i.z = (int32_t)CM_OF_M(enu_pos->z);
|
||||
|
||||
// Convert the ENU coordinates to ECEF
|
||||
ecef_of_enu_point_i(&gps_datalink.ecef_pos, <p_def, &enu_pos);
|
||||
ecef_of_enu_point_i(&gps_datalink.ecef_pos, <p_def, &enu_pos_i);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
|
||||
|
||||
lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||
|
||||
enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
|
||||
if (enu_speed.x & 0x400) {
|
||||
enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
|
||||
if (enu_speed.y & 0x400) {
|
||||
enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
|
||||
if (enu_speed.z & 0x200) {
|
||||
enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
|
||||
}
|
||||
enu_speed_i.x = (int32_t)CM_OF_M(enu_speed->x);
|
||||
enu_speed_i.y = (int32_t)CM_OF_M(enu_speed->y);
|
||||
enu_speed_i.z = (int32_t)CM_OF_M(enu_speed->z);
|
||||
|
||||
VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed);
|
||||
VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed_i);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||
|
||||
ecef_of_enu_vect_i(&gps_datalink.ecef_vel , <p_def , &enu_speed);
|
||||
ecef_of_enu_vect_i(&gps_datalink.ecef_vel , <p_def , &enu_speed_i);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
|
||||
|
||||
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
|
||||
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
|
||||
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed_i);
|
||||
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed_i);
|
||||
|
||||
gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
|
||||
gps_datalink.hmsl = ltp_def.hmsl + enu_pos_i.z * 10;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
|
||||
|
||||
gps_datalink.course = ((int32_t)heading) * 1e3;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps_datalink.num_sv = 30;
|
||||
gps_datalink.tow = tow;
|
||||
gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
|
||||
|
||||
// set gps msg time
|
||||
gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
|
||||
gps_datalink.last_msg_time = sys_time.nb_sec;
|
||||
|
||||
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
|
||||
gps_datalink.last_3dfix_time = sys_time.nb_sec;
|
||||
|
||||
// publish new GPS data
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
|
||||
}
|
||||
|
||||
/** Parse the REMOTE_GPS datalink packet */
|
||||
static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
|
||||
int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
|
||||
int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
|
||||
uint32_t tow, int32_t course)
|
||||
{
|
||||
gps_datalink.lla_pos.lat = lat;
|
||||
gps_datalink.lla_pos.lon = lon;
|
||||
gps_datalink.lla_pos.alt = alt;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||
|
||||
gps_datalink.hmsl = hmsl;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
|
||||
|
||||
gps_datalink.ecef_pos.x = ecef_x;
|
||||
gps_datalink.ecef_pos.y = ecef_y;
|
||||
gps_datalink.ecef_pos.z = ecef_z;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
|
||||
|
||||
gps_datalink.ecef_vel.x = ecef_xd;
|
||||
gps_datalink.ecef_vel.y = ecef_yd;
|
||||
gps_datalink.ecef_vel.z = ecef_zd;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
|
||||
|
||||
ned_of_ecef_vect_i(&gps_datalink.ned_vel, <p_def , &gps_datalink.ecef_vel);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||
|
||||
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(gps_datalink.ned_vel);
|
||||
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(gps_datalink.ned_vel);
|
||||
|
||||
gps_datalink.course = course;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps_datalink.num_sv = numsv;
|
||||
gps_datalink.tow = tow;
|
||||
gps_datalink.fix = GPS_FIX_3D;
|
||||
|
||||
// set gps msg time
|
||||
gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
|
||||
gps_datalink.last_msg_time = sys_time.nb_sec;
|
||||
|
||||
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
|
||||
gps_datalink.last_3dfix_time = sys_time.nb_sec;
|
||||
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
|
||||
// if selected, publish magnetometer data
|
||||
#if GPS_DATALINK_USE_MAG
|
||||
send_magnetometer(course, now_ts);
|
||||
#endif
|
||||
|
||||
// publish new GPS data
|
||||
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
|
||||
}
|
||||
|
||||
/** Parse the REMOTE_GPS_LOCAL datalink packet */
|
||||
static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
|
||||
float enu_xd, float enu_yd, float enu_zd,
|
||||
uint32_t tow, float course)
|
||||
{
|
||||
|
||||
struct EnuCoor_i enu_pos, enu_speed;
|
||||
|
||||
// Position in ENU coordinates
|
||||
enu_pos.x = (int32_t)CM_OF_M(enu_x);
|
||||
enu_pos.y = (int32_t)CM_OF_M(enu_y);
|
||||
enu_pos.z = (int32_t)CM_OF_M(enu_z);
|
||||
|
||||
// Convert the ENU coordinates to ECEF
|
||||
ecef_of_enu_point_i(&gps_datalink.ecef_pos, <p_def, &enu_pos);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
|
||||
|
||||
lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||
|
||||
enu_speed.x = (int32_t)CM_OF_M(enu_xd);
|
||||
enu_speed.y = (int32_t)CM_OF_M(enu_yd);
|
||||
enu_speed.z = (int32_t)CM_OF_M(enu_zd);
|
||||
|
||||
VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||
|
||||
ecef_of_enu_vect_i(&gps_datalink.ecef_vel , <p_def , &enu_speed);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
|
||||
|
||||
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
|
||||
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
|
||||
|
||||
gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
|
||||
|
||||
gps_datalink.course = (int32_t)(RadOfDeg(course)*1e7);
|
||||
gps_datalink.course = (int32_t)(course*1e7);
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps_datalink.num_sv = 7;
|
||||
@@ -260,54 +115,107 @@ static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
|
||||
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
|
||||
// if selected, publish magnetometer data
|
||||
#if GPS_DATALINK_USE_MAG
|
||||
send_magnetometer(course, now_ts);
|
||||
#endif
|
||||
|
||||
// Publish GPS data
|
||||
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS(uint8_t *buf)
|
||||
/** Parse the full EXTERNAL_POSE message and publish as GPS through ABI */
|
||||
void gps_datalink_parse_EXTERNAL_POSE(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
parse_gps_datalink(DL_REMOTE_GPS_numsv(buf),
|
||||
DL_REMOTE_GPS_ecef_x(buf),
|
||||
DL_REMOTE_GPS_ecef_y(buf),
|
||||
DL_REMOTE_GPS_ecef_z(buf),
|
||||
DL_REMOTE_GPS_lat(buf),
|
||||
DL_REMOTE_GPS_lon(buf),
|
||||
DL_REMOTE_GPS_alt(buf),
|
||||
DL_REMOTE_GPS_hmsl(buf),
|
||||
DL_REMOTE_GPS_ecef_xd(buf),
|
||||
DL_REMOTE_GPS_ecef_yd(buf),
|
||||
DL_REMOTE_GPS_ecef_zd(buf),
|
||||
DL_REMOTE_GPS_tow(buf),
|
||||
DL_REMOTE_GPS_course(buf));
|
||||
uint32_t tow = DL_EXTERNAL_POSE_timestamp(buf);
|
||||
|
||||
struct EnuCoor_f enu_pos, enu_speed;
|
||||
enu_pos.x = DL_EXTERNAL_POSE_enu_x(buf);
|
||||
enu_pos.y = DL_EXTERNAL_POSE_enu_y(buf);
|
||||
enu_pos.z = DL_EXTERNAL_POSE_enu_z(buf);
|
||||
enu_speed.x = DL_EXTERNAL_POSE_enu_xd(buf);
|
||||
enu_speed.y = DL_EXTERNAL_POSE_enu_yd(buf);
|
||||
enu_speed.z = DL_EXTERNAL_POSE_enu_zd(buf);
|
||||
|
||||
struct FloatQuat body_q; // Converted to NED for heading calculation
|
||||
body_q.qi = DL_EXTERNAL_POSE_body_qi(buf);
|
||||
body_q.qx = DL_EXTERNAL_POSE_body_qy(buf);
|
||||
body_q.qy = DL_EXTERNAL_POSE_body_qx(buf);
|
||||
body_q.qz = -DL_EXTERNAL_POSE_body_qz(buf);
|
||||
|
||||
struct FloatEulers body_e;
|
||||
float_eulers_of_quat(&body_e, &body_q);
|
||||
float heading = body_e.psi;
|
||||
|
||||
gps_datalink_publish(tow, &enu_pos, &enu_speed, heading);
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf)
|
||||
/** Parse the EXTERNAL_POSE_SMALL message and publish as GPS through ABI */
|
||||
void gps_datalink_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
if (DL_EXTERNAL_POSE_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
parse_gps_datalink_small(DL_REMOTE_GPS_SMALL_heading(buf),
|
||||
DL_REMOTE_GPS_SMALL_pos_xyz(buf),
|
||||
DL_REMOTE_GPS_SMALL_speed_xyz(buf),
|
||||
DL_REMOTE_GPS_SMALL_tow(buf));
|
||||
uint32_t tow = DL_EXTERNAL_POSE_SMALL_timestamp(buf);
|
||||
|
||||
struct EnuCoor_i enu_pos_cm, enu_speed_cm;
|
||||
struct EnuCoor_f enu_pos, enu_speed;
|
||||
|
||||
/* Convert the 32 bit xyz position to cm seperated */
|
||||
uint32_t enu_pos_u = DL_EXTERNAL_POSE_SMALL_enu_pos(buf);
|
||||
enu_pos_cm.x = (int32_t)((enu_pos_u >> 21) & 0x7FF); // bits 31-21 x position in cm
|
||||
if (enu_pos_cm.x & 0x400) {
|
||||
enu_pos_cm.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos_cm.y = (int32_t)((enu_pos_u >> 10) & 0x7FF); // bits 20-10 y position in cm
|
||||
if (enu_pos_cm.y & 0x400) {
|
||||
enu_pos_cm.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos_cm.z = (int32_t)(enu_pos_u & 0x3FF); // bits 9-0 z position in cm
|
||||
|
||||
/* Convert the cm position with a resolution to meters */
|
||||
enu_pos.x = enu_pos_cm.x / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
|
||||
enu_pos.y = enu_pos_cm.y / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
|
||||
enu_pos.z = enu_pos_cm.z / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
|
||||
|
||||
/* Convert the 32 bit xyz speed to cm seperated */
|
||||
uint32_t enu_speed_u = DL_EXTERNAL_POSE_SMALL_enu_speed(buf);
|
||||
enu_speed_cm.x = (int32_t)((enu_speed_u >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
|
||||
if (enu_speed_cm.x & 0x400) {
|
||||
enu_speed_cm.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed_cm.y = (int32_t)((enu_speed_u >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
|
||||
if (enu_speed_cm.y & 0x400) {
|
||||
enu_speed_cm.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed_cm.z = (int32_t)((enu_speed_u) & 0x3FF); // bits 9-0 speed z in cm/s
|
||||
if (enu_speed_cm.z & 0x200) {
|
||||
enu_speed_cm.z |= 0xFFFFFC00; // sign extend for twos complements
|
||||
}
|
||||
|
||||
/* Convert the cm/s speed with a resolution to meters/s */
|
||||
enu_speed.x = enu_speed_cm.x / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
|
||||
enu_speed.y = enu_speed_cm.y / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
|
||||
enu_speed.z = enu_speed_cm.z / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
|
||||
|
||||
/* Convert the heading with the 1e4 fraction to radians */
|
||||
float heading = DL_EXTERNAL_POSE_SMALL_heading(buf) / 1e4;
|
||||
|
||||
gps_datalink_publish(tow, &enu_pos, &enu_speed, heading);
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_LOCAL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_y(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_z(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_xd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_yd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_zd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_tow(buf),
|
||||
DL_REMOTE_GPS_LOCAL_course(buf));
|
||||
uint32_t tow = DL_REMOTE_GPS_LOCAL_tow(buf);
|
||||
|
||||
struct EnuCoor_f enu_pos, enu_speed;
|
||||
enu_pos.x = DL_REMOTE_GPS_LOCAL_enu_x(buf);
|
||||
enu_pos.y = DL_REMOTE_GPS_LOCAL_enu_y(buf);
|
||||
enu_pos.z = DL_REMOTE_GPS_LOCAL_enu_z(buf);
|
||||
|
||||
enu_speed.x = DL_REMOTE_GPS_LOCAL_enu_xd(buf);
|
||||
enu_speed.y = DL_REMOTE_GPS_LOCAL_enu_yd(buf);
|
||||
enu_speed.z = DL_REMOTE_GPS_LOCAL_enu_zd(buf);
|
||||
|
||||
float course = RadOfDeg(DL_REMOTE_GPS_LOCAL_course(buf));
|
||||
|
||||
gps_datalink_publish(tow, &enu_pos, &enu_speed, course);
|
||||
}
|
||||
|
||||
@@ -44,8 +44,8 @@ extern void gps_datalink_init(void);
|
||||
|
||||
#define gps_datalink_periodic_check() gps_periodic_check(&gps_datalink)
|
||||
|
||||
extern void gps_datalink_parse_REMOTE_GPS(uint8_t *buf);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf);
|
||||
extern void gps_datalink_parse_EXTERNAL_POSE(uint8_t *buf);
|
||||
extern void gps_datalink_parse_EXTERNAL_POSE_SMALL(uint8_t *buf);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf);
|
||||
|
||||
#endif /* GPS_DATALINK_H */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -36,7 +36,27 @@ extern "C" {
|
||||
#include "modules/ahrs/ahrs.h"
|
||||
#include "modules/ins/ins.h"
|
||||
|
||||
struct ekf2_parameters_t {
|
||||
/* Main EKF2 structure for keeping track of the status and use cross messaging */
|
||||
struct ekf2_t {
|
||||
uint32_t gyro_stamp; ///< Gyroscope last abi message timestamp
|
||||
uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages
|
||||
uint32_t accel_stamp; ///< Accelerometer last abi message timestamp
|
||||
uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages
|
||||
uint32_t flow_stamp; ///< Optic flow last abi message timestamp
|
||||
|
||||
struct FloatRates gyro; ///< Last gyroscope measurements
|
||||
struct FloatVect3 accel; ///< Last accelerometer measurements
|
||||
bool gyro_valid; ///< If we received a gyroscope measurement
|
||||
bool accel_valid; ///< If we received a acceleration measurement
|
||||
|
||||
float temp; ///< Latest temperature measurement in degrees celcius
|
||||
float qnh; ///< QNH value in hPa
|
||||
uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
|
||||
uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
|
||||
struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counterEKF2
|
||||
struct OrientationReps body_to_imu; ///< Body to IMU rotation
|
||||
bool got_imu_data; ///< If we received valid IMU data (any sensor)
|
||||
|
||||
int32_t mag_fusion_type;
|
||||
int32_t fusion_mode;
|
||||
};
|
||||
@@ -45,7 +65,9 @@ extern void ins_ekf2_init(void);
|
||||
extern void ins_ekf2_update(void);
|
||||
extern void ins_ekf2_change_param(int32_t unk);
|
||||
extern void ins_ekf2_remove_gps(int32_t mode);
|
||||
extern struct ekf2_parameters_t ekf2_params;
|
||||
extern void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf);
|
||||
extern void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf);
|
||||
extern struct ekf2_t ekf2;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
+1
-1
Submodule sw/ext/ecl updated: a85d3a43ed...b3fed06fe8
+1
-1
Submodule sw/ext/matrix updated: 6b0777d815...c8da7960a1
+1
-1
Submodule sw/ext/pprzlink updated: 72963f4daf...2de761911b
@@ -57,10 +57,10 @@ GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-co
|
||||
INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ $(IVY_INC)
|
||||
INCLUDES += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/ -I$(PAPARAZZI_SRC)/sw/airborne/modules/gps/librtcm3/ -I$(PAPARAZZI_SRC)/sw/airborne/arch/linux/
|
||||
|
||||
all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
|
||||
all: davis2ivy kestrel2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
|
||||
|
||||
clean:
|
||||
$(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
|
||||
$(Q)rm -f *.o davis2ivy kestrel2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
|
||||
|
||||
davis2ivy: davis2ivy.o
|
||||
@echo CC $@
|
||||
@@ -70,10 +70,6 @@ kestrel2ivy: kestrel2ivy.o
|
||||
@echo CC $@
|
||||
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(IVY_LDFLAGS)
|
||||
|
||||
natnet2ivy: natnet2ivy.o pprz_geodetic_double.o pprz_algebra_double.o udp_socket.o
|
||||
@echo CC $@
|
||||
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
|
||||
|
||||
sbp2ivy: sbp2ivy.o serial_port.o sbp.o edc.o
|
||||
@echo CC $@
|
||||
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(INCLUDES) $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
|
||||
@@ -94,18 +90,12 @@ sbs2ivy: sbs2ivy.o pprz_geodetic_double.o pprz_geodetic_float.o sbs_parser.o
|
||||
@echo CC $@
|
||||
$(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) -o $@ $^ $(LIBRARYS) $(GLIBIVY_LDFLAGS) $(GTK_LDFLAGS) -lm
|
||||
|
||||
pprz_algebra_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_algebra_double.c
|
||||
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
|
||||
|
||||
pprz_geodetic_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.c
|
||||
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
|
||||
|
||||
pprz_geodetic_float.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_float.c
|
||||
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
|
||||
|
||||
udp_socket.o : $(PAPARAZZI_SRC)/sw/airborne/arch/linux/udp_socket.c
|
||||
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
|
||||
|
||||
serial_port.o : $(PAPARAZZI_SRC)/sw/airborne/arch/linux/serial_port.c
|
||||
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -13,7 +13,7 @@ FloatValue = struct.Struct( '<f' )
|
||||
DoubleValue = struct.Struct( '<d' )
|
||||
|
||||
class NatNetClient:
|
||||
def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1510, dataPort=1511, rigidBodyListener=None, newFrameListener=None, rigidBodyListListener=None, verbose=False ):
|
||||
def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1510, dataPort=1511, rigidBodyListener=None, newFrameListener=None, rigidBodyListListener=None, verbose=False, version=(3,0,0,0) ):
|
||||
# IP address of the NatNet server.
|
||||
self.serverIPAddress = server
|
||||
|
||||
@@ -37,7 +37,7 @@ class NatNetClient:
|
||||
self.rigidBodyList = []
|
||||
|
||||
# NatNet stream version. This will be updated to the actual version the server is using during initialization.
|
||||
self.__natNetStreamVersion = (3,0,0,0)
|
||||
self.__natNetStreamVersion = version
|
||||
|
||||
# Trace verbose level
|
||||
self.verbose = verbose
|
||||
@@ -114,16 +114,16 @@ class NatNetClient:
|
||||
|
||||
# Marker positions
|
||||
for i in markerCountRange:
|
||||
pos = Vector3.unpack( data[offset:offset+12] )
|
||||
mpos = Vector3.unpack( data[offset:offset+12] )
|
||||
offset += 12
|
||||
self.__trace( "\tMarker", i, ":", pos[0],",", pos[1],",", pos[2] )
|
||||
self.__trace( "\tMarker", i, ":", mpos[0],",", mpos[1],",", mpos[2] )
|
||||
|
||||
if( self.__natNetStreamVersion[0] >= 2 ):
|
||||
# Marker ID's
|
||||
for i in markerCountRange:
|
||||
id = int.from_bytes( data[offset:offset+4], byteorder='little' )
|
||||
mid = int.from_bytes( data[offset:offset+4], byteorder='little' )
|
||||
offset += 4
|
||||
self.__trace( "\tMarker ID", i, ":", id )
|
||||
self.__trace( "\tMarker ID", i, ":", mid )
|
||||
|
||||
# Marker sizes
|
||||
for i in markerCountRange:
|
||||
|
||||
@@ -70,6 +70,11 @@ parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help
|
||||
parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="transmit frequency")
|
||||
parser.add_argument('-gr', '--ground_ref', dest='ground_ref', action='store_true', help="also send the GROUND_REF message")
|
||||
parser.add_argument('-vs', '--vel_samples', dest='vel_samples', default=4, type=int, help="amount of samples to compute velocity (should be greater than 2)")
|
||||
parser.add_argument('-rg', '--remote_gps', dest='rgl_msg', action='store_true', help="use the old REMOTE_GPS_LOCAL message")
|
||||
parser.add_argument('-sm', '--small', dest='small_msg', action='store_true', help="enable the EXTERNAL_POSE_SMALL message instead of the full")
|
||||
parser.add_argument('-o', '--old_natnet', dest='old_natnet', action='store_true', help="Change the NatNet version to 2.9")
|
||||
parser.add_argument('-zf', '--z_forward', dest='z_forward', action='store_true', help="Z-axis as forward")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.ac is None:
|
||||
@@ -125,6 +130,18 @@ def compute_velocity(ac_id):
|
||||
vel[2] /= nb
|
||||
return vel
|
||||
|
||||
# Rotate the Z-forward to X-forward frame
|
||||
def rotZtoX(in_vec3, quat = False):
|
||||
out_vec3 = {}
|
||||
out_vec3[0] = -in_vec3[0]
|
||||
out_vec3[1] = in_vec3[2]
|
||||
out_vec3[2] = in_vec3[1]
|
||||
|
||||
# Copy the qi
|
||||
if quat:
|
||||
out_vec3[3] = in_vec3[3]
|
||||
return out_vec3
|
||||
|
||||
def receiveRigidBodyList( rigidBodyList, stamp ):
|
||||
for (ac_id, pos, quat, valid) in rigidBodyList:
|
||||
if not valid:
|
||||
@@ -142,22 +159,57 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
|
||||
continue # too early for next message
|
||||
timestamp[i] = stamp
|
||||
|
||||
msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
|
||||
msg['ac_id'] = id_dict[i]
|
||||
msg['pad'] = 0
|
||||
msg['enu_x'] = pos[0]
|
||||
msg['enu_y'] = pos[1]
|
||||
msg['enu_z'] = pos[2]
|
||||
vel = compute_velocity(i)
|
||||
msg['enu_xd'] = vel[0]
|
||||
msg['enu_yd'] = vel[1]
|
||||
msg['enu_zd'] = vel[2]
|
||||
msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
|
||||
# convert quaternion to psi euler angle
|
||||
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
|
||||
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
|
||||
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
|
||||
ivy.send(msg)
|
||||
|
||||
# Rotate everything if Z-forward
|
||||
if args.z_forward:
|
||||
pos = rotZtoX(pos)
|
||||
vel = rotZtoX(vel)
|
||||
quat = rotZtoX(quat, True)
|
||||
|
||||
# Check which message to send
|
||||
if args.rgl_msg:
|
||||
msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
|
||||
msg['ac_id'] = id_dict[i]
|
||||
msg['pad'] = 0
|
||||
msg['enu_x'] = pos[0]
|
||||
msg['enu_y'] = pos[1]
|
||||
msg['enu_z'] = pos[2]
|
||||
msg['enu_xd'] = vel[0]
|
||||
msg['enu_yd'] = vel[1]
|
||||
msg['enu_zd'] = vel[2]
|
||||
msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
|
||||
# convert quaternion to psi euler angle
|
||||
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
|
||||
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
|
||||
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
|
||||
ivy.send(msg)
|
||||
elif args.small_msg:
|
||||
# First check if position fits in message
|
||||
if abs(pos[0]*100.0) > pow(2, 10) or abs(pos[1]*100.0) > pow(2, 10) or pos[2]*100.0 > pow(2, 10) or pos[2] < 0.:
|
||||
print("Position out of range for small message")
|
||||
continue
|
||||
|
||||
# TODO calculate everything
|
||||
msg = PprzMessage("datalink", "EXTERNAL_POSE_SMALL")
|
||||
msg['ac_id'] = id_dict[i]
|
||||
msg['timestamp'] = int(1000. * stamp) # Time in ms
|
||||
ivy.send(msg)
|
||||
else:
|
||||
msg = PprzMessage("datalink", "EXTERNAL_POSE")
|
||||
msg['ac_id'] = id_dict[i]
|
||||
msg['timestamp'] = int(1000. * stamp) # Time in ms
|
||||
msg['enu_x'] = pos[0]
|
||||
msg['enu_y'] = pos[1]
|
||||
msg['enu_z'] = pos[2]
|
||||
msg['enu_xd'] = vel[0]
|
||||
msg['enu_yd'] = vel[1]
|
||||
msg['enu_zd'] = vel[2]
|
||||
msg['body_qi'] = quat[3]
|
||||
msg['body_qx'] = quat[0]
|
||||
msg['body_qy'] = quat[1]
|
||||
msg['body_qz'] = quat[2]
|
||||
ivy.send(msg)
|
||||
|
||||
# send GROUND_REF message if needed
|
||||
if args.ground_ref:
|
||||
@@ -174,12 +226,16 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
|
||||
|
||||
|
||||
# start natnet interface
|
||||
natnet_version = (3,0,0,0)
|
||||
if args.old_natnet:
|
||||
natnet_version = (2,9,0,0)
|
||||
natnet = NatNetClient(
|
||||
server=args.server,
|
||||
rigidBodyListListener=receiveRigidBodyList,
|
||||
dataPort=args.data_port,
|
||||
commandPort=args.command_port,
|
||||
verbose=args.verbose)
|
||||
verbose=args.verbose,
|
||||
version=natnet_version)
|
||||
|
||||
|
||||
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
|
||||
|
||||
Reference in New Issue
Block a user