From 692281b27374104c562b170c09ca45b30d052552 Mon Sep 17 00:00:00 2001
From: jmachuca77 <jaime@element.aero>
Date: Sun, 30 Dec 2018 14:43:39 -0600
Subject: [PATCH] Rover: adding AP_RPM

---
 APMrover2/APMrover2.cpp   |  1 +
 APMrover2/GCS_Mavlink.cpp | 15 ++++++++++++++-
 APMrover2/Parameters.cpp  |  4 ++++
 APMrover2/Parameters.h    |  3 ++-
 APMrover2/Rover.h         |  6 ++++++
 APMrover2/sensors.cpp     | 12 ++++++++++++
 APMrover2/system.cpp      |  3 +++
 7 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 7242b69d82..7d5b4ec1b5 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -66,6 +66,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK_CLASS(RC_Channels,         (RC_Channels*)&rover.g2.rc_channels, read_aux_all,           10,    200),
     SCHED_TASK_CLASS(AP_BattMonitor,      &rover.battery,          read,           10,  300),
     SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events,  50,  200),
+    SCHED_TASK(rpm_update,             10,    100),
 #if MOUNT == ENABLED
     SCHED_TASK_CLASS(AP_Mount,            &rover.camera_mount,     update,         50,  200),
 #endif
diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index bd48aed36f..c7c0387785 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -160,6 +160,19 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
         voltage);
 }
 
+/*
+  send RPM packet
+ */
+void Rover::send_rpm(mavlink_channel_t chan)
+{
+    if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
+        mavlink_msg_rpm_send(
+            chan,
+            rpm_sensor.get_rpm(0),
+            rpm_sensor.get_rpm(1));
+    }
+}
+
 /*
   send PID tuning message
  */
@@ -325,7 +338,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
 
     case MSG_RPM:
         CHECK_PAYLOAD_SIZE(RPM);
-        rover.send_wheel_encoder(chan);
+        rover.send_rpm(chan);
         break;
 
     case MSG_WHEEL_DISTANCE:
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 7f388a3160..f3ad8f4962 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -409,6 +409,10 @@ const AP_Param::Info Rover::var_info[] = {
     GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3),
 #endif
 
+    // @Group: RPM
+    // @Path: ../libraries/AP_RPM/AP_RPM.cpp
+    GOBJECT(rpm_sensor, "RPM", AP_RPM),
+
     // @Group: MIS_
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index a1b7fd7da9..a9e7279d60 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -58,7 +58,8 @@ public:
 
         // 97: RSSI
         k_param_rssi = 97,
-
+        k_param_rpm_sensor,     // rpm sensor 98
+        
         // 100: Arming parameters
         k_param_arming = 100,
 
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 5942c557fc..b6c43e9ee5 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -54,6 +54,7 @@
 #include <AP_RangeFinder/AP_RangeFinder.h>          // Range finder library
 #include <AP_RCMapper/AP_RCMapper.h>                // RC input mapping library
 #include <AP_Relay/AP_Relay.h>                      // APM relay
+#include <AP_RPM/AP_RPM.h>
 #include <AP_RSSI/AP_RSSI.h>                        // RSSI Library
 #include <AP_Scheduler/AP_Scheduler.h>              // main loop scheduler
 #include <AP_SerialManager/AP_SerialManager.h>      // Serial manager library
@@ -177,6 +178,9 @@ private:
     AP_Int8 *modes;
     const uint8_t num_modes = 6;
 
+    // AP_RPM Module
+    AP_RPM rpm_sensor;
+
     // Inertial Navigation EKF
 #if AP_AHRS_NAVEKF_AVAILABLE
     NavEKF2 EKF2{&ahrs, rangefinder};
@@ -441,6 +445,7 @@ private:
     void send_nav_controller_output(mavlink_channel_t chan);
     void send_servo_out(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);
+    void send_rpm(mavlink_channel_t chan);
     void send_wheel_encoder(mavlink_channel_t chan);
     void send_wheel_encoder_distance(mavlink_channel_t chan);
 
@@ -509,6 +514,7 @@ private:
     void set_servos(void);
 
     // system.cpp
+    void rpm_update(void);
     void init_ardupilot();
     void startup_ground(void);
     void update_ahrs_flyforward();
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index 8384208dfb..173a7f2aea 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -252,6 +252,18 @@ void Rover::read_airspeed(void)
     g2.airspeed.update(should_log(MASK_LOG_IMU));
 }
 
+/*
+  update RPM sensors
+ */
+void Rover::rpm_update(void)
+{
+    rpm_sensor.update();
+    if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
+        if (should_log(MASK_LOG_RC)) {
+            logger.Write_RPM(rpm_sensor);
+        }
+    }
+}
 // update error mask of sensors and subsystems. The mask
 // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
 // then it indicates that the sensor or subsystem is present but
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index fe9ab7140b..08e38e5fac 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -62,6 +62,9 @@ void Rover::init_ardupilot()
 
     battery.init();
 
+    // Initialise RPM sensor
+    rpm_sensor.init();
+
     rssi.init();
 
     g2.airspeed.init();