src: mavlink_vehicle: Add initial heartbeat burst

* Add a initial heartbeat burst to awake MAVLink connection in case of
  a PX4

src/mavlink_vehicle.rs

@@ -69,6 +69,12 @@ impl<
             MAVLinkVehicle::<M>::new(connection_string, version, system_id, component_id),
         ));
 
+        // Send initial heartbeats to wake up MAVLink connection in case of a PX4
+        for _ in 0..10 {
+            send_heartbeat(mavlink_vehicle.clone());
+            std::thread::sleep(std::time::Duration::from_millis(100));
+        }
+
         let heartbeat_mavlink_vehicle = mavlink_vehicle.clone();
         let receive_message_mavlink_vehicle = mavlink_vehicle.clone();
 
@@ -116,18 +122,24 @@ fn receive_message_loop<
     }
 }
 
+fn send_heartbeat<M: mavlink::Message + From<mavlink::common::MavMessage>>(
+    mavlink_vehicle: Arc<Mutex<MAVLinkVehicle<M>>>,
+) {
+    let mavlink_vehicle = mavlink_vehicle.as_ref().lock().unwrap();
+    let vehicle = mavlink_vehicle.vehicle.clone();
+    let mut header = mavlink_vehicle.header.lock().unwrap();
+    if let Err(error) = vehicle.as_ref().send(&header, &heartbeat_message().into()) {
+        error!("Failed to send heartbeat: {:?}", error);
+    }
+    header.sequence = header.sequence.wrapping_add(1);
+}
+
 fn heartbeat_loop<M: mavlink::Message + From<mavlink::common::MavMessage>>(
     mavlink_vehicle: Arc<Mutex<MAVLinkVehicle<M>>>,
 ) {
     loop {
         std::thread::sleep(std::time::Duration::from_secs(1));
-        let mavlink_vehicle = mavlink_vehicle.as_ref().lock().unwrap();
-        let vehicle = mavlink_vehicle.vehicle.clone();
-        let mut header = mavlink_vehicle.header.lock().unwrap();
-        if let Err(error) = vehicle.as_ref().send(&header, &heartbeat_message().into()) {
-            error!("Failed to send heartbeat: {:?}", error);
-        }
-        header.sequence = header.sequence.wrapping_add(1);
+        send_heartbeat(mavlink_vehicle.clone());
     }
 }