修复can方式remoteid功能。

uav_can/user_inc/dronecan.h

@@ -21,6 +21,8 @@ extern "C" {
 #define DRONECAN_CANARD_IFACE2_MASK (1 << 1)
 #define DRONECAN_CANARD_IFACE_MAX_NUM 2
 
+#define REMOTEID_NODEID 100
+
 /** The dronecan instance. */
 struct dronecan {
   /* canard 对象 */

uav_can/user_inc/user_rid.h

@@ -24,7 +24,7 @@ bool remoteid_shouldAcceptTransfer(const CanardInstance *ins,
                                            uint16_t data_type_id,
                                            CanardTransferType transfer_type,
                                            uint8_t source_node_id);
-int remoteid_OnRecieved(CanardInstance *canardIns,
+extern int remoteid_OnRecieved(CanardInstance *canardIns,
                                 CanardRxTransfer *transfer);
 extern Connect_check remoteid_link;
 //remoteid

uav_can/user_src/dronecan.c

@@ -2,6 +2,10 @@
 #include "drv_vkweigher_vls300.h"
 #include <string.h>
 #include "soft_timer.h"
+#include <dronecan.remoteid.ArmStatus.h>
+#include <uavcan.protocol.NodeStatus.h>
+#include "user_rid.h"
+
 static struct dronecan __dronecan = {0};
 
 uint8_t memory_pool[DRONECAN_CANARD_MEMORY_POOL_SIZE] = {0};
@@ -47,6 +51,10 @@ static void onTransferReceived(CanardInstance *ins,
   if (vkWeigher_Vls300_OnRecieved(ins, transfer)) {
     return;
   }
+  if (remoteid_OnRecieved(ins, transfer)) {
+    return;
+  }
+  
 }
 
 static bool shouldAcceptTransfer(const CanardInstance *ins,
@@ -55,6 +63,7 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
                                  CanardTransferType transfer_type,
                                  uint8_t source_node_id) {
 
+  bool ret = false;
   // /* 火萤降落伞相关消息 */
   // if (flyfireShouldAcceptTransfer(ins, out_data_type_signature, data_type_id,
   //                                 transfer_type, source_node_id)) {
@@ -90,10 +99,22 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
   if (vkWeigher_Vls300_shouldAcceptTransfer(ins, out_data_type_signature,
                                             data_type_id, transfer_type,
                                             source_node_id)) {
-    return true;
+    ret = true;
   }
 
-  return 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;
 }
 
 // static void send_node_status(struct dronecan *dcan) {

user_src/user_rid.c

@@ -1,293 +0,0 @@
-#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();
-}
-
-