proximity_uavcan_converter/UAVCAN/uavcan.h

#ifndef __UAVCAN_H
#define __UAVCAN_H

#ifdef __cplusplus
extern "C"
{
#endif

#include <stdio.h>
#include <stdint.h>
#include "canard.h"
#include "canard_stm32.h"
#include "main.h"
#include "node_timestamp.h"
#include "node_coarse_orientation.h"
#include "node_proximity.h"
#include "uavcan_filter.h"
#include "bootloader_interface.h"

#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0]))

#define APP_VERSION_MAJOR 1
#define APP_VERSION_MINOR 1
#define HARDWARE_VERSION_MAJOR 1
#define APP_NODE_NAME "zrzk.proxi.1"
#define GIT_HASH 0xBADC0FFE

#define UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE 0xee468a8121c46a9e
#define UAVCAN_GET_NODE_INFO_DATA_TYPE_ID 1
#define UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE ((3015 + 7) / 8)

#define UAVCAN_NODE_HEALTH_OK 0
#define UAVCAN_NODE_HEALTH_WARNING 1
#define UAVCAN_NODE_HEALTH_ERROR 2
#define UAVCAN_NODE_HEALTH_CRITICAL 3

#define UAVCAN_NODE_MODE_OPERATIONAL 0
#define UAVCAN_NODE_MODE_INITIALIZATION 1

#define UAVCAN_NODE_STATUS_MESSAGE_SIZE 7
#define UAVCAN_NODE_STATUS_DATA_TYPE_ID 341
#define UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE 0x0f0868d0c1a7c6f1

#define UAVCAN_PROTOCOL_DEBUG_KEYVALUE_ID 16370
#define UAVCAN_PROTOCOL_DEBUG_KEYVALUE_SIGNATURE 0xe02f25d6e0c98ae0
#define UAVCAN_PROTOCOL_DEBUG_KEYVALUE_MESSAGE_SIZE 62

#define UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_ID 16383
#define UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_SIGNATURE 0xd654a48e0c049d75
#define UAVCAN_DEBUG_LOG_MESSAGE_MESSAGE_SIZE ((983 + 7) / 8)

#define UNIQUE_ID_LENGTH_BYTES 16
#define UNIQUE_STM32_DEVICE_ID_LENGTH 12

  // #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1
  // #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40
  // #define UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US 600U
  // #define UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US 1000U
  // #define UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US 0U
  // #define UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US 400U
  // #define UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET 8
  // #define UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST 6

  // #define UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC 400UL
  // #define UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC 600UL

#define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID 16383
#define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE (0xD654A48E0C049D75ULL)
#define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE ((983 + 7) / 8)

#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID 40
#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE 0xb7d725df72724126
#define UAVCAN_BEGIN_FIRMWARE_UPDATE_RESPONSE_MAX_SIZE ((1031 + 7) / 8)
#define UAVCAN_BEGIN_FIRMWARE_UPDATE_REQUEST_MAX_SIZE ((1616 + 7) / 8)

#define UAVCAN_FILE_READ_DATA_TYPE_ID 48
#define UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE 0x8dcdca939f33f678
#define UAVCAN_FILE_READ_REQUEST_MAX_SIZE ((1648 + 7) / 8)
#define UAVCAN_FILE_READ_RESPONSE_MAX_SIZE ((2073 + 7) / 8)

#define UAVCAN_RESTART_NODE_DATA_TYPE_ID 5
#define UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE 0x569e05394a3017f0
#define UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE ((40 + 7) / 8)
#define UAVCAN_RESTART_NODE_RESPONSE_MAX_SIZE ((1 + 7) / 8)

#define UAVCAN_PARAM_GETSET_REQUEST_MAX_SIZE ((1791 + 7) / 8)
#define UAVCAN_PARAM_GETSET_RESPONSE_MAX_SIZE ((2967 + 7) / 8)
#define UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE 0xa7b622f939d1a4d5
#define UAVCAN_PARAM_GETSET_DATA_TYPE_ID 11

#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1
#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40
#define UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US 600000U
#define UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US 1000000U
#define UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US 0U
#define UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US 400000U
#define UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET 8
#define UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST 6

#define RCPWM_PERIOD_uS 20000
#define RCPWM_NEUTRAL_uS 1500
#define RCPWM_MAGNITUDE_uS 500

#define UAVCAN_MODE_GET_STATE 21
#define UABCAN_MODE_GET_SN 20

#define CANARD_SPIN_PERIOD 1000
#define PUBLISHER_PERIOD_mS 25
#define PUBLISHER_PERIOD_mS_low 1000
#define CANARD_STATUS_PERIOD 1050

#define FIlTER_USED_AMOUNT 2

  //#define UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG                                  0 // 0
  //#define UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_INFO                                   1 // 1
  //#define UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING                                2 // 2
  //#define UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR                                  3 // 3

#define USE_CAN_LOG 1

#undef MIN
#undef MAX
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#define MAX(a, b) (((a) > (b)) ? (a) : (b))

  static const uint8_t uavcan_local_node = 65;

  typedef struct
  {
    // FieldTypes
    uint8_t id;      // bit len 8
    uint8_t command; // bit len 8
    uint8_t param1;  // bit len 8
    uint8_t param2;  // bit len 8
    uint8_t param3;  // bit len 8
    uint8_t param4;  // bit len 8
  } uavcan_zrzk_request_t;

  typedef enum
  {
    UAVCAN_LOGLEVEL_DEBUG = 0,
    UAVCAN_LOGLEVEL_INFO = 1,
    UAVCAN_LOGLEVEL_WARNING = 2,
    UAVCAN_LOGLEVEL_ERROR = 3
  } uavcan_log_level_t;

  typedef struct
  {
    uavcan_log_level_t log_level; // bit len 8                          // bit len 8
    char source[31];              // Static Array 8bit[12] max items
    char text[90];                // Static Array 8bit[12] max items
  } uavcan_log_msg_t;

  typedef struct
  {
    uint64_t tick_ms;
    CanardCANFrame canard_CAN_frame;
  } uavcan_rev_t;

  typedef struct
  {
    uint32_t uptime_sec;
    uint16_t vendor_specific_status_code;
    uint8_t health;
    uint8_t mode;
    uint8_t sub_mode;
  } uavcan_node_status_t;

  typedef struct
  {
    uint32_t id;
    union
    {
      uavcan_node_status_t node_state;
      uavcan_proximity_t proximity;
    } content;
  } uavcan_send_data_t;

  typedef enum
  {
    SENSOR_TYPE_UNDEFINED = 0,
    SENSOR_TYPE_SONAR,
    SENSOR_TYPE_LIDAR,
    SENSOR_TYPE_RADAR
  } sensor_type_e;

  typedef enum
  {
    READING_TYPE_UNDEFINED = 0,
    READING_TYPE_VALID_RANGE = 1,
    READING_TYPE_TOO_CLOSE = 2,
    READING_TYPE_TOO_FAR = 3
  } reading_type_e;
  //------------------------------Filter-Start-----------------------------------------
  //----------------------------Filter-END----------------------------------

  void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *transfer);
  void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *transfer);
  void action_transfer_received_response(CanardInstance *ins, CanardRxTransfer *transfer);

  void make_node_status_message(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]);
  void read_unique_id(uint8_t *out_uid);
  void publish_node_status(void);
  void uavcan_init(void);
  void canard_publish(void);

  //void camera_geocmd_handleCanard(CanardRxTransfer* transfer);
  static void handle_canard_get_node_info(CanardRxTransfer *transfer);
  void send_canard(void);
  void receive_canard(void);
  void publish_node_status(void);
  void publishCanard(void);
  //void getsetHandleCanard(CanardRxTransfer *transfer);
  //void getNodeInfoHandleCanard(CanardRxTransfer *transfer);
  static uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]);

  void getNodeStateHandleCanard(CanardRxTransfer *transfer);

  void init_allocation(void);
  void handle_allocation_data(CanardInstance *ins, CanardRxTransfer *transfer);

  //uint8_t umin(uint8_t a,uint8_t b);
  static float get_random_float(void);

  void send_to_can_log(uavcan_log_msg_t *msg);

  void publish_zr_request(uavcan_zrzk_request_t *source);
  //uint32_t test_index_value(uint32_t new_tick, uint32_t *last_tick, uint32_t *tick_inter_max);
  void handle_uavcan_rx_frame(const uavcan_rev_t *msg);
  void handle_uav_state(CanardRxTransfer *transfer);
  void uavcan_init_with_cubemx(void);
  void send_uavcan_data(uavcan_send_data_t *send_data);
  void publish_proximity(uavcan_proximity_t *msg);
  void send_uavcan_proximity(uavcan_proximity_t *data);

  void request_node_restart(void);
  void check_need_to_jump_boot(void);
  uint8_t get_node_id(void);
  void uavcan_init_fifo(uint8_t node_id);
  void uavcan_cleanup_stale_transfers(uint64_t time_us);
  void handle_restart_node(CanardRxTransfer *transfer);
  void uavcan_init_by_allocation(void);
  void allocation_update(void);
#ifdef __cplusplus
}
#endif

#endif