Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 13:27:32 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ins] EKF2 add extra GPS heading checks (#3337)

Browse Source 
* add some extra checks if GPS_COURSE_YAW is enabled

* Added antennae distances to airframe files, scale distance error by factor instead of hardcode
This commit is contained in:
NoahWe
2024-07-23 11:09:26 +02:00
committed by GitHub
parent d610ef1bef
commit 862f69267e
7 changed files with 20 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+1
View File
@@ -306,6 +306,7 @@
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_ANTENNA_DISTANCE" value="2.40"/> <!-- In meters -->
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3d.xml
+3
View File
@@ -448,6 +448,9 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3e.xml
+3
View File
@@ -448,6 +448,9 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3f.xml
+3
View File
@@ -448,6 +448,9 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3g.xml
+1
View File
@@ -455,6 +455,7 @@
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3h.xml
+1
View File
@@ -455,6 +455,7 @@
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
sw/airborne/modules/ins/ins_ekf2.cpp
+8 -2
View File
@@ -49,6 +49,11 @@
#define USE_INS_NAV_INIT TRUE
#endif
/** Maximum allowed error in distance between dual GPS antennae */
#ifndef INS_EKF2_MAX_REL_LENGTH_ERROR
#define INS_EKF2_MAX_REL_LENGTH_ERROR 0.2 // Factor which gets multiplied by the reference distance
#endif
/** Special configuration for Optitrack */
#if INS_EKF2_OPTITRACK
#ifndef INS_EKF2_FUSION_MODE
@@ -927,8 +932,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
#if INS_EKF2_GPS_COURSE_YAW
gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
gps_msg.yaw_offset = 0;
#elif defined(INS_EKF2_GPS_YAW_OFFSET)
if(ISFINITE(gps_relposned.relPosHeading)) {
#elif defined(INS_EKF2_GPS_YAW_OFFSET) && defined(INS_EKF2_ANTENNA_DISTANCE)
if(ISFINITE(gps_relposned.relPosHeading) && gps_relposned.relPosValid && gps_relposned.diffSoln && gps_relposned.carrSoln >= 1
&& fabsf(gps_relposned.relPosLength - INS_EKF2_ANTENNA_DISTANCE) <= INS_EKF2_MAX_REL_LENGTH_ERROR * INS_EKF2_ANTENNA_DISTANCE) {
gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading - INS_EKF2_GPS_YAW_OFFSET));
} else {
gps_msg.yaw = NAN;