AP_Periph: break out node status send

Tools/AP_Periph/can.cpp

@@ -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()) {