mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-10-02 12:23:17 +08:00
EKF2 Optical Flow Interface (#2779)
Co-authored-by: Dennis van Wijngaarden <wijngaarden.dennis@gmail.com>
This commit is contained in:
Pietro Campolucci
co-authored by
Dennis van Wijngaarden
Dennis van Wijngaarden
parent
3172453e74
commit
69c55a0663
Vendored
+6
@@ -8,4 +8,10 @@
|
||||
"[python]": {
|
||||
"editor.tabSize": 4,
|
||||
},
|
||||
"files.associations": {
|
||||
"mateksys_3901_l0x.h": "c",
|
||||
"stdlib.h": "c",
|
||||
"iterator": "cpp",
|
||||
"tensor": "cpp"
|
||||
},
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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"/>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
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;
|
||||
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 */
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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(<p_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);
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: fca8d15eff...c6e88ccbb9
Reference in New Issue
Block a user