From 7f0bd06e8b59eb256f1afdbb7d485cf94c0ae15f Mon Sep 17 00:00:00 2001
From: Stephen Dade <stephen_dade@hotmail.com>
Date: Tue, 6 Jul 2021 20:53:38 +1000
Subject: [PATCH] Rover: Add support for HIGH_LATENCY2 messages

---
 Rover/GCS_Mavlink.cpp | 50 +++++++++++++++++++++++++++++++++++++++++++
 Rover/GCS_Mavlink.h   |  7 ++++++
 2 files changed, 57 insertions(+)

diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp
index 9684197853..694c1e1200 100644
--- a/Rover/GCS_Mavlink.cpp
+++ b/Rover/GCS_Mavlink.cpp
@@ -1054,3 +1054,53 @@ uint64_t GCS_MAVLINK_Rover::capabilities() const
             MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
             GCS_MAVLINK::capabilities());
 }
+
+#if HAL_HIGH_LATENCY2_ENABLED
+uint8_t GCS_MAVLINK_Rover::high_latency_tgt_heading() const
+{
+    const Mode *control_mode = rover.control_mode;
+    if (rover.control_mode->is_autopilot_mode()) {
+        // need to convert -180->180 to 0->360/2
+        return wrap_360(control_mode->wp_bearing()) / 2;
+    }
+    return 0;      
+}
+    
+uint16_t GCS_MAVLINK_Rover::high_latency_tgt_dist() const
+{
+    const Mode *control_mode = rover.control_mode;
+    if (rover.control_mode->is_autopilot_mode()) {
+        // return units are dm
+        return MIN((control_mode->get_distance_to_destination()) / 10, UINT16_MAX);
+    }
+    return 0;  
+}
+
+uint8_t GCS_MAVLINK_Rover::high_latency_tgt_airspeed() const
+{
+    const Mode *control_mode = rover.control_mode;
+    if (rover.control_mode->is_autopilot_mode()) {
+        // return units are m/s*5
+        return MIN((vfr_hud_airspeed() - control_mode->speed_error()) * 5, UINT8_MAX);
+    }
+    return 0;
+}
+
+uint8_t GCS_MAVLINK_Rover::high_latency_wind_speed() const
+{
+    if (rover.g2.windvane.enabled()) {
+        // return units are m/s*5
+        return MIN(rover.g2.windvane.get_true_wind_speed() * 5, UINT8_MAX);
+    }
+    return 0; 
+}
+
+uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
+{
+    if (rover.g2.windvane.enabled()) {
+        // return units are deg/2
+        return wrap_360(degrees(rover.g2.windvane.get_true_wind_direction_rad())) / 2;
+    }
+    return 0; 
+}
+#endif // HAL_HIGH_LATENCY2_ENABLED
\ No newline at end of file
diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h
index 3ede3bf492..d5a2e2db21 100644
--- a/Rover/GCS_Mavlink.h
+++ b/Rover/GCS_Mavlink.h
@@ -53,4 +53,11 @@ private:
 
     void send_rangefinder() const override;
 
+#if HAL_HIGH_LATENCY2_ENABLED
+    uint8_t high_latency_tgt_heading() const override;
+    uint16_t high_latency_tgt_dist() const override;
+    uint8_t high_latency_tgt_airspeed() const override;
+    uint8_t high_latency_wind_speed() const override;
+    uint8_t high_latency_wind_direction() const override;
+#endif // HAL_HIGH_LATENCY2_ENABLED
 };