From 25d231187f329aa31456c259d3aeb962a2363cad Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 28 May 2022 09:27:39 +1000
Subject: [PATCH] AP_Periph: added RNGFND_MAX_RATE parameter

prevent duplicate samples and allow setting of maximum rate we sample
rangefinder
---
 Tools/AP_Periph/AP_Periph.h    |  1 +
 Tools/AP_Periph/Parameters.cpp |  9 +++++++++
 Tools/AP_Periph/Parameters.h   |  2 ++
 Tools/AP_Periph/can.cpp        | 11 +++++++++--
 4 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index b3b136299a..8ab1bfd742 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -174,6 +174,7 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     RangeFinder rangefinder;
+    uint32_t last_sample_ms;
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 5eabb89bb0..240769eaaa 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -276,6 +276,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
 
+    // @Param: RNGFND_MAX_RATE
+    // @DisplayName: Rangefinder max rate
+    // @Description: This is the maximum rate we send rangefinder data in Hz. Zero means no limit
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Advanced
+    GSCALAR(rangefinder_max_rate, "RNGFND_MAX_RATE", 50),
+    
     // Rangefinder driver
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 520715cf7c..52d45f0b77 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -55,6 +55,7 @@ public:
         k_param_can_fdbaudrate0,
         k_param_can_fdbaudrate1,
         k_param_node_stats,
+        k_param_rangefinder_max_rate,
     };
 
     AP_Int16 format_version;
@@ -81,6 +82,7 @@ public:
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     AP_Int32 rangefinder_baud;
     AP_Int8 rangefinder_port;
+    AP_Int16 rangefinder_max_rate;
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_ADSB
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 93d4278521..b3b626df55 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -2239,8 +2239,9 @@ void AP_Periph_FW::can_rangefinder_update(void)
 #endif
     uint32_t now = AP_HAL::native_millis();
     static uint32_t last_update_ms;
-    if (now - last_update_ms < 20) {
-        // max 50Hz data
+    if (g.rangefinder_max_rate > 0 &&
+        now - last_update_ms < 1000/g.rangefinder_max_rate) {
+        // limit to max rate
         return;
     }
     last_update_ms = now;
@@ -2250,6 +2251,12 @@ void AP_Periph_FW::can_rangefinder_update(void)
         // don't send any data
         return;
     }
+    const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE);
+    if (last_sample_ms == sample_ms) {
+        return;
+    }
+    last_sample_ms = sample_ms;
+
     uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
     uavcan_equipment_range_sensor_Measurement pkt {};
     pkt.sensor_id = rangefinder.get_address(0);