From e34b42cd7f1b83062aab1098bde452c03f54d27e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 25 Jun 2025 12:36:48 +0200
Subject: [PATCH] Fixedwing updates: Calibrate MAG, replay SD-log, Set WP
 (#3486)

---
 conf/telemetry/default_fixedwing.xml         |  1 +
 conf/telemetry/fixedwing_flight_recorder.xml | 19 +++++++++++++++++++
 sw/airborne/modules/nav/common_nav.h         | 13 +++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml
index 9e194b9850..a137b71f3a 100644
--- a/conf/telemetry/default_fixedwing.xml
+++ b/conf/telemetry/default_fixedwing.xml
@@ -70,6 +70,7 @@
       <message name="BARO_RAW"          period="0.5"/>
       <message name="IMU_GYRO_RAW"      period="0.1"/>
       <message name="AIRSPEED_RAW"      period="0.5"/>
+      <message name="IMU_MAG_RAW"       period="0.5"/>
     </mode>
   </process>
   <process name="Fbw">
diff --git a/conf/telemetry/fixedwing_flight_recorder.xml b/conf/telemetry/fixedwing_flight_recorder.xml
index 32b1483bb4..ba2f7a53c9 100644
--- a/conf/telemetry/fixedwing_flight_recorder.xml
+++ b/conf/telemetry/fixedwing_flight_recorder.xml
@@ -119,6 +119,7 @@
     <mode name="default">
       <message name="ENERGY"              period="0.5"/>
       <message name="GPS"                 period="0.2"/>
+      <message name="GPS_SOL"             period="0.5"/>
       <message name="ATTITUDE"            period="0.05"/>
       <message name="DESIRED"             period="0.05"/>
       <message name="IMU_ACCEL"           period=".02"/>
@@ -128,6 +129,7 @@
       <message name="AOA"                 period="0.05"/>
       <message name="RC"                  period="0.05"/>
       <message name="COMMANDS"            period="0.05"/>
+      <message name="ESTIMATOR"           period="0.05"/>
       <message name="ACTUATORS"           period="0.05"/>
       <message name="PPRZ_MODE"           period="1.0"/>
       <message name="FBW_STATUS"          period="1.0"/>
@@ -136,6 +138,23 @@
       <message name="ESC"                 period="0.1"/>
       <message name="ADC_GENERIC"         period="0.05"/>
       <message name="WIND_INFO_RET"       period="0.1"/>
+      <message name="AUTOPILOT_VERSION"   period="11.1"/>
+      <message name="AIRSPEED"            period="1"/>
+      <message name="ALIVE"               period="5.1"/>
+      <message name="WP_MOVED"            period="0.5"/>
+      <message name="CIRCLE"              period="1.05"/>
+      <message name="WP_MOVED"             period="0.5"/>
+      <message name="SEGMENT"             period="1.2"/>
+      <message name="NAVIGATION_REF"      period="9."/>
+    </mode>
+  </process>
+  <process name="Mavlink"  type="mavlink">
+    <mode name="default">
+      <message name="HEARTBEAT"             period="0.5"/>
+      <message name="SYSTEM_TIME"           period="0.2"/>
+      <message name="SYS_STATUS"            period="0.2"/>
+      <message name="GPS_RAW_INT"           period="0.05"/>
+      <message name="ATTITUDE"              period="0.02"/>
     </mode>
   </process>
 </telemetry>
diff --git a/sw/airborne/modules/nav/common_nav.h b/sw/airborne/modules/nav/common_nav.h
index 873899cb47..9dc0372669 100644
--- a/sw/airborne/modules/nav/common_nav.h
+++ b/sw/airborne/modules/nav/common_nav.h
@@ -84,6 +84,13 @@ extern float get_time_to_home(void); /* estimated time to home point in seconds
     false; \
   })
 
+#define NavSetWaypointHereSend(_wp) ({ \
+    waypoints[_wp].x = stateGetPositionEnu_f()->x; \
+    waypoints[_wp].y = stateGetPositionEnu_f()->y; \
+    RunOnceEvery(NAVIGATION_FREQUENCY/1, {nav_send_waypoint(_wp);}) \
+    false; \
+  })
+
 #define NavSetWaypointPosAndAltHere(_wp) ({ \
     waypoints[_wp].x = stateGetPositionEnu_f()->x; \
     waypoints[_wp].y = stateGetPositionEnu_f()->y; \
@@ -91,4 +98,10 @@ extern float get_time_to_home(void); /* estimated time to home point in seconds
     false; \
   })
 
+#define NavSetWaypointDistBehind(_wp, _ref, dist) ({ \
+  waypoints[_wp].x = waypoints[_ref].x - dist*sin(stateGetNedToBodyEulers_f()->psi); \
+  waypoints[_wp].y = waypoints[_ref].y - dist*cos(stateGetNedToBodyEulers_f()->psi); \
+  false; \
+})
+
 #endif /* COMMON_NAV_H */