From 5ecaa2bd939c8af6e6bb527c57ce917b370d9afb Mon Sep 17 00:00:00 2001 From: BlueMark Innovations BV Date: Sat, 6 Jan 2024 15:36:39 +0100 Subject: [PATCH] store static fields of system ID message in parameters --- RemoteIDModule/DroneCAN.cpp | 38 +++++++++++++++++++++++++++++++ RemoteIDModule/RemoteIDModule.ino | 17 +++++++------- RemoteIDModule/mavlink.cpp | 38 +++++++++++++++++++++++++++++++ RemoteIDModule/parameters.cpp | 8 +++++++ RemoteIDModule/parameters.h | 8 +++++++ 5 files changed, 101 insertions(+), 8 deletions(-) diff --git a/RemoteIDModule/DroneCAN.cpp b/RemoteIDModule/DroneCAN.cpp index 63c82c3..e0b5899 100644 --- a/RemoteIDModule/DroneCAN.cpp +++ b/RemoteIDModule/DroneCAN.cpp @@ -543,6 +543,44 @@ void DroneCAN::handle_System(CanardRxTransfer* transfer) COPY_FIELD(class_eu); COPY_FIELD(operator_altitude_geo); COPY_FIELD(timestamp); + + //save values to parameters if they are different + check range + if (system.class_eu != g.class_eu) { + if (system.class_eu <= 3) { + g.class_eu = system.class_eu; + } + } + if (system.category_eu != g.category_eu) { + if (system.category_eu <= 7) { + g.category_eu = system.category_eu; + } + } + if (system.classification_type != g.classification_type) { + if (system.classification_type <= 1) { + g.classification_type = system.classification_type; + } + } + if (system.operator_location_type != g.operator_location_type) { + if (system.operator_location_type <= 2) { + g.operator_location_type = system.operator_location_type; + } + } + if (system.area_count != g.area_count) { + if (system.area_count <= 65000) { + g.area_count = system.area_count; + } + } + if (system.area_radius != g.area_radius) { + if (system.area_radius <= 250){ + g.area_radius = system.area_radius; + } + } + if (system.area_ceiling != g.area_ceiling) { + g.area_ceiling = system.area_ceiling; + } + if (system.area_floor != g.area_floor) { + g.area_floor = system.area_floor; + } } void DroneCAN::handle_OperatorID(CanardRxTransfer* transfer) diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index fd3bcd7..0127824 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -288,16 +288,17 @@ static void set_data(Transport &t) // System if (system.timestamp != 0) { - UAS_data.System.OperatorLocationType = (ODID_operator_location_type_t)system.operator_location_type; - UAS_data.System.ClassificationType = (ODID_classification_type_t)system.classification_type; + UAS_data.System.OperatorLocationType = (ODID_operator_location_type_t)g.operator_location_type; + UAS_data.System.ClassificationType = (ODID_classification_type_t)g.classification_type; UAS_data.System.OperatorLatitude = system.operator_latitude * 1.0e-7; UAS_data.System.OperatorLongitude = system.operator_longitude * 1.0e-7; - UAS_data.System.AreaCount = system.area_count; - UAS_data.System.AreaRadius = system.area_radius; - UAS_data.System.AreaCeiling = system.area_ceiling; - UAS_data.System.AreaFloor = system.area_floor; - UAS_data.System.CategoryEU = (ODID_category_EU_t)system.category_eu; - UAS_data.System.ClassEU = (ODID_class_EU_t)system.class_eu; + + UAS_data.System.AreaCount = g.area_count; + UAS_data.System.AreaRadius = g.area_radius; + UAS_data.System.AreaCeiling = g.area_ceiling; + UAS_data.System.AreaFloor = g.area_floor; + UAS_data.System.CategoryEU = (ODID_category_EU_t)g.category_eu; + UAS_data.System.ClassEU = (ODID_class_EU_t)g.class_eu; UAS_data.System.OperatorAltitudeGeo = system.operator_altitude_geo; UAS_data.System.Timestamp = system.timestamp; UAS_data.SystemValid = 1; diff --git a/RemoteIDModule/mavlink.cpp b/RemoteIDModule/mavlink.cpp index 88086c0..78681c6 100644 --- a/RemoteIDModule/mavlink.cpp +++ b/RemoteIDModule/mavlink.cpp @@ -192,6 +192,44 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & //only update the timestamp if we receive information with a different timestamp last_system_ms = millis(); last_system_timestamp = system.timestamp; + + //save values to parameters if they are different + check range + if (system.class_eu != g.class_eu) { + if (system.class_eu <= 3) { + g.class_eu = system.class_eu; + } + } + if (system.category_eu != g.category_eu) { + if (system.category_eu <= 7) { + g.category_eu = system.category_eu; + } + } + if (system.classification_type != g.classification_type) { + if (system.classification_type <= 1) { + g.classification_type = system.classification_type; + } + } + if (system.operator_location_type != g.operator_location_type) { + if (system.operator_location_type <= 2) { + g.operator_location_type = system.operator_location_type; + } + } + if (system.area_count != g.area_count) { + if (system.area_count <= 65000) { + g.area_count = system.area_count; + } + } + if (system.area_radius != g.area_radius) { + if (system.area_radius <= 250){ + g.area_radius = system.area_radius; + } + } + if (system.area_ceiling != g.area_ceiling) { + g.area_ceiling = system.area_ceiling; + } + if (system.area_floor != g.area_floor) { + g.area_floor = system.area_floor; + } } break; } diff --git a/RemoteIDModule/parameters.cpp b/RemoteIDModule/parameters.cpp index fbeb3a0..4d50337 100644 --- a/RemoteIDModule/parameters.cpp +++ b/RemoteIDModule/parameters.cpp @@ -18,6 +18,14 @@ const Parameters::Param Parameters::params[] = { { "UAS_TYPE_2", Parameters::ParamType::UINT8, (const void*)&g.ua_type_2, 0, 0, 15 }, { "UAS_ID_TYPE_2", Parameters::ParamType::UINT8, (const void*)&g.id_type_2, 0, 0, 4 }, { "UAS_ID_2", Parameters::ParamType::CHAR20, (const void*)&g.uas_id_2[0], 0, 0, 0 }, + { "EU_CLASS", Parameters::ParamType::UINT8, (const void*)&g.class_eu, 0, 0, 3 }, + { "EU_CATEGORY", Parameters::ParamType::UINT8, (const void*)&g.category_eu, 0, 0, 7 }, + { "CLASSIFICATION", Parameters::ParamType::UINT8, (const void*)&g.classification_type, 0, 0, 1 }, + { "OP_LOC_TYPE", Parameters::ParamType::UINT8, (const void*)&g.operator_location_type, 0, 0, 2 }, + { "AREA_COUNT", Parameters::ParamType::UINT32, (const void*)&g.area_count, 1, 1, 65000 }, //based on F3411-22 range + { "AREA_RADIUS", Parameters::ParamType::UINT8, (const void*)&g.area_radius, 0, 0, 250 }, //based on F3411-22 range + { "AREA_CEILING", Parameters::ParamType::UINT32, (const void*)&g.area_ceiling, 0, 0, 65535 }, //based on F3411-22 range + { "AREA_FLOOR", Parameters::ParamType::UINT32, (const void*)&g.area_floor, 0, 0, 65535 }, //based on F3411-22 range { "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_BCN_RATE", Parameters::ParamType::FLOAT, (const void*)&g.wifi_beacon_rate, 0, 0, 5 }, diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h index b537c97..b850399 100644 --- a/RemoteIDModule/parameters.h +++ b/RemoteIDModule/parameters.h @@ -22,6 +22,14 @@ class Parameters { uint8_t ua_type_2; uint8_t id_type_2; char uas_id_2[21] = "ABCD123456789"; + uint8_t class_eu; + uint8_t category_eu; + uint8_t classification_type; + uint8_t operator_location_type; + uint16_t area_count; + uint8_t area_radius; + uint16_t area_ceiling; + uint16_t area_floor; float wifi_nan_rate; float wifi_beacon_rate; float wifi_power;