V9_PMU302/uav_can/user_src/user_rid.c

#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();
}