From 5e151ef96288e9cfc72249871edeb9304434bfbc Mon Sep 17 00:00:00 2001 From: Cybra Date: Thu, 11 Jul 2024 15:09:37 +0700 Subject: [PATCH 1/2] Modify changes --- AP_Periph/bcast_remoteid.cpp | 61 +++++++++++++++++++----------------- AP_Periph/bcast_remoteid.h | 4 +-- 2 files changed, 35 insertions(+), 30 deletions(-) diff --git a/AP_Periph/bcast_remoteid.cpp b/AP_Periph/bcast_remoteid.cpp index 3a3476f..b29bc32 100644 --- a/AP_Periph/bcast_remoteid.cpp +++ b/AP_Periph/bcast_remoteid.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -37,9 +38,9 @@ const AP_Param::GroupInfo Bcast_RemoteID::var_info[] = { // @Param: ENABLE // @DisplayName: Enable Bcast ODID subsystem - // @Description: Enable ODID subsystem + // @Description: Enable Bcast ODID subsystem // @Values: 0:Disabled,1:Enabled - AP_GROUPINFO_FLAGS("ENABLE", 1, Bcast_RemoteID, _enable, 0, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("_ENABLE", 1, Bcast_RemoteID, _enabled, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPEND }; @@ -64,9 +65,14 @@ Bcast_RemoteID::Bcast_RemoteID() void Bcast_RemoteID::init() { - if (_enable == 0) { + if (!_enabled) { return; } + float value; + AP_Param::get("GPS_RAW_DATA", value); + if (value != 0.0 && value != 1.0) { + AP_Param::set_and_save_by_name("GPS_RAW_DATA", 1); + } load_UAS_ID_from_persistent_memory(); _initialised = true; @@ -121,6 +127,7 @@ void Bcast_RemoteID::get_persistent_params(ExpandingString &str) const // str.printf("DID_UAS_ID=%s\nDID_UAS_ID_TYPE=%u\nDID_UA_TYPE=%u\n", pkt_basic_id.uas_id, pkt_basic_id.id_type, pkt_basic_id.ua_type); // } // TODO: implement this + } // Perform the pre-arm checks and prevent arming if they are not satisifed @@ -142,10 +149,10 @@ bool Bcast_RemoteID::pre_arm_check(char* failmsg, uint8_t failmsg_len) const uint32_t max_age_ms = 3000; const uint32_t now_ms = AP_HAL::millis(); - if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) { - strncpy(failmsg, "ARM_STATUS not available", failmsg_len); - return false; - } + // if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) { + // strncpy(failmsg, "ARM_STATUS not available", failmsg_len); + // return false; + // } if (last_system_ms == 0 || (now_ms - last_system_ms > max_age_ms && @@ -164,7 +171,7 @@ bool Bcast_RemoteID::pre_arm_check(char* failmsg, uint8_t failmsg_len) void Bcast_RemoteID::update() { - if (_enable == 0) { + if (_enabled == 0) { return; } @@ -338,8 +345,6 @@ void Bcast_RemoteID::send_location_message() // accuracy, as we use system timer to propogate time timestamp_accuracy_mav = create_enum_timestamp_accuracy(1.0); - - // Timestamp here is the number of seconds after into the current hour referenced to UTC time (up to one hour) // FIX we need to only set this if w have a GPS lock is 2D good enough for that? @@ -355,25 +360,25 @@ void Bcast_RemoteID::send_location_message() WITH_SEMAPHORE(_sem); // take semaphore so CAN gets a consistent packet pkt_location = mavlink_open_drone_id_location_t{ - latitude : latitude, - longitude : longitude, - altitude_barometric : ODID_INV_ALT, - altitude_geodetic : altitude_geodetic, - height : 0, - timestamp : timestamp, - direction : uint16_t(direction * 100.0), // Heading (centi-degrees) - speed_horizontal : uint16_t(speed_horizontal * 100.0), // Ground speed (cm/s) - speed_vertical : int16_t(climb_rate * 100.0), // Climb rate (cm/s) - target_system : 0, - target_component : 0, + latitude : latitude, // Required + longitude : longitude, // Required + altitude_barometric : ODID_INV_ALT, // Optional (not supported) + altitude_geodetic : altitude_geodetic, // Required + height : 0, // Optional + timestamp : timestamp, // Required + direction : uint16_t(direction * 100.0), // Required, Heading (centi-degrees) + speed_horizontal : uint16_t(speed_horizontal * 100.0), // Required, Ground speed (cm/s) + speed_vertical : int16_t(climb_rate * 100.0), // Required, Climb rate (cm/s) + target_system : 0, // Not used + target_component : 0, // Not used id_or_mac : {}, - status : uint8_t(uav_status), - height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // height reference enum: Above takeoff location or above ground - horizontal_accuracy : uint8_t(horizontal_accuracy_mav), - vertical_accuracy : uint8_t(vertical_accuracy_mav), - barometer_accuracy : MAV_ODID_VER_ACC_UNKNOWN, - speed_accuracy : uint8_t(speed_accuracy_mav), - timestamp_accuracy : uint8_t(timestamp_accuracy_mav) + status : uint8_t(uav_status), // Optional, 0 for not declared, 4 for emergency + height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // Optional, height reference enum: Above takeoff location or above ground + horizontal_accuracy : uint8_t(horizontal_accuracy_mav), // Required + vertical_accuracy : uint8_t(vertical_accuracy_mav), // Required + barometer_accuracy : MAV_ODID_VER_ACC_UNKNOWN, // Optional (not supported) + speed_accuracy : uint8_t(speed_accuracy_mav), // Required + timestamp_accuracy : uint8_t(timestamp_accuracy_mav) // Required }; } diff --git a/AP_Periph/bcast_remoteid.h b/AP_Periph/bcast_remoteid.h index aaca2df..ed697d2 100644 --- a/AP_Periph/bcast_remoteid.h +++ b/AP_Periph/bcast_remoteid.h @@ -72,7 +72,7 @@ class Bcast_RemoteID { void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg); bool enabled(void) const { - return _enable != 0; + return _enabled != 0; } void set_arm_status(mavlink_open_drone_id_arm_status_t &status); @@ -86,7 +86,7 @@ class Bcast_RemoteID { private: bool _initialised; // parameters - AP_Int8 _enable; + AP_Int8 _enabled; char ua_type[3]; char id_type[3]; From aecf516d13c0f5ad9b4e8f14061bc70917892508 Mon Sep 17 00:00:00 2001 From: Cybra Date: Thu, 5 Sep 2024 14:11:43 +0800 Subject: [PATCH 2/2] add Bcast Param --- AP_Periph/Parameters.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/AP_Periph/Parameters.cpp b/AP_Periph/Parameters.cpp index d233d88..ad21215 100644 --- a/AP_Periph/Parameters.cpp +++ b/AP_Periph/Parameters.cpp @@ -47,6 +47,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @User: Advanced GSCALAR(format_version, "FORMAT_VERSION", 0), + // @PARAM: BCAST_REMOTEID + // @DisplayName: Broadcast RemoteID + // @Description: Broadcast RemoteID + // @Range: 0 1 + // @User: Advanced + GSCALAR(bcast_remoteid, "BCAST_REMOTEID", 0), + // @Param: CAN_NODE // @DisplayName: UAVCAN node that is used for this network // @Description: UAVCAN node should be set implicitly or 0 for dynamic node allocation