added validty checks of packets

RemoteIDModule/DroneCAN.cpp

@@ -105,6 +105,8 @@ void DroneCAN::arm_status_send(void)
         reason = "Bad location";
     } else if (msg_System.operator_latitude == 0 && msg_System.operator_longitude == 0) {
         reason = "Bad operator location";
+    } else if (parse_fail != nullptr) {
+        reason = parse_fail;
     } else {
         arm_status.status = DRONECAN_REMOTEID_ARMSTATUS_ODID_ARM_STATUS_GOOD_TO_ARM;
     }

RemoteIDModule/DroneCAN.h

@@ -18,6 +18,10 @@ public:
     void init(void);
     void update(void);
 
+    void set_parse_fail(const char *msg) {
+        parse_fail = msg;
+    }
+    
 private:
     uint32_t last_node_status_ms;
     CANDriver can_driver;
@@ -59,6 +63,8 @@ private:
     uint32_t last_self_id_ms;
     uint32_t last_operator_id_ms;
     uint32_t last_system_ms;
+
+    const char *parse_fail;
     
 public:
     void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);

RemoteIDModule/RemoteIDModule.ino

@@ -60,6 +60,45 @@ void setup()
 #define MIN(x,y) ((x)<(y)?(x):(y))
 #define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, MIN(sizeof(to), sizeof(from)))
 
+/*
+  check parsing of UAS_data, this checks ranges of values to ensure we
+  will produce a valid pack
+ */
+static const char *check_parse(void)
+{
+    {
+        ODID_Location_encoded encoded {};
+        if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
+            return "bad LOCATION data";
+        }
+    }
+    {
+        ODID_System_encoded encoded {};
+        if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
+            return "bad SYSTEM data";
+        }
+    }
+    {
+        ODID_BasicID_encoded encoded {};
+        if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
+            return "bad BASIC_ID data";
+        }
+    }
+    {
+        ODID_SelfID_encoded encoded {};
+        if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
+            return "bad SELF_ID data";
+        }
+    }
+    {
+        ODID_OperatorID_encoded encoded {};
+        if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
+            return "bad OPERATOR_ID data";
+        }
+    }
+    return nullptr;
+}
+
 static void set_data_mavlink(MAVLinkSerial &m)
 {
     const auto &operator_id = m.get_operator_id();
@@ -117,6 +156,8 @@ static void set_data_mavlink(MAVLinkSerial &m)
     UAS_data.Location.TSAccuracy = (ODID_Timestamp_accuracy_t)location.timestamp_accuracy;
     UAS_data.Location.TimeStamp = location.timestamp;
     UAS_data.LocationValid = 1;
+
+    m.set_parse_fail(check_parse());
 }
 
 #undef ODID_COPY_STR
@@ -179,6 +220,8 @@ static void set_data_dronecan(void)
     UAS_data.Location.TSAccuracy = (ODID_Timestamp_accuracy_t)location.timestamp_accuracy;
     UAS_data.Location.TimeStamp = location.timestamp;
     UAS_data.LocationValid = 1;
+
+    dronecan.set_parse_fail(check_parse());
 }
 
 static uint8_t beacon_payload[256];

RemoteIDModule/mavlink.cpp

@@ -198,6 +198,8 @@ void MAVLinkSerial::arm_status_send(void)
         reason = "Bad location";
     } else if (system.operator_latitude == 0 && system.operator_longitude == 0) {
         reason = "Bad operator location";
+    } else if (parse_fail != nullptr) {
+        reason = parse_fail;
     } else {
         status = MAV_ODID_GOOD_TO_ARM;
     }

RemoteIDModule/mavlink.h

@@ -70,6 +70,10 @@ public:
     bool system_valid(void);
     bool location_valid(void);
 
+    void set_parse_fail(const char *msg) {
+        parse_fail = msg;
+    }
+
 private:
     HardwareSerial &serial;
     mavlink_channel_t chan;
@@ -98,4 +102,5 @@ private:
     mavlink_open_drone_id_operator_id_t operator_id;
 
     uint32_t packets_received_mask;
+    const char *parse_fail;
 };