From ddc70058d535fa6305fabe13385ff3d48e17e5c3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 21 Jul 2017 11:56:39 +0900
Subject: [PATCH] Rover: reporting wheel encoder rpm

---
 APMrover2/GCS_Mavlink.cpp | 15 ++++++++++++++-
 APMrover2/Rover.h         |  2 ++
 APMrover2/sensors.cpp     |  8 ++++++++
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 29fe8ef538..d0dd65fbba 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -269,6 +269,14 @@ void Rover::send_current_waypoint(mavlink_channel_t chan)
     mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
 }
 
+void Rover::send_wheel_encoder(mavlink_channel_t chan)
+{
+    // send wheel encoder data using rpm message
+    if (g2.wheel_encoder.enabled(0) || g2.wheel_encoder.enabled(1)) {
+        mavlink_msg_rpm_send(chan, wheel_encoder_rpm[0], wheel_encoder_rpm[1]);
+    }
+}
+
 uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const
 {
     return rover.g.sysid_my_gcs;
@@ -415,6 +423,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         send_distance_sensor(rover.rangefinder);
         break;
 
+    case MSG_RPM:
+        CHECK_PAYLOAD_SIZE(RPM);
+        rover.send_wheel_encoder(chan);
+        break;
+
     case MSG_MOUNT_STATUS:
 #if MOUNT == ENABLED
         CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
@@ -481,7 +494,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
     case MSG_TERRAIN:
     case MSG_OPTICAL_FLOW:
     case MSG_GIMBAL_REPORT:
-    case MSG_RPM:
     case MSG_POSITION_TARGET_GLOBAL_INT:
     case MSG_LANDING:
         break;  // just here to prevent a warning
@@ -691,6 +703,7 @@ GCS_MAVLINK_Rover::data_stream_send(void)
         send_message(MSG_MOUNT_STATUS);
         send_message(MSG_EKF_STATUS_REPORT);
         send_message(MSG_VIBRATION);
+        send_message(MSG_RPM);
     }
 }
 
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index cdd35b294d..1e322af1e5 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -415,6 +415,7 @@ private:
     float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES];     // distance in radians at time of last update to EKF
     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
 
     // True when we are doing motor test
     bool motor_test;
@@ -460,6 +461,7 @@ private:
     void send_pid_tuning(mavlink_channel_t chan);
     void send_rangefinder(mavlink_channel_t chan);
     void send_current_waypoint(mavlink_channel_t chan);
+    void send_wheel_encoder(mavlink_channel_t chan);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_retry_deferred(void);
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index e720d9d400..a8b2cd6bdb 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -148,6 +148,14 @@ void Rover::update_wheel_encoder()
          * posOffset is the XYZ body frame position of the wheel hub (m)
          */
         EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_position(i), g2.wheel_encoder.get_wheel_radius(i));
+
+        // calculate rpm for reporting to GCS
+        if (is_positive(delta_time)) {
+            wheel_encoder_rpm[i] = (delta_angle / M_2PI) / (delta_time / 60.0f);
+        } else {
+            wheel_encoder_rpm[i] = 0.0f;
+        }
+
     }
 
     // record system time update for next iteration