From 78115fbf6bb4dd20cbee99a663e2aa89e663e7e5 Mon Sep 17 00:00:00 2001 From: Roel Schiphorst Date: Mon, 6 Feb 2023 20:36:25 +0100 Subject: [PATCH 1/2] give longer description of pre-arm failure reasons --- RemoteIDModule/RemoteIDModule.ino | 31 ++++++++++---- RemoteIDModule/transport.cpp | 69 ++++++++++++++++++++++++------- 2 files changed, 76 insertions(+), 24 deletions(-) diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index abb688f..97531fd 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/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); diff --git a/RemoteIDModule/transport.cpp b/RemoteIDModule/transport.cpp index d3e48cf..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; @@ -128,4 +168,3 @@ bool Transport::check_signature(uint8_t sig_length, uint8_t data_len, uint32_t s } return false; } - From ca64a22e926b9cf2e51d3134b4f2445ddc2f1974 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 15 Oct 2023 08:51:41 +1100 Subject: [PATCH 2/2] cleanup string handling --- RemoteIDModule/RemoteIDModule.ino | 24 ++++++++++------- RemoteIDModule/transport.cpp | 45 +++++++++++-------------------- 2 files changed, 29 insertions(+), 40 deletions(-) diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index 97531fd..6b3200a 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -131,52 +131,56 @@ void setup() /* check parsing of UAS_data, this checks ranges of values to ensure we will produce a valid pack + returns nullptr on no error, or a string error */ 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 "); + String ret = ""; { ODID_Location_encoded encoded {}; if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) { - strcat(return_string, "LOC "); + ret += "LOC "; } } { ODID_System_encoded encoded {}; if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) { - strcat(return_string, "SYS "); + ret += "SYS "; } } { ODID_BasicID_encoded encoded {}; if (UAS_data.BasicIDValid[0] == 1) { if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) { - strcat(return_string, "ID_1 "); + ret += "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 "); + ret += "ID_2 "; } } } { ODID_SelfID_encoded encoded {}; if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) { - strcat(return_string, "SELF_ID "); + ret += "SELF_ID "; } } { ODID_OperatorID_encoded encoded {}; if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) { - strcat(return_string, "OP_ID "); + ret += "OP_ID "; } } - if (strlen(return_string) > 4) { //only return error messag if one or more encoding functions failed - strcat(return_string, "data "); + 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()); return return_string; } return nullptr; diff --git a/RemoteIDModule/transport.cpp b/RemoteIDModule/transport.cpp index 736b3f1..98cac62 100644 --- a/RemoteIDModule/transport.cpp +++ b/RemoteIDModule/transport.cpp @@ -45,64 +45,49 @@ 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); + String ret = ""; if (last_location_ms == 0 || now_ms - last_location_ms > max_age_location_ms) { - strcat(return_string, "LOC "); + ret += "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 "); + ret += "ID "; } if (last_self_id_ms == 0 || now_ms - last_self_id_ms > max_age_other_ms) { - strcat(return_string, "SELF_ID "); + ret += "SELF_ID "; } if (last_operator_id_ms == 0 || now_ms - last_operator_id_ms > max_age_other_ms) { - strcat(return_string, "OP_ID "); + ret += "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 - strcat(return_string, "SYS "); + ret += "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 "); - } + ret += "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 "); - } + ret += "OP_LOC "; } - if (return_string == nullptr && reason == nullptr) { + if (ret.length() == 0 && reason == nullptr) { status = MAV_ODID_ARM_STATUS_GOOD_TO_ARM; } else { - if (reason!=nullptr) { - strncpy(return_string_full, reason, 49); + static char return_string[200]; + memset(return_string, 0, sizeof(return_string)); + if (reason != nullptr) { + strlcpy(return_string, reason, sizeof(return_string)); } - strncat(return_string_full, return_string, 49 - strlen(return_string_full)); - reason = return_string_full; + strlcat(return_string, ret.c_str(), sizeof(return_string)); + reason = return_string; } return status;