added options.h to control behaviour

and fixed remote ID failure status

RemoteIDModule/DroneCAN.cpp

@@ -471,13 +471,6 @@ void DroneCAN::readUniqueID(uint8_t id[6])
     esp_efuse_mac_get_default(id);
 }
 
-bool DroneCAN::location_valid(void)
-{
-    uint32_t now_ms = millis();
-    uint32_t max_ms = 2000;
-    return last_location_ms != 0 && now_ms - last_location_ms < max_ms;
-}
-
 #if 0
 // xprintf is useful when debugging in C code such as libcanard
 extern "C" {

RemoteIDModule/DroneCAN.h

@@ -21,6 +21,10 @@ public:
     void set_parse_fail(const char *msg) {
         parse_fail = msg;
     }
+
+    uint32_t get_last_location_ms(void) {
+        return last_location_ms;
+    }
     
 private:
     uint32_t last_node_status_ms;
@@ -79,6 +83,4 @@ public:
     const dronecan_remoteid_SelfID &get_self_id(void) { return msg_SelfID; }
     const dronecan_remoteid_System &get_system(void) { return msg_System; }
     const dronecan_remoteid_OperatorID &get_operator_id(void) { return msg_OperatorID; }
-
-    bool location_valid(void);
 };

RemoteIDModule/RemoteIDModule.ino

@@ -47,6 +47,7 @@ static BLE_TX ble;
 
 // OpenDroneID output data structure
 static ODID_UAS_Data UAS_data;
+static uint32_t last_location_ms;
 
 /*
   setup serial ports
@@ -185,6 +186,13 @@ static void set_data_mavlink(MAVLinkSerial &m)
     }
 
     m.set_parse_fail(check_parse());
+
+    uint32_t now_ms = millis();
+    uint32_t location_age_ms = now_ms - m.get_last_location_ms();
+    uint32_t last_location_age_ms = now_ms - last_location_ms;
+    if (location_age_ms < last_location_age_ms) {
+        last_location_ms = m.get_last_location_ms();
+    }
 }
 
 #undef ODID_COPY_STR
@@ -254,6 +262,13 @@ static void set_data_dronecan(void)
     }
 
     dronecan.set_parse_fail(check_parse());
+
+    uint32_t now_ms = millis();
+    uint32_t location_age_ms = now_ms - dronecan.get_last_location_ms();
+    uint32_t last_location_age_ms = now_ms - last_location_ms;
+    if (location_age_ms < last_location_age_ms) {
+        last_location_ms = dronecan.get_last_location_ms();
+    }
 }
 #endif // AP_DRONECAN_ENABLED
 
@@ -274,25 +289,37 @@ void loop()
         return;
     }
 
-    bool data_ok = false;
+    bool have_location = false;
 
 #if AP_MAVLINK_ENABLED
-    const bool mavlink1_ok = mavlink1.location_valid();
-    const bool mavlink2_ok = mavlink2.location_valid();
-    data_ok |= mavlink1_ok || mavlink2_ok;
+    const bool mavlink1_ok = mavlink1.get_last_location_ms() != 0;
+    const bool mavlink2_ok = mavlink2.get_last_location_ms() != 0;
+    have_location |= mavlink1_ok || mavlink2_ok;
 #endif
 
 #if AP_DRONECAN_ENABLED
-    const bool dronecan_ok = dronecan.location_valid();
-    data_ok |= dronecan_ok;
+    const bool dronecan_ok = dronecan.get_last_location_ms() != 0;
+    have_location |= dronecan_ok;
 #endif
 
-#if !AP_BROADCAST_ON_POWER_UP
-    if (!data_ok) {
+#if AP_BROADCAST_ON_POWER_UP
+    // if we are broadcasting on powerup we always mark location valid
+    // so the location with default data is sent
+    if (!UAS_data.LocationValid) {
+        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
+        UAS_data.LocationValid = 1;
+    }
+#else
+    // only broadcast if we have received a location at least once
+    if (!have_location) {
         return;
     }
 #endif
 
+    if (now_ms - last_location_ms > 5000) {
+        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
+    }
+
 #if AP_MAVLINK_ENABLED
     if (mavlink1_ok) {
         set_data_mavlink(mavlink1);

RemoteIDModule/mavlink.cpp

@@ -203,10 +203,3 @@ void MAVLinkSerial::arm_status_send(void)
         status,
         reason);
 }
-
-bool MAVLinkSerial::location_valid(void)
-{
-    uint32_t now_ms = millis();
-    uint32_t max_ms = 2000;
-    return last_location_ms != 0 && now_ms - last_location_ms < max_ms;
-}

RemoteIDModule/mavlink.h

@@ -64,7 +64,9 @@ public:
     uint32_t last_operator_id_ms;
     uint32_t last_system_ms;
 
-    bool location_valid(void);
+    uint32_t get_last_location_ms(void) {
+        return last_location_ms;
+    }
 
     void set_parse_fail(const char *msg) {
         parse_fail = msg;

RemoteIDModule/options.h

@@ -1,5 +1,5 @@
 /*
-  control optional behaviour in the firmware
+  control optional behaviour in the firmware at build time
  */
 
 // enable WiFi NAN support
@@ -17,4 +17,3 @@
 
 // do we support MAVLink connnection to flight controller?
 #define AP_MAVLINK_ENABLED 1
-