zrzk
/
proximity_uavcan_converter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
4 Commits
7 Branches
0 Tags
10 MiB
 
 
 
Branch: master
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'master'
${ noResults }
proximity_uavcan_converter/UAVCAN/uavcan.c

653 lines
21 KiB
Raw Permalink Blame History

#include "uavcan.h"
#include <string.h>
#include <stdlib.h>
#include "main.h"
#include "math.h"
#include "node_flow_water.h"
#define MEMPOOL_OBJECTS 16
static CanardInstance g_canard; //The library instance
static uint8_t g_canard_memory_pool[2048];
static uint32_t spin_time = 0;
uint8_t feedback_status =0;
//static const uint8_t gimbal_id = 0xC5; //Arena for memory allocation, used by the library
//static uint32_t g_uptime = 0;
uint8_t sver_p1 = 1;
uint8_t sver_p2 = 0;
uint8_t sver_p3 = 0;
//static uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
//static uint8_t node_mode = UAVCAN_NODE_MODE_INITIALIZATION;
static uint64_t send_next_node_id_allocation_request_at; ///< When the next node ID allocation request should be sent
static uint8_t node_id_allocation_unique_id_offset;
static uint8_t node_unique_id[UNIQUE_ID_LENGTH_BYTES];
static uint8_t transfer_id = 0;
//static uint32_t last_state_time = 0;
//extern QueueHandle_t uavcan_rev_queue;
typedef struct
{
// FieldTypes
uint8_t id; // bit len 8
float pitch; // float16 Saturate
float roll; // float16 Saturate
float yaw; // float16 Saturate
} zrzk_equipment_uav_Euler;
#ifndef CANARD_INTERNAL_SATURATE
#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
#endif
#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) );
#endif
#if defined(__GNUC__)
# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
#else
# define CANARD_MAYBE_UNUSED(x) x
#endif
bool shouldAcceptTransfer(const CanardInstance *ins,
uint64_t *out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type,
uint8_t source_node_id)
{
// (void)source_node_id;
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID)
{
// If we're in the process of allocation of dynamic node ID, accept only relevant transfers.
if ((transfer_type == CanardTransferTypeBroadcast) &&
(data_type_id == UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID))
{
*out_data_type_signature = UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE;
return true;
}
else
{
return false;
}
}
else
{
switch (data_type_id)
{
case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID:
*out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE;
return true;
case ZRZK_EQUIPMENT_FLOW_WATER_ID:
*out_data_type_signature = ZRZK_EQUIPMENT_FLOW_WATER_SIGNATURE;
return true;
default:
return false;
}
}
}
void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
{
if(transfer->transfer_type == CanardTransferTypeBroadcast)
{
action_transfer_received_broadcast(ins, transfer);
}
else if (transfer->transfer_type == CanardTransferTypeResponse)
{
action_transfer_received_response(ins, transfer);
}
else
{
action_transfer_received_request(ins, transfer);
}
}
void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID:
handle_canard_get_node_info(transfer);
break;
default:
break;
}
}
void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
case UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID:
if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID)
{
handle_allocation_data(&g_canard, transfer);
}
break;
default:
break;
}
}
void action_transfer_received_response(CanardInstance *ins, CanardRxTransfer *transfer)
{
switch (transfer->data_type_id)
{
default:
break;
}
}
void receive_canard(void)
{
CanardCANFrame rx_frame;
uint32_t time_stame = HAL_GetTick();
while (canardSTM32Receive(&rx_frame) > 0)
{
canardHandleRxFrame(&g_canard, &rx_frame, HAL_GetTick());
if ((HAL_GetTick() - time_stame) > 100)
{
return;
}
}
}
void handle_uavcan_rx_frame(const uavcan_rev_t *msg)
{
canardHandleRxFrame(&g_canard, &msg->canard_CAN_frame,msg->tick_ms*1000);
}
static void handle_canard_get_node_info(CanardRxTransfer *transfer)
{
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
uint16_t len = 0;
len = make_node_info_message(buffer);
int result = canardRequestOrRespond(&g_canard,
transfer->source_node_id,
UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&buffer[0],
(uint16_t)len);
}
void uavcan_init(void)
{
set_unique_id("com.zr3d.proxi");
CanardSTM32CANTimings timings;
int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings);
if (result)
{
__ASM volatile("BKPT #01");
}
result = canardSTM32Init(&timings, CanardSTM32IfaceModeNormal);
if (result)
{
__ASM volatile("BKPT #01");
}
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
canardSetLocalNodeID(&g_canard, uavcan_local_node);
if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0)
{
can_setup_filters_none();
}
}
void uavcan_init_with_cubemx(void)
{
set_unique_id("com.zr3d.proxi");
initCanOnlyFilter();
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
canardSetLocalNodeID(&g_canard, uavcan_local_node);
if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0)
{
can_setup_filters_none();
}
init_CAN_IRQ();
}
void init_allocation(void)
{
set_unique_id("com.zr3d.proxi");
initCanOnlyFilter();
canardInit(&g_canard, // Uninitialized library instance
g_canard_memory_pool, // Raw memory chunk used for dynamic allocation
sizeof(g_canard_memory_pool), // Size of the above, in bytes
onTransferReceived, // Callback, see CanardOnTransferReception
shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer
NULL);
// if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0)
// {
// can_setup_filters_none();
// }
init_CAN_IRQ();
//执行动态节点ID分配过程
static const uint8_t preferred_node_id = CANARD_BROADCAST_NODE_ID;
node_id_allocation_unique_id_offset = 0;
uint8_t node_id_allocation_transfer_id = 0;
while (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID)
{
//puts("Waiting for dynamic node ID allocation...");
// vTaskDelay(50);
send_next_node_id_allocation_request_at =
HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC +
(uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC);
// (uint64_t)(0.1 * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC);
while ((HAL_GetTick() < send_next_node_id_allocation_request_at) &&
(canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID))
{
send_canard();
receive_canard();
osDelay(5);
}
if (canardGetLocalNodeID(&g_canard) != CANARD_BROADCAST_NODE_ID)
{
break;
}
// Structure of the request is documented in the DSDL definition 请求的结构记录在DSDL定义中
// See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1];
allocation_request[0] = (uint8_t)(preferred_node_id << 1U);
if (node_id_allocation_unique_id_offset == 0)
{
allocation_request[0] |= 1; // First part of unique ID
}
uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES];
get_unique_id(my_unique_id);
static const uint8_t max_len_of_unique_id_in_request = 6;
uint8_t uid_size = (uint8_t)(UNIQUE_ID_LENGTH_BYTES - node_id_allocation_unique_id_offset);
if (uid_size > max_len_of_unique_id_in_request)
{
uid_size = max_len_of_unique_id_in_request;
}
// Paranoia time
assert(node_id_allocation_unique_id_offset < UNIQUE_ID_LENGTH_BYTES);
assert(uid_size <= max_len_of_unique_id_in_request);
assert(uid_size > 0);
assert((uid_size + node_id_allocation_unique_id_offset) <= UNIQUE_ID_LENGTH_BYTES);
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
// Broadcasting the request
const int16_t bcast_res = canardBroadcast(&g_canard,
UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE,
UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID,
&node_id_allocation_transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&allocation_request[0],
(uint16_t)(uid_size + 1));
if (bcast_res < 0)
{
//(void)fprintf(stderr, "Could not broadcast dynamic node ID allocation request; error %d\n", bcast_res); //TODO ////my_printf()
}
// Preparing for timeout; if response is received, this value will be updated from the callback.
node_id_allocation_unique_id_offset = 0;
}
//过滤设置
if (canardGetLocalNodeID(&g_canard) != 0) //TODO 其实这个判断可以删除，稳妥起见，再判断一次
{
if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0)
{
can_setup_filters_none();
}
}
init_CAN_IRQ();
}
void send_canard(void)
{
osStatus_t status;
if (uavcan_send_mutexHandle != NULL)
{
status = osMutexAcquire(uavcan_send_mutexHandle, 20U);
}
for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&g_canard)) != NULL;)
{
const int16_t tx_res = canardSTM32Transmit(txf);
if (tx_res < 0) // Failure - drop the frame and report
{
break;
}
else if (tx_res > 0) // Success - just drop the frame
{
canardPopTxQueue(&g_canard);
}
else // Timeout - just exit and try again later
{
break;
}
}
if(status ==osOK && uavcan_send_mutexHandle != NULL)
{
status = osMutexRelease(uavcan_send_mutexHandle);
}
}
void publish_node_status(void)
{
if (HAL_GetTick() < spin_time + CANARD_SPIN_PERIOD)
return; // rate limiting
spin_time = HAL_GetTick();
uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE];
static uint8_t transfer_id = 0; // This variable MUST BE STATIC; refer to the libcanard documentation for the background
make_node_status_message(buffer);
canardBroadcast(&g_canard,
UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE,
UAVCAN_NODE_STATUS_DATA_TYPE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
UAVCAN_NODE_STATUS_MESSAGE_SIZE);
//some indication
}
void make_node_status_message(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE])
{
uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL;
memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE);
uint32_t uptime_sec = (HAL_GetTick() / 1000);
canardEncodeScalar(buffer, 0, 32, &uptime_sec);
canardEncodeScalar(buffer, 32, 2, &node_health);
canardEncodeScalar(buffer, 34, 3, &node_mode);
}
uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE])
{
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
make_node_status_message(buffer);
buffer[7] = APP_VERSION_MAJOR;
buffer[8] = APP_VERSION_MINOR;
buffer[9] = 1; // Optional field flags, VCS commit is set
uint32_t githash = GIT_HASH;
canardEncodeScalar(buffer, 80, 32, &githash);
buffer[22] = 0;//get_hw_version_minor();
buffer[23] = 0;//get_hw_version_patch();
get_unique_id(&buffer[24]);
const size_t name_len = strlen(APP_NODE_NAME);
memcpy(&buffer[41], APP_NODE_NAME, name_len);
return 41 + name_len;
}
//动态节点消息接收处理 //on_allocation_message
void handle_allocation_data(CanardInstance *ins, CanardRxTransfer *transfer)
{
// Rule C - updating the randomized time interval
send_next_node_id_allocation_request_at =
HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + //getMonotonicTimestampUSec()
(uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC);
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
{
// puts("Allocation request from another allocatee");
node_id_allocation_unique_id_offset = 0;
return;
}
//Copying the unique ID from the message unique_id_bit_offset：位偏移量 --uint8
static const uint8_t unique_id_bit_offset = 8;
uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES];
uint8_t received_unique_id_len = 0;
for (; received_unique_id_len < (transfer->payload_len - (unique_id_bit_offset / 8U)); received_unique_id_len++)
{
assert(received_unique_id_len < UNIQUE_ID_LENGTH_BYTES);
const uint8_t bit_offset = (uint8_t)(unique_id_bit_offset + received_unique_id_len * 8U);
(void)canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]);
}
// Obtaining the local unique ID 获取本地 unique ID
uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES];
get_unique_id(my_unique_id);
// Matching the received UID against the local one
if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0)
{
node_id_allocation_unique_id_offset = 0;
return; // No match, return
}
if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES)
{
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
node_id_allocation_unique_id_offset = received_unique_id_len;
send_next_node_id_allocation_request_at -= UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC;
}
else
{
// Allocation complete - copying the allocated node ID from the message
uint8_t allocated_node_id = 0;
(void)canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id);
assert(allocated_node_id <= 127);
canardSetLocalNodeID(ins, allocated_node_id);
}
}
void canard_get_node_info_handle(CanardRxTransfer *transfer)
{
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE);
uint16_t len = 0;
len = make_node_info_message(buffer);
int result = canardRequestOrRespond(&g_canard,
transfer->source_node_id,
UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
&transfer->transfer_id,
// transfer->priority,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardResponse,
&buffer[0],
(uint16_t)len);
}
/**
* Returns a pseudo random float in the range [0, 1].
*/
static float get_random_float(void)
{
static bool initialized = false;
if (!initialized)
{
initialized = true;
// desig_get_unique_id((uint32_t*)&node_unique_id[0]);
// const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0];
// srand(HAL_GetTick() ^ *unique_32);
srand(HAL_GetTick());
}
return (float)rand() / (float)RAND_MAX;
}
void get_unique_id(uint8_t *out_uid)
{
for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++)
{
out_uid[i] = node_unique_id[i];
}
}
void set_unique_id(char *str_unique_id)
{
for (uint8_t i = 0; i < strlen(str_unique_id); i++)
{
node_unique_id[i] = str_unique_id[i];
}
}
void publish_log_message( uavcan_log_level_t log_level, const char *source, const char *text)
{
uint8_t buffer[UAVCAN_DEBUG_LOG_MESSAGE_MESSAGE_SIZE] ={0};
uint8_t source_len = strlen(source) & 31;
uint8_t msg_len = strlen(text);
static uint8_t transfer_id;
if(msg_len > 90)
{
msg_len = 90;
}
/* Use manual mutex locking here to lock the global buffer as well */
buffer[0] = (log_level << 5) | source_len;
memcpy(&buffer[1], source, source_len);
memcpy(&buffer[1+source_len], text, msg_len);
canardBroadcast(
&g_canard,
UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_SIGNATURE,
UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOWEST,
buffer,
1 + source_len + msg_len
);
}
void send_uavcan_data(uavcan_send_data_t *send_data)
{
switch (send_data->id)
{
case UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID:
/* code */
break;
//case ZRZK_EQUIPMENT_FLOW_WATERDEPTH_ID:
// publish_water_dapth(&send_data->content.water_depth_msg);
//break;
case UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID:
publish_device_temperature(&send_data->content.device_temp_msg);
break;
case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID:
publish_measurement_canard(&send_data->content.measurement_msg);
break;
case UAVCAN_NODE_STATUS_DATA_TYPE_ID:
publish_node_status();
break;
case ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID:
publish_proximity(&send_data->content.proximity);
break;
default:
break;
}
}
void publish_measurement_canard(uavcan_measurement_t *msg) //MEASUREMENT
{
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE];
uint32_t len = uavcan_equipment_range_sensor_Measurement_encode(msg, &buffer[0]);
if (UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE != len)
return;
canardBroadcast(&g_canard,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
&buffer[0],
len);
// HAL_UART_AbortReceive(&huart1);
// HAL_NVIC_DisableIRQ(USART1_IRQn);
// send_canard();
// HAL_NVIC_EnableIRQ(USART1_IRQn);
}
void publish_device_temperature(uavcan_equipment_device_Temperature *data)
{
uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE] = {0};
uint32_t len = uavcan_equipment_device_Temperature_encode(data, &buffer);
if (len > 0)
{
canardBroadcast(
&g_canard,
UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE,
UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOWEST,
buffer,
len);
}
}
void publish_proximity(uavcan_proximity_t *msg)
{
uint8_t buffer[ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_MAX_SIZE] = {0};
uint32_t len = zrzk_equipment_range_sensor_Proximity_encode(msg, &buffer);
if (len > 0)
{
canardBroadcast(
&g_canard,
ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_SIGNATURE,
ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
buffer,
len);
}
}