From a302b59baabd443c2c255eebf65f46169c88342d Mon Sep 17 00:00:00 2001 From: Roel Schiphorst Date: Mon, 6 Feb 2023 20:36:25 +0100 Subject: [PATCH] PR 1 PR2 PR2, forgot to include transport.cpp rebase --- RemoteIDModule/RemoteIDModule.ino | 33 +++++++++----- RemoteIDModule/transport.cpp | 71 ++++++++++++++++++++++++------- 2 files changed, 78 insertions(+), 26 deletions(-) diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index 9d22037..e697470 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -49,7 +49,7 @@ static WebInterface webif; #include "soc/rtc_cntl_reg.h" static bool arm_check_ok = false; // goes true for LED arm check status -static bool pfst_check_ok = false; +static bool pfst_check_ok = false; /* setup serial ports @@ -127,36 +127,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; } @@ -281,9 +296,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); diff --git a/RemoteIDModule/transport.cpp b/RemoteIDModule/transport.cpp index 9f98eac..736b3f1 100644 --- a/RemoteIDModule/transport.cpp +++ b/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; @@ -101,7 +141,7 @@ bool Transport::check_signature(uint8_t sig_length, uint8_t data_len, uint32_t s // monocypher signatures are 64 bytes return false; } - + /* loop over all public keys, if one matches then we are OK */ @@ -128,4 +168,3 @@ bool Transport::check_signature(uint8_t sig_length, uint8_t data_len, uint32_t s } return false; } -