From d7c659f11e34c2cf884be9603dff63ee5ef26fd9 Mon Sep 17 00:00:00 2001 From: muramura Date: Thu, 22 Sep 2022 11:39:43 +0900 Subject: [PATCH] SelfID: Make SELF ID messages optional --- RemoteIDModule/DroneCAN.cpp | 20 +++++++++++++++++ RemoteIDModule/RemoteIDModule.ino | 16 ++++++++++--- RemoteIDModule/parameters.cpp | 37 +++++++++++++++++++++++++++++++ RemoteIDModule/parameters.h | 8 +++++++ 4 files changed, 78 insertions(+), 3 deletions(-) diff --git a/RemoteIDModule/DroneCAN.cpp b/RemoteIDModule/DroneCAN.cpp index 4e2ca87..b263e85 100644 --- a/RemoteIDModule/DroneCAN.cpp +++ b/RemoteIDModule/DroneCAN.cpp @@ -614,6 +614,19 @@ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transf } break; } + case Parameters::ParamType::CHAR23: { + if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { + return; + } + char v[24] {}; + strncpy(v, (const char *)&req.value.string_value.data[0], req.value.string_value.len); + if (vp->min_len > 0 && strlen(v) < vp->min_len) { + can_printf("%s too short - min %u", vp->name, vp->min_len); + } else { + vp->set_char23(v); + } + break; + } case Parameters::ParamType::CHAR64: { if (req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) { return; @@ -670,6 +683,13 @@ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transf pkt.value.string_value.len = strlen(s); break; } + case Parameters::ParamType::CHAR23: { + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + const char *s = vp->get_char23(); + strncpy((char*)pkt.value.string_value.data, s, sizeof(pkt.value.string_value.data)); + pkt.value.string_value.len = strlen(s); + break; + } case Parameters::ParamType::CHAR64: { pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; const char *s = vp->get_char64(); diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index 3dcecf7..68276f9 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -183,9 +183,19 @@ static void set_data(Transport &t) UAS_data.OperatorIDValid = 1; // SelfID - UAS_data.SelfID.DescType = (ODID_desctype_t)self_id.description_type; - ODID_COPY_STR(UAS_data.SelfID.Desc, self_id.description); - UAS_data.SelfIDValid = 1; + if (g.have_self_id_info()) { + // from parameters + UAS_data.SelfID.DescType = (ODID_desctype_t)g.description_type; + ODID_COPY_STR(UAS_data.SelfID.Desc, g.description); + UAS_data.SelfIDValid = 1; + } else { + // from transport + if (strlen(self_id.description) > 0) { + UAS_data.SelfID.DescType = (ODID_desctype_t)self_id.description_type; + ODID_COPY_STR(UAS_data.SelfID.Desc, self_id.description); + UAS_data.SelfIDValid = 1; + } + } // System if (system.timestamp != 0) { diff --git a/RemoteIDModule/parameters.cpp b/RemoteIDModule/parameters.cpp index 66bcd3c..05e668e 100644 --- a/RemoteIDModule/parameters.cpp +++ b/RemoteIDModule/parameters.cpp @@ -15,6 +15,8 @@ const Parameters::Param Parameters::params[] = { { "UAS_TYPE", Parameters::ParamType::UINT8, (const void*)&g.ua_type, 0, 0, 15 }, { "UAS_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 }, + { "DESCRIPTION_TYPE", Parameters::ParamType::UINT8, (const void*)&g.description_type, 0, 0, 255 }, + { "DESCRIPTION", Parameters::ParamType::CHAR23, (const void*)&g.description[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 }, { "WIFI_POWER", Parameters::ParamType::FLOAT, (const void*)&g.wifi_power, 20, 2, 20 }, @@ -166,6 +168,16 @@ void Parameters::Param::set_char20(const char *v) const nvs_set_str(handle, name, v); } +void Parameters::Param::set_char23(const char *v) const +{ + if (min_len > 0 && strlen(v) < min_len) { + return; + } + memset((void*)ptr, 0, 24); + strncpy((char *)ptr, v, 23); + nvs_set_str(handle, name, v); +} + void Parameters::Param::set_char64(const char *v) const { if (min_len > 0 && strlen(v) < min_len) { @@ -200,6 +212,12 @@ const char *Parameters::Param::get_char20() const return p; } +const char *Parameters::Param::get_char23() const +{ + const char *p = (const char *)ptr; + return p; +} + const char *Parameters::Param::get_char64() const { const char *p = (const char *)ptr; @@ -291,6 +309,11 @@ void Parameters::init(void) nvs_get_str(handle, p.name, (char *)p.ptr, &len); break; } + case ParamType::CHAR23: { + size_t len = 24; + nvs_get_str(handle, p.name, (char *)p.ptr, &len); + break; + } case ParamType::CHAR64: { size_t len = 65; nvs_get_str(handle, p.name, (char *)p.ptr, &len); @@ -329,6 +352,17 @@ bool Parameters::have_basic_id_info(void) const return strlen(g.uas_id) > 0 && g.id_type > 0 && g.ua_type > 0; } +/** + * check if SelfID info is filled in with parameters + * + * @retval true Has SELF ID information. + * @retval false Does not have SELF ID information. + */ +bool Parameters::have_self_id_info(void) const +{ + return strlen(g.description) > 0; +} + bool Parameters::set_by_name_uint8(const char *name, uint8_t v) { const auto *f = find(name); @@ -368,6 +402,9 @@ bool Parameters::set_by_name_string(const char *name, const char *s) case ParamType::CHAR20: f->set_char20(s); return true; + case ParamType::CHAR23: + f->set_char23(s); + return true; case ParamType::CHAR64: f->set_char64(s); return true; diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h index d8e1cc9..86190a3 100644 --- a/RemoteIDModule/parameters.h +++ b/RemoteIDModule/parameters.h @@ -19,6 +19,10 @@ class Parameters { uint8_t ua_type; uint8_t id_type; char uas_id[21] = "ABCD123456789"; + // + uint8_t description_type; + char description[24]; // For debug = "Pesticides are sprayed"; + // float wifi_nan_rate; float wifi_power; float bt4_rate; @@ -41,6 +45,7 @@ class Parameters { FLOAT=3, CHAR20=4, CHAR64=5, + CHAR23=6, }; struct Param { @@ -56,11 +61,13 @@ class Parameters { void set_uint8(uint8_t v) const; void set_uint32(uint32_t v) const; void set_char20(const char *v) const; + void set_char23(const char *v) const; void set_char64(const char *v) const; uint8_t get_uint8() const; uint32_t get_uint32() const; float get_float() const; const char *get_char20() const; + const char *get_char23() const; const char *get_char64() const; bool get_as_float(float &v) const; void set_as_float(float v) const; @@ -74,6 +81,7 @@ class Parameters { void init(void); bool have_basic_id_info(void) const; + bool have_self_id_info(void) const; bool set_by_name_uint8(const char *name, uint8_t v); bool set_by_name_char64(const char *name, const char *s);