V9_PMU302/uav_can/user_src/drv_vkweigher_vls300.c

#include "drv_vkweigher_vls300.h"
#include "dronecan.h"
#include "sys/time.h"
#include <fcntl.h>
#include <stdint.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include "vkweigher.Version_res.h"
#include "vkweigher.WeigherStatus.h"
#include "vkweigher.WeigherCalib_req.h"
#include "vkweigher.WeigherCalib_res.h"
#include "vkweigher.FuseBreak_req.h"
#include "vkweigher.FuseBreak_res.h"
#include "vkweigher.Version_req.h"
#include "soft_timer.h"
#include "string.h"
#include "soft_seed_device.h"

struct vk_weigher_vls300 _vk_vls300;

// static rt_size_t read_data(rt_device_t dev, weigher_data_t *data) {

//   struct weigher_device *weigher =
//       rt_container_of(dev, struct weigher_device, parent);

//   struct vk_weigher_vls300 *vls300 =
//       rt_container_of(weigher, struct vk_weigher_vls300, weigher_dev);

//   rt_size_t size = 0;

//   rt_enter_critical();
//   if (vls300->status_data.timestamp > 0 && vls300->data_got == RT_TRUE) {
//     vls300->data_got = RT_FALSE;
//     data->timestamp_ms = vls300->status_data.timestamp;
//     data->weight = vls300->status_data.weight;
//     data->weight_dot = vls300->status_data.weight_dot;
//     data->ucode[0] = vls300->status_data.status;
//     data->ucode[1] = vls300->status_data.status >> 8;

//     for (int i = 0; i < 3; ++i) {
//       data->acce[i] = vls300->status_data.acce[i];
//       data->gyro[i] = vls300->status_data.gyro[i];
//     }

//     for (int i = 0; i < 4; ++i) {
//       data->Q[i] = vls300->status_data.Q[i];
//     }
//     size = sizeof(weigher_data_t);
//   }
//   rt_exit_critical();

//   return size;
// }

// static rt_ssize_t read_version(rt_device_t dev, weigher_version_t *version) {
//   RT_ASSERT(dev != RT_NULL);
//   RT_ASSERT(version != RT_NULL);

//   struct weigher_device *weigher =
//       rt_container_of(dev, struct weigher_device, parent);

//   struct vk_weigher_vls300 *vls300 =
//       rt_container_of(weigher, struct vk_weigher_vls300, weigher_dev);

//   rt_ssize_t size = 0;

//   rt_enter_critical();
//   if (vls300->version.timestamp > 0) {
//     version->timestamp_ms = vls300->version.timestamp;
//     rt_snprintf(version->sn_num, sizeof(version->sn_num), "%u",
//                 vls300->version.SN_num);
//     rt_strncpy(version->fw_ver, (char *)vls300->version.fw_ver,
//                sizeof(version->fw_ver));
//     rt_strncpy(version->hw_ver, "VLS300", sizeof(version->hw_ver));
//     rt_strncpy(version->manufactory, "VKFLY", sizeof(version->manufactory));
//     size = sizeof(weigher_version_t);
//   }
//   rt_exit_critical();

//   return size;
// }

// uint8_t vklift_weight_calibrate(uint32_t weight) 
// {
//   static uint8_t transfer_id = 0;

//   struct vkweigher_WeigherCalibRequest msg = {0};
//   msg.weight = weight;

//   uint8_t buffer[VKWEIGHER_WEIGHERCALIB_REQUEST_MAX_SIZE] = {0};
//   memcpy(buffer,&msg.weight,sizeof(msg.weight));
//   uint8_t len = vkweigher_WeigherCalibRequest_encode(&msg, buffer);
//   CanardTxTransfer tranxfer = {
//       .inout_transfer_id = &transfer_id,
//       .transfer_type = CanardTransferTypeRequest,
//       .data_type_id = VKWEIGHER_WEIGHERCALIB_REQUEST_ID,
//       .data_type_signature = VKWEIGHER_WEIGHERCALIB_REQUEST_SIGNATURE,
//       .priority = CANARD_TRANSFER_PRIORITY_LOW,
//       .payload = buffer,
//       .payload_len = len,
//   };

//    dronecan_request_or_respond(DRONECAN_WEIGHER_NODE_ID, &tranxfer);

//   return 1;
// }

// static rt_err_t fuse_prot(rt_device_t dev, void *args) {

//   RT_ASSERT(dev != RT_NULL);

//   struct weigher_device *weigher =
//       rt_container_of(dev, struct weigher_device, parent);

//   struct vk_weigher_vls300 *vls300 =
//       rt_container_of(weigher, struct vk_weigher_vls300, weigher_dev);

//   static uint8_t transfer_id = 0;

//   struct vkweigher_FuseBreakRequest msg = {0};
//   msg.break_cmd = *(uint8_t *)args;

//   uint8_t buffer[VKWEIGHER_FUSEBREAK_REQUEST_MAX_SIZE] = {0};
//   uint8_t len = vkweigher_FuseBreakRequest_encode(&msg, buffer);

//   if (vls300->canard != RT_NULL) {
//     CanardTxTransfer tranxfer = {
//         .inout_transfer_id = &transfer_id,
//         .transfer_type = CanardTransferTypeRequest,
//         .data_type_id = VKWEIGHER_FUSEBREAK_REQUEST_ID,
//         .data_type_signature = VKWEIGHER_FUSEBREAK_REQUEST_SIGNATURE,
//         .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
//         .payload = buffer,
//         .payload_len = len,
//         .iface_mask = vls300->iface_mask,
//     };
//     struct dronecan *dcan = (struct dronecan *)vls300->canard->user_reference;
//     dronecan_request_or_respond(DRONECAN_WEIGHER_NODE_ID, &tranxfer);
//   }

//   return RT_EOK;
// }

// static rt_err_t factory_reset(rt_device_t dev, void *args) { return RT_EOK; }

// static rt_err_t set_sn(rt_device_t dev, uint32_t SN) {
//   RT_ASSERT(dev != RT_NULL);
//   RT_ASSERT(data != RT_NULL);

//   static uint8_t transfer_id = 0;

//   struct weigher_device *weigher =
//       rt_container_of(dev, struct weigher_device, parent);

//   struct vk_weigher_vls300 *vls300 =
//       rt_container_of(weigher, struct vk_weigher_vls300, weigher_dev);

//   struct vkweigher_VersionRequest msg = {0};
//   msg.cmd = 1;
//   msg.SN_num = SN;

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

//   uint8_t len = vkweigher_VersionRequest_encode(&msg, buffer);

//   CanardTxTransfer tranxfer = {
//       .inout_transfer_id = &transfer_id,
//       .transfer_type = CanardTransferTypeRequest,
//       .data_type_id = VKWEIGHER_VERSION_REQUEST_ID,
//       .data_type_signature = VKWEIGHER_VERSION_REQUEST_SIGNATURE,
//       .priority = CANARD_TRANSFER_PRIORITY_LOW,
//       .payload = buffer,
//       .payload_len = len,
//       .iface_mask = vls300->iface_mask,
//   };
//   struct dronecan *dcan = (struct dronecan *)vls300->canard->user_reference;
//   dronecan_request_or_respond(DRONECAN_WEIGHER_NODE_ID, &tranxfer);

//   return RT_EOK;
// }
static void req_version(struct vk_weigher_vls300 *vls300) {

  static uint8_t transfer_id = 0;
  if (vls300->canard != NULL) {
    struct vkweigher_VersionRequest msg = {0};
    msg.cmd = 0;

    uint8_t buffer[VKWEIGHER_VERSION_REQUEST_MAX_SIZE] = {0};

    uint8_t len = vkweigher_VersionRequest_encode(&msg, buffer);

    CanardTxTransfer tranxfer = {
        .inout_transfer_id = &transfer_id,
        .transfer_type = CanardTransferTypeRequest,
        .data_type_id = VKWEIGHER_VERSION_REQUEST_ID,
        .data_type_signature = VKWEIGHER_VERSION_REQUEST_SIGNATURE,
        .priority = CANARD_TRANSFER_PRIORITY_LOW,
        .payload = buffer,
        .payload_len = len,
    };
    dronecan_request_or_respond(DRONECAN_WEIGHER_NODE_ID, &tranxfer);
  }
}


// static void handle_fw_update(CanardRxTransfer *transfer) {
//   RT_ASSERT(transfer != RT_NULL);
//   if (transfer->transfer_type == CanardTransferTypeRequest) {
//     switch (transfer->data_type_id) {
//     case UAVCAN_PROTOCOL_FILE_GETINFO_REQUEST_ID: {
//       /* 处理固件信息请求 */
//       struct uavcan_protocol_file_GetInfoRequest req = {0};
//       if (!uavcan_protocol_file_GetInfoRequest_decode(transfer, &req)) {
//         if (rt_strncmp((char *)req.path.path.data, "VLS300.bin",
//                        req.path.path.len) == 0) {
//           rt_event_send(_vk_vls300->_event,
//                         VLS300_EVENT_GOT_FW_UPDATE_INFO_ACK);
//         }
//       }
//     } break;
//     case UAVCAN_PROTOCOL_FILE_READ_REQUEST_ID: {
//       /* 处理固件数据读取请求 */
//       rt_event_send(_vk_vls300->_event, VLS300_EVENT_GOT_FW_UPDATE_DATA_ACK);
//     } break;
//     default:
//       break;
//     }
//   } else if (transfer->transfer_type == CanardTransferTypeResponse) {
//     switch (transfer->data_type_id) {
//     case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ID: {
//       rt_event_send(_vk_vls300->_event, VLS300_EVENT_GOT_FW_UPDATE_START_ACK);
//     } break;
//     default:
//       break;
//     }
//   }
// };

int vkWeigher_Vls300_OnRecieved(CanardInstance *canardIns,
                                CanardRxTransfer *transfer) {

  int ret = 0;

  if (transfer->transfer_type == CanardTransferTypeBroadcast) {
    /* weigher 广播帧 */
    switch (transfer->data_type_id) {

    case VKWEIGHER_WEIGHERSTATUS_ID: {
      struct vkweigher_WeigherStatus status = {0};
      if (!vkweigher_WeigherStatus_decode(transfer, &status)) {

          Dev.Lift_Weight_Link.connect_status = COMP_NORMAL;
          Dev.Lift_Weight_Link.recv_time = HAL_GetTick();
          Dev.Lift_Weight.facid = FAC_VK;

          _vk_vls300.data_got = true;
          _vk_vls300.status_data.timestamp = Get_Systimer_Us();
          _vk_vls300.status_data.weight = status.weight;
          _vk_vls300.status_data.weight_dot = status.rate;
          _vk_vls300.status_data.status = status.status;
          _vk_vls300.status_data.acce[0] = status.ax;
          _vk_vls300.status_data.acce[1] = status.ay;
          _vk_vls300.status_data.acce[2] = status.az;
          _vk_vls300.status_data.gyro[0] = status.gx;
          _vk_vls300.status_data.gyro[1] = status.gy;
          _vk_vls300.status_data.gyro[2] = status.gz;

          for (int i = 0; i < 4; i++) {
            _vk_vls300.status_data.Q[i] = status.Q[i];
          }


        /* 若未收到版本信息, 则主动请求一次版本信息 */
        if (_vk_vls300.version.timestamp == 0) {
          req_version(&_vk_vls300);
        }
      }
      ret = 1;
    } break;

    default:
      break;
    }
  } else if (transfer->transfer_type == CanardTransferTypeResponse) {
    /* weigher 应答帧 */
    switch (transfer->data_type_id) {

    case VKWEIGHER_VERSION_RESPONSE_ID: {
      struct vkweigher_VersionResponse msg = {0};
      if (!vkweigher_VersionResponse_decode(transfer, &msg)) {
        /* 收到 version 消息回令 */
          _vk_vls300.version.timestamp = Get_Systimer_Us();
          memcpy(_vk_vls300.version.fw_ver, msg.fw_ver, sizeof(msg.fw_ver));
          _vk_vls300.version.SN_num = msg.SN_num;

      }
      ret = 1;
    } break;

    case VKWEIGHER_FUSEBREAK_RESPONSE_ID: {
      struct vkweigher_FuseBreakResponse msg = {0};
      if (!vkweigher_FuseBreakResponse_decode(transfer, &msg)) {
        /* 收到 fuse break 消息回令 */
      }
      ret = 1;
    } break;

    case VKWEIGHER_WEIGHERCALIB_RESPONSE_ID: {
      struct vkweigher_WeigherCalibResponse msg = {0};
      if (!vkweigher_WeigherCalibResponse_decode(transfer, &msg)) {
        /* 收到 calibrate 消息回令 */
      }
      ret = 1;
    } break;

    // case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ID:
    // case UAVCAN_PROTOCOL_FILE_GETINFO_REQUEST_ID:
    // case UAVCAN_PROTOCOL_FILE_READ_REQUEST_ID:
    //   handle_fw_update(transfer);
    //   break;

    default:
      break;
    }
  }

  return ret;
}

bool vkWeigher_Vls300_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 == VKWEIGHER_WEIGHERSTATUS_ID &&
      transfer_type == CanardTransferTypeBroadcast) {
    *out_data_type_signature = VKWEIGHER_WEIGHERSTATUS_SIGNATURE;
    ret = true;
  } else if (data_type_id == VKWEIGHER_VERSION_RESPONSE_ID &&
             transfer_type == CanardTransferTypeResponse) {
    *out_data_type_signature = VKWEIGHER_VERSION_RESPONSE_SIGNATURE;
    ret = true;
  } else if (data_type_id == VKWEIGHER_FUSEBREAK_RESPONSE_ID &&
             transfer_type == CanardTransferTypeResponse) {
    *out_data_type_signature = VKWEIGHER_FUSEBREAK_RESPONSE_SIGNATURE;
    ret = true;
  } else if (data_type_id == VKWEIGHER_WEIGHERCALIB_RESPONSE_ID &&
             transfer_type == CanardTransferTypeResponse) {
    *out_data_type_signature = VKWEIGHER_WEIGHERCALIB_RESPONSE_SIGNATURE;
    ret = true;
  }
  // } else if (data_type_id ==
  //                UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ID &&
  //            (transfer_type == CanardTransferTypeResponse)) {
  //   *out_data_type_signature =
  //       UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_SIGNATURE;
  //   ret = true;
  // } else if (data_type_id == UAVCAN_PROTOCOL_FILE_GETINFO_REQUEST_ID &&
  //            transfer_type == CanardTransferTypeRequest) {
  //   *out_data_type_signature = UAVCAN_PROTOCOL_FILE_GETINFO_REQUEST_SIGNATURE;
  //   ret = true;
  // } else if (data_type_id == UAVCAN_PROTOCOL_FILE_READ_REQUEST_ID &&
  //            transfer_type == CanardTransferTypeRequest) {
  //   *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_REQUEST_SIGNATURE;
  //   ret = true;
  // }

  return ret;
}

// static void fw_update_start(struct vk_weigher_vls300 *vls300,
//                             CanardInstance *canardIns) {
//   /* 发送升级开始 */
//   struct uavcan_protocol_file_BeginFirmwareUpdateRequest req = {0};
//   uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_REQUEST_MAX_SIZE] = {
//       0};
//   static uint8_t transfer_id = 0;

//   req.source_node_id = DRONECAN_FMU_NODE_ID;
//   req.image_file_remote_path.path.len = rt_strlen("VLS300.bin");
//   rt_memcpy((char *)req.image_file_remote_path.path.data, "VLS300.bin",
//             req.image_file_remote_path.path.len);

//   uint32_t len =
//       uavcan_protocol_file_BeginFirmwareUpdateRequest_encode(&req, buffer);

//   CanardTxTransfer tx_transfer = {
//       .data_type_signature =
//           UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_REQUEST_SIGNATURE,
//       .data_type_id = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_REQUEST_ID,
//       .inout_transfer_id = &transfer_id,
//       .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
//       .transfer_type = CanardTransferTypeRequest,
//       .payload = buffer,
//       .payload_len = len,
//   };
//   struct dronecan *dcan = (struct dronecan *)canardIns->user_reference;
//   dronecan_request_or_respond(DRONECAN_FMU_NODE_ID, &tx_transfer);
// }

// static void fw_update_send_info(struct vk_weigher_vls300 *vls300,
//                                 uint32_t fw_size, CanardInstance *canardIns) {
//   /* 发送升级信息 */
//   struct uavcan_protocol_file_GetInfoResponse res = {0};
//   static uint8_t transfer_id = 0;
//   uint8_t buffer[UAVCAN_PROTOCOL_FILE_GETINFO_RESPONSE_MAX_SIZE] = {0};

//   res.size = fw_size;
//   res.error.value = UAVCAN_PROTOCOL_FILE_ERROR_OK;

//   uint32_t len = uavcan_protocol_file_GetInfoResponse_encode(&res, buffer);
//   CanardTxTransfer tx_transfer = {
//       .data_type_signature = UAVCAN_PROTOCOL_FILE_GETINFO_RESPONSE_SIGNATURE,
//       .data_type_id = UAVCAN_PROTOCOL_FILE_GETINFO_RESPONSE_ID,
//       .inout_transfer_id = &transfer_id,
//       .priority = CANARD_TRANSFER_PRIORITY_MEDIUM,
//       .transfer_type = CanardTransferTypeResponse,
//       .payload = buffer,
//       .payload_len = len,
//   };
//   struct dronecan *dcan = (struct dronecan *)canardIns->user_reference;
//   dronecan_request_or_respond(DRONECAN_FMU_NODE_ID, &tx_transfer);
// }

// static void fw_update_data(struct vk_weigher_vls300 *vls300, uint16_t pack_num,
//                            const uint8_t data[128 + 2]) {
//   //   send_data_to_vkgps(vls300, _MSGID_MCTOGPS_UPDATE_DATA, data, 128 + 4);
// }

// static int do_fw_update(struct vk_weigher_vls300 *vls300,
//                         const char *fw_file_name, CanardInstance *canardIns,
//                         CanardRxTransfer *transfer) {
//   RT_ASSERT(vls300 != RT_NULL);
//   RT_ASSERT(fw_file_name != RT_NULL);
//   const uint32_t PACK_BYTES_LEN = 128;
//   uint8_t buff[PACK_BYTES_LEN + 2];
//   /* 获取fd文件的大小 */
//   struct stat stat;
//   uint32_t fw_byte_size = 0;
//   int fd = open(fw_file_name, O_RDONLY);

//   if (fd < 0) {
//     LOG_E("open file %s failed!", fw_file_name);
//     return -1;
//   }

//   if (fstat(fd, &stat) == 0) {
//     fw_byte_size = stat.st_size;
//   } else {
//     /* 固件文件描述符异常 */
//     close(fd);
//     return -1;
//   }

//   uint8_t retry_cnt = 0;
//   uint32_t event;
//   uint16_t send_pack_num = 0;
//   off_t fd_offset = 0;

//   /* 发送升级开始 */
//   do {
//     LOG_D("send fw update sta.");
//     fw_update_start(vls300, vls300->canard);
//     rt_err_t ret =
//         rt_event_recv(vls300->_event, VLS300_EVENT_GOT_FW_UPDATE_START_ACK,
//                       RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 1000, &event);
//     /* 等待升级开始回令 */
//     if (RT_EOK == ret && (event & VLS300_EVENT_GOT_FW_UPDATE_START_ACK)) {
//       /* 收到升级开始回令 */
//       LOG_D("got update sta ack.");
//       rt_thread_mdelay(200);
//       send_pack_num = 1;
//       break;

//     } else {
//       retry_cnt++;
//       /* 升级开始失败 */
//       rt_thread_mdelay(200);
//       if (retry_cnt >= 5) {
//         LOG_E("update sta retry failed!");
//         close(fd);
//         return -1;
//       }
//     }
//   } while (1);

//   /* 发送升级数据信息 */
//   do {
//     fw_update_send_info(vls300, fw_byte_size, vls300->canard);
//     rt_err_t ret =
//         rt_event_recv(vls300->_event, VLS300_EVENT_GOT_FW_UPDATE_INFO_ACK,
//                       RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 500, &event);

//     if (ret == RT_EOK && (event & VLS300_EVENT_GOT_FW_UPDATE_INFO_ACK)) {
//       /* 收到回令, 升级结束 */
//       LOG_D("got update end ack.");
//       break;
//     } else {
//       /* 超时次未有回令, 升级结束失败 */
//       retry_cnt++;
//       if (retry_cnt >= 5) {
//         LOG_E("gps_id %d: update end ack retry failed!");
//         close(fd);
//         return -4;
//       }
//     };
//   } while (1);

//   /* 发送升级数据 */
//   do {
//     lseek(fd, fd_offset, SEEK_SET);
//     rt_memcpy(buff, &send_pack_num, 2);
//     uint16_t read_len = read(fd, buff + 2, PACK_BYTES_LEN);
//     LOG_D("send fw update data, pack number %d.", send_pack_num);
//     fw_update_data(vls300, send_pack_num, buff);
//     vls300->_fw_update_send_pack_num = send_pack_num;
//     rt_err_t ret =
//         rt_event_recv(vls300->_event, VLS300_EVENT_GOT_FW_UPDATE_DATA_ACK,
//                       RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 500, &event);
//     if (RT_EOK == ret && (event & VLS300_EVENT_GOT_FW_UPDATE_DATA_ACK)) {
//       /* 接收到升级数据回令 */
//       fd_offset += read_len;
//       send_pack_num += 1;
//       if (fd_offset >= fw_byte_size) {
//         break;
//       }
//     } else {
//       retry_cnt++;
//       if (retry_cnt >= 5) {
//         LOG_E("update data retry failed!");
//         close(fd);
//         return -3;
//       }
//     }
//   } while (1);

//   close(fd);
//   return 0;
// }

// static void vk_vls300_fw_update_entry(void *arg) {

//   struct vk_weigher_vls300 *vls300 = (struct vk_weigher_vls300 *)arg;

//   do_fw_update(vls300, vls300->_fw_file_name, vls300->canard, RT_NULL);
// }

// int vls300_fw_update(const char *fw_file_name) {
//   /* 检查固件文件是否存在 */
//   if (access(fw_file_name, F_OK) != 0) {
//     return -1;
//   }

//   _vk_vls300->_fw_file_name = fw_file_name;

//   rt_thread_t tid =
//       rt_thread_create("VK_VLS300_FW_UPDATE", vk_vls300_fw_update_entry,
//                        _vk_vls300, 2048, 15, 5);
//   rt_thread_startup(tid);

//   return 0;
// }