Basic support for softRF

docs/ADSB.md

@@ -34,6 +34,7 @@ All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/m
 * [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested)
 * [TT-SC1](https://www.aerobits.pl/product/aero/) (tested)
 * [ADSBee1090](https://pantsforbirds.com/adsbee-1090/) (tested)
+* [SoftRF](https://github.com/lyusupov/SoftRF/wiki/Nano-Edition) (tested)
 
 ## TT-SC1 settings
 * download software for ADSB TT-SC1 from https://www.aerobits.pl/product/aero/ , file Micro_ADSB_App-vX.XX.X_win_setup.zip and install it
@@ -63,3 +64,12 @@ AT+SETTINGS=SAVE
 * in INAV configurator ports TAB set telemetry MAVLINK, and baudrate 115200
 * https://pantsforbirds.com/adsbee-1090/quick-start/
 
+## SoftRF settings
+SoftRF supports only MAVLink version 1.
+```
+set mavlink_version = 1
+save
+```
+The baud rate for SoftRF is 57600. INAV provides minimal support for SoftRF and supports only 
+the mandatory MAVLink messages: `MAVLINK_MSG_ID_HEARTBEAT`, `MAVLINK_MSG_ID_SYSTEM_TIME`, and `MAVLINK_MSG_ID_GPS_RAW_INT`.
+

src/main/telemetry/mavlink.c

@@ -171,10 +171,19 @@ static uint8_t mavRates[] = {
     [MAV_DATA_STREAM_RC_CHANNELS] = 1,          // 1Hz
     [MAV_DATA_STREAM_POSITION] = 2,             // 2Hz
     [MAV_DATA_STREAM_EXTRA1] = 3,               // 3Hz
-    [MAV_DATA_STREAM_EXTRA2] = 2,               // 2Hz, HEARTBEATs are important
-    [MAV_DATA_STREAM_EXTRA3] = 1                // 1Hz
+    [MAV_DATA_STREAM_EXTRA2] = 2,               // 2Hz
+    [MAV_DATA_STREAM_EXTRA3] = 1,               // 1Hz
 };
 
+/* HEARTBEAT and SYSTEM_TIME are not part of any MAV_DATA_STREAM; scheduled independently. */
+typedef struct mavlinkScheduledMessage_s {
+    uint8_t rateHz;     // desired transmission rate in Hz
+    uint8_t ticks;      // countdown decremented at TELEMETRY_MAVLINK_MAXRATE
+} mavlinkScheduledMessage_t;
+
+static mavlinkScheduledMessage_t mavHeartbeat  = { .rateHz = 1, .ticks = 0 };
+static mavlinkScheduledMessage_t mavSystemTime = { .rateHz = 1, .ticks = TELEMETRY_MAVLINK_MAXRATE / 2 }; // Stagger SYSTEM_TIME by half a period so it doesn't ride in the same TX burst as HEARTBEAT.
+
 #define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
 
 static timeUs_t lastMavlinkMessage = 0;
@@ -286,6 +295,22 @@ static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
     return 0;
 }
 
+static bool mavlinkScheduledTrigger(mavlinkScheduledMessage_t *msg)
+{
+    if (msg->rateHz == 0) {
+        return false;
+    }
+
+    if (msg->ticks == 0) {
+        uint8_t rate = msg->rateHz > TELEMETRY_MAVLINK_MAXRATE ? TELEMETRY_MAVLINK_MAXRATE : msg->rateHz;
+        msg->ticks = TELEMETRY_MAVLINK_MAXRATE / rate;
+        return true;
+    }
+
+    msg->ticks--;
+    return false;
+}
+
 void freeMAVLinkTelemetryPort(void)
 {
     closeSerialPort(mavlinkPort);
@@ -593,6 +618,8 @@ void mavlinkSendRCChannelsAndRSSI(void)
 void mavlinkSendPosition(timeUs_t currentTimeUs)
 {
     uint8_t gpsFixType = 0;
+    rtcTime_t rtcTime;
+    uint64_t timeUnixUsec = currentTimeUs;
 
     if (!(sensors(SENSOR_GPS)
 #ifdef USE_GPS_FIX_ESTIMATION
@@ -601,16 +628,21 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
         ))
         return;
 
-    if (gpsSol.fixType == GPS_NO_FIX)
+    if (gpsSol.fixType == GPS_NO_FIX) {
         gpsFixType = 1;
-    else if (gpsSol.fixType == GPS_FIX_2D)
-            gpsFixType = 2;
-    else if (gpsSol.fixType == GPS_FIX_3D)
-            gpsFixType = 3;
+    } else if (gpsSol.fixType == GPS_FIX_2D) {
+        gpsFixType = 2;
+    } else if (gpsSol.fixType == GPS_FIX_3D) {
+        gpsFixType = 3;
+    }
+
+    if (rtcGet(&rtcTime)) {
+        timeUnixUsec = (uint64_t)rtcTime * 1000ULL;
+    }
 
     mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg,
         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
-        currentTimeUs,
+        timeUnixUsec,
         // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
         gpsFixType,
         // lat Latitude in 1E7 degrees
@@ -704,52 +736,22 @@ void mavlinkSendAttitude(void)
     mavlinkSendMessage();
 }
 
-void mavlinkSendHUDAndHeartbeat(void)
+void mavlinkSendSystemTime(void)
 {
-    float mavAltitude = 0;
-    float mavGroundSpeed = 0;
-    float mavAirSpeed = 0;
-    float mavClimbRate = 0;
-
-#if defined(USE_GPS)
-    // use ground speed if source available
-    if (sensors(SENSOR_GPS)
-#ifdef USE_GPS_FIX_ESTIMATION
-            || STATE(GPS_ESTIMATED_FIX)
-#endif
-        ) {
-        mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
-    }
-#endif
+    uint64_t timeUnixUsec = 0;
+    rtcTime_t rtcTime;
 
-#if defined(USE_PITOT)
-    if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
-        mavAirSpeed = getAirspeedEstimate() / 100.0f;
+    if (rtcGet(&rtcTime)) {
+        //timeUnixUsec = (uint64_t)rtcTime * 1000ULL + (uint64_t)(micros() % 1000); // extrapolation to uS
+        timeUnixUsec = (uint64_t)rtcTime * 1000ULL; // mS resolution
     }
-#endif
-
-    // select best source for altitude
-    mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
-    mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
-
-    int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
-    mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
-        // airspeed Current airspeed in m/s
-        mavAirSpeed,
-        // groundspeed Current ground speed in m/s
-        mavGroundSpeed,
-        // heading Current heading in degrees, in compass units (0..360, 0=north)
-        DECIDEGREES_TO_DEGREES(attitude.values.yaw),
-        // throttle Current throttle setting in integer percent, 0 to 100
-        thr,
-        // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
-        mavAltitude,
-        // climb Current climb rate in meters/second
-        mavClimbRate);
 
+    mavlink_msg_system_time_pack(mavSystemId, mavComponentId, &mavSendMsg, timeUnixUsec, millis());
     mavlinkSendMessage();
+}
 
-
+void mavlinkSendHeartbeat(void)
+{
     uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
     if (ARMING_FLAG(ARMED))
         mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -821,16 +823,62 @@ void mavlinkSendHUDAndHeartbeat(void)
     }
 
     mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg,
-        // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
-        mavSystemType,
-        // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
-        mavType,
-        // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
-        mavModes,
-        // custom_mode A bitfield for use for autopilot-specific flags.
-        mavCustomMode,
-        // system_status System status flag, see MAV_STATE ENUM
-        mavSystemState);
+            // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
+                               mavSystemType,
+            // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
+                               mavType,
+            // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
+                               mavModes,
+            // custom_mode A bitfield for use for autopilot-specific flags.
+                               mavCustomMode,
+            // system_status System status flag, see MAV_STATE ENUM
+                               mavSystemState);
+
+    mavlinkSendMessage();
+}
+
+void mavlinkSendHUD(void)
+{
+    float mavAltitude = 0;
+    float mavGroundSpeed = 0;
+    float mavAirSpeed = 0;
+    float mavClimbRate = 0;
+
+#if defined(USE_GPS)
+    // use ground speed if source available
+    if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
+        mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
+    }
+#endif
+
+#if defined(USE_PITOT)
+    if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
+        mavAirSpeed = getAirspeedEstimate() / 100.0f;
+    }
+#endif
+
+    // select best source for altitude
+    mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
+    mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
+
+    int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
+    mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
+        // airspeed Current airspeed in m/s
+        mavAirSpeed,
+        // groundspeed Current ground speed in m/s
+        mavGroundSpeed,
+        // heading Current heading in degrees, in compass units (0..360, 0=north)
+        DECIDEGREES_TO_DEGREES(attitude.values.yaw),
+        // throttle Current throttle setting in integer percent, 0 to 100
+        thr,
+        // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
+        mavAltitude,
+        // climb Current climb rate in meters/second
+        mavClimbRate);
 
     mavlinkSendMessage();
 }
@@ -952,13 +1000,20 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs)
     }
 
     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
-        mavlinkSendHUDAndHeartbeat();
+        mavlinkSendHUD();
     }
 
     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
         mavlinkSendBatteryTemperatureStatusText();
     }
 
+    if (mavlinkScheduledTrigger(&mavHeartbeat)) {
+        mavlinkSendHeartbeat();
+    }
+
+    if (mavlinkScheduledTrigger(&mavSystemTime)) {
+        mavlinkSendSystemTime();
+    }
 }
 
 static bool handleIncoming_MISSION_CLEAR_ALL(void)