From 4d71ebfbfd502d4fa42c0e9dd6a4163cc8f80196 Mon Sep 17 00:00:00 2001 From: Roel Schiphorst Date: Thu, 2 Feb 2023 22:39:23 +0100 Subject: [PATCH] added region parameter --- RemoteIDModule/RemoteIDModule.ino | 99 ++++++++++++++++++++++++++++++- RemoteIDModule/parameters.cpp | 25 +++++++- RemoteIDModule/parameters.h | 25 ++++++++ RemoteIDModule/transport.cpp | 16 ++--- 4 files changed, 156 insertions(+), 9 deletions(-) diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index 6b3200a..5e32c3d 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -128,6 +128,33 @@ void setup() #define IMIN(x,y) ((x)<(y)?(x):(y)) #define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, IMIN(sizeof(to), sizeof(from))) +/* + check if a UASID is valid as a serial number + */ +static bool valid_UASID_serialnum(const char *uasid) +{ + const auto uasid_len = strnlen(uasid, sizeof(UAS_data.BasicID[0].UASID)); + // 5th char is length field + if (uasid_len < 5 || !isxdigit(uasid[4]) != 0) { + return false; + } + char str[2] {}; + str[0] = uasid[4]; + const uint8_t sn_length = strtol(str, NULL, 16) + 5; + if (sn_length != uasid_len && sn_length > 5) { + return false; + } + //check also for invalid chars + for (unsigned i = 0; i < uasid_len; i++) { + const auto c = uasid[i]; + if (islower(c) || c == 'I' || c == 'O') { + // only capital letters and digits are allowed. Char I and O are not allowed. + return false; + } + } + return true; +} + /* check parsing of UAS_data, this checks ranges of values to ensure we will produce a valid pack @@ -136,6 +163,7 @@ void setup() static const char *check_parse(void) { String ret = ""; + const uint32_t required = g.get_required_features(); { ODID_Location_encoded encoded {}; @@ -175,14 +203,83 @@ static const char *check_parse(void) ret += "OP_ID "; } } + + bool serial_number_configured = false; + bool session_ID_configured = false; + bool CAA_registration_ID_configured = false; //only mandatory in Japan + + // BasicID checks + if (UAS_data.BasicIDValid[0] == 1) { + if (UAS_data.BasicID[0].IDType == ODID_IDTYPE_SERIAL_NUMBER) { + serial_number_configured = true; + } else if (UAS_data.BasicID[0].IDType == ODID_IDTYPE_CAA_REGISTRATION_ID) { + CAA_registration_ID_configured = true; + } else if (UAS_data.BasicID[0].IDType == ODID_IDTYPE_SPECIFIC_SESSION_ID) { + session_ID_configured = true; + } + } + + if (UAS_data.BasicIDValid[1] == 1) { + if (UAS_data.BasicID[1].IDType == ODID_IDTYPE_SERIAL_NUMBER) { + if (!serial_number_configured) { //only one serial number is allowed + serial_number_configured = true; + } else { + ret += "DBL_SER_NUM "; + } + } else if (UAS_data.BasicID[1].IDType == ODID_IDTYPE_CAA_REGISTRATION_ID) { + if (!CAA_registration_ID_configured) { //only one CAA registration ID is allowed + CAA_registration_ID_configured = true; + } else { + ret += "DBL_CAA_ID "; + } + } else if (UAS_data.BasicID[1].IDType== ODID_IDTYPE_SPECIFIC_SESSION_ID) { + if (!session_ID_configured) { //only one session ID is allowed + session_ID_configured = true; + } else { + ret += "DBL_SESS_ID "; + } + } + } + + if (!serial_number_configured && (required & REG_REQUIRE_SERIAL_NUM)) { + ret += "SER_NUM "; + } + + if (!serial_number_configured && !session_ID_configured && (required & REG_REQUIRE_SERIAL_OR_SESSION)) { + ret += "SER_SESS "; + } + + if (!CAA_registration_ID_configured && (required & REG_REQUIRE_REG_ID)) { + ret += "REG_ID "; + } + + if (strlen(UAS_data.OperatorID.OperatorId) <= 0 && (required & REG_REQUIRE_OPERATOR_ID)) { + // basic check if the operator field has data. For the EU legislation an additional check could be implemented. + ret += "OP_ID "; + } + + if (UAS_data.BasicIDValid[0] == 1 && + serial_number_configured && + UAS_data.BasicID[0].IDType == ODID_IDTYPE_SERIAL_NUMBER && + !valid_UASID_serialnum(UAS_data.BasicID[0].UASID)) { + ret += "SER_NUM_VAL "; + } + + if (UAS_data.BasicIDValid[1] == 1 && + UAS_data.BasicID[1].IDType == ODID_IDTYPE_SERIAL_NUMBER && + !valid_UASID_serialnum(UAS_data.BasicID[1].UASID)) { + ret += "SER_NUM_VAL "; + } + if (ret.length() > 0) { // if all errors would occur in this function, it will fit in // 50 chars that is also the max for the arm status message static char return_string[50]; memset(return_string, 0, sizeof(return_string)); - snprintf(return_string, sizeof(return_string-1), "bad %s data", ret.c_str()); + snprintf(return_string, sizeof(return_string)-1, "bad %s data", ret.c_str()); return return_string; } + return nullptr; } diff --git a/RemoteIDModule/parameters.cpp b/RemoteIDModule/parameters.cpp index fbeb3a0..d361aba 100644 --- a/RemoteIDModule/parameters.cpp +++ b/RemoteIDModule/parameters.cpp @@ -38,7 +38,8 @@ const Parameters::Param Parameters::params[] = { { "PUBLIC_KEY5", Parameters::ParamType::CHAR64, (const void*)&g.public_keys[4], }, { "MAVLINK_SYSID", Parameters::ParamType::UINT8, (const void*)&g.mavlink_sysid, 0, 0, 254 }, { "OPTIONS", Parameters::ParamType::UINT8, (const void*)&g.options, 0, 0, 254 }, - { "TO_DEFAULTS", Parameters::ParamType::UINT8, (const void*)&g.to_factory_defaults, 0, 0, 1 }, //if set to 1, reset to factory defaults and make 0. + { "TO_DEFAULTS", Parameters::ParamType::UINT8, (const void*)&g.to_factory_defaults, 0, 0, 1 }, //if set to 1, reset to factory defaults and make 0. + { "REGION", Parameters::ParamType::UINT8, (const void*)&g.region, 0, 0, 3 }, // 0 = world, 1 = USA, 2 = Japan, 3 = EU { "DONE_INIT", Parameters::ParamType::UINT8, (const void*)&g.done_init, 0, 0, 0, PARAM_FLAG_HIDDEN}, { "", Parameters::ParamType::NONE, nullptr, }, }; @@ -496,3 +497,25 @@ bool Parameters::remove_public_key(uint8_t i) name[strlen(name)-2] = '1'+i; return set_by_name_char64(name, ""); } + +/* + get required features based on region + There are 4 regions: 0 = world, 1 = USA, 2 = Japan, 3 = EU + for now region world is treated the same as the USA + checks are based on this table: https://github.com/opendroneid/opendroneid-core-c#comparison + and requirements on the serial number (ANSI/CTA-2063-A) +*/ +uint32_t Parameters::get_required_features(void) const +{ + switch (region) { + case Region::EU: + return REG_REQUIRE_LOC | REG_REQUIRE_BASIC_ID | REG_REQUIRE_OPERATOR_ID | REG_REQUIRE_OPERATOR_LOC | REG_REQUIRE_OPERATOR_ID | REG_REQUIRE_SERIAL_NUM; + case Region::JAPAN: + return REG_REQUIRE_LOC | REG_REQUIRE_BASIC_ID | REG_REQUIRE_OPERATOR_ID | REG_REQUIRE_OPERATOR_LOC | REG_REQUIRE_SERIAL_NUM; + case Region::WORLD: + case Region::USA: + default: + return REG_REQUIRE_LOC | REG_REQUIRE_BASIC_ID | REG_REQUIRE_OPERATOR_ID | REG_REQUIRE_SYSTEM | REG_REQUIRE_OPERATOR_LOC | REG_REQUIRE_SERIAL_OR_SESSION; + } +} + diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h index 764c8f9..6be941c 100644 --- a/RemoteIDModule/parameters.h +++ b/RemoteIDModule/parameters.h @@ -10,6 +10,26 @@ #define PARAM_FLAG_PASSWORD (1U<<0) #define PARAM_FLAG_HIDDEN (1U<<1) +enum class Region : uint8_t { + WORLD=0, + USA=1, + JAPAN=2, + EU=3, +}; + +/* + bits for which fields are required. Based on the region + */ +#define REG_REQUIRE_LOC (1U<<0) +#define REG_REQUIRE_BASIC_ID (1U<<1) +#define REG_REQUIRE_SELF_ID (1U<<2) +#define REG_REQUIRE_OPERATOR_ID (1U<<3) +#define REG_REQUIRE_SYSTEM (1U<<4) +#define REG_REQUIRE_OPERATOR_LOC (1U<<5) +#define REG_REQUIRE_SERIAL_NUM (1U<<6) +#define REG_REQUIRE_SERIAL_OR_SESSION (1U<<7) +#define REG_REQUIRE_REG_ID (1U<<8) + class Parameters { public: int8_t lock_level; @@ -37,6 +57,8 @@ class Parameters { uint8_t wifi_channel = 6; uint8_t to_factory_defaults = 0; uint8_t options; + Region region; + struct { char b64_key[64]; } public_keys[MAX_PUBLIC_KEYS]; @@ -102,6 +124,9 @@ class Parameters { static uint16_t param_count_float(void); static int16_t param_index_float(const Param *p); + // get the REG_REQUIRE features for the region + uint32_t get_required_features(void) const; + private: void load_defaults(void); }; diff --git a/RemoteIDModule/transport.cpp b/RemoteIDModule/transport.cpp index 98cac62..be7b8e4 100644 --- a/RemoteIDModule/transport.cpp +++ b/RemoteIDModule/transport.cpp @@ -6,6 +6,7 @@ #include "parameters.h" #include "util.h" #include "monocypher.h" +#include const char *Transport::parse_fail = "uninitialised"; @@ -36,6 +37,7 @@ uint8_t Transport::arm_status_check(const char *&reason) const uint32_t max_age_location_ms = 3000; const uint32_t max_age_other_ms = 22000; const uint32_t now_ms = millis(); + const uint32_t required = g.get_required_features(); uint8_t status = MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC; @@ -47,34 +49,34 @@ uint8_t Transport::arm_status_check(const char *&reason) String ret = ""; - if (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms) { + if ((required & REG_REQUIRE_LOC) && (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms)) { ret += "LOC "; } - if (!g.have_basic_id_info()) { + if ((required & REG_REQUIRE_BASIC_ID) && !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 ret += "ID "; } - if (last_self_id_ms == 0 || now_ms - last_self_id_ms > max_age_other_ms) { + if ((required & REG_REQUIRE_SELF_ID) && (last_self_id_ms == 0 || now_ms - last_self_id_ms > max_age_other_ms)) { ret += "SELF_ID "; } - if (last_operator_id_ms == 0 || now_ms - last_operator_id_ms > max_age_other_ms) { + if ((required & REG_REQUIRE_OPERATOR_ID) && (last_operator_id_ms == 0 || now_ms - last_operator_id_ms > max_age_other_ms)) { ret += "OP_ID "; } - if (last_system_ms == 0 || now_ms - last_system_ms > max_age_location_ms) { + if ((required & REG_REQUIRE_SYSTEM) && (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 ret += "SYS "; } - if (location.latitude == 0 && location.longitude == 0) { + if ((required & REG_REQUIRE_LOC) && (location.latitude == 0 && location.longitude == 0)) { ret += "LOC "; } - if (system.operator_latitude == 0 && system.operator_longitude == 0) { + if ((required & REG_REQUIRE_OPERATOR_LOC) && (system.operator_latitude == 0 && system.operator_longitude == 0)) { ret += "OP_LOC "; }