Selaa lähdekoodia

give longer description of pre-arm failure reasons

Roel Schiphorst 3 vuotta sitten
vanhempi
sitoutus
1f02c9cafd
2 muutettua tiedostoa jossa 76 lisäystä ja 24 poistoa
  1. 22 9
      RemoteIDModule/RemoteIDModule.ino
  2. 54 15
      RemoteIDModule/transport.cpp

+ 22 - 9
RemoteIDModule/RemoteIDModule.ino

@@ -134,36 +134,51 @@ void setup()
  */
 static const char *check_parse(void)
 {
+    static char return_string[50]; //if all errors would occur in this function, it will fit in 50 chars that is also the max for the arm status message
+    strcpy (return_string, "bad ");
+
     {
         ODID_Location_encoded encoded {};
         if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
-            return "bad LOCATION data";
+            strcat(return_string, "LOC ");
         }
     }
     {
         ODID_System_encoded encoded {};
         if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
-            return "bad SYSTEM data";
+            strcat(return_string, "SYS ");
         }
     }
     {
         ODID_BasicID_encoded encoded {};
-        if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
-            return "bad BASIC_ID data";
+        if (UAS_data.BasicIDValid[0] == 1) {
+            if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
+                strcat(return_string, "ID_1 ");
+            }
+        }
+        memset(&encoded, 0, sizeof(encoded));
+        if (UAS_data.BasicIDValid[1] == 1) {
+            if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[1]) != ODID_SUCCESS) {
+                strcat(return_string, "ID_2 ");
+            }
         }
     }
     {
         ODID_SelfID_encoded encoded {};
         if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
-            return "bad SELF_ID data";
+            strcat(return_string, "SELF_ID ");
         }
     }
     {
         ODID_OperatorID_encoded encoded {};
         if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
-            return "bad OPERATOR_ID data";
+            strcat(return_string, "OP_ID ");
         }
     }
+    if (strlen(return_string) > 4) { //only return  error messag if one or more encoding functions failed
+        strcat(return_string, "data ");
+        return return_string;
+    }
     return nullptr;
 }
 
@@ -306,9 +321,7 @@ static void set_data(Transport &t)
     }
 
     const char *reason = check_parse();
-    if (reason == nullptr) {
-        t.arm_status_check(reason);
-    }
+    t.arm_status_check(reason);
     t.set_parse_fail(reason);
 
     arm_check_ok = (reason==nullptr);

+ 54 - 15
RemoteIDModule/transport.cpp

@@ -45,24 +45,64 @@ uint8_t Transport::arm_status_check(const char *&reason)
         return status;
     }
 
+    char return_string[200]; //make it bigger to accomodate all possible errors
+    memset(return_string, 0, 200);
+    strcpy (return_string, "missing ");
+
+    static char return_string_full[50];
+    memset(return_string_full, 0, 50);
+
     if (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms) {
-        reason = "missing location message";
-    } else if (!g.have_basic_id_info() && (last_basic_id_ms == 0 || now_ms - last_basic_id_ms > max_age_other_ms)) {
-        reason = "missing basic_id message";
-    } else if (last_self_id_ms == 0  || now_ms - last_self_id_ms > max_age_other_ms) {
-        reason = "missing self_id message";
-    } else if (last_operator_id_ms == 0 || now_ms - last_operator_id_ms > max_age_other_ms) {
-        reason = "missing operator_id message";
-    } else if (last_system_ms == 0 || now_ms - last_system_ms > max_age_location_ms) {
+        strcat(return_string, "LOC ");
+    }
+    if (!g.have_basic_id_info()) {
+        // if there is no basic ID data stored in the parameters give warning. If basic ID data are streamed to RID device,
+        // it will store them in the parameters
+        strcat(return_string, "ID ");
+    }
+
+    if (last_self_id_ms == 0  || now_ms - last_self_id_ms > max_age_other_ms) {
+        strcat(return_string, "SELF_ID ");
+    }
+
+    if (last_operator_id_ms == 0 || now_ms - last_operator_id_ms > max_age_other_ms) {
+        strcat(return_string, "OP_ID ");
+    }
+
+    if (last_system_ms == 0 || now_ms - last_system_ms > max_age_location_ms) {
         // we use location age limit for system as the operator location needs to come in as fast
         // as the vehicle location for FAA standard
-        reason = "missing system message";
-    } else if (location.latitude == 0 && location.longitude == 0) {
-        reason = "Bad location";
-    } else if (system.operator_latitude == 0 && system.operator_longitude == 0) {
-        reason = "Bad operator location";
-    } else if (reason == nullptr) {
+        strcat(return_string, "SYS ");
+    }
+
+    if (location.latitude == 0 && location.longitude == 0) {
+        if (strcmp(return_string ,"missing ") == 0) {
+            //if the return string only contains the word missing, there is no error.
+            strcpy(return_string, "zero LOC ");
+        }
+        else {
+             strcat(return_string, "zero LOC ");
+        }
+    }
+
+    if (system.operator_latitude == 0 && system.operator_longitude == 0) {
+        if (strcmp(return_string ,"missing ") == 0) {
+            //if the return string only contains the word missing, there is no error.
+            strcpy(return_string, "zero OP_LOC ");
+        }
+        else {
+             strcat(return_string, "zero OP_LOC ");
+        }
+    }
+
+    if (return_string == nullptr && reason == nullptr) {
         status = MAV_ODID_ARM_STATUS_GOOD_TO_ARM;
+    } else {
+        if (reason!=nullptr) {
+            strncpy(return_string_full, reason, 49);
+        }
+        strncat(return_string_full, return_string, 49 - strlen(return_string_full));
+        reason = return_string_full;
     }
 
     return status;
@@ -128,4 +168,3 @@ bool Transport::check_signature(uint8_t sig_length, uint8_t data_len, uint32_t s
     }
     return false;
 }
-