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

[airframe] Update TUD - [ekf] dual GPS heading (#3323)

Browse Source 
* Fix messages

* Enable GPS heading 25kg

* Larger heater errors and 25kg checklist

* AGL fix for 3

* Calibrated 25kg

* [ekf2] Fix GPS heading offset

* Fix heading EKF2 and correct GPS ports

---------

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
This commit is contained in:
Christophe De Wagter
2024-06-22 14:49:06 +02:00
committed by GitHub
parent 36828c0560
commit 7100d157c7
12 changed files with 23 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+6 -2
View File
@@ -304,6 +304,8 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<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"/>
</section>
<section name="GROUND_DETECT">
@@ -326,8 +328,8 @@
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="95,217,-115" type="int[]"/>
<field name="scale" value="{{29417,8274,1853},{49873,15133,3195}}"/>
<field name="neutral" value="-66,372,-134" type="int[]"/>
<field name="scale" value="{{34618,27035,27547},{57579,48737,47275}}"/>
<field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/>
</field>
</define>
@@ -571,12 +573,14 @@
<item name="basic law">Location, airspace and weather</item>
<item name="RC Battery">Check the RC battery</item>
<item name="wings">Check wings secured (and taped)</item>
<item name="rotation">Initialize and check wing rotation</item>
<item name="attitude">Check attitude and heading</item>
<item name="airspeed">Calibrate airspeed sensor</item>
<item name="takeoff location">Put UAV on take-off location</item>
<item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item>
<item name="camera">Switch on camera</item>
<item name="parachute">Arm parachute</item>
<item name="announce">Announce flight to other airspace users</item>
</checklist>
</airframe>
conf/airframes/tudelft/rot_wing_v3b.xml
+1 -1
View File
@@ -467,7 +467,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
+1 -1
View File
@@ -240,7 +240,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
+1 -1
View File
@@ -240,7 +240,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3d.xml
+1 -1
View File
@@ -455,7 +455,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3e.xml
+1 -1
View File
@@ -455,7 +455,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3f.xml
+1 -1
View File
@@ -455,7 +455,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3g.xml
+1 -1
View File
@@ -462,7 +462,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/airframes/tudelft/rot_wing_v3h.xml
+1 -1
View File
@@ -462,7 +462,7 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
conf/telemetry/highspeed_rotorcraft.xml
+2 -4
View File
@@ -49,7 +49,6 @@
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="AOA" period="0.2"/>
<message name="ACTUATORS_RAW" period="0.23"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
@@ -98,6 +97,7 @@
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="ACTUATORS_RAW" period="0.1"/>
</mode>
<mode name="scaled_sensors">
@@ -251,11 +251,9 @@
<message name="HYBRID_GUIDANCE" period="0.02"/>
<message name="ESC" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.002"/>
<message name="PPM" period="0.05"/>
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/>
<message name="IMU_HEATER" period="6.2"/>
<message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
<message name="EXTERNAL_POSE_DOWN" period="0.002"/>
sw/airborne/modules/imu/imu_heater.c
+3 -3
View File
@@ -81,7 +81,7 @@ static struct preflight_check_t imu_heater_pfc;
/** Maximum error in percentile */
#ifndef IMU_HEATER_MAX_ERROR
#define IMU_HEATER_MAX_ERROR 0.15f
#define IMU_HEATER_MAX_ERROR 0.20f
#endif
/** ABI id from either guro or accel */
@@ -92,8 +92,8 @@ static struct preflight_check_t imu_heater_pfc;
#endif
static void imu_heater_preflight(struct preflight_result_t *result) {
if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+2*IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+2*IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
} else {
preflight_success(result, "IMU %d temperature ok", IMU_HEATER_ABI_ID);
}
sw/airborne/modules/ins/ins_ekf2.cpp
+4 -2
View File
@@ -929,11 +929,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
gps_msg.yaw_offset = 0;
#elif defined(INS_EKF2_GPS_YAW_OFFSET)
if(ISFINITE(gps_relposned.relPosHeading)) {
gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading));
gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading - INS_EKF2_GPS_YAW_OFFSET));
} else {
gps_msg.yaw = NAN;
}
gps_msg.yaw_offset = INS_EKF2_GPS_YAW_OFFSET;
// Offset also needs to be substracted from the heading (this is for roll/pitch angle limits)
gps_msg.yaw_offset = RadOfDeg(INS_EKF2_GPS_YAW_OFFSET);
#else
gps_msg.yaw = NAN;
gps_msg.yaw_offset = NAN;