Canard sending

gistfile1.txt

static void BSP_CAN_SendFrame(const CanardCANFrame* const frame) {
	if (!frame) {
		return;
	}
    uint32_t mailbox = 0;

    // DroneCAN uses 29-bit extended identifiers
    const CAN_TxHeaderTypeDef header = {
		.IDE = CAN_ID_EXT,
		.ExtId = frame->id & 0x1FFFFFFF,
		.RTR = CAN_RTR_DATA,
		.DLC = frame->data_len,
		.TransmitGlobalTime = DISABLE,
    };

    // Wait until a transmit mailbox is available
    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) {
        // wait
    }

    // Submit frame to CAN peripheral
    HAL_StatusTypeDef rc = HAL_CAN_AddTxMessage(&hcan1, &header, frame->data, &mailbox);
    if (HAL_OK != rc) {
    	cnts.uavcan_tx_fail++;
    	// fail
    }
}

static void uavcan_transmit(void) {
    // Flush TX queue
    const CanardCANFrame* frame = NULL;
    while ((frame = canardPeekTxQueue(&canard)) != NULL) {
        BSP_CAN_SendFrame(frame);
        canardPopTxQueue(&canard);
    }
}

u8 uavcan_send_heartbeat(void) {
	// Create a heart-beat message
	struct uavcan_protocol_NodeStatus test_heartbeat = {
		.uptime_sec = getCurrentMicros() / 1000000,
		.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK,
		.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL
	};

	// Serialize the heartbeat message
	i16 res = uavcan_protocol_NodeStatus_encode(&test_heartbeat, hbeat_ser_buf);
	if (res < 0) {
		cnts.uavcan_enc_heartbeat++;
		return 1;
	}
	res = canardBroadcast(
		&canard,
		UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID,
		&bia_transfer_sn,
		CANARD_TRANSFER_PRIORITY_LOW,
		hbeat_ser_buf, hbeat_ser_buf_size
	);
	if (res < 0) {
		cnts.uavcan_send_heartbeat++;
		return 1; // out of memory or error
	}
	uavcan_transmit();
	return 0;
}