aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: break out node status send

Browse Source
This commit is contained in:
Andrew Tridgell 2020-07-09 07:30:45 +10:00
parent 1a4f094025
commit 9fa60be1ba
1 changed files with 25 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
Tools/AP_Periph/can.cpp
View File

@ -852,6 +852,30 @@ static uint16_t pool_peak_percent(void)
return peak_percent;
}
static void node_status_send(void)
{
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
node_status.uptime_sec = AP_HAL::millis() / 1000U;
node_status.vendor_specific_status_code = hal.util->available_memory();
uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
const int16_t bc_res = canardBroadcast(&canard,
UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
UAVCAN_PROTOCOL_NODESTATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
if (bc_res <= 0) {
printf("broadcast fail %d\n", bc_res);
} else {
//printf("broadcast node status OK\n");
}
}
/**
* This function is called at 1 Hz rate from the main loop.
*/
@ -878,27 +902,7 @@ static void process1HzTasks(uint64_t timestamp_usec)
/*
* Transmitting the node status message periodically.
*/
{
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE] {};
node_status.uptime_sec = AP_HAL::native_millis() / 1000U;
node_status.vendor_specific_status_code = hal.util->available_memory();
uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
const int16_t bc_res = canardBroadcast(&canard,
UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
UAVCAN_PROTOCOL_NODESTATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
if (bc_res <= 0) {
printf("broadcast fail %d\n", bc_res);
} else {
//printf("broadcast node status OK\n");
}
}
node_status_send();
#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
if (periph.g.flash_bootloader.get()) {