From 7ae5f91a2a0966ea26e0d3d24b00a115e536e801 Mon Sep 17 00:00:00 2001
From: Freek van Tienen <freek.v.tienen@gmail.com>
Date: Wed, 20 Sep 2023 18:08:59 +0200
Subject: [PATCH] [modules] Fix flight recorder and MAVLink frequency to
 TELEMETRY_FREQUENCY and configuration (#3085)

---
 Makefile.ac                             | 10 ----------
 conf/airframes/examples/cube_orange.xml |  2 ++
 conf/modules/datalink_common.xml        |  2 ++
 conf/modules/extra_dl.xml               |  6 +++---
 conf/modules/flight_recorder.xml        |  4 +++-
 conf/modules/mavlink.xml                |  4 ++--
 sw/airborne/main_ap.c                   |  5 ++++-
 sw/airborne/main_chibios.c              |  2 +-
 8 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/Makefile.ac b/Makefile.ac
index a99adaca3b..2ce27693ba 100644
--- a/Makefile.ac
+++ b/Makefile.ac
@@ -110,16 +110,6 @@ ifeq ($(MAKECMDGOALS),all_ac_h)
 
 -include $(AIRBORNE)/Makefile
 
-ifdef PERIODIC_FREQUENCY
-# telemetry and module periodic frequency default to PERIODIC_FREQUENCY
-TELEMETRY_FREQUENCY ?= $(PERIODIC_FREQUENCY)
-DEFAULT_MODULES_FREQUENCY = $(PERIODIC_FREQUENCY)
-else
-$(warning Info: PERIODIC_FREQUENCY not configured, defaulting to 60Hz for modules and telemetry)
-TELEMETRY_FREQUENCY ?= 60
-DEFAULT_MODULES_FREQUENCY = 60
-endif
-
 endif
 
 
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index ecba76cc44..ee2a0d930d 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -16,6 +16,8 @@
   <firmware name="rotorcraft">
     <target name="ap" board="cube_orangeplus">
       <configure name="PERIODIC_FREQUENCY"  value="500"/>
+      <define name="SYS_TIME_FREQUENCY" value="1500"/><!-- To be able to send 3x IMU measurements -->
+      <configure name="TELEMETRY_FREQUENCY" value="1500"/><!-- To be able to send 3x IMU measurements -->
       <!--configure name="RTOS_DEBUG" value="1"/-->
 
       <module name="radio_control" type="sbus">
diff --git a/conf/modules/datalink_common.xml b/conf/modules/datalink_common.xml
index 972a3fd581..f7d7cf5917 100644
--- a/conf/modules/datalink_common.xml
+++ b/conf/modules/datalink_common.xml
@@ -19,6 +19,8 @@
   <makefile>
     <define name="DOWNLINK"/>
     <define name="PERIODIC_TELEMETRY"/>
+    <configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
+    <define name="TELEMETRY_FREQUENCY" value="$(TELEMETRY_FREQUENCY)"/>
     <file name="downlink.c"/>
     <file name="datalink.c"/>
     <file name="telemetry.c"/>
diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml
index d016ad720c..21be70f68b 100644
--- a/conf/modules/extra_dl.xml
+++ b/conf/modules/extra_dl.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="extra_dl" dir="datalink">
+<module name="extra_dl" dir="datalink" task="datalink">
   <doc>
     <description>Extra datalink (PPRZ transport)</description>
     <configure name="EXTRA_DL_PORT" value="UARTX|UDPX|usb_serial" description="Select port for extra datalink"/>
@@ -10,7 +10,7 @@
     <file name="extra_pprz_dl.h"/>
   </header>
   <init fun="extra_pprz_dl_init()"/>
-  <periodic fun="extra_pprz_dl_periodic()" freq="TELEMETRY_FREQUENCY" autorun="TRUE"/>
+  <periodic fun="extra_pprz_dl_periodic()" autorun="TRUE"/>
 
   <!--
   The event function listens on the extra telemetry port (from the payload computer for example)
@@ -23,9 +23,9 @@
 
   <makefile>
     <configure name="EXTRA_DL_PORT" default="uart1" case="upper|lower"/>
-    <configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
     <define name="EXTRA_DOWNLINK_DEVICE" value="$(EXTRA_DL_PORT_LOWER)"/>
     <define name="USE_$(EXTRA_DL_PORT_UPPER)"/>
+    <configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
     <define name="TELEMETRY_FREQUENCY" value="$(TELEMETRY_FREQUENCY)"/>
     <raw>
       # Check for UDP port
diff --git a/conf/modules/flight_recorder.xml b/conf/modules/flight_recorder.xml
index d41f2d69a1..b4fb3bed62 100644
--- a/conf/modules/flight_recorder.xml
+++ b/conf/modules/flight_recorder.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="flight_recorder" dir="loggers" task="core">
+<module name="flight_recorder" dir="loggers" task="datalink">
   <doc>
     <description>
       Record flight data according to your telemetry file.
@@ -24,6 +24,8 @@
   <makefile target="ap">
     <file name="flight_recorder.c"/>
     <define name="FLIGHTRECORDER_SDLOG" cond="ifneq (FALSE,$(findstring $(FLIGHTRECORDER_SDLOG),FALSE))"/>
+    <configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
+    <define name="TELEMETRY_FREQUENCY" value="$(TELEMETRY_FREQUENCY)"/>
   </makefile>
 </module>
 
diff --git a/conf/modules/mavlink.xml b/conf/modules/mavlink.xml
index 9d597cf823..7850e343ff 100644
--- a/conf/modules/mavlink.xml
+++ b/conf/modules/mavlink.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="mavlink" dir="datalink">
+<module name="mavlink" dir="datalink" task="datalink">
   <doc>
     <description>Basic MAVLink implementation</description>
     <configure name="MAVLINK_PORT" value="UARTx|UDPx|UsbS" description="The port device to use for mavlink (default: UART1)"/>
@@ -12,7 +12,7 @@
 
   <init fun="mavlink_init()"/>
   <periodic fun="mavlink_periodic()" freq="10" autorun="TRUE"/>
-  <periodic fun="mavlink_periodic_telemetry()" freq="TELEMETRY_FREQUENCY" autorun="TRUE"/>
+  <periodic fun="mavlink_periodic_telemetry()" autorun="TRUE"/>
   <event fun="mavlink_event()"/>
 
   <makefile>
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index e6dc415a2e..88ac0026c9 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -49,8 +49,11 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
 #endif
 
 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
- * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
+ * defaults to PERIODIC_FREQUENCY or set by TELEMETRY_FREQUENCY configure option in airframe file
  */
+#if (TELEMETRY_FREQUENCY > SYS_TIME_FREQUENCY) || !(SYS_TIME_FREQUENCY/TELEMETRY_FREQUENCY*TELEMETRY_FREQUENCY == SYS_TIME_FREQUENCY)
+#warning "The TELEMETRY_FREQUENCY can not be faster than the SYS_TIME_FREQUENCY and needs to be dividable by the SYS_TIME_FREQUENCY."
+#endif
 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
 
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
diff --git a/sw/airborne/main_chibios.c b/sw/airborne/main_chibios.c
index 0bf65c65ff..3db344f42e 100644
--- a/sw/airborne/main_chibios.c
+++ b/sw/airborne/main_chibios.c
@@ -33,7 +33,7 @@
 
 #ifndef  SYS_TIME_FREQUENCY
 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
-#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) && SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
+#elif  CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) || SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
 #endif