[modules] Fix flight recorder and MAVLink frequency to TELEMETRY_FREQUENCY and configuration (#3085)

This commit is contained in:
Freek van Tienen
2023-09-20 18:08:59 +02:00
committed by GitHub
parent a98fd3ab49
commit 7ae5f91a2a
8 changed files with 17 additions and 18 deletions

View File

@@ -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

View File

@@ -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">

View File

@@ -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"/>

View File

@@ -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

View File

@@ -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>

View File

@@ -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>

View File

@@ -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)

View File

@@ -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