Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: provide separate instances of canard objects per iface

Browse Source
This commit is contained in:
Siddharth Purohit 2021-06-19 21:49:07 +05:30 committed by Andrew Tridgell
parent 9fb7ef915e
commit 3ed53c58e9
3 changed files with 219 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/AP_Periph/AP_Periph.cpp
View File

@ -86,7 +86,9 @@ void AP_Periph_FW::init()
// always run with watchdog enabled. This should have already been
// setup by the bootloader, but if not then enable now
#ifndef DISABLE_WATCHDOG
stm32_watchdog_init();
#endif
stm32_watchdog_pat();

2
Tools/AP_Periph/AP_Periph.h
View File

@ -130,7 +130,7 @@ public:
#if HAL_NUM_CAN_IFACES >= 2
// This allows you to change the protocol and it continues to use the one at boot.
// Without this, changing away from UAVCAN causes loss of comms and you can't
// change the rest of your params or veryofy it suceeded.
// change the rest of your params or verify it succeeded.
AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES];
#endif

389
Tools/AP_Periph/can.cpp
View File

@ -79,13 +79,13 @@ extern AP_Periph_FW periph;
#define HAL_CAN_POOL_SIZE 4000
#endif
static CanardInstance canard;
static uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)];
static CanardInstance canard[HAL_NUM_CAN_IFACES];
static uint32_t canard_memory_pool[HAL_NUM_CAN_IFACES][HAL_CAN_POOL_SIZE/sizeof(uint32_t)];
#ifndef HAL_CAN_DEFAULT_NODE_ID
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID
#endif
static uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID;
static uint8_t transfer_id;
static uint8_t transfer_id[HAL_NUM_CAN_IFACES];
#ifndef CAN_APP_NODE_NAME
#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph"
@ -113,14 +113,27 @@ HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
* Variables used for dynamic node ID allocation.
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
*/
static uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent
static uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request
static uint32_t send_next_node_id_allocation_request_at_ms[HAL_NUM_CAN_IFACES]; ///< When the next node ID allocation request should be sent
static uint8_t node_id_allocation_unique_id_offset[HAL_NUM_CAN_IFACES]; ///< Depends on the stage of the next request
/*
* Node status variables
*/
static uavcan_protocol_NodeStatus node_status;
/**
* Get interface id given canard object pointer
*/
static uint8_t get_canard_iface_id(CanardInstance* ins)
{
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
if (ins == &canard[i]) {
return i;
}
}
// something is not right if we got here
return 0;
}
/**
* Returns a pseudo random integer in a given range
@ -350,6 +363,11 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr
total_size);
}
static int16_t canard_broadcast(uint64_t data_type_signature,
uint16_t data_type_id,
uint8_t priority,
const void* payload,
uint16_t payload_len);
static void processTx(void);
static void processRx(void);
@ -409,15 +427,16 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
{
const uint8_t iface = get_canard_iface_id(ins);
// Rule C - updating the randomized time interval
send_next_node_id_allocation_request_at_ms =
send_next_node_id_allocation_request_at_ms[iface] =
AP_HAL::native_millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
{
printf("Allocation request from another allocatee\n");
node_id_allocation_unique_id_offset = 0;
node_id_allocation_unique_id_offset[iface] = 0;
return;
}
@ -437,14 +456,14 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
// 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\n");
node_id_allocation_unique_id_offset = 0;
node_id_allocation_unique_id_offset[iface] = 0;
return; // No match, return
}
if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) {
// 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_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
node_id_allocation_unique_id_offset[iface] = received_unique_id_len;
send_next_node_id_allocation_request_at_ms[iface] -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
printf("Matching allocation response: %d\n", received_unique_id_len);
} else {
@ -792,10 +811,8 @@ static void can_safety_button_update(void)
uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {};
uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer);
canardBroadcast(&canard,
ARDUPILOT_INDICATION_BUTTON_SIGNATURE,
canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE,
ARDUPILOT_INDICATION_BUTTON_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -977,37 +994,72 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
return false;
}
static void cleanup_stale_transactions(uint64_t &timestamp_usec)
{
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
canardCleanupStaleTransfers(&canard[i], timestamp_usec);
}
}
static int16_t canard_broadcast(uint64_t data_type_signature,
uint16_t data_type_id,
uint8_t priority,
const void* payload,
uint16_t payload_len)
{
int16_t cache = -1;
bool success = false;
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
if (canardGetLocalNodeID(&canard[i]) == CANARD_BROADCAST_NODE_ID) {
continue;
}
cache = canardBroadcast(&canard[i],
data_type_signature,
data_type_id,
&transfer_id[i],
priority,
payload,
payload_len);
if (cache == 0) {
// we had atleast one interface where transaction was successful
success = true;
}
}
return success?0:cache;
}
static void processTx(void)
{
static uint8_t fail_count;
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
AP_HAL::CANFrame txmsg {};
txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout
bool sent_ok = false;
const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
#if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
#endif
sent_ok |= (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0);
static uint8_t fail_count[HAL_NUM_CAN_IFACES];
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
if (AP::periph().can_iface_periph[i] == NULL) {
continue;
}
if (sent_ok) {
canardPopTxQueue(&canard);
fail_count = 0;
} else {
// just exit and try again later. If we fail 8 times in a row
// then start discarding to prevent the pool filling up
if (fail_count < 8) {
fail_count++;
#if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
#endif
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard[i])) != NULL;) {
AP_HAL::CANFrame txmsg {};
txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout
const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
if (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0) {
canardPopTxQueue(&canard[i]);
fail_count[i] = 0;
} else {
canardPopTxQueue(&canard);
// just exit and try again later. If we fail 8 times in a row
// then start discarding to prevent the pool filling up
if (fail_count[i] < 8) {
fail_count[i]++;
} else {
canardPopTxQueue(&canard[i]);
}
break;
}
return;
}
}
}
@ -1015,41 +1067,39 @@ static void processTx(void)
static void processRx(void)
{
AP_HAL::CANFrame rxmsg;
while (true) {
bool got_pkt = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
for (uint8_t iface=0; iface<HAL_NUM_CAN_IFACES; iface++) {
if (AP::periph().can_iface_periph[iface] == NULL) {
continue;
}
#if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
if (periph.can_protocol_cached[iface] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
#endif
while (true) {
bool read_select = true;
bool write_select = false;
AP::periph().can_iface_periph[i]->select(read_select, write_select, nullptr, 0);
if (!read_select) {
continue;
AP::periph().can_iface_periph[iface]->select(read_select, write_select, nullptr, 0);
if (!read_select) { // No data pending
break;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
AP::periph().can_iface_periph[i]->receive(rxmsg, timestamp, flags);
AP::periph().can_iface_periph[iface]->receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id;
canardHandleRxFrame(&canard, &rx_frame, timestamp);
got_pkt = true;
}
if (!got_pkt) {
break;
canardHandleRxFrame(&canard[iface], &rx_frame, timestamp);
}
}
}
static uint16_t pool_peak_percent(void)
static uint16_t pool_peak_percent(const uint8_t iface)
{
const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard);
const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard[iface]);
const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks);
return peak_percent;
}
@ -1063,10 +1113,8 @@ static void node_status_send(void)
uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
const int16_t bc_res = canardBroadcast(&canard,
UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
const int16_t bc_res = canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
UAVCAN_PROTOCOL_NODESTATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
@ -1086,7 +1134,7 @@ static void process1HzTasks(uint64_t timestamp_usec)
/*
* Purging transfers that are no longer transmitted. This will occasionally free up some memory.
*/
canardCleanupStaleTransfers(&canard, timestamp_usec);
cleanup_stale_transactions(timestamp_usec);
/*
* Printing the memory usage statistics.
@ -1096,8 +1144,10 @@ static void process1HzTasks(uint64_t timestamp_usec)
* The recommended way to establish the minimal size of the memory pool is to stress-test the application and
* record the worst case memory usage.
*/
if (pool_peak_percent() > 70) {
printf("WARNING: ENLARGE MEMORY POOL\n");
for (uint8_t iface = 0; iface < HAL_NUM_CAN_IFACES; iface++) {
if (pool_peak_percent(iface) > 70) {
printf("WARNING: ENLARGE MEMORY POOL on Iface %d\n", iface);
}
}
}
@ -1163,31 +1213,95 @@ static void process1HzTasks(uint64_t timestamp_usec)
/*
wait for dynamic allocation of node ID
*/
static void can_wait_node_id(void)
static bool no_iface_finished_dna = true;
static bool can_do_dna(const uint8_t iface)
{
if (canardGetLocalNodeID(&canard[iface]) != CANARD_BROADCAST_NODE_ID) {
no_iface_finished_dna = false;
return true;
}
uint8_t node_id_allocation_transfer_id = 0;
if (no_iface_finished_dna) {
printf("Waiting for dynamic node ID allocation on IF%d... (pool %u)\n", iface, pool_peak_percent(iface));
}
const uint32_t now = AP_HAL::native_millis();
send_next_node_id_allocation_request_at_ms[iface] =
now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
// 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 (node_id_allocation_unique_id_offset[iface] == 0) {
allocation_request[0] |= 1; // First part of unique ID
}
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH];
readUniqueID(my_unique_id);
static const uint8_t MaxLenOfUniqueIDInRequest = 6;
uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - node_id_allocation_unique_id_offset[iface]);
if (uid_size > MaxLenOfUniqueIDInRequest) {
uid_size = MaxLenOfUniqueIDInRequest;
}
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset[iface]], uid_size);
// Broadcasting the request
const int16_t bcast_res = canardBroadcast(&canard[iface],
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
&node_id_allocation_transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&allocation_request[0],
(uint16_t) (uid_size + 1));
if (bcast_res < 0) {
printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, iface);
}
// Preparing for timeout; if response is received, this value will be updated from the callback.
node_id_allocation_unique_id_offset[iface] = 0;
if (no_iface_finished_dna) {
printf("Dynamic node ID allocation complete [IF%d][%d]\n", iface, canardGetLocalNodeID(&canard[iface]));
}
return false;
}
/*
wait until atleast one iface has Node Allocation finished
*/
static void can_wait_node_id() {
uint32_t now = AP_HAL::native_millis();
const uint32_t led_pattern = 0xAAAA;
uint8_t led_idx = 0;
uint32_t last_led_change = AP_HAL::native_millis();
const uint32_t led_change_period = 50;
while (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID)
{
printf("Waiting for dynamic node ID allocation... (pool %u)\n", pool_peak_percent());
while(true) {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
if (can_do_dna(i)) {
return;
}
}
uint32_t wait_until_ms = UINT32_MAX;
stm32_watchdog_pat();
uint32_t now = AP_HAL::native_millis();
send_next_node_id_allocation_request_at_ms =
now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
while (((now=AP_HAL::native_millis()) < send_next_node_id_allocation_request_at_ms) &&
(canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID))
{
while (((now=AP_HAL::native_millis()) < wait_until_ms) && no_iface_finished_dna) {
processTx();
processRx();
canardCleanupStaleTransfers(&canard, AP_HAL::native_micros64());
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
wait_until_ms = MIN(wait_until_ms, send_next_node_id_allocation_request_at_ms[i]);
}
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
canardCleanupStaleTransfers(&canard[i], AP_HAL::native_micros64());
}
stm32_watchdog_pat();
if (now - last_led_change > led_change_period) {
@ -1209,53 +1323,7 @@ static void can_wait_node_id(void)
periph.check_for_serial_reboot_cmd(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT);
#endif
}
if (canardGetLocalNodeID(&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 (node_id_allocation_unique_id_offset == 0)
{
allocation_request[0] |= 1; // First part of unique ID
}
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH];
readUniqueID(my_unique_id);
static const uint8_t MaxLenOfUniqueIDInRequest = 6;
uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - node_id_allocation_unique_id_offset);
if (uid_size > MaxLenOfUniqueIDInRequest)
{
uid_size = MaxLenOfUniqueIDInRequest;
}
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
// Broadcasting the request
const int16_t bcast_res = canardBroadcast(&canard,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
&node_id_allocation_transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&allocation_request[0],
(uint16_t) (uid_size + 1));
if (bcast_res < 0)
{
printf("Could not broadcast ID allocation req; error %d\n", bcast_res);
}
// Preparing for timeout; if response is received, this value will be updated from the callback.
node_id_allocation_unique_id_offset = 0;
}
printf("Dynamic node ID allocation complete [%d]\n", canardGetLocalNodeID(&canard));
}
void AP_Periph_FW::can_start()
@ -1298,16 +1366,15 @@ void AP_Periph_FW::can_start()
if (can_iface_periph[i] != nullptr) {
can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
}
canardInit(&canard[i], (uint8_t *)canard_memory_pool[i], sizeof(canard_memory_pool[i]),
onTransferReceived, shouldAcceptTransfer, NULL);
if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) {
canardSetLocalNodeID(&canard[i], PreferredNodeID);
}
}
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, NULL);
if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) {
canardSetLocalNodeID(&canard, PreferredNodeID);
}
// wait for dynamic node ID allocation
// wait for dynamic node ID allocation on atleast one iface
can_wait_node_id();
}
@ -1356,10 +1423,8 @@ void AP_Periph_FW::pwm_hardpoint_update()
uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE,
UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1390,10 +1455,8 @@ void AP_Periph_FW::hwesc_telem_update()
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
UAVCAN_EQUIPMENT_ESC_STATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1403,6 +1466,10 @@ void AP_Periph_FW::hwesc_telem_update()
void AP_Periph_FW::can_update()
{
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
can_do_dna(i);
}
static uint32_t last_1Hz_ms;
uint32_t now = AP_HAL::native_millis();
if (now - last_1Hz_ms >= 1000) {
@ -1481,10 +1548,8 @@ void AP_Periph_FW::can_mag_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,
UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1547,10 +1612,8 @@ void AP_Periph_FW::can_battery_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {};
const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE,
UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1647,10 +1710,8 @@ void AP_Periph_FW::can_gps_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_FIX_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1740,10 +1801,8 @@ void AP_Periph_FW::can_gps_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_FIX2_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1761,10 +1820,8 @@ void AP_Periph_FW::can_gps_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1790,10 +1847,8 @@ void AP_Periph_FW::can_gps_update(void)
uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {};
const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer);
canardBroadcast(&canard,
ARDUPILOT_GNSS_STATUS_SIGNATURE,
canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE,
ARDUPILOT_GNSS_STATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1833,10 +1888,8 @@ void AP_Periph_FW::can_baro_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE,
UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1853,10 +1906,8 @@ void AP_Periph_FW::can_baro_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE,
UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1918,10 +1969,8 @@ void AP_Periph_FW::can_airspeed_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,
UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -1999,10 +2048,8 @@ void AP_Periph_FW::can_rangefinder_update(void)
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -2066,10 +2113,8 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg)
uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {};
uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer);
canardBroadcast(&canard,
ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE,
canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE,
ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
@ -2091,10 +2136,8 @@ void can_printf(const char *fmt, ...)
uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);