z8359531l 14 часов назад
Родитель
Сommit
e13a502c05

+ 105 - 0
uav_can/user_inc/dronecan.remoteid.ArmStatus.h

@@ -0,0 +1,105 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE 52
+#define DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE (0xFEDF72CCF06F3BDDULL)
+#define DRONECAN_REMOTEID_ARMSTATUS_ID 20035
+
+#define DRONECAN_REMOTEID_ARMSTATUS_ODID_ARM_STATUS_GOOD_TO_ARM 0
+#define DRONECAN_REMOTEID_ARMSTATUS_ODID_ARM_STATUS_FAIL_GENERIC 1
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_ArmStatus_cxx_iface;
+#endif
+
+struct dronecan_remoteid_ArmStatus {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_ArmStatus_cxx_iface;
+#endif
+    uint8_t status;
+    struct { uint8_t len; uint8_t data[50]; }error;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_ArmStatus_encode(struct dronecan_remoteid_ArmStatus* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_ArmStatus_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_ArmStatus* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_ArmStatus_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_ArmStatus* msg, bool tao);
+static inline bool _dronecan_remoteid_ArmStatus_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_ArmStatus* msg, bool tao);
+void _dronecan_remoteid_ArmStatus_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_ArmStatus* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->status);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t error_len = msg->error.len > 50 ? 50 : msg->error.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 6, &error_len);
+        *bit_ofs += 6;
+    }
+    for (size_t i=0; i < error_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->error.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_ArmStatus, return true on failure, false on success
+*/
+bool _dronecan_remoteid_ArmStatus_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_ArmStatus* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->status);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 6, false, &msg->error.len);
+        *bit_ofs += 6;
+    } else {
+        msg->error.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->error.len > 50) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->error.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->error.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_ArmStatus sample_dronecan_remoteid_ArmStatus_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_ArmStatus, DRONECAN_REMOTEID_ARMSTATUS_ID, DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE, DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE);
+#endif
+#endif

+ 154 - 0
uav_can/user_inc/dronecan.remoteid.BasicID.h

@@ -0,0 +1,154 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_BASICID_MAX_SIZE 44
+#define DRONECAN_REMOTEID_BASICID_SIGNATURE (0x5B1C624A8E4FC533ULL)
+#define DRONECAN_REMOTEID_BASICID_ID 20030
+
+#define DRONECAN_REMOTEID_BASICID_ODID_ID_TYPE_NONE 0
+#define DRONECAN_REMOTEID_BASICID_ODID_ID_TYPE_SERIAL_NUMBER 1
+#define DRONECAN_REMOTEID_BASICID_ODID_ID_TYPE_CAA_REGISTRATION_ID 2
+#define DRONECAN_REMOTEID_BASICID_ODID_ID_TYPE_UTM_ASSIGNED_UUID 3
+#define DRONECAN_REMOTEID_BASICID_ODID_ID_TYPE_SPECIFIC_SESSION_ID 4
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_NONE 0
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_AEROPLANE 1
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR 2
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_GYROPLANE 3
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_HYBRID_LIFT 4
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_ORNITHOPTER 5
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_GLIDER 6
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_KITE 7
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_FREE_BALLOON 8
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_CAPTIVE_BALLOON 9
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_AIRSHIP 10
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_FREE_FALL_PARACHUTE 11
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_ROCKET 12
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_TETHERED_POWERED_AIRCRAFT 13
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_GROUND_OBSTACLE 14
+#define DRONECAN_REMOTEID_BASICID_ODID_UA_TYPE_OTHER 15
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_BasicID_cxx_iface;
+#endif
+
+struct dronecan_remoteid_BasicID {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_BasicID_cxx_iface;
+#endif
+    struct { uint8_t len; uint8_t data[20]; }id_or_mac;
+    uint8_t id_type;
+    uint8_t ua_type;
+    struct { uint8_t len; uint8_t data[20]; }uas_id;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_BasicID_encode(struct dronecan_remoteid_BasicID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_BasicID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_BasicID* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_BasicID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_BasicID* msg, bool tao);
+static inline bool _dronecan_remoteid_BasicID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_BasicID* msg, bool tao);
+void _dronecan_remoteid_BasicID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_BasicID* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t id_or_mac_len = msg->id_or_mac.len > 20 ? 20 : msg->id_or_mac.len;
+#pragma GCC diagnostic pop
+    canardEncodeScalar(buffer, *bit_ofs, 5, &id_or_mac_len);
+    *bit_ofs += 5;
+    for (size_t i=0; i < id_or_mac_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_type);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->ua_type);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t uas_id_len = msg->uas_id.len > 20 ? 20 : msg->uas_id.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 5, &uas_id_len);
+        *bit_ofs += 5;
+    }
+    for (size_t i=0; i < uas_id_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->uas_id.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_BasicID, return true on failure, false on success
+*/
+bool _dronecan_remoteid_BasicID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_BasicID* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->id_or_mac.len);
+    *bit_ofs += 5;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->id_or_mac.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->id_or_mac.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_type);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->ua_type);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->uas_id.len);
+        *bit_ofs += 5;
+    } else {
+        msg->uas_id.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->uas_id.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->uas_id.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->uas_id.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_BasicID sample_dronecan_remoteid_BasicID_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_BasicID, DRONECAN_REMOTEID_BASICID_ID, DRONECAN_REMOTEID_BASICID_SIGNATURE, DRONECAN_REMOTEID_BASICID_MAX_SIZE);
+#endif
+#endif

+ 232 - 0
uav_can/user_inc/dronecan.remoteid.Location.h

@@ -0,0 +1,232 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_LOCATION_MAX_SIZE 58
+#define DRONECAN_REMOTEID_LOCATION_SIGNATURE (0xEAA3A2C5BCB14CAAULL)
+#define DRONECAN_REMOTEID_LOCATION_ID 20031
+
+#define DRONECAN_REMOTEID_LOCATION_ODID_STATUS_UNDECLARED 0
+#define DRONECAN_REMOTEID_LOCATION_ODID_STATUS_GROUND 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_STATUS_AIRBORNE 2
+#define DRONECAN_REMOTEID_LOCATION_ODID_STATUS_EMERGENCY 3
+#define DRONECAN_REMOTEID_LOCATION_ODID_HEIGHT_REF_OVER_TAKEOFF 0
+#define DRONECAN_REMOTEID_LOCATION_ODID_HEIGHT_REF_OVER_GROUND 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_UNKNOWN 0
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_10NM 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_4NM 2
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_2NM 3
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_1NM 4
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_0_5NM 5
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_0_3NM 6
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_0_1NM 7
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_0_05NM 8
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_30_METER 9
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_10_METER 10
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_3_METER 11
+#define DRONECAN_REMOTEID_LOCATION_ODID_HOR_ACC_1_METER 12
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_UNKNOWN 0
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_150_METER 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_45_METER 2
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_25_METER 3
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_10_METER 4
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_3_METER 5
+#define DRONECAN_REMOTEID_LOCATION_ODID_VER_ACC_1_METER 6
+#define DRONECAN_REMOTEID_LOCATION_ODID_SPEED_ACC_UNKNOWN 0
+#define DRONECAN_REMOTEID_LOCATION_ODID_SPEED_ACC_10_METERS_PER_SECOND 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_SPEED_ACC_3_METERS_PER_SECOND 2
+#define DRONECAN_REMOTEID_LOCATION_ODID_SPEED_ACC_1_METERS_PER_SECOND 3
+#define DRONECAN_REMOTEID_LOCATION_ODID_SPEED_ACC_0_3_METERS_PER_SECOND 4
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_1_SECOND 1
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_2_SECOND 2
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_3_SECOND 3
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_4_SECOND 4
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_5_SECOND 5
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_6_SECOND 6
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_7_SECOND 7
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_8_SECOND 8
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_0_9_SECOND 9
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_0_SECOND 10
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_1_SECOND 11
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_2_SECOND 12
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_3_SECOND 13
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_4_SECOND 14
+#define DRONECAN_REMOTEID_LOCATION_ODID_TIME_ACC_1_5_SECOND 15
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_Location_cxx_iface;
+#endif
+
+struct dronecan_remoteid_Location {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_Location_cxx_iface;
+#endif
+    struct { uint8_t len; uint8_t data[20]; }id_or_mac;
+    uint8_t status;
+    uint16_t direction;
+    uint16_t speed_horizontal;
+    int16_t speed_vertical;
+    int32_t latitude;
+    int32_t longitude;
+    float altitude_barometric;
+    float altitude_geodetic;
+    uint8_t height_reference;
+    float height;
+    uint8_t horizontal_accuracy;
+    uint8_t vertical_accuracy;
+    uint8_t barometer_accuracy;
+    uint8_t speed_accuracy;
+    float timestamp;
+    uint8_t timestamp_accuracy;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_Location_encode(struct dronecan_remoteid_Location* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_Location_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_Location* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_Location_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_Location* msg, bool tao);
+static inline bool _dronecan_remoteid_Location_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_Location* msg, bool tao);
+void _dronecan_remoteid_Location_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_Location* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t id_or_mac_len = msg->id_or_mac.len > 20 ? 20 : msg->id_or_mac.len;
+#pragma GCC diagnostic pop
+    canardEncodeScalar(buffer, *bit_ofs, 5, &id_or_mac_len);
+    *bit_ofs += 5;
+    for (size_t i=0; i < id_or_mac_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->status);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->direction);
+    *bit_ofs += 16;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->speed_horizontal);
+    *bit_ofs += 16;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->speed_vertical);
+    *bit_ofs += 16;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->latitude);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->longitude);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->altitude_barometric);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->altitude_geodetic);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->height_reference);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->height);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->horizontal_accuracy);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->vertical_accuracy);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->barometer_accuracy);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->speed_accuracy);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->timestamp);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->timestamp_accuracy);
+    *bit_ofs += 8;
+}
+
+/*
+ decode dronecan_remoteid_Location, return true on failure, false on success
+*/
+bool _dronecan_remoteid_Location_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_Location* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->id_or_mac.len);
+    *bit_ofs += 5;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->id_or_mac.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->id_or_mac.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->status);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, false, &msg->direction);
+    *bit_ofs += 16;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, false, &msg->speed_horizontal);
+    *bit_ofs += 16;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, true, &msg->speed_vertical);
+    *bit_ofs += 16;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->latitude);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->longitude);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->altitude_barometric);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->altitude_geodetic);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->height_reference);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->height);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->horizontal_accuracy);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->vertical_accuracy);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->barometer_accuracy);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->speed_accuracy);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->timestamp);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->timestamp_accuracy);
+    *bit_ofs += 8;
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_Location sample_dronecan_remoteid_Location_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_Location, DRONECAN_REMOTEID_LOCATION_ID, DRONECAN_REMOTEID_LOCATION_SIGNATURE, DRONECAN_REMOTEID_LOCATION_MAX_SIZE);
+#endif
+#endif

+ 128 - 0
uav_can/user_inc/dronecan.remoteid.OperatorID.h

@@ -0,0 +1,128 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_OPERATORID_MAX_SIZE 43
+#define DRONECAN_REMOTEID_OPERATORID_SIGNATURE (0x581E7FC7F03AF935ULL)
+#define DRONECAN_REMOTEID_OPERATORID_ID 20034
+
+#define DRONECAN_REMOTEID_OPERATORID_ODID_OPERATOR_ID_TYPE_CAA 0
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_OperatorID_cxx_iface;
+#endif
+
+struct dronecan_remoteid_OperatorID {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_OperatorID_cxx_iface;
+#endif
+    struct { uint8_t len; uint8_t data[20]; }id_or_mac;
+    uint8_t operator_id_type;
+    struct { uint8_t len; uint8_t data[20]; }operator_id;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_OperatorID_encode(struct dronecan_remoteid_OperatorID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_OperatorID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_OperatorID* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_OperatorID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_OperatorID* msg, bool tao);
+static inline bool _dronecan_remoteid_OperatorID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_OperatorID* msg, bool tao);
+void _dronecan_remoteid_OperatorID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_OperatorID* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t id_or_mac_len = msg->id_or_mac.len > 20 ? 20 : msg->id_or_mac.len;
+#pragma GCC diagnostic pop
+    canardEncodeScalar(buffer, *bit_ofs, 5, &id_or_mac_len);
+    *bit_ofs += 5;
+    for (size_t i=0; i < id_or_mac_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->operator_id_type);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t operator_id_len = msg->operator_id.len > 20 ? 20 : msg->operator_id.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 5, &operator_id_len);
+        *bit_ofs += 5;
+    }
+    for (size_t i=0; i < operator_id_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->operator_id.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_OperatorID, return true on failure, false on success
+*/
+bool _dronecan_remoteid_OperatorID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_OperatorID* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->id_or_mac.len);
+    *bit_ofs += 5;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->id_or_mac.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->id_or_mac.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->operator_id_type);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->operator_id.len);
+        *bit_ofs += 5;
+    } else {
+        msg->operator_id.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->operator_id.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->operator_id.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->operator_id.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_OperatorID sample_dronecan_remoteid_OperatorID_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_OperatorID, DRONECAN_REMOTEID_OPERATORID_ID, DRONECAN_REMOTEID_OPERATORID_SIGNATURE, DRONECAN_REMOTEID_OPERATORID_MAX_SIZE);
+#endif
+#endif

+ 11 - 0
uav_can/user_inc/dronecan.remoteid.SecureCommand.h

@@ -0,0 +1,11 @@
+#pragma once
+#include <dronecan.remoteid.SecureCommand_req.h>
+#include <dronecan.remoteid.SecureCommand_res.h>
+
+#define DRONECAN_REMOTEID_SECURECOMMAND_ID 64
+#define DRONECAN_REMOTEID_SECURECOMMAND_SIGNATURE (0x126A47C9C17A8BD7ULL)
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+#include <canard/cxx_wrappers.h>
+SERVICE_MESSAGE_CXX_IFACE(dronecan_remoteid_SecureCommand, DRONECAN_REMOTEID_SECURECOMMAND_ID, DRONECAN_REMOTEID_SECURECOMMAND_SIGNATURE, DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_MAX_SIZE, DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_MAX_SIZE);
+#endif

+ 121 - 0
uav_can/user_inc/dronecan.remoteid.SecureCommand_req.h

@@ -0,0 +1,121 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_MAX_SIZE 230
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SIGNATURE (0x126A47C9C17A8BD7ULL)
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_ID 64
+
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_SESSION_KEY 0
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_REMOTEID_SESSION_KEY 1
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_REMOVE_PUBLIC_KEYS 2
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_PUBLIC_KEYS 3
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_SET_PUBLIC_KEYS 4
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_REMOTEID_CONFIG 5
+#define DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_SET_REMOTEID_CONFIG 6
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_SecureCommand_cxx_iface;
+#endif
+
+struct dronecan_remoteid_SecureCommandRequest {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_SecureCommand_cxx_iface;
+#endif
+    uint32_t sequence;
+    uint32_t operation;
+    uint8_t sig_length;
+    struct { uint8_t len; uint8_t data[220]; }data;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_SecureCommandRequest_encode(struct dronecan_remoteid_SecureCommandRequest* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_SecureCommandRequest_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SecureCommandRequest* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_SecureCommandRequest_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandRequest* msg, bool tao);
+static inline bool _dronecan_remoteid_SecureCommandRequest_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandRequest* msg, bool tao);
+void _dronecan_remoteid_SecureCommandRequest_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandRequest* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->sequence);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->operation);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->sig_length);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t data_len = msg->data.len > 220 ? 220 : msg->data.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &data_len);
+        *bit_ofs += 8;
+    }
+    for (size_t i=0; i < data_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->data.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_SecureCommandRequest, return true on failure, false on success
+*/
+bool _dronecan_remoteid_SecureCommandRequest_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandRequest* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->sequence);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->operation);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->sig_length);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->data.len);
+        *bit_ofs += 8;
+    } else {
+        msg->data.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->data.len > 220) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->data.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->data.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SecureCommandRequest sample_dronecan_remoteid_SecureCommandRequest_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+#endif
+#endif

+ 119 - 0
uav_can/user_inc/dronecan.remoteid.SecureCommand_res.h

@@ -0,0 +1,119 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_MAX_SIZE 230
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_SIGNATURE (0x126A47C9C17A8BD7ULL)
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_ID 64
+
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_ACCEPTED 0
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_TEMPORARILY_REJECTED 1
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_DENIED 2
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_UNSUPPORTED 3
+#define DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_FAILED 4
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_SecureCommand_cxx_iface;
+#endif
+
+struct dronecan_remoteid_SecureCommandResponse {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_SecureCommand_cxx_iface;
+#endif
+    uint32_t sequence;
+    uint32_t operation;
+    uint8_t result;
+    struct { uint8_t len; uint8_t data[220]; }data;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_SecureCommandResponse_encode(struct dronecan_remoteid_SecureCommandResponse* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_SecureCommandResponse_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SecureCommandResponse* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_SecureCommandResponse_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandResponse* msg, bool tao);
+static inline bool _dronecan_remoteid_SecureCommandResponse_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandResponse* msg, bool tao);
+void _dronecan_remoteid_SecureCommandResponse_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandResponse* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->sequence);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->operation);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->result);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t data_len = msg->data.len > 220 ? 220 : msg->data.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &data_len);
+        *bit_ofs += 8;
+    }
+    for (size_t i=0; i < data_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->data.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_SecureCommandResponse, return true on failure, false on success
+*/
+bool _dronecan_remoteid_SecureCommandResponse_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SecureCommandResponse* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->sequence);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->operation);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->result);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->data.len);
+        *bit_ofs += 8;
+    } else {
+        msg->data.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->data.len > 220) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->data.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->data.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SecureCommandResponse sample_dronecan_remoteid_SecureCommandResponse_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+#endif
+#endif

+ 128 - 0
uav_can/user_inc/dronecan.remoteid.SelfID.h

@@ -0,0 +1,128 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_SELFID_MAX_SIZE 46
+#define DRONECAN_REMOTEID_SELFID_SIGNATURE (0x59BE81DC4C06A185ULL)
+#define DRONECAN_REMOTEID_SELFID_ID 20032
+
+#define DRONECAN_REMOTEID_SELFID_ODID_DESC_TYPE_TEXT 0
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_SelfID_cxx_iface;
+#endif
+
+struct dronecan_remoteid_SelfID {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_SelfID_cxx_iface;
+#endif
+    struct { uint8_t len; uint8_t data[20]; }id_or_mac;
+    uint8_t description_type;
+    struct { uint8_t len; uint8_t data[23]; }description;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_SelfID_encode(struct dronecan_remoteid_SelfID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_SelfID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SelfID* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_SelfID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SelfID* msg, bool tao);
+static inline bool _dronecan_remoteid_SelfID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SelfID* msg, bool tao);
+void _dronecan_remoteid_SelfID_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_SelfID* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t id_or_mac_len = msg->id_or_mac.len > 20 ? 20 : msg->id_or_mac.len;
+#pragma GCC diagnostic pop
+    canardEncodeScalar(buffer, *bit_ofs, 5, &id_or_mac_len);
+    *bit_ofs += 5;
+    for (size_t i=0; i < id_or_mac_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->description_type);
+    *bit_ofs += 8;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t description_len = msg->description.len > 23 ? 23 : msg->description.len;
+#pragma GCC diagnostic pop
+    if (!tao) {
+        canardEncodeScalar(buffer, *bit_ofs, 5, &description_len);
+        *bit_ofs += 5;
+    }
+    for (size_t i=0; i < description_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->description.data[i]);
+        *bit_ofs += 8;
+    }
+}
+
+/*
+ decode dronecan_remoteid_SelfID, return true on failure, false on success
+*/
+bool _dronecan_remoteid_SelfID_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_SelfID* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->id_or_mac.len);
+    *bit_ofs += 5;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->id_or_mac.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->id_or_mac.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->description_type);
+    *bit_ofs += 8;
+
+    if (!tao) {
+        canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->description.len);
+        *bit_ofs += 5;
+    } else {
+        msg->description.len = ((transfer->payload_len*8)-*bit_ofs)/8;
+    }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->description.len > 23) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->description.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->description.data[i]);
+        *bit_ofs += 8;
+    }
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SelfID sample_dronecan_remoteid_SelfID_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_SelfID, DRONECAN_REMOTEID_SELFID_ID, DRONECAN_REMOTEID_SELFID_SIGNATURE, DRONECAN_REMOTEID_SELFID_MAX_SIZE);
+#endif
+#endif

+ 178 - 0
uav_can/user_inc/dronecan.remoteid.System.h

@@ -0,0 +1,178 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define DRONECAN_REMOTEID_SYSTEM_MAX_SIZE 53
+#define DRONECAN_REMOTEID_SYSTEM_SIGNATURE (0x9AC872F49BF32437ULL)
+#define DRONECAN_REMOTEID_SYSTEM_ID 20033
+
+#define DRONECAN_REMOTEID_SYSTEM_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF 0
+#define DRONECAN_REMOTEID_SYSTEM_ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS 1
+#define DRONECAN_REMOTEID_SYSTEM_ODID_OPERATOR_LOCATION_TYPE_FIXED 2
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASSIFICATION_TYPE_EU 0
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CATEGORY_EU_UNDECLARED 0
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CATEGORY_EU_OPEN 1
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CATEGORY_EU_SPECIFIC 2
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CATEGORY_EU_CERTIFIED 3
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_UNDECLARED 0
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_0 1
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_1 2
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_2 3
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_3 4
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_4 5
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_5 6
+#define DRONECAN_REMOTEID_SYSTEM_ODID_CLASS_EU_CLASS_6 7
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class dronecan_remoteid_System_cxx_iface;
+#endif
+
+struct dronecan_remoteid_System {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = dronecan_remoteid_System_cxx_iface;
+#endif
+    struct { uint8_t len; uint8_t data[20]; }id_or_mac;
+    uint8_t operator_location_type;
+    uint8_t classification_type;
+    int32_t operator_latitude;
+    int32_t operator_longitude;
+    uint16_t area_count;
+    uint16_t area_radius;
+    float area_ceiling;
+    float area_floor;
+    uint8_t category_eu;
+    uint8_t class_eu;
+    float operator_altitude_geo;
+    uint32_t timestamp;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t dronecan_remoteid_System_encode(struct dronecan_remoteid_System* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool dronecan_remoteid_System_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_System* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _dronecan_remoteid_System_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_System* msg, bool tao);
+static inline bool _dronecan_remoteid_System_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_System* msg, bool tao);
+void _dronecan_remoteid_System_encode(uint8_t* buffer, uint32_t* bit_ofs, struct dronecan_remoteid_System* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    const uint8_t id_or_mac_len = msg->id_or_mac.len > 20 ? 20 : msg->id_or_mac.len;
+#pragma GCC diagnostic pop
+    canardEncodeScalar(buffer, *bit_ofs, 5, &id_or_mac_len);
+    *bit_ofs += 5;
+    for (size_t i=0; i < id_or_mac_len; i++) {
+        canardEncodeScalar(buffer, *bit_ofs, 8, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->operator_location_type);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->classification_type);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->operator_latitude);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->operator_longitude);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->area_count);
+    *bit_ofs += 16;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->area_radius);
+    *bit_ofs += 16;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->area_ceiling);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->area_floor);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->category_eu);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 8, &msg->class_eu);
+    *bit_ofs += 8;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->operator_altitude_geo);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->timestamp);
+    *bit_ofs += 32;
+}
+
+/*
+ decode dronecan_remoteid_System, return true on failure, false on success
+*/
+bool _dronecan_remoteid_System_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct dronecan_remoteid_System* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 5, false, &msg->id_or_mac.len);
+    *bit_ofs += 5;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+    if (msg->id_or_mac.len > 20) {
+        return true; /* invalid value */
+    }
+#pragma GCC diagnostic pop
+    for (size_t i=0; i < msg->id_or_mac.len; i++) {
+        canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->id_or_mac.data[i]);
+        *bit_ofs += 8;
+    }
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->operator_location_type);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->classification_type);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->operator_latitude);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->operator_longitude);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, false, &msg->area_count);
+    *bit_ofs += 16;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, false, &msg->area_radius);
+    *bit_ofs += 16;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->area_ceiling);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->area_floor);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->category_eu);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 8, false, &msg->class_eu);
+    *bit_ofs += 8;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, true, &msg->operator_altitude_geo);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->timestamp);
+    *bit_ofs += 32;
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_System sample_dronecan_remoteid_System_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(dronecan_remoteid_System, DRONECAN_REMOTEID_SYSTEM_ID, DRONECAN_REMOTEID_SYSTEM_SIGNATURE, DRONECAN_REMOTEID_SYSTEM_MAX_SIZE);
+#endif
+#endif

+ 8 - 0
uav_can/user_inc/dronecan_msgs.h

@@ -0,0 +1,8 @@
+#pragma once
+#include "remoteid.ArmStatus.h"
+#include "remoteid.BasicID.h"
+#include "remoteid.Location.h"
+#include "remoteid.OperatorID.h"
+#include "remoteid.SecureCommand.h"
+#include "remoteid.SelfID.h"
+#include "remoteid.System.h"

+ 108 - 0
uav_can/user_inc/uavcan.protocol.NodeStatus.h

@@ -0,0 +1,108 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <canard.h>
+
+
+#define UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE 7
+#define UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE (0xF0868D0C1A7C6F1ULL)
+#define UAVCAN_PROTOCOL_NODESTATUS_ID 341
+
+#define UAVCAN_PROTOCOL_NODESTATUS_MAX_BROADCASTING_PERIOD_MS 1000
+#define UAVCAN_PROTOCOL_NODESTATUS_MIN_BROADCASTING_PERIOD_MS 2
+#define UAVCAN_PROTOCOL_NODESTATUS_OFFLINE_TIMEOUT_MS 3000
+#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK 0
+#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING 1
+#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR 2
+#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL 3
+#define UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL 0
+#define UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION 1
+#define UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE 2
+#define UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE 3
+#define UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE 7
+
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+class uavcan_protocol_NodeStatus_cxx_iface;
+#endif
+
+struct uavcan_protocol_NodeStatus {
+#if defined(__cplusplus) && defined(DRONECAN_CXX_WRAPPERS)
+    using cxx_iface = uavcan_protocol_NodeStatus_cxx_iface;
+#endif
+    uint32_t uptime_sec;
+    uint8_t health;
+    uint8_t mode;
+    uint8_t sub_mode;
+    uint16_t vendor_specific_status_code;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+uint32_t uavcan_protocol_NodeStatus_encode(struct uavcan_protocol_NodeStatus* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+);
+bool uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, struct uavcan_protocol_NodeStatus* msg);
+
+#if defined(CANARD_DSDLC_INTERNAL)
+static inline void _uavcan_protocol_NodeStatus_encode(uint8_t* buffer, uint32_t* bit_ofs, struct uavcan_protocol_NodeStatus* msg, bool tao);
+static inline bool _uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct uavcan_protocol_NodeStatus* msg, bool tao);
+void _uavcan_protocol_NodeStatus_encode(uint8_t* buffer, uint32_t* bit_ofs, struct uavcan_protocol_NodeStatus* msg, bool tao) {
+    (void)buffer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+
+    canardEncodeScalar(buffer, *bit_ofs, 32, &msg->uptime_sec);
+    *bit_ofs += 32;
+    canardEncodeScalar(buffer, *bit_ofs, 2, &msg->health);
+    *bit_ofs += 2;
+    canardEncodeScalar(buffer, *bit_ofs, 3, &msg->mode);
+    *bit_ofs += 3;
+    canardEncodeScalar(buffer, *bit_ofs, 3, &msg->sub_mode);
+    *bit_ofs += 3;
+    canardEncodeScalar(buffer, *bit_ofs, 16, &msg->vendor_specific_status_code);
+    *bit_ofs += 16;
+}
+
+/*
+ decode uavcan_protocol_NodeStatus, return true on failure, false on success
+*/
+bool _uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, struct uavcan_protocol_NodeStatus* msg, bool tao) {
+    (void)transfer;
+    (void)bit_ofs;
+    (void)msg;
+    (void)tao;
+    canardDecodeScalar(transfer, *bit_ofs, 32, false, &msg->uptime_sec);
+    *bit_ofs += 32;
+
+    canardDecodeScalar(transfer, *bit_ofs, 2, false, &msg->health);
+    *bit_ofs += 2;
+
+    canardDecodeScalar(transfer, *bit_ofs, 3, false, &msg->mode);
+    *bit_ofs += 3;
+
+    canardDecodeScalar(transfer, *bit_ofs, 3, false, &msg->sub_mode);
+    *bit_ofs += 3;
+
+    canardDecodeScalar(transfer, *bit_ofs, 16, false, &msg->vendor_specific_status_code);
+    *bit_ofs += 16;
+
+    return false; /* success */
+}
+#endif
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct uavcan_protocol_NodeStatus sample_uavcan_protocol_NodeStatus_msg(void);
+#endif
+#ifdef __cplusplus
+} // extern "C"
+
+#ifdef DRONECAN_CXX_WRAPPERS
+#include <canard/cxx_wrappers.h>
+BROADCAST_MESSAGE_CXX_IFACE(uavcan_protocol_NodeStatus, UAVCAN_PROTOCOL_NODESTATUS_ID, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE);
+#endif
+#endif

+ 121 - 0
uav_can/user_inc/user_rid.h

@@ -0,0 +1,121 @@
+#ifndef _USER_RID_H
+#define _USER_RID_H
+
+#include "stdint.h"
+#include "stdbool.h"
+#include "common.h"
+#include "canard.h"
+void send_msg_to_remoteid(void);
+
+enum Remoteid_type
+{
+    LOCATION_TYPE = 1,
+    SYS_OPERATOR_TYPE = 2,
+    BASICID_TYPE = 3,
+    SELFID_TYPE = 4,
+    OPERATOR_TYPE = 5,
+    ARMSTATUS_TYPE = 6,
+
+    RID_TYPE_END,
+};
+
+bool remoteid_shouldAcceptTransfer(const CanardInstance *ins,
+                                           uint64_t *out_data_type_signature,
+                                           uint16_t data_type_id,
+                                           CanardTransferType transfer_type,
+                                           uint8_t source_node_id);
+int remoteid_OnRecieved(CanardInstance *canardIns,
+                                CanardRxTransfer *transfer);
+extern Connect_check remoteid_link;
+//remoteid
+#pragma pack(1)
+typedef struct{
+ int32_t latitude; /*< [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/
+ int32_t longitude; /*< [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/
+ float altitude_barometric; /*< [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m.*/
+ float altitude_geodetic; /*< [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m.*/
+ float height; /*< [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m.*/
+ float timestamp; /*< [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF.*/
+ uint16_t direction; /*< [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees.*/
+ uint16_t speed_horizontal; /*< [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s.*/
+ int16_t speed_vertical; /*< [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.*/
+ uint8_t target_system; /*<  System ID (0 for broadcast).*/
+ uint8_t target_component; /*<  Component ID (0 for broadcast).*/
+ uint8_t id_or_mac[20]; /*<  Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */
+ uint8_t status; /*<  Indicates whether the unmanned aircraft is on the ground or in the air.*/
+ uint8_t height_reference; /*<  Indicates the reference point for the height field.*/
+ uint8_t horizontal_accuracy; /*<  The accuracy of the horizontal position.*/
+ uint8_t vertical_accuracy; /*<  The accuracy of the vertical position.*/
+ uint8_t barometer_accuracy; /*<  The accuracy of the barometric altitude.*/
+ uint8_t speed_accuracy; /*<  The accuracy of the horizontal and vertical speed.*/
+ uint8_t timestamp_accuracy; /*<  The accuracy of the timestamps.*/
+} _location_msg;
+#pragma pack()
+extern _location_msg locationId;
+
+#pragma pack(1)
+typedef struct{
+ int32_t operator_latitude; /*< [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon).*/
+ int32_t operator_longitude; /*< [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon).*/
+ float area_ceiling; /*< [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA.*/
+ float area_floor; /*< [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA.*/
+ float operator_altitude_geo; /*< [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m.*/
+ uint32_t timestamp; /*< [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.*/
+ uint16_t area_count; /*<  Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA.*/
+ uint16_t area_radius; /*< [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA.*/
+ uint8_t target_system; /*<  System ID (0 for broadcast).*/
+ uint8_t target_component; /*<  Component ID (0 for broadcast).*/
+ uint8_t id_or_mac[20]; /*<  Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */
+ uint8_t operator_location_type; /*<  Specifies the operator location type.*/
+ uint8_t classification_type; /*<  Specifies the classification type of the UA.*/
+ uint8_t category_eu; /*<  When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA.*/
+ uint8_t class_eu; /*<  When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA.*/
+} _sys_operator_msg;
+#pragma pack()
+extern _sys_operator_msg systemId;
+
+#pragma pack(1)
+typedef struct  {
+ uint8_t target_system; /*<  System ID (0 for broadcast).*/
+ uint8_t target_component; /*<  Component ID (0 for broadcast).*/
+ uint8_t id_or_mac[20]; /*<  Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */
+ uint8_t id_type; /*<  Indicates the format for the uas_id field of this message.*/
+ uint8_t ua_type; /*<  Indicates the type of UA (Unmanned Aircraft).*/
+ uint8_t uas_id[20]; /*<  UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field.*/
+} _basicId_msg;
+#pragma pack()
+extern _basicId_msg basicId;
+
+
+#pragma pack(1)
+typedef struct{
+ uint8_t target_system; /*<  System ID (0 for broadcast).*/
+ uint8_t target_component; /*<  Component ID (0 for broadcast).*/
+ uint8_t id_or_mac[20]; /*<  Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */
+ uint8_t description_type; /*<  Indicates the type of the description field.*/
+ char description[23]; /*<  Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/
+} _selfId_msg;
+#pragma pack()
+extern _selfId_msg selfId;
+
+#pragma pack(1)
+typedef struct{
+ uint8_t target_system; /*<  System ID (0 for broadcast).*/
+ uint8_t target_component; /*<  Component ID (0 for broadcast).*/
+ uint8_t id_or_mac[20]; /*<  Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */
+ uint8_t operator_id_type; /*<  Indicates the type of the operator_id field.*/
+ char operator_id[20]; /*<  Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/
+} _operatorId_msg;
+#pragma pack()
+extern _operatorId_msg operatorId;
+
+#pragma pack(1)
+typedef struct{
+ uint8_t status;
+ uint8_t len; 
+ uint8_t data[50];
+} _armStatusID_msg;
+#pragma pack()
+extern _armStatusID_msg StatusId;
+
+#endif

+ 68 - 0
uav_can/user_src/dronecan.remoteid.ArmStatus.c

@@ -0,0 +1,68 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.ArmStatus.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_ArmStatus_encode(struct dronecan_remoteid_ArmStatus* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE);
+    _dronecan_remoteid_ArmStatus_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_ArmStatus_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_ArmStatus* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_ARMSTATUS_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_ArmStatus_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_ArmStatus sample_dronecan_remoteid_ArmStatus_msg(void) {
+    struct dronecan_remoteid_ArmStatus msg;
+
+    msg.status = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.error.len = (uint8_t)random_range_unsigned_val(0, 50);
+    for (size_t i=0; i < msg.error.len; i++) {
+        msg.error.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 73 - 0
uav_can/user_src/dronecan.remoteid.BasicID.c

@@ -0,0 +1,73 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.BasicID.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_BasicID_encode(struct dronecan_remoteid_BasicID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_BASICID_MAX_SIZE);
+    _dronecan_remoteid_BasicID_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_BasicID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_BasicID* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_BASICID_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_BasicID_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_BasicID sample_dronecan_remoteid_BasicID_msg(void) {
+    struct dronecan_remoteid_BasicID msg;
+
+    msg.id_or_mac.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.id_or_mac.len; i++) {
+        msg.id_or_mac.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    msg.id_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.ua_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.uas_id.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.uas_id.len; i++) {
+        msg.uas_id.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 83 - 0
uav_can/user_src/dronecan.remoteid.Location.c

@@ -0,0 +1,83 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.Location.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_Location_encode(struct dronecan_remoteid_Location* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_LOCATION_MAX_SIZE);
+    _dronecan_remoteid_Location_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_Location_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_Location* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_LOCATION_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_Location_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_Location sample_dronecan_remoteid_Location_msg(void) {
+    struct dronecan_remoteid_Location msg;
+
+    msg.id_or_mac.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.id_or_mac.len; i++) {
+        msg.id_or_mac.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    msg.status = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.direction = (uint16_t)random_bitlen_unsigned_val(16);
+    msg.speed_horizontal = (uint16_t)random_bitlen_unsigned_val(16);
+    msg.speed_vertical = (int16_t)random_bitlen_signed_val(16);
+    msg.latitude = (int32_t)random_bitlen_signed_val(32);
+    msg.longitude = (int32_t)random_bitlen_signed_val(32);
+    msg.altitude_barometric = random_float_val();
+    msg.altitude_geodetic = random_float_val();
+    msg.height_reference = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.height = random_float_val();
+    msg.horizontal_accuracy = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.vertical_accuracy = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.barometer_accuracy = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.speed_accuracy = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.timestamp = random_float_val();
+    msg.timestamp_accuracy = (uint8_t)random_bitlen_unsigned_val(8);
+    return msg;
+}
+#endif

+ 72 - 0
uav_can/user_src/dronecan.remoteid.OperatorID.c

@@ -0,0 +1,72 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.OperatorID.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_OperatorID_encode(struct dronecan_remoteid_OperatorID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_OPERATORID_MAX_SIZE);
+    _dronecan_remoteid_OperatorID_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_OperatorID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_OperatorID* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_OPERATORID_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_OperatorID_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_OperatorID sample_dronecan_remoteid_OperatorID_msg(void) {
+    struct dronecan_remoteid_OperatorID msg;
+
+    msg.id_or_mac.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.id_or_mac.len; i++) {
+        msg.id_or_mac.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    msg.operator_id_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.operator_id.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.operator_id.len; i++) {
+        msg.operator_id.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 71 - 0
uav_can/user_src/dronecan.remoteid.SecureCommand_req.c

@@ -0,0 +1,71 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.SecureCommand_req.h>
+#include <dronecan.remoteid.SecureCommand_res.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_SecureCommandRequest_encode(struct dronecan_remoteid_SecureCommandRequest* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_MAX_SIZE);
+    _dronecan_remoteid_SecureCommandRequest_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_SecureCommandRequest_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SecureCommandRequest* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_SecureCommandRequest_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SecureCommandRequest sample_dronecan_remoteid_SecureCommandRequest_msg(void) {
+    struct dronecan_remoteid_SecureCommandRequest msg;
+
+    msg.sequence = (uint32_t)random_bitlen_unsigned_val(32);
+    msg.operation = (uint32_t)random_bitlen_unsigned_val(32);
+    msg.sig_length = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.data.len = (uint8_t)random_range_unsigned_val(0, 220);
+    for (size_t i=0; i < msg.data.len; i++) {
+        msg.data.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 70 - 0
uav_can/user_src/dronecan.remoteid.SecureCommand_res.c

@@ -0,0 +1,70 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.SecureCommand_res.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_SecureCommandResponse_encode(struct dronecan_remoteid_SecureCommandResponse* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_MAX_SIZE);
+    _dronecan_remoteid_SecureCommandResponse_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_SecureCommandResponse_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SecureCommandResponse* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_SecureCommandResponse_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SecureCommandResponse sample_dronecan_remoteid_SecureCommandResponse_msg(void) {
+    struct dronecan_remoteid_SecureCommandResponse msg;
+
+    msg.sequence = (uint32_t)random_bitlen_unsigned_val(32);
+    msg.operation = (uint32_t)random_bitlen_unsigned_val(32);
+    msg.result = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.data.len = (uint8_t)random_range_unsigned_val(0, 220);
+    for (size_t i=0; i < msg.data.len; i++) {
+        msg.data.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 72 - 0
uav_can/user_src/dronecan.remoteid.SelfID.c

@@ -0,0 +1,72 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.SelfID.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_SelfID_encode(struct dronecan_remoteid_SelfID* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_SELFID_MAX_SIZE);
+    _dronecan_remoteid_SelfID_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_SelfID_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_SelfID* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_SELFID_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_SelfID_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_SelfID sample_dronecan_remoteid_SelfID_msg(void) {
+    struct dronecan_remoteid_SelfID msg;
+
+    msg.id_or_mac.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.id_or_mac.len; i++) {
+        msg.id_or_mac.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    msg.description_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.description.len = (uint8_t)random_range_unsigned_val(0, 23);
+    for (size_t i=0; i < msg.description.len; i++) {
+        msg.description.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    return msg;
+}
+#endif

+ 79 - 0
uav_can/user_src/dronecan.remoteid.System.c

@@ -0,0 +1,79 @@
+#define CANARD_DSDLC_INTERNAL
+#include <dronecan.remoteid.System.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t dronecan_remoteid_System_encode(struct dronecan_remoteid_System* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, DRONECAN_REMOTEID_SYSTEM_MAX_SIZE);
+    _dronecan_remoteid_System_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool dronecan_remoteid_System_decode(const CanardRxTransfer* transfer, struct dronecan_remoteid_System* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > DRONECAN_REMOTEID_SYSTEM_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_dronecan_remoteid_System_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct dronecan_remoteid_System sample_dronecan_remoteid_System_msg(void) {
+    struct dronecan_remoteid_System msg;
+
+    msg.id_or_mac.len = (uint8_t)random_range_unsigned_val(0, 20);
+    for (size_t i=0; i < msg.id_or_mac.len; i++) {
+        msg.id_or_mac.data[i] = (uint8_t)random_bitlen_unsigned_val(8);
+    }
+    msg.operator_location_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.classification_type = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.operator_latitude = (int32_t)random_bitlen_signed_val(32);
+    msg.operator_longitude = (int32_t)random_bitlen_signed_val(32);
+    msg.area_count = (uint16_t)random_bitlen_unsigned_val(16);
+    msg.area_radius = (uint16_t)random_bitlen_unsigned_val(16);
+    msg.area_ceiling = random_float_val();
+    msg.area_floor = random_float_val();
+    msg.category_eu = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.class_eu = (uint8_t)random_bitlen_unsigned_val(8);
+    msg.operator_altitude_geo = random_float_val();
+    msg.timestamp = (uint32_t)random_bitlen_unsigned_val(32);
+    return msg;
+}
+#endif

+ 68 - 0
uav_can/user_src/uavcan.protocol.NodeStatus.c

@@ -0,0 +1,68 @@
+#define CANARD_DSDLC_INTERNAL
+#include <uavcan.protocol.NodeStatus.h>
+#include <string.h>
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+#include <test_helpers.h>
+#endif
+
+uint32_t uavcan_protocol_NodeStatus_encode(struct uavcan_protocol_NodeStatus* msg, uint8_t* buffer
+#if CANARD_ENABLE_TAO_OPTION
+    , bool tao
+#endif
+) {
+    uint32_t bit_ofs = 0;
+    memset(buffer, 0, UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE);
+    _uavcan_protocol_NodeStatus_encode(buffer, &bit_ofs, msg, 
+#if CANARD_ENABLE_TAO_OPTION
+    tao
+#else
+    true
+#endif
+    );
+    return ((bit_ofs+7)/8);
+}
+
+/*
+  return true if the decode is invalid
+ */
+bool uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, struct uavcan_protocol_NodeStatus* msg) {
+#if CANARD_ENABLE_TAO_OPTION
+    if (transfer->tao && (transfer->payload_len > UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE)) {
+        return true; /* invalid payload length */
+    }
+#endif
+    uint32_t bit_ofs = 0;
+    if (_uavcan_protocol_NodeStatus_decode(transfer, &bit_ofs, msg,
+#if CANARD_ENABLE_TAO_OPTION
+    transfer->tao
+#else
+    true
+#endif
+    )) {
+        return true; /* invalid payload */
+    }
+
+    const uint32_t byte_len = (bit_ofs+7U)/8U;
+#if CANARD_ENABLE_TAO_OPTION
+    // if this could be CANFD then the dlc could indicating more bytes than
+    // we actually have
+    if (!transfer->tao) {
+        return byte_len > transfer->payload_len;
+    }
+#endif
+    return byte_len != transfer->payload_len;
+}
+
+#ifdef CANARD_DSDLC_TEST_BUILD
+struct uavcan_protocol_NodeStatus sample_uavcan_protocol_NodeStatus_msg(void) {
+    struct uavcan_protocol_NodeStatus msg;
+
+    msg.uptime_sec = (uint32_t)random_bitlen_unsigned_val(32);
+    msg.health = (uint8_t)random_bitlen_unsigned_val(2);
+    msg.mode = (uint8_t)random_bitlen_unsigned_val(3);
+    msg.sub_mode = (uint8_t)random_bitlen_unsigned_val(3);
+    msg.vendor_specific_status_code = (uint16_t)random_bitlen_unsigned_val(16);
+    return msg;
+}
+#endif

+ 293 - 0
uav_can/user_src/user_rid.c

@@ -0,0 +1,293 @@
+#include "user_rid.h"
+#include "dronecan.h"
+#include <dronecan.remoteid.Location.h>
+#include <dronecan.remoteid.ArmStatus.h>
+#include <dronecan.remoteid.BasicID.h>
+#include <uavcan.protocol.NodeStatus.h>
+#include <dronecan.remoteid.OperatorID.h>
+#include <dronecan.remoteid.SelfID.h>
+#include <dronecan.remoteid.System.h>
+#include "string.h"
+
+Connect_check remoteid_link;
+_armStatusID_msg StatusId;
+_location_msg locationId;
+_operatorId_msg operatorId;
+_basicId_msg basicId;
+_selfId_msg selfId;
+_sys_operator_msg systemId;
+
+void send_location_msg( void )
+{
+    static uint8_t transfer_id = 0;
+
+    struct dronecan_remoteid_Location msg;
+
+    uint8_t buffer[DRONECAN_REMOTEID_LOCATION_MAX_SIZE] = {0};
+
+
+    msg.id_or_mac.len = 20;
+    memcpy(&msg.id_or_mac.data, &locationId.id_or_mac, msg.id_or_mac.len);
+    msg.status = locationId.status;
+    msg.direction = locationId.direction;
+    msg.speed_horizontal = locationId.speed_horizontal;
+    msg.speed_vertical = locationId.speed_vertical;
+    msg.latitude = locationId.latitude;
+    msg.longitude = locationId.longitude;
+    msg.altitude_barometric = locationId.altitude_barometric;
+    msg.altitude_geodetic = locationId.altitude_geodetic;
+    msg.height_reference = locationId.height_reference;
+    msg.height = locationId.height;
+    msg.horizontal_accuracy = locationId.horizontal_accuracy;
+    msg.vertical_accuracy = locationId.vertical_accuracy;
+    msg.barometer_accuracy = locationId.barometer_accuracy;
+    msg.speed_accuracy = locationId.speed_accuracy;
+    msg.timestamp = locationId.timestamp;
+    msg.timestamp_accuracy = locationId.timestamp_accuracy;
+
+    uint32_t len = dronecan_remoteid_Location_encode(&msg, buffer);
+
+    CanardTxTransfer tx_transfer = {
+        .data_type_signature = DRONECAN_REMOTEID_LOCATION_SIGNATURE,
+        .data_type_id = DRONECAN_REMOTEID_LOCATION_ID,
+        .inout_transfer_id = &transfer_id,
+        .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+        .transfer_type = CanardTransferTypeBroadcast,
+        .payload = buffer,
+        .payload_len = len,
+    };
+
+    dronecan_broadcast(&tx_transfer);
+}
+
+void send_basicID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_BasicID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_BASICID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &basicId.id_or_mac, msg.id_or_mac.len);
+  msg.id_type = basicId.id_type;
+  msg.ua_type = basicId.ua_type;
+  msg.uas_id.len = 20;
+  memcpy(&msg.uas_id.data, &basicId.uas_id, msg.uas_id.len);
+
+  uint32_t len = dronecan_remoteid_BasicID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_BASICID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_BASICID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+void send_operatorID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_OperatorID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_OPERATORID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &operatorId.id_or_mac, msg.id_or_mac.len);
+  msg.operator_id_type = operatorId.operator_id_type;
+  msg.operator_id.len = 20;
+  memcpy(&msg.operator_id.data, &operatorId.operator_id, msg.operator_id.len);
+
+  uint32_t len = dronecan_remoteid_OperatorID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_OPERATORID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_OPERATORID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+
+void send_sysOperatorID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_System msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_SYSTEM_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &systemId.id_or_mac, msg.id_or_mac.len);
+  msg.operator_location_type = systemId.operator_location_type;
+  msg.classification_type = systemId.classification_type;
+  msg.operator_latitude = systemId.operator_latitude;
+  msg.operator_longitude = systemId.operator_longitude;
+  msg.area_count = systemId.area_count;
+  msg.area_radius = systemId.area_radius;
+  msg.area_ceiling = systemId.area_ceiling;
+  msg.area_floor = systemId.area_floor;
+  msg.category_eu = systemId.category_eu;
+  msg.class_eu = systemId.class_eu;
+  msg.operator_altitude_geo = systemId.operator_altitude_geo;
+  msg.timestamp = systemId.timestamp;
+
+  uint32_t len = dronecan_remoteid_System_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_SYSTEM_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_SYSTEM_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+void send_selfID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_SelfID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_SELFID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &selfId.id_or_mac, msg.id_or_mac.len);
+  msg.description_type = selfId.description_type;
+  msg.description.len = 23;
+  memcpy(&msg.description.data, &selfId.description, msg.description.len);
+
+  uint32_t len = dronecan_remoteid_SelfID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_SELFID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_SELFID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+int remoteid_OnRecieved(CanardInstance *canardIns,
+                                CanardRxTransfer *transfer) 
+{
+  int ret = 0;
+  struct dronecan_remoteid_ArmStatus info;
+  static int TimeDelay = 0;
+  if (transfer->transfer_type == CanardTransferTypeBroadcast) 
+  {
+    if( transfer->data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID)
+    {
+        if (!dronecan_remoteid_ArmStatus_decode(transfer, &info)) {
+            StatusId.status = info.status;
+            if( StatusId.status != 0 && HAL_GetTick() - TimeDelay > 2500)
+            {
+              memcpy(&StatusId.status, &info.status, sizeof(struct dronecan_remoteid_ArmStatus));
+            }
+            else
+            {
+              StatusId.status = 0;
+              TimeDelay = HAL_GetTick();
+              memset(&StatusId.len,0,51);
+            }
+            StatusId.len = 50;
+            remoteid_link.connect_status = COMP_NORMAL;
+        }
+    }
+  }
+
+  return ret;
+}
+
+bool remoteid_shouldAcceptTransfer(const CanardInstance *ins,
+                                           uint64_t *out_data_type_signature,
+                                           uint16_t data_type_id,
+                                           CanardTransferType transfer_type,
+                                           uint8_t source_node_id) {
+
+  bool ret = false;
+    
+  if (data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID &&
+      transfer_type == CanardTransferTypeBroadcast) {
+    *out_data_type_signature =  DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE;
+    ret = true;
+  } 
+  if (data_type_id == UAVCAN_PROTOCOL_NODESTATUS_ID &&
+        transfer_type == CanardTransferTypeBroadcast) {
+    *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE;
+    ret = true;
+  } 
+
+  return ret;
+}
+
+
+
+void send_msg_to_remoteid(void)
+{
+    static uint32_t SendTime_N2HZ = 0;
+    static uint32_t SendTime_N3HZ = 0;
+    static uint32_t SendTime_N4HZ = 0;
+    static uint32_t SendTime_N5HZ = 0;
+    static uint32_t SendTime_N6HZ = 0;
+
+    if ( HAL_GetTick() - SendTime_N2HZ > 555 )
+    {
+        //send_node_status();
+        send_location_msg();
+        
+        SendTime_N2HZ = HAL_GetTick();
+    }
+    if ( HAL_GetTick() - SendTime_N3HZ > 666 )
+    {
+        send_sysOperatorID();
+        SendTime_N3HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N4HZ > 333 )
+    {
+        send_basicID();
+        SendTime_N4HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N6HZ > 1777 )
+    {
+        send_operatorID();
+        SendTime_N6HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N5HZ > 1888 )
+    {
+        send_selfID();
+        SendTime_N5HZ = HAL_GetTick();
+    }
+
+    
+
+    dronecan_tx_processing();
+}
+
+

+ 236 - 0
user_inc/soft_hd_water_pump.h

@@ -0,0 +1,236 @@
+#ifndef _SOFT_HD_WATER_PUMP_H
+#define _SOFT_HD_WATER_PUMP_H
+#include "stdint.h"
+#include "string.h"
+#include <stdbool.h>
+#include "soft_water_device.h"
+
+//================================MSG FRAM================================================
+#define  HD_CANID_PRI_POS         (26)
+#define  HD_CANID_PRI_MASK        (0x1C000000)   // 1 1100 0000 0000 0000 0000 0000 0000
+#define  HD_PRI_HIGHEST            0x00
+
+#define  HD_CANID_ANONYMOUS          (0)
+#define  HD_CANID_ANONYMOUS_POS      (24)
+#define  HD_CANID_ANONYMOUS_MASK     (0x01000000)	// 0 0001 0000 0000 0000 0000 0000 0000
+
+#define  HD_CANID_SUBJECT_ID		(0)
+#define  HD_CANID_SUBJECT_POS       (14)
+#define  HD_CANID_SUBJECT_MASK      (0x00FFFF00)   // 0 0000 1111 1111 1111 1111 0000 0000
+
+#define  HD_CANID_SNM_POS         (25)
+#define  HD_CANID_SNM_MASK        (0x02000000)   // 0 0010 0000 0000 0000 0000 0000 0000
+#define  HD_TYPE_MSG               (0x0)           // �㲥֡(message farme)
+#define  HD_TYPE_SER               (0x1)           // ����֡
+
+#define HD_CANID_REV_7            (0)
+#define HD_CANID_REV_7_POS        (7)
+#define HD_CANID_REV_7_MASK       (0x00000080)		// 0 0000 0000 0000 0000 0000 1000 0000
+
+#define  HD_CANID_SRCNODE_POS     (0)
+#define  HD_CANID_SRCNODE_MASK    (0x0000007F)   // 0 0000 0000 0000 0000 0000 0111 1111
+//#define  HD_NODE_BDC               0x0A          // �㲥�ڵ�
+
+//================================SER FRAM==============================================
+#define  HD_CANID_RNR_POS         (24)
+#define  HD_CANID_RNR_MASK        (0x01000000)   // 0 0001 0000 0000 0000 0000 0000 0000
+#define  HD_TYPE_RES               (0x0)
+#define  HD_TYPE_REQ               (0x1)
+
+#define  HD_CANID_REV_23			(0)
+#define  HD_CANID_REV_23_POS		(23)
+#define  HD_CANID_REV_23_MASK		(0x800000)	// 0 0000 1000 0000 0000 0000 0000 0000
+
+
+#define  HD_CANID_SERID_POS       (14)
+#define  HD_CANID_SERID_MASK      (0x007FC000)   // 0 0000 0111 1111 1100 0000 0000 0000
+#define  HD_CANID_SERID           (0)
+
+
+#define  HD_CANID_DESNODE_POS     (7)
+#define  HD_CANID_DESNODE_MASK    (0x00003F80)   // 0 0000 0000 0000 0011 1111 1000 0000
+#define  FMU_NODE_ID_HD             (0x01)          // �ɿؽڵ�
+#define  HD_CANID_PUMP_ID			(0x08)			//ˮ�ýڵ�
+
+#define  HD_CANID_NOZZLE_0_ID		(0x40)			//喷头节点
+#define  HD_CANID_NOZZLE_1_ID		(0x41)			//
+#define  HD_CANID_NOZZLE_2_ID		(0x42)			//
+#define  HD_CANID_NOZZLE_3_ID		(0x43)			//
+#define  HD_CANID_NOZZLE_4_ID		(0x44)			//
+
+//MSG ID
+//水泵
+#define HD_HIGH_FREQ_REPORT_ID   (0x34)		//水泵控制器-高频状态上报
+#define HD_LOW_FREQ_REPORT_ID  	 (0x35)		//水泵控制器-低频状态上报
+#define HD_WATER_PUMP_CONTROL_ID (0x36)		//水泵控制命令
+#define HD_FLOWMETER_PULSE_ID 	 (0x44)		//水泵控制器-流量计脉冲
+#define HD_HEART_ID			 	 (0x05)		//水泵控制器-心跳
+
+//喷头
+#define HD_NOZZLE_HIGH_FREQ_ID   (0x37)		//喷头控制器-状态上报高频
+#define HD_NOZZLE_LOW_FREQ_ID  	 (0x38)		//喷头控制器-状态上报低频
+#define HD_NOZZLE_CONTROL_ID 	 (0x39)		//喷头控制器-控制命令
+#define HD_IDENTIFY_ID			 (0x0D)		//喷头控制器-识别
+#define HD_SET_NOZZLE_ID		 (0x09)		//喷头控制器-设置id
+
+//msg_id crc payload_len 在多帧中占用的字节数量
+#define msg_id_len 	1
+#define crc_len		2
+#define length_len  2
+
+//多帧消息类型
+#define none 0 			//非多帧消息
+#define PSL	 1			///水泵控制器低频状态上报
+
+#define SSL 2			//喷头控制器低频状态上报
+
+
+//多帧消息长度
+#define PSL_LEN     21
+#define SSL_LEN     14
+
+//HD��֡��Ϣ�ṹ��
+typedef struct _HDMsg
+{	
+	uint8_t Msg_id;
+	uint8_t buffer[50];
+	uint16_t rxindex;
+	uint16_t length;
+	HWTail Tail;
+	HWTail LastTail;
+	bool finish;	
+	uint16_t crc;
+	uint8_t len_wrong;
+	uint8_t crc_wrong;
+	uint32_t setEscIdTime;
+	
+}HDMsg;
+
+typedef struct _pump_status
+{
+	uint8_t LST : 2;						//逻辑状态,0:spin; 1:Stop。
+	uint8_t SST : 3;						//子状态,0:Calib; 1:Ready; 2:Align; 3:Startup; 4:Spin; 5:Freewheel。
+	uint8_t STA : 2;						 //水泵状态,0:Fault; 1:Init; 2:Stop; 3:Run。
+	uint8_t rev: 1;
+} pump_status;
+
+typedef struct _MCU_STATUS
+{
+	uint8_t CMST : 1;						//从 MCU 通讯状态,0:在位;1:失联
+	uint8_t VQST : 1; 						//从 MCU 版本查询状态,0:版本查询成功;1:版本查询失败
+	uint8_t REST : 2;						//从 MCU 重启状态,0:运行中;1:重启中;2:重启失败。
+	uint8_t VERS : 2;						//从 MCU 版本同步状态,0:已同步;1:版本同步中;2:版本同步失败。
+	uint8_t PWMS : 1; 						//从 MCU 同步 PWM 状态,0:信号正常;1:信号丢失。
+	uint8_t rev: 1;
+}_MCU_STATUS;
+
+typedef struct _fault_code
+{
+	uint16_t current_high : 1;   		//母线电流过大
+	uint16_t vol_low : 1;   		//母线电压过低
+	uint16_t vol_high : 1;   		//母线电压过高
+	uint16_t overload : 1;   		// 过载
+	uint16_t overspeed : 1;   		// 超速
+	uint16_t blockage : 1;   		// 堵转
+	uint16_t phase_current_high : 1;   		// 相电流过大
+	uint16_t phase_current_abnormal : 1;   		// 相电流偏移值异常
+	uint16_t temp_high : 1;   		// 温度过高
+	uint16_t phase_line_missing : 1;   		// 相线缺失
+	uint16_t thr_loss : 1;   		//油门信号丢失
+	uint16_t selif_check_abnormal : 1;   		// 自检异常
+	uint16_t hard_over_current : 1;   		// 硬件过流
+	uint16_t power_abnormal : 1;   		// 驱动电源异常
+	uint16_t overheat_open : 1;   		// 电机过热开路
+}_fault_code;
+
+typedef struct _pump
+{	
+	uint32_t devID;
+	
+	uint16_t flow1;
+	uint16_t flow2;
+	pump_status pump1_status;
+	pump_status pump2_status;
+	
+	uint16_t pump1_pwm;
+	uint16_t pump2_pwm;
+	uint16_t pump1_speed;
+	uint16_t pump2_speed;
+	_fault_code pump1_fault_code;
+	_fault_code pump2_fault_code;
+	uint16_t pump1_vol;
+	uint16_t pump2_vol;
+	uint8_t pump1_current;
+	uint8_t pump2_current;
+	uint8_t pump1_temp;
+	uint8_t pump2_temp;
+	_MCU_STATUS MCU_status;
+	uint16_t flowmeter1_pulse_interval;
+	uint16_t flowmeter2_pulse_interval;
+
+	
+	
+	HDMsg MultiMsg;
+}HDpump;
+
+typedef struct _solenoid_valve_status
+{
+	uint8_t VDS : 1; 			//电磁阀检测状态,0:关; 1:开。
+	uint8_t VCS : 1; 			//电磁阀控制状态,0:关; 1:开。
+	uint8_t rev : 5;
+}_solenoid_valve_status;
+
+typedef struct _nozzle_status
+{	
+	uint8_t LST : 2;				//逻辑状态,0:spin; 1:Stop; 2:Identify_Spin。
+	uint8_t SST : 3; 			//喷头子状态,0:Calib; 1:Ready; 2:Align; 3:Startup; 4:Spin; 5:Freewheel。
+	uint8_t STA : 2; 			//喷头状态,0:Fault; 1:Init; 2:Stop; 3:Run。
+	uint8_t PLS : 1; 			//功率限制标志,0:未限制; 1 限制中。
+	
+}_nozzle_status;
+
+typedef struct _nozzle
+{
+	uint32_t devID;
+	uint16_t speed;
+	_solenoid_valve_status solenoid_valve_status;
+	uint16_t speed_command;
+	_nozzle_status nozzle_status;
+
+	_fault_code fault_code;
+	uint16_t nozzle_vol;
+	uint8_t nozzle_current;
+	uint8_t solenoid_valve_current;
+	uint8_t nozzle_temp;
+	uint16_t motor_power;
+	uint16_t over_current_num;
+	uint16_t hard_over_current_num;
+	uint8_t blockage_frequency;
+
+	HDMsg MultiMsg;
+
+	uint8_t nodeId;
+	comp_status deviceLink;
+    uint32_t linkTime;
+
+}HDnozzle;
+
+typedef struct _nozzle_id
+{
+	uint32_t devID;
+	uint8_t nodeId;
+	uint32_t linkTime;
+}_nozzle_id;
+
+//extern HDnozzle HD_nozzle;
+extern HDnozzle NozzleMsg[5];
+extern HDpump HD_pump; 
+void HD_pump_func(void);
+extern void HuiDaCanRecvHookFunction(uint32_t id, uint8_t *recv_buf, uint8_t len);
+void HD_frame_process(HDMsg *_HDMsg , uint8_t *recv_buf, uint32_t len);
+void HD_can_sendmsg( uint8_t msg_id, _nozzle_id* Nozzle_id);
+void little_to_big_16(uint16_t* value) ;
+void little_to_big_32(uint32_t* value) ;
+void add_devId(uint8_t SrcId, uint32_t devId);
+uint32_t millis(void);
+#endif

+ 638 - 0
user_src/soft_hd_water_pump.c

@@ -0,0 +1,638 @@
+#include "soft_hd_water_pump.h"
+#include "soft_water_device.h"
+#include "common.h"
+#include "string.h"
+#include "soft_p_2_c.h"
+#include "soft_flow.h"
+#include "soft_seed_device.h"
+#include "soft_crc.h"
+#include "soft_version.h"
+
+// 惠达采用小端存储
+
+HWTail PumpControlTail = {0};
+HWTail NozzleControlTail = {0};
+HDpump HD_pump = {0};
+HDMsg *_HDMsg = NULL;
+
+HDnozzle NozzleMsg[5] = {0};
+_nozzle_id Nozzle_id[4] = {0};
+
+// 设置喷头id标志位
+bool SetNozzleId = false;
+// 识别喷头标志位
+bool IdentifyNozzle = false;
+// 设置喷头id
+uint8_t SetNozzleIdNum = 4;
+// 设置喷头设备id
+uint32_t SetDevId = 1399067365;
+// uint16_t curNodeID = 0;
+// 超时时间
+uint32_t overtime = 0;
+
+void HuiDaCanRecvHookFunction(uint32_t id, uint8_t *recv_buf, uint8_t len)
+{
+	uint16_t TypeID = 0;
+	uint8_t SrcNodeID = (id & HD_CANID_SRCNODE_MASK) >> HD_CANID_SRCNODE_POS;
+
+	if (SrcNodeID == HD_CANID_PUMP_ID)
+	{
+	}
+	else if (SrcNodeID >= HD_CANID_NOZZLE_0_ID && SrcNodeID <= HD_CANID_NOZZLE_4_ID)
+	{
+	}
+	else
+	{
+		return;
+	}
+
+	TypeID = recv_buf[0];
+
+	switch (SrcNodeID)
+	{
+	case HD_CANID_PUMP_ID:
+		_HDMsg = &HD_pump.MultiMsg;
+
+		Dev.Pump_Link.connect_status = COMP_NORMAL;
+		Dev.Pump_Link.recv_time = HAL_GetTick();
+		Dev.Pump.facid = FAC_HD_PUMP;
+
+		// 多帧消息处理
+		// 多帧发送需要二次打包
+		// 判断接收的消息类型
+		switch (_HDMsg->Msg_id)
+		{
+		// 非多帧消息
+		case none:
+			break;
+
+		case PSL:
+			HD_frame_process(_HDMsg, recv_buf, len);
+			if (_HDMsg->finish == true)
+			{
+				memcpy(&HD_pump.pump1_pwm, &_HDMsg->buffer[3], 2);
+				little_to_big_16(&HD_pump.pump1_pwm);
+
+				memcpy(&HD_pump.pump2_pwm, &_HDMsg->buffer[5], 2);
+				little_to_big_16(&HD_pump.pump2_pwm);
+
+				memcpy(&HD_pump.pump1_speed, &_HDMsg->buffer[7], 2);
+				little_to_big_16(&HD_pump.pump1_speed);
+
+				memcpy(&HD_pump.pump2_speed, &_HDMsg->buffer[9], 2);
+				little_to_big_16(&HD_pump.pump2_speed);
+
+				uint16_t fault_code = 0;
+				memcpy(&fault_code, &_HDMsg->buffer[11], 2);
+				little_to_big_16(&fault_code);
+				memcpy(&HD_pump.pump1_fault_code, &fault_code, 2);
+
+				fault_code = 0;
+				memcpy(&fault_code, &_HDMsg->buffer[13], 2);
+				little_to_big_16(&fault_code);
+				memcpy(&HD_pump.pump2_fault_code, &fault_code, 2);
+
+				memcpy(&HD_pump.pump1_vol, &_HDMsg->buffer[15], 2);
+				little_to_big_16(&HD_pump.pump1_vol);
+
+				memcpy(&HD_pump.pump2_vol, &_HDMsg->buffer[17], 2);
+				little_to_big_16(&HD_pump.pump2_vol);
+
+				HD_pump.pump1_current = _HDMsg->buffer[19];
+				HD_pump.pump2_current = _HDMsg->buffer[20];
+				HD_pump.pump1_temp = _HDMsg->buffer[21];
+				HD_pump.pump2_temp = _HDMsg->buffer[22];
+
+				memcpy(&HD_pump.MCU_status, &_HDMsg->buffer[23], 1);
+
+				_HDMsg->finish = false;
+				_HDMsg->Msg_id = none;
+			}
+			// 处理完毕跳过单帧消息处理,防止重复处理错误
+			goto next;
+			// break;
+		}
+
+		switch (TypeID)
+		{
+		case HD_HIGH_FREQ_REPORT_ID:
+			memcpy(&HD_pump.flow1, &recv_buf[1], 2);
+			little_to_big_16(&HD_pump.flow1);
+
+			memcpy(&HD_pump.flow2, &recv_buf[3], 2);
+			little_to_big_16(&HD_pump.flow2);
+
+			memcpy(&HD_pump.pump1_status, &recv_buf[5], 1);
+			memcpy(&HD_pump.pump2_status, &recv_buf[6], 1);
+
+			break;
+		case HD_LOW_FREQ_REPORT_ID:
+			memcpy(&_HDMsg->length, &recv_buf[1], 2);
+
+			little_to_big_16(&_HDMsg->length);
+			// 如果接收的消息长度正确
+			if (_HDMsg->length == PSL_LEN)
+			{
+				_HDMsg->rxindex = 0;
+				_HDMsg->LastTail.HWTailByte = recv_buf[7];
+
+				memcpy(&_HDMsg->buffer, recv_buf, 7);
+				_HDMsg->rxindex += 7;
+				_HDMsg->Msg_id = PSL;
+			}
+			else
+			{
+				_HDMsg->rxindex = 0;
+				_HDMsg->Msg_id = none;
+			}
+			break;
+
+		case HD_FLOWMETER_PULSE_ID:
+			memcpy(&HD_pump.flowmeter1_pulse_interval, &recv_buf[1], 2);
+			little_to_big_16(&HD_pump.flowmeter1_pulse_interval);
+
+			memcpy(&HD_pump.flowmeter2_pulse_interval, &recv_buf[3], 2);
+			little_to_big_16(&HD_pump.flowmeter2_pulse_interval);
+			break;
+		case HD_HEART_ID:
+			memcpy(&HD_pump.devID, &recv_buf[1], 4);
+			little_to_big_32(&HD_pump.devID);
+			break;
+		}
+		break;
+
+	case HD_CANID_NOZZLE_0_ID ... HD_CANID_NOZZLE_4_ID:
+
+	{
+		HDnozzle *HD_nozzle = NULL;
+		HD_nozzle = &NozzleMsg[SrcNodeID - HD_CANID_NOZZLE_0_ID];
+		_HDMsg = &HD_nozzle->MultiMsg;
+
+		Dev.Nozzle_Link.connect_status = COMP_NORMAL;
+		Dev.Nozzle_Link.recv_time = HAL_GetTick();
+		Dev.Nozzle.facid = FAC_HD_NOZZLE;
+
+		HD_nozzle->deviceLink = COMP_NORMAL;
+		HD_nozzle->linkTime = HAL_GetTick();
+		HD_nozzle->nodeId = SrcNodeID;
+
+		// 多帧消息处理
+		// 多帧发送需要二次打包
+		// 判断接收的消息类型
+		switch (_HDMsg->Msg_id)
+		{
+		// 非多帧消息
+		case none:
+		{
+			break;
+		}
+
+		case SSL:
+		{
+			HD_frame_process(_HDMsg, recv_buf, len);
+			if (_HDMsg->finish == true)
+			{
+				uint16_t _fault_code = 0;
+				memcpy(&_fault_code, &_HDMsg->buffer[3], 2);
+				little_to_big_16(&_fault_code);
+				memcpy(&HD_nozzle->fault_code, &_fault_code, 2);
+
+				memcpy(&HD_nozzle->nozzle_vol, &_HDMsg->buffer[5], 2);
+				little_to_big_16(&HD_nozzle->nozzle_vol);
+
+				memcpy(&HD_nozzle->nozzle_current, &_HDMsg->buffer[7], 1);
+				memcpy(&HD_nozzle->solenoid_valve_current, &_HDMsg->buffer[8], 1);
+				memcpy(&HD_nozzle->nozzle_temp, &_HDMsg->buffer[9], 1);
+
+				memcpy(&HD_nozzle->motor_power, &_HDMsg->buffer[10], 2);
+				little_to_big_16(&HD_nozzle->motor_power);
+
+				memcpy(&HD_nozzle->over_current_num, &_HDMsg->buffer[12], 2);
+				little_to_big_16(&HD_nozzle->over_current_num);
+
+				memcpy(&HD_nozzle->hard_over_current_num, &_HDMsg->buffer[14], 2);
+				little_to_big_16(&HD_nozzle->hard_over_current_num);
+
+				memcpy(&HD_nozzle->blockage_frequency, &_HDMsg->buffer[15], 1);
+
+				_HDMsg->finish = false;
+				_HDMsg->Msg_id = none;
+			}
+			// 跳过单帧消息处理,防止重复处理错误
+			goto next;
+			// break;
+		}
+		}
+
+		switch (TypeID)
+		{
+		case HD_NOZZLE_HIGH_FREQ_ID:
+		{
+			memcpy(&HD_nozzle->speed, &recv_buf[1], 2);
+			little_to_big_16(&HD_nozzle->speed);
+
+			memcpy(&HD_nozzle->solenoid_valve_status, &recv_buf[3], 1);
+
+			memcpy(&HD_nozzle->speed_command, &recv_buf[4], 2);
+			little_to_big_16(&HD_nozzle->speed_command);
+
+			memcpy(&HD_nozzle->nozzle_status, &recv_buf[6], 1);
+			break;
+		}
+		case HD_NOZZLE_LOW_FREQ_ID:
+		{
+			memcpy(&_HDMsg->length, &recv_buf[1], 2);
+
+			little_to_big_16(&_HDMsg->length);
+			// 如果接收的消息长度正确
+			if (_HDMsg->length == SSL_LEN)
+			{
+				_HDMsg->rxindex = 0;
+				_HDMsg->LastTail.HWTailByte = recv_buf[7];
+
+				memcpy(&_HDMsg->buffer, recv_buf, 7);
+				_HDMsg->rxindex += 7;
+				_HDMsg->Msg_id = SSL;
+			}
+			else
+			{
+				_HDMsg->rxindex = 0;
+				_HDMsg->Msg_id = none;
+			}
+			break;
+		}
+		case HD_HEART_ID:
+		{
+			memcpy(&HD_nozzle->devID, &recv_buf[1], 4);
+			little_to_big_32(&HD_nozzle->devID);
+			// 添加设备id
+			add_devId(SrcNodeID, HD_nozzle->devID);
+
+			if (SetNozzleId == true && HD_nozzle->devID == SetDevId)
+			{
+				if (SrcNodeID == SetNozzleIdNum + HD_CANID_NOZZLE_0_ID)
+				{
+					SetNozzleId = false;
+				}
+				else
+					overtime = millis();
+			}
+			break;
+		}
+		case HD_IDENTIFY_ID:
+		{
+			if (recv_buf[1] == 0 && IdentifyNozzle == true)
+			{
+				uint32_t dev_id;
+				memcpy(&dev_id, &recv_buf[2], 4);
+				little_to_big_32(&dev_id);
+				if (dev_id == SetDevId)
+					IdentifyNozzle = false;
+				else
+					overtime = millis();
+			}
+			else
+				overtime = millis();
+
+			break;
+		}
+		}
+	}
+	break;
+	}
+
+next:;
+}
+
+void HD_pump_func(void)
+{
+	// test
+
+	// static uint32_t time_1hz = 0;
+	if (Dev.Pump_Link.connect_status == COMP_NORMAL && Dev.Pump.facid == FAC_HD_PUMP)
+	{
+		/*
+		if(Check_Timer_Ready(&time_1hz,_1_HZ_))
+		{
+			HW_CanGetESCInfomation();
+			HW_CanSetESCInfomation();
+		}
+*/
+
+		HD_can_sendmsg(HD_WATER_PUMP_CONTROL_ID, NULL);
+		// HD_can_sendmsg( 1300 , 1300);
+	}
+
+	if (Dev.Nozzle_Link.connect_status == COMP_NORMAL && Dev.Nozzle.facid == FAC_HD_NOZZLE)
+	{
+		/*
+		if(Check_Timer_Ready(&time_1hz,_1_HZ_))
+		{
+			HW_CanGetESCInfomation();
+			HW_CanSetESCInfomation();
+		}
+*/
+		if (IdentifyNozzle == true)
+		{
+			/*
+			if (millis() - overtime > 5000)
+			{
+				IdentifyNozzle = false;
+			}*/
+
+			for (uint8_t i = 0; i < 4; i++)
+			{
+				if (Nozzle_id[i].devID == SetDevId)
+				{
+					HD_can_sendmsg(HD_IDENTIFY_ID, &Nozzle_id[i]);
+				}
+			}
+		}
+		else if (SetNozzleId == true)
+		{
+			/*
+			if (millis() - overtime > 5000)
+			{
+				IdentifyNozzle = false;
+			}*/
+
+			for (uint8_t i = 0; i < 4; i++)
+			{
+				if (Nozzle_id[i].devID == SetDevId)
+				{
+					HD_can_sendmsg(HD_SET_NOZZLE_ID, &Nozzle_id[i]);
+				}
+			}
+		}
+		else
+			HD_can_sendmsg(HD_NOZZLE_CONTROL_ID, NULL);
+		// HD_can_sendmsg( 1300 , 1300);
+	}
+}
+
+void HD_can_sendmsg(uint8_t msg_id, _nozzle_id *Nozzle_id)
+{
+	uint32_t canID;
+	uint32_t _dev_ID;
+	uint8_t can_buf[8] = {0};
+	switch (msg_id)
+	{
+	case HD_WATER_PUMP_CONTROL_ID:
+	{
+		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
+				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
+				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
+				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
+				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
+				((HD_CANID_PUMP_ID << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
+				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);
+
+		can_buf[0] = HD_WATER_PUMP_CONTROL_ID;
+
+		uint16_t _pwm1 = (uint16_t)pmu_pin.pump1;
+		uint16_t _pwm2 = (uint16_t)pmu_pin.pump2;
+		if (_pwm1 > 1800)
+			_pwm1 = 1800;
+		if (_pwm2 > 1800)
+			_pwm2 = 1800;
+		little_to_big_16(&_pwm1);
+		little_to_big_16(&_pwm2);
+		memcpy(&can_buf[1], &_pwm1, 2);
+		memcpy(&can_buf[3], &_pwm2, 2);
+
+		PumpControlTail.HWTailBit.start = 1;
+		PumpControlTail.HWTailBit.end = 1;
+		PumpControlTail.HWTailBit.toggle = 1;
+		can_buf[5] = PumpControlTail.HWTailByte;
+
+		can_send_msg_normal(can_buf, 6, canID);
+
+		PumpControlTail.HWTailBit.tranid++;
+		break;
+	}
+	case HD_NOZZLE_CONTROL_ID:
+	{
+		uint16_t rpm = 0;
+		uint8_t open;
+		for (uint8_t i = 1; i <= 4; i++)
+		{
+			switch (i)
+			{
+			case 1:
+				rpm = pmu_pin.nozz1_fm;
+				break;
+			case 2:
+				rpm = pmu_pin.nozz2_zp;
+				break;
+			case 3:
+				rpm = pmu_pin.nozz3;
+				break;
+			case 4:
+				rpm = pmu_pin.nozz4;
+				break;
+			default:
+				break;
+			}
+			canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
+					((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
+					((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
+					((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
+					((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
+					(((HD_CANID_NOZZLE_0_ID + i) << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
+					((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);
+			open = 1;
+			if (rpm <= 1020)
+			{
+				rpm = 0;
+				open = 0;
+			}
+			else if (rpm >= 2000)
+				rpm = 14000;
+			else
+				rpm = (rpm - 1020) * ((14000 - 2000) / (2000 - 1000));
+			can_buf[0] = HD_NOZZLE_CONTROL_ID;
+			// rpm = 14000;
+			little_to_big_16(&rpm);
+			memcpy(&can_buf[1], &rpm, 2);
+			memcpy(&can_buf[3], &open, 1);
+
+			PumpControlTail.HWTailBit.start = 1;
+			PumpControlTail.HWTailBit.end = 1;
+			PumpControlTail.HWTailBit.toggle = 1;
+			can_buf[4] = PumpControlTail.HWTailByte;
+
+			can_send_msg_normal(can_buf, 5, canID);
+
+			PumpControlTail.HWTailBit.tranid++;
+		}
+
+		break;
+	}
+	case HD_IDENTIFY_ID:
+	{
+		if (Nozzle_id == NULL)
+			return;
+		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
+				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
+				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
+				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
+				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
+				((Nozzle_id->nodeId << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
+				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);
+
+		can_buf[0] = HD_IDENTIFY_ID;
+		// rpm = 14000;
+		_dev_ID = SetDevId;
+		little_to_big_32(&_dev_ID);
+		memcpy(&can_buf[1], &_dev_ID, 4);
+		can_buf[5] = 0x00;
+		can_buf[6] = 0x00;
+		PumpControlTail.HWTailBit.start = 1;
+		PumpControlTail.HWTailBit.end = 1;
+		PumpControlTail.HWTailBit.toggle = 1;
+		can_buf[7] = PumpControlTail.HWTailByte;
+
+		can_send_msg_normal(can_buf, 8, canID);
+
+		PumpControlTail.HWTailBit.tranid++;
+		break;
+	}
+	case HD_SET_NOZZLE_ID:
+	{
+		if (Nozzle_id == NULL)
+			return;
+		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
+				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
+				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
+				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
+				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
+				((Nozzle_id->nodeId << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
+				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);
+
+		can_buf[0] = HD_SET_NOZZLE_ID;
+
+		_dev_ID = SetDevId;
+		little_to_big_32(&_dev_ID);
+		memcpy(&can_buf[1], &_dev_ID, 4);
+		can_buf[5] = SetNozzleIdNum + HD_CANID_NOZZLE_0_ID;
+		can_buf[6] = 0;
+
+		PumpControlTail.HWTailBit.start = 1;
+		PumpControlTail.HWTailBit.end = 1;
+		PumpControlTail.HWTailBit.toggle = 1;
+		can_buf[7] = PumpControlTail.HWTailByte;
+
+		can_send_msg_normal(can_buf, 8, canID);
+
+		PumpControlTail.HWTailBit.tranid++;
+		break;
+	}
+	default:
+		break;
+	}
+}
+
+// 多帧消息处理
+void HD_frame_process(HDMsg *_HDMsg, uint8_t *recv_buf, uint32_t len)
+{
+	_HDMsg->Tail.HWTailByte = recv_buf[len - 1];
+
+	// 如果收帧连续,数据进入buffer
+	if (_HDMsg->Tail.HWTailBit.toggle != _HDMsg->LastTail.HWTailBit.toggle)
+	{
+		memcpy(&_HDMsg->buffer[_HDMsg->rxindex], recv_buf, (len - 1));
+		_HDMsg->LastTail.HWTailByte = _HDMsg->Tail.HWTailByte;
+		_HDMsg->rxindex = _HDMsg->rxindex + len - 1;
+	}
+	// 如果是最后一帧
+	if (_HDMsg->Tail.HWTailBit.end == 1 && _HDMsg->rxindex == _HDMsg->length + length_len + msg_id_len + crc_len)
+	{
+		memcpy(&_HDMsg->crc, &_HDMsg->buffer[_HDMsg->rxindex - 2], 2);
+		little_to_big_16(&_HDMsg->crc);
+
+		// 校验
+		if (_HDMsg->crc == crcAdd(0xFFFF, _HDMsg->buffer, _HDMsg->rxindex - 2))
+		{
+			_HDMsg->finish = true;
+		}
+		else
+			_HDMsg->crc_wrong++;
+
+		// 无论是否校验通过,都重置
+		_HDMsg->Msg_id = none;
+		_HDMsg->rxindex = 0;
+	}
+	// 超出字节重置
+	else if (_HDMsg->rxindex >= (_HDMsg->length + length_len + msg_id_len + crc_len))
+	{
+		_HDMsg->rxindex = 0;
+		_HDMsg->Msg_id = none;
+		_HDMsg->len_wrong++;
+	}
+}
+
+// 添加设备id到Nozzle_id数组
+void add_devId(uint8_t SrcId, uint32_t devId)
+{
+	uint8_t p = 0;
+	for (uint8_t i = 0; i < 4; i++)
+	{
+		// 超时视为断开连接,清除设备id
+		if (Nozzle_id[i].devID != 0 && millis() - Nozzle_id[i].linkTime >= 2000)
+			Nozzle_id[i].devID = 0;
+	}
+	for (uint8_t i = 0; i < 4; i++)
+	{
+		// 如果设备id已经存在,更新时间
+		if (Nozzle_id[i].devID == devId)
+		{
+			Nozzle_id[i].linkTime = millis();
+			Nozzle_id[i].nodeId = SrcId;
+			return;
+		}
+		// 空位,记录
+		else if (Nozzle_id[i].devID == 0)
+		{
+			p = i;
+		}
+	}
+	// 当前id与之前的id都不相同,添加
+	// 如果数组已满,不添加
+	if (p < 4)
+	{
+		Nozzle_id[p].devID = devId;
+		Nozzle_id[p].nodeId = SrcId;
+		Nozzle_id[p].linkTime = millis();
+	}
+}
+
+void little_to_big_32(uint32_t *value)
+{
+	if (value == NULL)
+	{
+		return;
+	}
+	uint32_t _value = *value;
+	_value = ((_value & 0xFF) << 24) |
+			 ((_value & 0xFF00) << 8) |
+			 ((_value >> 8) & 0xFF00) |
+			 ((_value >> 24) & 0xFF);
+	*value = _value;
+}
+
+void little_to_big_16(uint16_t *value)
+{
+	if (value == NULL)
+	{
+		return;
+	}
+	uint16_t _value = *value;
+	_value = ((_value & 0xFF) << 8) |
+			 ((_value >> 8) & 0xFF);
+	*value = _value;
+}
+
+uint32_t millis()
+{
+	return HAL_GetTick();
+}

+ 293 - 0
user_src/user_rid.c

@@ -0,0 +1,293 @@
+#include "user_rid.h"
+#include "dronecan.h"
+#include <dronecan.remoteid.Location.h>
+#include <dronecan.remoteid.ArmStatus.h>
+#include <dronecan.remoteid.BasicID.h>
+#include <uavcan.protocol.NodeStatus.h>
+#include <dronecan.remoteid.OperatorID.h>
+#include <dronecan.remoteid.SelfID.h>
+#include <dronecan.remoteid.System.h>
+#include "string.h"
+
+Connect_check remoteid_link;
+_armStatusID_msg StatusId;
+_location_msg locationId;
+_operatorId_msg operatorId;
+_basicId_msg basicId;
+_selfId_msg selfId;
+_sys_operator_msg systemId;
+
+void send_location_msg( void )
+{
+    static uint8_t transfer_id = 0;
+
+    struct dronecan_remoteid_Location msg;
+
+    uint8_t buffer[DRONECAN_REMOTEID_LOCATION_MAX_SIZE] = {0};
+
+
+    msg.id_or_mac.len = 20;
+    memcpy(&msg.id_or_mac.data, &locationId.id_or_mac, msg.id_or_mac.len);
+    msg.status = locationId.status;
+    msg.direction = locationId.direction;
+    msg.speed_horizontal = locationId.speed_horizontal;
+    msg.speed_vertical = locationId.speed_vertical;
+    msg.latitude = locationId.latitude;
+    msg.longitude = locationId.longitude;
+    msg.altitude_barometric = locationId.altitude_barometric;
+    msg.altitude_geodetic = locationId.altitude_geodetic;
+    msg.height_reference = locationId.height_reference;
+    msg.height = locationId.height;
+    msg.horizontal_accuracy = locationId.horizontal_accuracy;
+    msg.vertical_accuracy = locationId.vertical_accuracy;
+    msg.barometer_accuracy = locationId.barometer_accuracy;
+    msg.speed_accuracy = locationId.speed_accuracy;
+    msg.timestamp = locationId.timestamp;
+    msg.timestamp_accuracy = locationId.timestamp_accuracy;
+
+    uint32_t len = dronecan_remoteid_Location_encode(&msg, buffer);
+
+    CanardTxTransfer tx_transfer = {
+        .data_type_signature = DRONECAN_REMOTEID_LOCATION_SIGNATURE,
+        .data_type_id = DRONECAN_REMOTEID_LOCATION_ID,
+        .inout_transfer_id = &transfer_id,
+        .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+        .transfer_type = CanardTransferTypeBroadcast,
+        .payload = buffer,
+        .payload_len = len,
+    };
+
+    dronecan_broadcast(&tx_transfer);
+}
+
+void send_basicID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_BasicID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_BASICID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &basicId.id_or_mac, msg.id_or_mac.len);
+  msg.id_type = basicId.id_type;
+  msg.ua_type = basicId.ua_type;
+  msg.uas_id.len = 20;
+  memcpy(&msg.uas_id.data, &basicId.uas_id, msg.uas_id.len);
+
+  uint32_t len = dronecan_remoteid_BasicID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_BASICID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_BASICID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+void send_operatorID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_OperatorID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_OPERATORID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &operatorId.id_or_mac, msg.id_or_mac.len);
+  msg.operator_id_type = operatorId.operator_id_type;
+  msg.operator_id.len = 20;
+  memcpy(&msg.operator_id.data, &operatorId.operator_id, msg.operator_id.len);
+
+  uint32_t len = dronecan_remoteid_OperatorID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_OPERATORID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_OPERATORID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+
+void send_sysOperatorID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_System msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_SYSTEM_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &systemId.id_or_mac, msg.id_or_mac.len);
+  msg.operator_location_type = systemId.operator_location_type;
+  msg.classification_type = systemId.classification_type;
+  msg.operator_latitude = systemId.operator_latitude;
+  msg.operator_longitude = systemId.operator_longitude;
+  msg.area_count = systemId.area_count;
+  msg.area_radius = systemId.area_radius;
+  msg.area_ceiling = systemId.area_ceiling;
+  msg.area_floor = systemId.area_floor;
+  msg.category_eu = systemId.category_eu;
+  msg.class_eu = systemId.class_eu;
+  msg.operator_altitude_geo = systemId.operator_altitude_geo;
+  msg.timestamp = systemId.timestamp;
+
+  uint32_t len = dronecan_remoteid_System_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_SYSTEM_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_SYSTEM_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+void send_selfID(void) 
+{
+  static uint8_t transfer_id = 0;
+
+  struct dronecan_remoteid_SelfID msg;
+
+  uint8_t buffer[DRONECAN_REMOTEID_SELFID_MAX_SIZE] = {0};
+
+  msg.id_or_mac.len = 20;
+  memcpy(&msg.id_or_mac.data, &selfId.id_or_mac, msg.id_or_mac.len);
+  msg.description_type = selfId.description_type;
+  msg.description.len = 23;
+  memcpy(&msg.description.data, &selfId.description, msg.description.len);
+
+  uint32_t len = dronecan_remoteid_SelfID_encode(&msg, buffer);
+
+  CanardTxTransfer tx_transfer = {
+      .data_type_signature = DRONECAN_REMOTEID_SELFID_SIGNATURE,
+      .data_type_id = DRONECAN_REMOTEID_SELFID_ID,
+      .inout_transfer_id = &transfer_id,
+      .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
+      .transfer_type = CanardTransferTypeBroadcast,
+      .payload = buffer,
+      .payload_len = len,
+  };
+
+  dronecan_broadcast(&tx_transfer);
+}
+
+
+int remoteid_OnRecieved(CanardInstance *canardIns,
+                                CanardRxTransfer *transfer) 
+{
+  int ret = 0;
+  struct dronecan_remoteid_ArmStatus info;
+  static int TimeDelay = 0;
+  if (transfer->transfer_type == CanardTransferTypeBroadcast) 
+  {
+    if( transfer->data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID)
+    {
+        if (!dronecan_remoteid_ArmStatus_decode(transfer, &info)) {
+            StatusId.status = info.status;
+            if( StatusId.status != 0 && HAL_GetTick() - TimeDelay > 2500)
+            {
+              memcpy(&StatusId.status, &info.status, sizeof(struct dronecan_remoteid_ArmStatus));
+            }
+            else
+            {
+              StatusId.status = 0;
+              TimeDelay = HAL_GetTick();
+              memset(&StatusId.len,0,51);
+            }
+            StatusId.len = 50;
+            remoteid_link.connect_status = COMP_NORMAL;
+        }
+    }
+  }
+
+  return ret;
+}
+
+bool remoteid_shouldAcceptTransfer(const CanardInstance *ins,
+                                           uint64_t *out_data_type_signature,
+                                           uint16_t data_type_id,
+                                           CanardTransferType transfer_type,
+                                           uint8_t source_node_id) {
+
+  bool ret = false;
+    
+  if (data_type_id == DRONECAN_REMOTEID_ARMSTATUS_ID &&
+      transfer_type == CanardTransferTypeBroadcast) {
+    *out_data_type_signature =  DRONECAN_REMOTEID_ARMSTATUS_SIGNATURE;
+    ret = true;
+  } 
+  if (data_type_id == UAVCAN_PROTOCOL_NODESTATUS_ID &&
+        transfer_type == CanardTransferTypeBroadcast) {
+    *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE;
+    ret = true;
+  } 
+
+  return ret;
+}
+
+
+
+void send_msg_to_remoteid(void)
+{
+    static uint32_t SendTime_N2HZ = 0;
+    static uint32_t SendTime_N3HZ = 0;
+    static uint32_t SendTime_N4HZ = 0;
+    static uint32_t SendTime_N5HZ = 0;
+    static uint32_t SendTime_N6HZ = 0;
+
+    if ( HAL_GetTick() - SendTime_N2HZ > 555 )
+    {
+        //send_node_status();
+        send_location_msg();
+        
+        SendTime_N2HZ = HAL_GetTick();
+    }
+    if ( HAL_GetTick() - SendTime_N3HZ > 666 )
+    {
+        send_sysOperatorID();
+        SendTime_N3HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N4HZ > 333 )
+    {
+        send_basicID();
+        SendTime_N4HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N6HZ > 1777 )
+    {
+        send_operatorID();
+        SendTime_N6HZ = HAL_GetTick();
+    }
+
+    if ( HAL_GetTick() - SendTime_N5HZ > 1888 )
+    {
+        send_selfID();
+        SendTime_N5HZ = HAL_GetTick();
+    }
+
+    
+
+    dronecan_tx_processing();
+}
+
+