From 3d160b783230401105dfe1f65aaaecb73ba86aaa Mon Sep 17 00:00:00 2001
From: Pavlo Kolomiiets <pkolomiets@gmail.com>
Date: Sat, 26 Jan 2019 11:14:00 +0900
Subject: [PATCH] Rover: send mavlink WHEEL_DISTANCE message

---
 APMrover2/GCS_Mavlink.cpp | 18 ++++++++++++++++++
 APMrover2/Rover.h         |  2 ++
 APMrover2/sensors.cpp     |  3 +++
 3 files changed, 23 insertions(+)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 6a4edfcc9a..7064657473 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -287,6 +287,18 @@ void Rover::send_wheel_encoder(mavlink_channel_t chan)
     }
 }
 
+void Rover::send_wheel_encoder_distance(mavlink_channel_t chan)
+{
+    // send wheel encoder data using wheel_distance message
+    if (g2.wheel_encoder.num_sensors() > 0) {
+        double distances[16] {};
+        for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
+            distances[i] = wheel_encoder_last_distance_m[i];
+        }
+        mavlink_msg_wheel_distance_send(chan, 1000UL * wheel_encoder_last_ekf_update_ms, g2.wheel_encoder.num_sensors(), distances);
+    }
+}
+
 uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const
 {
     return rover.g.sysid_my_gcs;
@@ -346,6 +358,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         rover.send_wheel_encoder(chan);
         break;
 
+    case MSG_WHEEL_DISTANCE:
+        CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
+        rover.send_wheel_encoder_distance(chan);
+        break;
+
     case MSG_FENCE_STATUS:
         CHECK_PAYLOAD_SIZE(FENCE_STATUS);
         rover.send_fence_status(chan);
@@ -518,6 +535,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_EKF_STATUS_REPORT,
     MSG_VIBRATION,
     MSG_RPM,
+    MSG_WHEEL_DISTANCE,
     MSG_ESC_TELEMETRY,
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 86b9a43237..8520330b7a 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -336,6 +336,7 @@ private:
 
     // last wheel encoder update times
     float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES];     // distance in radians at time of last update to EKF
+    float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES];    // distance in meters at time of last update to EKF (for reporting to GCS)
     uint32_t wheel_encoder_last_update_ms[WHEELENCODER_MAX_INSTANCES];  // system time of last ping from each encoder
     uint32_t wheel_encoder_last_ekf_update_ms;                          // system time of last encoder data push to EKF
     float wheel_encoder_rpm[WHEELENCODER_MAX_INSTANCES];                // for reporting to GCS
@@ -443,6 +444,7 @@ private:
     void send_servo_out(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);
     void send_wheel_encoder(mavlink_channel_t chan);
+    void send_wheel_encoder_distance(mavlink_channel_t chan);
     void send_fence_status(mavlink_channel_t chan);
 
     // Log.cpp
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index abc38be44a..8384208dfb 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -99,6 +99,9 @@ void Rover::update_wheel_encoder()
         const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i];
         wheel_encoder_last_angle_rad[i] = curr_angle_rad;
 
+        // save cumulative distances at current time (in meters)
+        wheel_encoder_last_distance_m[i] = g2.wheel_encoder.get_distance(i);
+
         // calculate delta time
         float delta_time;
         const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);