V9_PMU302/uav_can/user_src/dronecan.c

#include "dronecan.h"
#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};
CanardInstance canard_ins;

bool dronecan_lock_status = false;
void dronecan_lock() {
  dronecan_lock_status = 1;
}
void dronecan_unlock() {
  dronecan_lock_status = 0;
}


int dronecan_request_or_respond(uint8_t dest_id,
                                CanardTxTransfer *tx_transfer) {
  int ret = 0;

  if (dronecan_lock_status == UNLOCK) {
    dronecan_lock();
    ret = canardRequestOrRespondObj(&canard_ins, dest_id, tx_transfer);
    dronecan_unlock();
  }

  return ret;
}


static void onTransferReceived(CanardInstance *ins,
                               CanardRxTransfer *transfer) {

  // if (flyfire_onRecieved(ins, transfer)) {
  //   return;
  // } else if (hbwEscOnRecieved(ins, transfer)) {
  //   return;
  // } else if (sinemotorEscOnRecieved(ins, transfer)) {
  //   return;
  // } else if (tmotorEscOnRecieved(ins, transfer)) {
  //   return;
  // } else if (uavcanEscOnRecieved(ins, transfer)) {
  //   return;
  // } else 
  if (vkWeigher_Vls300_OnRecieved(ins, transfer)) {
    return;
  }
  if (remoteid_OnRecieved(ins, transfer)) {
    return;
  }
  
}

static bool 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 (flyfireShouldAcceptTransfer(ins, out_data_type_signature, data_type_id,
  //                                 transfer_type, source_node_id)) {
  //   return true;
  // }

  // /* 好盈电调相关消息 */
  // if (hbwEscShouldAcceptTransfer(ins, out_data_type_signature, data_type_id,
  //                                transfer_type, source_node_id)) {
  //   return true;
  // }

  // /* 弦动电调相关消息 */
  // if (sinemotorEscShouldAcceptTransfer(ins, out_data_type_signature,
  //                                      data_type_id, transfer_type,
  //                                      source_node_id)) {
  //   return true;
  // }

  // /* tmotor电调相关消息 */
  // if (tmotorEscShouldAcceptTransfer(ins, out_data_type_signature, data_type_id,
  //                                   transfer_type, source_node_id)) {
  //   return true;
  // }

  // /* uavcan电调相关消息 */
  // if (uavcanEscShouldAcceptTransfer(ins, out_data_type_signature, data_type_id,
  //                                   transfer_type, source_node_id)) {
  //   return true;
  // }

  /* dronecan weigher 称重器相关消息 */
  if (vkWeigher_Vls300_shouldAcceptTransfer(ins, out_data_type_signature,
                                            data_type_id, transfer_type,
                                            source_node_id)) {
    ret = true;
  }

  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) {
//   static uint8_t transfer_id = 0;

//   struct uavcan_protocol_NodeStatus msg;

//   uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE] = {0};

//   msg.uptime_sec = systime_now_ms() / 1000;
//   msg.sub_mode = 0;
//   msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
//   msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;

//   uint32_t len = uavcan_protocol_NodeStatus_encode(&msg, buffer);

//   CanardTxTransfer tx_transfer = {
//       .data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
//       .data_type_id = UAVCAN_PROTOCOL_NODESTATUS_ID,
//       .inout_transfer_id = &transfer_id,
//       .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
//       .transfer_type = CanardTransferTypeBroadcast,
//       .payload = buffer,
//       .payload_len = len,
//       .iface_mask = 0xff, /* 发送到所有接口 */
//   };

//   dronecan_broadcast(dcan, &tx_transfer);
// }

void dronecan_tx_processing(void) {

  struct dronecan *dcan = &__dronecan;

  // Transmit the frames in the queue.
  while (1) {
    CanardCANFrame tx_frame = {0};

    CanardCANFrame *tx_frame_p = NULL;
    if( dronecan_lock_status == UNLOCK )
    {
      dronecan_lock();
      tx_frame_p = canardPeekTxQueue(&canard_ins);
      if (tx_frame_p == NULL) 
      {
        /* 没有发送消息, 结束发送 */
        dronecan_unlock();
        break;
      }
    }
    else
    {
      break;
    }
    
    /* 将消息拷贝出来 */
    memcpy(&tx_frame, tx_frame_p, sizeof(tx_frame));
    canardPopTxQueue(&canard_ins);
    dronecan_unlock(dcan);
    /* 通过 can 接口发送消息 */ 

    can_send_msg_normal(tx_frame.data, tx_frame.data_len, tx_frame.id);

  }

  static int clear_time = 0;
  /* 每秒清理一次 canard 过期消息 */
  if( HAL_GetTick() - clear_time > 1000 )
  {
    if( dronecan_lock_status == UNLOCK )
    {
      clear_time = HAL_GetTick();
      dronecan_lock();
      canardCleanupStaleTransfers(&canard_ins, Get_Systimer_Us());
      dronecan_unlock();
    }
  }
}

/**
 * @brief dronecan 回调函数, 当 can 接口接收到消息时调用
 *
 * @param msg can 消息
 * @param iface_id can 接口 id, 用于管理多个不同 can 接口
 * @return int 1-是 dronecan 消息, 0-不是 dronecan 消息
 */
int dronecan_rx_callback(CAN_RxHeaderTypeDef Rxhead,uint8_t data[]) {
  int ret = 0;

  CanardCANFrame rx_frame = {0};
  rx_frame.id = Rxhead.ExtId;
  if (Rxhead.IDE == CAN_ID_EXT) {
    rx_frame.id |= CANARD_CAN_FRAME_EFF;
  }
  if (Rxhead.RTR == CAN_RTR_REMOTE) {
    rx_frame.id |= CANARD_CAN_FRAME_RTR;
  }
  rx_frame.data_len = Rxhead.DLC;

  memcpy(rx_frame.data, &data[0], Rxhead.DLC);
  uint64_t timestamp = Get_Systimer_Us();

  int16_t res;
  if(dronecan_lock_status == UNLOCK)
  {
    dronecan_lock();
    res = canardHandleRxFrame(&canard_ins, &rx_frame, timestamp);
    dronecan_unlock();
  }

  if (res == CANARD_OK) {
    ret = 1;
  }

  return ret;
}

// static void timer_out_cb(void *arg) {
//   if (arg) {
//     struct dronecan *dcan = arg;
//     send_node_status(dcan);
//   }
// }


void dronecan_init(void) {

  struct dronecan *dcan = &__dronecan;
  canardInit(&canard_ins, &memory_pool,
             DRONECAN_CANARD_MEMORY_POOL_SIZE, onTransferReceived,
             shouldAcceptTransfer, dcan);
  canardSetLocalNodeID(&canard_ins,
                       DRONECAN_CANARD_DEFAULT_LOCAL_NODE_ID);

}

/**
 * @brief dronecan 发送广播消息
 *
 * @param dcan dronecan 对象
 * @param tx_transfer 发送消息对象
 * @return int 发送到 canard 发送队列的消息计数
 */
int dronecan_broadcast(CanardTxTransfer *tx_transfer) {
  int ret = 0;

  if (dronecan_lock_status == UNLOCK) {
    dronecan_lock();
    ret = canardBroadcastObj(&canard_ins, tx_transfer);
    dronecan_unlock();
  }

  return ret;
}