/* * This demo application is distributed under the terms of CC0 (public domain dedication). * More info: https://creativecommons.org/publicdomain/zero/1.0/ */ // This is needed to enable necessary declarations in sys/ #ifndef _GNU_SOURCE # define _GNU_SOURCE #endif #include // #include // CAN backend driver for SocketCAN, distributed with Libcanard #include #include #include #include #include #include #include "log.h" #include "stm32f4xx_hal.h" #include "mycan.h" #include "tmotor_ctrl.h" #include "elmo.h" /* * Application constants */ #define P_MIN -12.5f #define P_MAX 12.5f #define V_MIN -45.0f #define V_MAX 45.0f #define KP_MIN 0.0f #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f #define T_MIN -18.0f #define T_MAX 18.0f #define APP_VERSION_MAJOR 1 #define APP_VERSION_MINOR 0 #define APP_NODE_NAME "org.uavcan.libcanard.demo" #define TMOTOR_COUNT (6) /* * Some useful constants defined by the UAVCAN specification. * Data type signature values can be easily obtained with the script show_data_type_info.py */ #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1 #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40 #define UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC 400000UL #define UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC 600000UL #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_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_GET_NODE_INFO_RESPONSE_MAX_SIZE ((3015 + 7) / 8) #define UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE 0xee468a8121c46a9e #define UAVCAN_GET_NODE_INFO_DATA_TYPE_ID 1 #define UNIQUE_ID_LENGTH_BYTES 16 #define UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_ID 15 #define UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_SIGNATURE 0x196ae06426a3b5d8 #define UAVCAN_PROTOCOL_ENUMERATION_BEGIN_RESPONSE_MAX_SIZE (759 / 8) #define UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_ID 380 #define UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_SIGNATURE 0x884cb63050a84f35 #define UAVCAN_PROTOCOL_ENUMERATION_INDICATION_RESPONSE_MAX_SIZE (815/8) // uavcan.protocol.param.GetSet 11 0xa7b622f939d1a4d5 1791 / 2967 #define UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_ID 11 #define UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_SIGNATURE 0xa7b622f939d1a4d5 #define UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE (2967 / 8) // uavcan.protocol.param.ExcuteOpcode 10 0x3b131ac5eb69d2cd 56 / 49 #define UAVCAN_PROTOCOL_PARAM_OPCODE_DATA_TYPE_ID 10 #define UAVCAN_PROTOCOL_PARAM_OPCODE_DATA_TYPE_SIGNATURE 0x3b131ac5eb69d2cd #define UAVCAN_PROTOCOL_PARAM_RESPONSE_MAX_SIZE (56 / 8) //uavcan.equipment.indication.BeepCommand 1080 0xbe9ea9fec2b15d52 #define UAVCAN_EQUIPMENT_INDICATION_BEEP_CMD_DATA_TYPE_ID 1080 #define UAVCAN_EQUIPMENT_INDICATION_BEEP_CMD_DATA_TYPE_SIGNATURE 0xbe9ea9fec2b15d52 #define UAVCAN_EQUIPMENT_INDICATION_BEEP_CMD_RESPONSE_MAX_SIZE (32 / 8) // uavcan.equipment.esc.RawCommand 1030 0x217f5c87d7ec951d 285 #define UAVCAN_EQUIPMENT_ESC_RAW_CMD_DATA_TYPE_ID 1030 #define UAVCAN_EQUIPMENT_ESC_RAW_CMD_DATA_TYPE_SIGNATURE 0x217f5c87d7ec951d #define UAVCAN_EQUIPMENT_ESC_RAW_CMD_RESPONSE_MAX_SIZE (285 / 8) // uavcan.equipment.esc.RPMCommand 1031 0xce0f9f621cf7e70b 365 #define UAVCAN_EQUIPMENT_ESC_RPM_CMD_DATA_TYPE_ID 1031 #define UAVCAN_EQUIPMENT_ESC_RPM_CMD_DATA_TYPE_SIGNATURE 0xce0f9f621cf7e70b #define UAVCAN_EQUIPMENT_ESC_RPM_CMD_RESPONSE_MAX_SIZE (365 / 8) //uavcan.equipment.actuator.Status 1011 0x5e9bba44faf1ea04 64 #define UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_TYPE_ID 1011 #define UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_SIGNATURE 0x5e9bba44faf1ea04 #define UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE (64/8) // uavcan.equipment.esc.Status 1034 0xa9af28aea2fbb254 110 #define UAVCAN_EQUIPMENT_ESC_STATUS_DATA_TYPE_ID 1034 #define UAVCAN_EQUIPMENT_ESC_STATUS_DATA_TYPE_SIGNATURE 0xa9af28aea2fbb254 #define UAVCAN_EQUIPMENT_ESC_STATUS_RESPONSE_MAX_SIZE (110 / 8) //uavcan.equipment.actuator.ArrayCommand 1010 0xd8a7486238ec3af3 484 #define UAVCAN_MOTOR_STATUS_TYPE_ID 1010 #define UAVCAN_MOTOR_STATUS_TYPE_SIGNATURE 0xd8a7486238ec3af3 #define UAVCAN_MOTOR_STATUS_MAX_SIZE 484 //uavcan.protocol.param.Value N/A 0x29f14bf484727267 1035 //CAN_RxHeaderTypeDef RxHeader; //volatile int can_rx_frame_cnt; //uint8_t RxData[16]; // #define HCAN hcan1 #define HCAN2 hcan2 uint8_t MotorData[16]; struct _uavc_dttid_sign { /* data */ uint16_t transfer_type; //0 1 2 uint16_t data_type_id; uint64_t data_type_signature; } uavc_dttid_accept_tab[] = { { CanardTransferTypeRequest, UAVCAN_GET_NODE_INFO_DATA_TYPE_ID, UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE}, { CanardTransferTypeRequest, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_ID, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_SIGNATURE }, { CanardTransferTypeBroadcast, UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_ID, UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_SIGNATURE }, { CanardTransferTypeRequest, UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_ID, UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_SIGNATURE }, { CanardTransferTypeRequest, UAVCAN_PROTOCOL_PARAM_OPCODE_DATA_TYPE_ID, UAVCAN_PROTOCOL_PARAM_OPCODE_DATA_TYPE_SIGNATURE }, { CanardTransferTypeBroadcast, UAVCAN_EQUIPMENT_INDICATION_BEEP_CMD_DATA_TYPE_ID, UAVCAN_EQUIPMENT_INDICATION_BEEP_CMD_DATA_TYPE_SIGNATURE }, { CanardTransferTypeBroadcast, UAVCAN_EQUIPMENT_ESC_RAW_CMD_DATA_TYPE_ID, UAVCAN_EQUIPMENT_ESC_RAW_CMD_DATA_TYPE_SIGNATURE }, { CanardTransferTypeBroadcast, UAVCAN_EQUIPMENT_ESC_RPM_CMD_DATA_TYPE_ID, UAVCAN_EQUIPMENT_ESC_RPM_CMD_DATA_TYPE_SIGNATURE }, { CanardTransferTypeBroadcast, UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_TYPE_ID, UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_SIGNATURE},//增加的数据类型 {CanardTransferTypeBroadcast,UAVCAN_EQUIPMENT_ESC_STATUS_DATA_TYPE_ID,UAVCAN_EQUIPMENT_ESC_STATUS_DATA_TYPE_SIGNATURE}, {CanardTransferTypeBroadcast,UAVCAN_MOTOR_STATUS_TYPE_ID,UAVCAN_MOTOR_STATUS_TYPE_SIGNATURE} }; #define COUNT_OF(x) (sizeof(x)/sizeof(*x)) /* * Library instance. * In simple applications it makes sense to make it static, but it is not necessary. */ static CanardInstance g_canard; ///< The library instance static uint8_t g_canard_memory_pool[4096]; ///< Arena for memory allocation, used by the library /* * Variables used for dynamic node ID allocation. * RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation */ static uint64_t g_send_next_node_id_allocation_request_at; ///< When the next node ID allocation request should be sent static uint8_t g_node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request /* * Node status variables */ static uint8_t g_node_health = UAVCAN_NODE_HEALTH_OK; static uint8_t g_node_mode = UAVCAN_NODE_MODE_INITIALIZATION; static uint8_t g_node_enumrate_indication = 0; uint8_t nodeid = 0xa; uint8_t change; #include "canard_stm32.h" // static void canard_stm32_demo_init(void) // { // 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");+ // } // canardSetLocalNodeID(&g_canard, 100); // } static uint64_t getMonotonicTimestampUSec(void) { uint64_t time_ms = HAL_GetTick(); return time_ms * 1000; // struct timespec ts; // memset(&ts, 0, sizeof(ts)); // if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) // { // abort(); // } // return (uint64_t)(ts.tv_sec * 1000000LL + ts.tv_nsec / 1000LL); } /** * Returns a pseudo random float in the range [0, 1]. */ static float getRandomFloat(void) { static bool initialized = false; if (!initialized) // This is not thread safe, but a race condition here is not harmful. { initialized = true; srand((uint32_t) time(NULL)); } // coverity[dont_call] return (float)rand() / (float)RAND_MAX; } /** * This function uses a mock unique ID, this is not allowed in real applications! */ static void readUniqueID(uint8_t* out_uid) { for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++) { out_uid[i] = i; } } static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]) { memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE); static uint32_t started_at_sec = 0; if (started_at_sec == 0) { started_at_sec = (uint32_t)(getMonotonicTimestampUSec() / 1000000U); } const uint32_t uptime_sec = (uint32_t)((getMonotonicTimestampUSec() / 1000000U) - started_at_sec); // g_node_mode = UAVCAN_NODE_MODE_OPERATIONAL; /* * Here we're using the helper for demonstrational purposes; in this simple case it could be preferred to * encode the values manually. */ canardEncodeScalar(buffer, 0, 32, &uptime_sec); canardEncodeScalar(buffer, 32, 2, &g_node_health); //ok init value. canardEncodeScalar(buffer, 34, 3, &g_node_mode); } static uint16_t makeNodeInfoMessage(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]) { memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE); // NodeStatus makeNodeStatusMessage(buffer); // SoftwareVersion buffer[7] = APP_VERSION_MAJOR; buffer[8] = APP_VERSION_MINOR; buffer[9] = 1; // Optional field flags, VCS commit is set uint32_t u32 = 0x44332211;//GIT_HASH; canardEncodeScalar(buffer, 80, 32, &u32); //byte[10]~byte[13] // Image CRC skipped 2 // HardwareVersion // Major skipped // Minor skipped readUniqueID(&buffer[24]); //byte[24] // Certificate of authenticity skipped // Name const size_t name_len = strlen(APP_NODE_NAME); memcpy(&buffer[41], APP_NODE_NAME, name_len); return 41 + name_len; } static void getNodeInfoHandleCanard(CanardRxTransfer* transfer) { printf("GetNodeInfo request from %d\n", transfer->source_node_id); uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; const uint16_t total_size = makeNodeInfoMessage(buffer); /* * Transmitting; in this case we don't have to release the payload because it's empty anyway. */ const int16_t resp_res = 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)total_size); if (resp_res <= 0) { (void)fprintf(stderr, "Could not respond to GetNodeInfo; error %d\n", resp_res); } } typedef struct __packed _actuator_Status_b { uint8_t actuator_id; uint16_t position; uint16_t force; uint16_t speed; } _actuator_Status_b ; // static void BroadMotorInfoCanard(void) // { // //printf("GetMotorInfo request from %d\n", transfer->source_node_id); // uint16_t buffer[UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE]; // //getRX_Motor_Info(&HCAN); // memset(buffer,0,UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE); // _actuator_Status_b motor_status; // extern motor_id; // uint8_t i=motor_id; // LOGI("%d",i); // extern tmotor_info_t tmotor_info[TMOTOR_COUNT]; // tmotor_info_t *ps=tmotor_info; // ps=ps+i-1; // if(((ps->id)<=6)&&((ps->id)!=0)) // { // motor_status.actuator_id=ps->id; // static uint8_t transfer_id; // motor_status.position=canardConvertNativeFloatToFloat16(ps->position_f); // motor_status.speed=canardConvertNativeFloatToFloat16(ps->speed_f); // motor_status.force=canardConvertNativeFloatToFloat16(ps->torque_f); // uint8_t current=(uint8_t)(ps->torque_f/0.95*100); // LOGI("p:%f,S:%f,T:%f,C:%f",ps->position_f,ps->speed_f,ps->speed_f/0.95); // // motor_status.position=canardConvertNativeFloatToFloat16(3.0); // // motor_status.speed=canardConvertNativeFloatToFloat16(4.0); // // motor_status.force=canardConvertNativeFloatToFloat16(5.0); // int offset=0; // canardEncodeScalar(buffer, offset, 8, &motor_status.actuator_id); // offset += 8; // canardEncodeScalar(buffer, offset, 16, &motor_status.position); // offset += 16; // canardEncodeScalar(buffer, offset, 16, &motor_status.force); // offset += 16; // canardEncodeScalar(buffer, offset, 16, &motor_status.speed); // offset += 16; // canardEncodeScalar(buffer, offset, 8, ¤t); // const int16_t Broad_res=canardBroadcast(&g_canard, // UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_SIGNATURE, // UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_TYPE_ID, // &transfer_id, // CANARD_TRANSFER_PRIORITY_MEDIUM, // &buffer, // UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE); // LOGI("ttttt"); // if (Broad_res <= 0) // { // (void)fprintf(stderr, "Could not broad motor feedback; error %d\n", Broad_res); // }} // } static void BroadMotorInfoCanard(void) { uint16_t buffer[UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE]; memset(buffer,0,UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE); _actuator_Status_b motor_status; static uint8_t motor_transfer_id; for (uint8_t i=1;i<=6;i++) { //g_canard.node_id=nodeid+i; motor_status.actuator_id=i; motor_status.position=canardConvertNativeFloatToFloat16(3.0+i); motor_status.speed=canardConvertNativeFloatToFloat16(4.0); motor_status.force=canardConvertNativeFloatToFloat16(5.0); uint8_t current=5; int offset=0; canardEncodeScalar(buffer, offset, 8, &motor_status.actuator_id); offset += 8; canardEncodeScalar(buffer, offset, 16, &motor_status.position); offset += 16; canardEncodeScalar(buffer, offset, 16, &motor_status.force); offset += 16; canardEncodeScalar(buffer, offset, 16, &motor_status.speed); offset += 16; canardEncodeScalar(buffer, offset, 8, ¤t); const int16_t Broad_res=canardBroadcast(&g_canard, UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_SIGNATURE, UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_TYPE_ID, &motor_transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, &buffer, UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_MAX_SIZE); LOGI("%d,%d,%d",i,motor_transfer_id,Broad_res); if (Broad_res <= 0) { (void)fprintf(stderr, "Could not broad motor feedback; error %d\n", Broad_res); } } //g_canard.node_id=nodeid; } static void equipment_enumeration_begin_handler(CanardRxTransfer *transfer) { printf("equipment esc enum %s from %d\n", __FUNCTION__, transfer->source_node_id); // --- // uint8 ERROR_OK = 0 # Success // uint8 ERROR_INVALID_MODE = 1 # The node cannot perform enumeration in its current operating mode // uint8 ERROR_INVALID_PARAMETER = 2 # The node cannot enumerate on the requested parameter, or it doesn't exist // uint8 ERROR_UNSUPPORTED = 3 # The node cannot perform enumeration in its current configuration // uint8 ERROR_UNKNOWN = 255 # Generic error // uint8 error uint8_t buffer[1]; buffer[0] = 0; //no error. #if 0 const int16_t resp_res = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_SIGNATURE, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], (uint16_t)1); if (resp_res <= 0) { (void)fprintf(stderr, "Could not respond to enumration begin; error %d\n", resp_res); } #else for (int i=0; i<6; ++i) { g_canard.node_id = nodeid + i; const int16_t resp_res = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_SIGNATURE, UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], (uint16_t)1); if (resp_res <= 0) { (void)fprintf(stderr, "Could not respond to enumration begin; error %d\n", resp_res); } } //reset g_canard.node_id = nodeid; #endif } typedef union __packed _param_numeric_value { int64_t integer_value; float real_value; } param_numeric_value_u ; //66bit? struct __packed _indication_resp { /* data */ uint8_t void6; param_numeric_value_u value; uint8_t param_name[20]; }; typedef struct { uint8_t* name; int64_t val; int64_t min; int64_t max; int64_t defval; } param_t; static param_t parameters[] = { {"esc_index", 0, 10, 20, 15}, {"ctl_dir", 0, 0, 1, 0}, }; static inline param_t *getParamByIndex(uint16_t index) { if (index > COUNT_OF(parameters)) { return NULL; } return ¶meters[index]; } static inline param_t* getParamByName(uint8_t *name) { for (int i=0; isource_node_id, // UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_SIGNATURE, // UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_ID, // &transfer->transfer_id, // transfer->priority, // CanardRequest, // &resp, // (uint16_t)total_size); static uint8_t indication_transfer_id; const int16_t resp_res = canardBroadcast(&g_canard, UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_SIGNATURE, UAVCAN_PROTOCOL_ENUMERATION_INDICATION_DATA_TYPE_ID, &indication_transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, &resp, (uint16_t)total_size); if (resp_res <= 0) { (void)fprintf(stderr, "Could not respond to enumration indication; error %d\n", resp_res); } } typedef union __packed _param_value { /* data */ int64_t integer_value; float real_value; uint8_t boolean_value; uint8_t string_value[128]; } param_value_u ; typedef struct __packed _param_getset_req { uint16_t index; //actually uint13 param_value_u value; //8+2+8? uint8_t name[92]; } param_getset_req_t ; // typedef struct __packed _param_getset_resp // { // uint8_t void5; //actually uint13 // param_value_u value; // uint8_t void5_; // param_value_u default_value; // uint8_t void6; // param_numeric_value_u max_value; // uint8_t void6_; // param_numeric_value_u min_value; // uint8_t name[92]; // } param_getset_resp_t ; typedef struct __packed _param_getset_resp { uint16_t index : 13; //actually uint13 uint16_t tag : 3; //tag = 1 == int int64_t value; } param_getset_resp_t ; uint16_t canard_encode_param(param_t *p, uint8_t *buffer) { uint8_t n = 0; int offset = 0; uint8_t tag = 1; if ( NULL == p ) { tag = 0; canardEncodeScalar(buffer, offset, 5, &n); offset += 5; canardEncodeScalar(buffer, offset, 3, &tag); offset += 3; canardEncodeScalar(buffer, offset, 6, &n); offset += 6; canardEncodeScalar(buffer, offset, 2, &tag); offset += 2; canardEncodeScalar(buffer, offset, 6, &n); offset += 6; canardEncodeScalar(buffer, offset, 2, &tag); offset += 2; buffer[offset / 8] = 0; return ( offset / 8 + 1 ); } canardEncodeScalar(buffer, offset, 5, &n); offset += 5; canardEncodeScalar(buffer, offset, 3, &tag); offset += 3; canardEncodeScalar(buffer, offset, 64, &p->val); offset += 64; canardEncodeScalar(buffer, offset, 5, &n); offset += 5; canardEncodeScalar(buffer, offset, 3, &tag); offset += 3; canardEncodeScalar(buffer, offset, 64, &p->defval); offset += 64; canardEncodeScalar(buffer, offset, 6, &n); offset += 6; canardEncodeScalar(buffer, offset, 2, &tag); offset += 2; canardEncodeScalar(buffer, offset, 64, &p->max); offset += 64; canardEncodeScalar(buffer, offset, 6, &n); offset += 6; canardEncodeScalar(buffer, offset, 2, &tag); offset += 2; canardEncodeScalar(buffer, offset, 64, &p->min); offset += 64; memcpy(&buffer[offset / 8], p->name, strlen(p->name)); return (offset / 8 + strlen(p->name)); } static void param_getset_handler(CanardRxTransfer *transfer) { printf("%s, %d\n", __FUNCTION__, transfer->source_node_id); uint16_t index = 0xffff; uint8_t tag = 0; int offset; int64_t val = 0; canardDecodeScalar(transfer, offset, 13, false, &index); offset += 13; canardDecodeScalar(transfer, offset, 3, false, &tag); offset += 3; if ( 1 == tag ) { canardDecodeScalar(transfer, offset, 64, false, &val); offset += 64; } //uint16 + int64 so far uint16_t n = transfer->payload_len - offset / 8; uint8_t name[16] = ""; for (int i=0; ival = val; } uint8_t buffer[64] = ""; uint16_t total_size = canard_encode_param(p, buffer); const int16_t resp_res = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer, (uint16_t)total_size); if (resp_res <= 0) { (void)fprintf(stderr, "Could not respond to param getset; error %d\n", resp_res); } } static void param_opcode_save_handler(CanardRxTransfer *transfer) { } //idle value [5340 3750 5340 2750 2750 5340] int16_t esc_raw[8]; int16_t esc_raw_middle[3] = {4095, 4095, 4103}; float motor_v[2]; const int esc_center_raw = 5340; int32_t elmo_v[2]; static void equipment_esc_raw_cmd_handler(CanardRxTransfer *transfer) { // printf("equipment esc raw cmd from %d\n", transfer->source_node_id); if (transfer->payload_len < 9) { //int14 * 4 = 56bit //int14 * 6 = 84 / 8 = 10 // return;// } // union _esc_raw // { // /* data */ // uint8_t payload[7]; // struct // { // /* data */ // int64_t esc0 : 14; // int64_t esc1 : 14; // int64_t esc2 : 14; // int64_t esc3 : 14; // }channels; // }esc_raw; // memcpy(esc_raw.payload, transfer.) int offset = 0; const uint8_t bit_len = 14; canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[0]); offset += bit_len; canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[1]); offset += bit_len; canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[2]); // offset += bit_len; // canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[3]); // offset += bit_len; // canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[4]); // offset += bit_len; // canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[5]); // offset += bit_len; // canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[6]); // offset += bit_len; // canardDecodeScalar(transfer, offset, bit_len, true, &esc_raw[7]); printf("esc decode %4d %4d %4d\n", esc_raw[0], esc_raw[1], esc_raw[2]); elmo_v[1] = (esc_raw[2] - esc_raw_middle[2]) / 10; Elmo_PVM(9, elmo_v[1]); } /** * */ static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise. */ if ((canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) && (transfer->transfer_type == CanardTransferTypeBroadcast) && (transfer->data_type_id == UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID)) { // Rule C - updating the randomized time interval g_send_next_node_id_allocation_request_at = getMonotonicTimestampUSec() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + (uint64_t)(getRandomFloat() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { puts("Allocation request from another allocatee"); g_node_id_allocation_unique_id_offset = 0; return; } // Copying the unique ID from the message static const uint8_t UniqueIDBitOffset = 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 - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { assert(received_unique_id_len < UNIQUE_ID_LENGTH_BYTES); const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); (void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); } // Obtaining the local unique ID uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES]; readUniqueID(my_unique_id); // Matching the received UID against the local one if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { printf("Mismatching allocation response from %d:", transfer->source_node_id); for (uint8_t i = 0; i < received_unique_id_len; i++) { printf(" %02x/%02x", received_unique_id[i], my_unique_id[i]); } puts(""); g_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. g_node_id_allocation_unique_id_offset = received_unique_id_len; g_send_next_node_id_allocation_request_at -= UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC; printf("Matching allocation response from %d offset %d\n", transfer->source_node_id, g_node_id_allocation_unique_id_offset); } 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); printf("Node ID %d allocated by %d\n", allocated_node_id, transfer->source_node_id); } } if ((transfer->transfer_type == CanardTransferTypeRequest) && (transfer->data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) { getNodeInfoHandleCanard(transfer); } if ((transfer->data_type_id == UAVCAN_PROTOCOL_ENUMERATION_BEGIN_DATA_TYPE_ID)) { equipment_enumeration_begin_handler(transfer); g_node_enumrate_indication = 1; } if ((transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RAW_CMD_DATA_TYPE_ID)) { equipment_esc_raw_cmd_handler(transfer); } if ( UAVCAN_PROTOCOL_PARAM_GETSET_DATA_TYPE_ID == transfer->data_type_id) { param_getset_handler(transfer); } if (UAVCAN_PROTOCOL_PARAM_OPCODE_DATA_TYPE_ID == transfer->data_type_id ) { } if(UAVCAN_MOTOR_ENCODER_FEEDBACK_DATA_TYPE_ID == transfer->data_type_id) { //getMotorInfoCanard(transfer); } } /** * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received * by the local node. * If the callback returns true, the library will receive the transfer. * If the callback returns false, the library will ignore the transfer. * All transfers that are addressed to other nodes are always ignored. */ 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) { (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 { // if ((transfer_type == CanardTransferTypeRequest) && // (data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) // { // *out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE; // return true; // } // printf("data type id %d trans type %d\n", data_type_id, transfer_type); for (int i=0; i 70) { puts("WARNING: ENLARGE MEMORY POOL"); } } /* * Transmitting the node status message periodically. */ { uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]; makeNodeStatusMessage(buffer); static uint8_t transfer_id; // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)! const int16_t bc_res = 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); if (bc_res <= 0) { (void)fprintf(stderr, "Could not broadcast node status; error %d\n", bc_res); } } g_node_mode = UAVCAN_NODE_MODE_OPERATIONAL; if (g_node_enumrate_indication) { g_node_enumrate_indication = 0; equipment_enumration_indication(NULL); } } /** * Transmits all frames from the TX queue, receives up to one frame. */ static void processTxRxOnce(void* socketcan, int32_t timeout_msec) { // Transmitting for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&g_canard)) != NULL;) { // const int16_t tx_res = socketcanTransmit(socketcan, txf, 0); static int assign2; if (txf->id == 0x1801550a) { assign2++; } const int16_t tx_res = uavcan_transmit(txf); // const int16_t tx_res = canardSTM32Transmit(txf); if (tx_res < 0) // Failure - drop the frame and report { canardPopTxQueue(&g_canard); (void)fprintf(stderr, "Transmit error %d, frame dropped, errno '%s'\n", tx_res, strerror(errno)); } else if (tx_res > 0) // Success - just drop the frame { canardPopTxQueue(&g_canard); } else // Timeout - just exit and try again later { break; } } // Receiving CanardCANFrame rx_frame; const uint64_t timestamp = getMonotonicTimestampUSec(); // const int16_t rx_res = socketcanReceive(socketcan, &rx_frame, timeout_msec); const int16_t rx_res = uavcan_receive(&rx_frame); // const int16_t rx_res = canardSTM32Receive(&rx_frame); if (rx_res < 0) // Failure - report { (void)fprintf(stderr, "Receive error %d, errno '%s'\n", rx_res, strerror(errno)); } else if (rx_res > 0) // Success - process the frame { canardHandleRxFrame(&g_canard, &rx_frame, timestamp); } else { ; // Timeout - nothing to do } } int main_uavc_demo(int argc, char** argv) { /* * Initializing the Libcanard instance. */ canardInit(&g_canard, g_canard_memory_pool, sizeof(g_canard_memory_pool), onTransferReceived, shouldAcceptTransfer, NULL); canardSetLocalNodeID(&g_canard, nodeid); /* * Performing the dynamic node ID allocation procedure. */ static const uint8_t PreferredNodeID = CANARD_BROADCAST_NODE_ID; ///< This can be made configurable, obviously g_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..."); g_send_next_node_id_allocation_request_at = getMonotonicTimestampUSec() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + (uint64_t)(getRandomFloat() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); while ((getMonotonicTimestampUSec() < g_send_next_node_id_allocation_request_at) && (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID)) { processTxRxOnce(NULL, 1); } if (canardGetLocalNodeID(&g_canard) != CANARD_BROADCAST_NODE_ID) { break; } // Structure of the request is documented in the DSDL definition // 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)(PreferredNodeID << 1U); if (g_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]; readUniqueID(my_unique_id); static const uint8_t MaxLenOfUniqueIDInRequest = 6; uint8_t uid_size = (uint8_t)(UNIQUE_ID_LENGTH_BYTES - g_node_id_allocation_unique_id_offset); if (uid_size > MaxLenOfUniqueIDInRequest) { uid_size = MaxLenOfUniqueIDInRequest; } // Paranoia time assert(g_node_id_allocation_unique_id_offset < UNIQUE_ID_LENGTH_BYTES); assert(uid_size <= MaxLenOfUniqueIDInRequest); assert(uid_size > 0); assert((uid_size + g_node_id_allocation_unique_id_offset) <= UNIQUE_ID_LENGTH_BYTES); memmove(&allocation_request[1], &my_unique_id[g_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); } // Preparing for timeout; if response is received, this value will be updated from the callback. g_node_id_allocation_unique_id_offset = 0; } printf("Dynamic node ID allocation complete [%d]\n", canardGetLocalNodeID(&g_canard)); /* * Running the main loop. */ uint64_t next_1hz_service_at = getMonotonicTimestampUSec(); uint64_t next_100hz_service_at = getMonotonicTimestampUSec(); for (;;) { tmotor_set_v(1,5);// LOGI("444"); processTxRxOnce(NULL, 1); const uint64_t ts = getMonotonicTimestampUSec(); //LOGI("555"); if (ts >= next_1hz_service_at) { LOGI("666"); next_1hz_service_at += 1000000; process1HzTasks(ts); BroadMotorInfoCanard(); LOGI("777"); if (change) { change = 0; // canardSetLocalNodeID(&g_canard, nodeid); g_canard.node_id = nodeid; } } // if (ts >= next_100hz_service_at) { // next_100hz_service_at += 10 * 1000; // tmotor_set_v(1, v); // } } return 0; }