Explorar el Código

added ID_TYPE and UA_TYPE and fill in BasicID from parameters

Andrew Tridgell hace 3 años
padre
commit
8df78fd31c

+ 11 - 3
RemoteIDModule/RemoteIDModule.ino

@@ -149,9 +149,17 @@ static void set_data(Transport &t)
     const auto &location = t.get_location();
 
     // BasicID
-    UAS_data.BasicID[0].UAType = (ODID_uatype_t)basic_id.ua_type;
-    UAS_data.BasicID[0].IDType = (ODID_idtype_t)basic_id.id_type;
-    ODID_COPY_STR(UAS_data.BasicID[0].UASID, basic_id.uas_id);
+    if (g.have_basic_id_info()) {
+        // from parameters
+        UAS_data.BasicID[0].UAType = (ODID_uatype_t)g.ua_type;
+        UAS_data.BasicID[0].IDType = (ODID_idtype_t)g.id_type;
+        ODID_COPY_STR(UAS_data.BasicID[0].UASID, g.uas_id);
+    } else {
+        // from transport
+        UAS_data.BasicID[0].UAType = (ODID_uatype_t)basic_id.ua_type;
+        UAS_data.BasicID[0].IDType = (ODID_idtype_t)basic_id.id_type;
+        ODID_COPY_STR(UAS_data.BasicID[0].UASID, basic_id.uas_id);
+    }
     UAS_data.BasicIDValid[0] = 1;
 
     // OperatorID

+ 10 - 0
RemoteIDModule/parameters.cpp

@@ -10,6 +10,8 @@ static nvs_handle handle;
 const Parameters::Param Parameters::params[] = {
     { "LOCK_LEVEL",        Parameters::ParamType::UINT8,  (const void*)&g.lock_level,       0, 0, 2 },
     { "CAN_NODE",          Parameters::ParamType::UINT8,  (const void*)&g.can_node,         0, 0, 127 },
+    { "UA_TYPE",           Parameters::ParamType::UINT8,  (const void*)&g.ua_type,          0, 0, 15 },
+    { "ID_TYPE",           Parameters::ParamType::UINT8,  (const void*)&g.id_type,          0, 0, 4 },
     { "UAS_ID",            Parameters::ParamType::CHAR20, (const void*)&g.uas_id[0],        0, 0, 0 },
     { "BAUDRATE",          Parameters::ParamType::UINT32, (const void*)&g.baudrate,         57600, 9600, 921600 },
     { "WIFI_NAN_RATE",     Parameters::ParamType::FLOAT,  (const void*)&g.wifi_nan_rate,    0, 0, 5 },
@@ -183,3 +185,11 @@ void Parameters::init(void)
 
     }
 }
+
+/*
+  check if BasicID info is filled in with parameters
+ */
+bool Parameters::have_basic_id_info(void) const
+{
+    return strlen(g.uas_id) > 0 && g.id_type > 0 && g.ua_type > 0;
+}

+ 4 - 0
RemoteIDModule/parameters.h

@@ -14,6 +14,8 @@ public:
     uint8_t can_node;
     uint8_t bcast_powerup;
     uint32_t baudrate = 57600;
+    uint8_t ua_type;
+    uint8_t id_type;
     char uas_id[21] = "ABCD123456789";
     float wifi_nan_rate;
     float bt4_rate;
@@ -59,6 +61,8 @@ public:
     static const Param *find_by_index(uint16_t idx);
 
     void init(void);
+
+    bool have_basic_id_info(void) const;
 private:
     void load_defaults(void);
 };

+ 2 - 1
RemoteIDModule/transport.cpp

@@ -3,6 +3,7 @@
  */
 #include <Arduino.h>
 #include "transport.h"
+#include "parameters.h"
 
 const char *Transport::parse_fail = "uninitialised";
 
@@ -36,7 +37,7 @@ uint8_t Transport::arm_status_check(const char *&reason)
 
     if (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms) {
         reason = "missing location message";
-    } else if (last_basic_id_ms == 0 || now_ms - last_basic_id_ms > max_age_other_ms) {
+    } 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";