Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-10-02 12:23:17 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2 Optical Flow Interface (#2779)

Browse Source 
Co-authored-by: Dennis van Wijngaarden <wijngaarden.dennis@gmail.com>
This commit is contained in:
Pietro Campolucci
2021-09-15 22:26:28 +02:00
committed by GitHub
co-authored by Dennis van Wijngaarden
parent 3172453e74
commit 69c55a0663
17 changed files with 789 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+6
View File
@@ -8,4 +8,10 @@
"[python]": {
"editor.tabSize": 4,
},
"files.associations": {
"mateksys_3901_l0x.h": "c",
"stdlib.h": "c",
"iterator": "cpp",
"tensor": "cpp"
},
}
conf/airframes/tudelft/nederquad.xml
+309
View File
File diff suppressed because it is too large Load Diff
conf/modules/gps_datalink.xml
+1
View File
@@ -6,6 +6,7 @@
Remote GPS via datalink.
Parses the REMOTE_GPS and REMOTE_GPS_SMALL datalink messages and publishes it 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>
conf/modules/ins_ekf2.xml
+17 -5
View File
@@ -5,11 +5,21 @@
<description>
simple INS and AHRS using EKF2 from PX4
</description>
<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_SONAR_MIN_RANGE" value="0.001" description="Minimum range in meters for the AGL sensor"/>
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="Maximum range in meters for the AGL sensor"/>
<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"/>
@@ -22,6 +32,7 @@
<!-- 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="subsystems/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="subsystems/ins/ins_ekf2" handler="remove_gps" type="bool" persistent="true"/>
</dl_settings>
</dl_settings>
</settings>
@@ -45,9 +56,9 @@
<!-- 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 -->
<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"/>
@@ -67,5 +78,6 @@
<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/optical_flow_mateksys_3901_l0x.xml
+13 -8
View File
@@ -12,11 +12,24 @@
<configure name="MATEKSYS_3901_L0X_BAUD" value="115200" description="Sets the baudrate of the UART"/>
<define name="MATEKSYS_3901_L0X_MOTION_THRES" value="120" description="Sets the minimum motion quality to accept the flow measurement [0-255]"/>
<define name="MATEKSYS_3901_L0X_DISTANCE_THRES" value="200" description="Sets the minimum distance quality to accept the distance measurement [0-255]"/>
<define name="MATEKSYS_3901_L0X_MAX_DISTANCE" value="2500" description="Maximum allowable distance for rangefinder [mm]"/>
<define name="MATEKSYS_3901_L0X_MAX_FLOW" value="150" description="Maximum allowable flow rate measurable [deg/sec] "/>
<define name="USE_MATEKSYS_3901_L0X_AGL" value="0" description="Send AGL measurements on ABI bus"/>
<define name="USE_MATEKSYS_3901_L0X_OPTICAL_FLOW" value="0" description="Send optical flow measurements on ABI bus"/>
<define name="MATEKSYS_3901_L0X_COMPENSATE_ROTATION" value="0" description="Adjust distance from ground by compensating body rotation"/>
<define name="MATEKSYS_3901_L0X_FLOW_X_SCALER" value="1" description="Calibration scaling factor for flow in X direction (pitch)"/>
<define name="MATEKSYS_3901_L0X_FLOW_Y_SCALER" value="1" description="Calibration scaling factor for flow in Y direction (roll)"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="optical_flow">
<dl_setting MAX="2" MIN="-2" STEP="0.05" VAR="mateksys3901l0x.scaler_x" shortname="scaler X" module="modules/optical_flow/mateksys_3901_l0x" param="MATEKSYS_3901_L0X_FLOW_X_SCALER" handler="scale_X" type="float" persistent="true"/>
<dl_setting MAX="2" MIN="-2" STEP="0.05" VAR="mateksys3901l0x.scaler_y" shortname="scaler Y" module="modules/optical_flow/mateksys_3901_l0x" param="MATEKSYS_3901_L0X_FLOW_Y_SCALER" handler="scale_Y" type="float" persistent="true"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="mateksys_3901_l0x.h"/>
</header>
@@ -32,18 +45,10 @@
<!-- Enable UART and set baudrate -->
<define name="USE_$(MATEKSYS_3901_L0X_PORT_UPPER)"/>
<!-- If the driver is used on a serial port where the TX pin is not available, since the TX is not needed for this sensor it can be disabled -->
<define name="USE_$(MATEKSYS_3901_L0X_PORT)_TX" value="FALSE"/>
<define name="$(MATEKSYS_3901_L0X_PORT_UPPER)_BAUD" value="$(MATEKSYS_3901_L0X_BAUD)"/>
<define name="MATEKSYS_3901_L0X_PORT" value="$(MATEKSYS_3901_L0X_PORT_LOWER)"/>
<!-- Data processing configuration parameters -->
<define name="MATEKSYS_3901_L0X_MOTION_THRES" value="120"/>
<define name="MATEKSYS_3901_L0X_DISTANCE_THRES" value="200"/>
<define name="USE_MATEKSYS_3901_L0X_AGL" value="1"/>
<define name="USE_MATEKSYS_3901_L0X_OPTICAL_FLOW" value="1"/>
<define name="MATEKSYS_3901_L0X_COMPENSATE_ROTATION" value="0"/>
<file name="mateksys_3901_l0x.c"/>
</makefile>
conf/userconf/tudelft/conf.xml
+11
View File
@@ -309,6 +309,17 @@
gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="Nederquad"
ac_id="89"
airframe="airframes/tudelft/nederquad.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="[settings/rotorcraft_basic.xml] [settings/control/stabilization_rate.xml] [settings/control/stabilization_indi.xml]"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/lidar_lite.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_mateksys_3901_l0x.xml modules/stabilization_indi_simple.xml"
gui_color="#406b937dccd0"
/>
<aircraft
name="OrigamiMXS_wifi_indi_stereocam"
ac_id="15"
conf/userconf/tudelft/control_panel.xml
+72
View File
@@ -67,6 +67,78 @@
<program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
</section>
<section name="sessions">
<session name="EKF Full">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-layout" constant="bottom_settings.xml"/>
</program>
<program name="Messages"/>
<program name="Real-time Plotter">
<arg flag="-g" constant="800x250-0+0"/>
<arg flag="-t" constant="ACC"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-0+250"/>
<arg flag="-t" constant="GYRO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-0+500"/>
<arg flag="-t" constant="MAG"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-0+750"/>
<arg flag="-t" constant="INNOVATION_STATUS"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_vel"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_pos"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_hgt"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_tas"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_hagl"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_flow"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_beta"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+0"/>
<arg flag="-t" constant="POSITION"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:east:0.0039063"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:north:0.0039063"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:up:0.0039063"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+250"/>
<arg flag="-t" constant="VELOCITY"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:veast:0.0000019"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:vnorth:0.0000019"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+500"/>
<arg flag="-t" constant="OPTICAL FLOW"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:flow_x"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:flow_x"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:distance_compensated"/>
</program>
</session>
<session name="helidd">
<program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM0" constant="00:07:80:2d:d6:d9"/>
sw/airborne/modules/optical_flow/mateksys_3901_l0x.c
+89 -48
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/optical_flow/mateksys_3901_l0x.h
+19 -16
View File
@@ -22,7 +22,6 @@
/** @file modules/optical_flow/mateksys_3901_l0x.h
* @brief Driver for the mateksys_3901_l0x sensor via MSPx protocol output
*
*/
/*
@@ -37,7 +36,6 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2
#include "std.h"
#include "stdbool.h"
//#include "filters/median_filter.h" //Who knows we need it ;)
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF)
@@ -45,7 +43,7 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
enum Mateksys3901l0XParseStatus {
MATEKSYS_3901_L0X_INITIALIZE, // initialization
MATEKSYS_3901_L0X_INITIALIZE,
MATEKSYS_3901_L0X_PARSE_HEAD,
MATEKSYS_3901_L0X_PARSE_HEAD2,
MATEKSYS_3901_L0X_PARSE_DIRECTION,
@@ -53,13 +51,13 @@ enum Mateksys3901l0XParseStatus {
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1,
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2,
MATEKSYS_3901_L0X_PARSE_SIZE,
MATEKSYS_3901_L0X_PARSE_POINTER, // ??
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY, // used if lidar message
MATEKSYS_3901_L0X_PARSE_POINTER,
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B1,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B2,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B3,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B4,
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY, // used if flow message
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B1,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B2,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B3,
@@ -74,19 +72,22 @@ enum Mateksys3901l0XParseStatus {
struct Mateksys3901l0X {
struct link_device *device;
enum Mateksys3901l0XParseStatus parse_status;
float time_sec;
float time_usec;
uint8_t sensor_id;
uint8_t motion_quality;
uint8_t motion_quality;
int32_t motionX_temp;
int32_t motionX;
int32_t motionY_temp;
int32_t motionY;
int32_t motionX_clean;
int32_t motionY_clean;
uint8_t distancemm_quality;
int32_t distancemm;
float distance_clean;
float velocityX;
float velocityY;
uint8_t parse_crc;
uint8_t distancemm_quality;
int32_t distancemm_temp;
float distancemm;
float distance_compensated;
float velocityX;
float velocityY;
uint8_t parse_crc;
float scaler_x;
float scaler_y;
};
extern struct Mateksys3901l0X mateksys3901l0x;
@@ -94,6 +95,8 @@ extern struct Mateksys3901l0X mateksys3901l0x;
extern void mateksys3901l0x_init(void);
extern void mateksys3901l0x_event(void);
extern void mateksys3901l0x_downlink(void);
extern void mateksys_3901_l0x_scale_X(float scalex);
extern void mateksys_3901_l0x_scale_Y(float scaley);
#endif /* MATEKSYS_3901_L0X_H */
sw/airborne/modules/optical_flow/px4flow.c
+2
View File
@@ -102,6 +102,7 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
optical_flow.distance = optical_flow.ground_distance;
optical_flow.ground_distance = optical_flow.ground_distance / gain;
}
@@ -172,6 +173,7 @@ void px4flow_downlink(void)
&optical_flow.flow_comp_m_x,
&optical_flow.flow_comp_m_y,
&optical_flow.quality,
&optical_flow.distance,
&optical_flow.ground_distance,
&distance_quality);
}
sw/airborne/modules/optical_flow/px4flow.h
+1
View File
@@ -47,6 +47,7 @@ struct mavlink_optical_flow {
float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated [meters/sec]
float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated [meters/sec]
float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
float distance; ///< Distance measured without compensation in meters
int32_t flow_x; ///< Flow in pixels in x-sensor direction
int32_t flow_y; ///< Flow in pixels in y-sensor direction
uint8_t sensor_id; ///< Sensor ID
sw/airborne/modules/optical_flow/px4flow_i2c.c
+3
View File
@@ -278,6 +278,7 @@ void px4flow_i2c_downlink(void)
uint8_t quality = px4flow.i2c_int_frame.qual;
float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0;
float distance = 0;
#else
int32_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum;
int32_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum;
@@ -287,6 +288,7 @@ void px4flow_i2c_downlink(void)
uint8_t quality = px4flow.i2c_frame.qual;
float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0;
float distance;
#endif
DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice,
@@ -298,5 +300,6 @@ void px4flow_i2c_downlink(void)
&flow_comp_m_y,
&quality,
&ground_distance,
&distance,
&distance_quality);
}
sw/airborne/subsystems/abi_sender_ids.h
+4
View File
@@ -209,6 +209,10 @@
#define MAG_RM3100_SENDER_ID 5
#endif
#ifndef MAG_DATALINK_SENDER_ID
#define MAG_DATALINK_SENDER_ID 6
#endif
#ifndef IMU_MAG_PITOT_ID
#define IMU_MAG_PITOT_ID 50
#endif
sw/airborne/subsystems/gps/gps_datalink.c
+43 -2
View File
@@ -1,3 +1,4 @@
/*
* Copyright (C) 2014 Freek van Tienen
*
@@ -31,7 +32,15 @@
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "subsystems/imu.h"
#include "subsystems/datalink/datalink.h"
#include "subsystems/datalink/downlink.h"
/** Set to 1 to receive also magnetometer ABI messages */
#ifndef GPS_DATALINK_USE_MAG
#define GPS_DATALINK_USE_MAG 1
#endif
PRINT_CONFIG_VAR(GPS_DATALINK_USE_MAG)
struct LtpDef_i ltp_def;
@@ -57,6 +66,26 @@ void gps_datalink_init(void)
ltp_def_from_lla_i(&ltp_def, &llh_nav0);
}
// Send GPS heading info as magnetometer messages
static void send_magnetometer(int32_t course, uint32_t now_ts)
{
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);
// 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)
{
@@ -168,8 +197,14 @@ static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in
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();
// 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);
}
@@ -223,8 +258,14 @@ static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
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();
// 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);
}
sw/airborne/subsystems/ins/ins_ekf2.cpp
+196 -22
View File
File diff suppressed because it is too large Load Diff
sw/airborne/subsystems/ins/ins_ekf2.h
+2
View File
@@ -38,11 +38,13 @@ extern "C" {
struct ekf2_parameters_t {
int32_t mag_fusion_type;
int32_t fusion_mode;
};
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;
#ifdef __cplusplus
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: fca8d15eff...c6e88ccbb9