From 6dcb0af697ff0ab406f43b7dab472ff1089130ee Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 14 Mar 2024 00:07:27 +0000
Subject: [PATCH] AP_Periph: add support for streaming dedicated RPM message

---
 Tools/AP_Periph/AP_Periph.h    |  9 ++++++
 Tools/AP_Periph/Parameters.cpp | 11 +++++++
 Tools/AP_Periph/Parameters.h   |  5 ++++
 Tools/AP_Periph/can.cpp        |  3 ++
 Tools/AP_Periph/rpm.cpp        | 55 ++++++++++++++++++++++++++++++++++
 5 files changed, 83 insertions(+)
 create mode 100644 Tools/AP_Periph/rpm.cpp

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index eff5ccd6b04..b6db8f6a11c 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -91,6 +91,10 @@
     #endif
 #endif
 
+#if defined(HAL_PERIPH_ENABLE_RPM_STREAM) && !defined(HAL_PERIPH_ENABLE_RPM)
+    #error "HAL_PERIPH_ENABLE_RPM_STREAM requires HAL_PERIPH_ENABLE_RPM"
+#endif
+
 #ifndef AP_PERIPH_SAFETY_SWITCH_ENABLED
 #define AP_PERIPH_SAFETY_SWITCH_ENABLED defined(HAL_PERIPH_ENABLE_RC_OUT)
 #endif
@@ -229,7 +233,12 @@ public:
 #ifdef HAL_PERIPH_ENABLE_RPM
     AP_RPM rpm_sensor;
     uint32_t rpm_last_update_ms;
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    void rpm_sensor_send();
+    uint32_t rpm_last_send_ms;
+    uint8_t rpm_last_sent_index;
 #endif
+#endif // HAL_PERIPH_ENABLE_RPM
 
 #ifdef HAL_PERIPH_ENABLE_BATTERY
     void handle_battery_failsafe(const char* type_str, const int8_t action) { }
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 28768361706..61e42c29f34 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -692,6 +692,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @User: Standard
     GSCALAR(options, "OPTIONS", AP_PERIPH_PROBE_CONTINUOUS),
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    // @Param: RPM_MSG_RATE
+    // @DisplayName: RPM sensor message rate
+    // @Description: This is the rate RPM sensor data is sent in Hz. Zero means no send. Each sensor with a set ID is sent in turn.
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(rpm_msg_rate, "RPM_MSG_RATE", 0),
+#endif
+
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 9d51c97cb04..d68e38f7b4e 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -94,6 +94,7 @@ public:
         k_param_rangefinder_baud1,
         k_param_rangefinder_port1,
         k_param_options,
+        k_param_rpm_msg_rate,
     };
 
     AP_Int16 format_version;
@@ -212,6 +213,10 @@ public:
     AP_Int8 temperature_msg_rate;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    AP_Int16 rpm_msg_rate;
+#endif
+
 #if HAL_CANFD_SUPPORTED
     AP_Int8 can_fdmode;
     AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES];
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 80ae6fbc4fc..1b5607cdab1 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1848,6 +1848,9 @@ void AP_Periph_FW::can_update()
     #endif
 #ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
         temperature_sensor_update();
+#endif
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+        rpm_sensor_send();
 #endif
     }
     const uint32_t now_us = AP_HAL::micros();
diff --git a/Tools/AP_Periph/rpm.cpp b/Tools/AP_Periph/rpm.cpp
new file mode 100644
index 00000000000..5a39866013b
--- /dev/null
+++ b/Tools/AP_Periph/rpm.cpp
@@ -0,0 +1,55 @@
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+
+#include <dronecan_msgs.h>
+
+// Send rpm message occasionally
+void AP_Periph_FW::rpm_sensor_send(void)
+{
+    if (g.rpm_msg_rate <= 0) {
+        return;
+    }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - rpm_last_send_ms < (1000U / g.rpm_msg_rate)) {
+        return;
+    }
+    rpm_last_send_ms = now_ms;
+
+    {
+        const uint8_t num_sensors = rpm_sensor.num_sensors();
+        for (uint8_t i = 0; i < num_sensors; i++) {
+            // Send each sensor in turn
+            const uint8_t index = (rpm_last_sent_index + 1 + i) % num_sensors;
+
+            const int8_t sensor_id = rpm_sensor.get_dronecan_sensor_id(index);
+            if (sensor_id < 0) {
+                // disabled or not configured to send
+                continue;
+            }
+
+            dronecan_sensors_rpm_RPM pkt {};
+            pkt.sensor_id = sensor_id;
+
+            // Get rpm and set health flag
+            if (!rpm_sensor.get_rpm(index, pkt.rpm)) {
+                pkt.flags |= DRONECAN_SENSORS_RPM_RPM_FLAGS_UNHEALTHY;
+            }
+
+            uint8_t buffer[DRONECAN_SENSORS_RPM_RPM_MAX_SIZE] {};
+            const uint16_t total_size = dronecan_sensors_rpm_RPM_encode(&pkt, buffer, !canfdout());
+
+            canard_broadcast(DRONECAN_SENSORS_RPM_RPM_SIGNATURE,
+                             DRONECAN_SENSORS_RPM_RPM_ID,
+                             CANARD_TRANSFER_PRIORITY_LOW,
+                             &buffer[0],
+                             total_size);
+
+            rpm_last_sent_index = index;
+            break;
+        }
+    }
+}
+
+#endif // HAL_PERIPH_ENABLE_RPM_STREAM