From c9267f30971c3f7c1dfc56213cf469de8b2d44b9 Mon Sep 17 00:00:00 2001
From: stevendaniluk <stevendaniluk8@gmail.com>
Date: Sun, 30 Jun 2019 19:36:29 -0700
Subject: [PATCH] Apply a simple moving average to incoming transmitter
 commands

---
 arduino/CMakeLists.txt          |  1 +
 arduino/car_interface.cpp       | 18 +++++++---
 arduino/simple_moving_average.h | 61 +++++++++++++++++++++++++++++++++
 3 files changed, 76 insertions(+), 4 deletions(-)
 create mode 100644 arduino/simple_moving_average.h

diff --git a/arduino/CMakeLists.txt b/arduino/CMakeLists.txt
index e63ce1a..c570bff 100644
--- a/arduino/CMakeLists.txt
+++ b/arduino/CMakeLists.txt
@@ -7,6 +7,7 @@ generate_arduino_firmware(car_interface
   HDRS interrupt_pwm_signal.h
   HDRS PinChangeInt.h
   HDRS signal_map.h
+  HDRS simple_moving_average.h
   BOARD nano328
   PORT /dev/ttyUSB0
 )
diff --git a/arduino/car_interface.cpp b/arduino/car_interface.cpp
index 92dd456..921ef7e 100644
--- a/arduino/car_interface.cpp
+++ b/arduino/car_interface.cpp
@@ -18,6 +18,7 @@ the controller commands (from the incoming topic) are issued.
 #include "Arduino.h"
 #include "interrupt_pwm_signal.h"
 #include "signal_map.h"
+#include "simple_moving_average.h"
 
 // Use PinChangeInt library to detect rising/falling/change on any pin
 // Declare which ports will not be used to save memory
@@ -43,8 +44,8 @@ the controller commands (from the incoming topic) are issued.
 // positions for the servo positions and incoming PWM signals Argument order is: min, center, max.
 const SignalMap steering_servo_map(63, 100, 137);
 const SignalMap throttle_servo_map(58, 87, 132);
-const SignalMap steering_pwm_map(1020, 1475, 1970);
-const SignalMap throttle_pwm_map(1020, 1392, 1970);
+const SignalMap steering_pwm_map(1020, 1485, 1965);
+const SignalMap throttle_pwm_map(1020, 1392, 1965);
 
 // Threshold PWM value for 3rd channel switch to trigger override mode
 const uint16_t override_pwm_thresh = 1500;
@@ -63,6 +64,11 @@ const int16_t cmd_timout = 500;
 bool override_active = false;
 bool prev_override_active = false;
 
+// Helpers to compute a moving average of the incoming controls from the transmitter, since the
+// signals can be noisy
+SimpleMovingAverage<uint16_t, 5> steering_sma(steering_pwm_map.center);
+SimpleMovingAverage<uint16_t, 5> throttle_sma(throttle_pwm_map.center);
+
 // Arduino servo objects for controlling the steering servo and ESC
 Servo steering;
 Servo throttle;
@@ -156,9 +162,13 @@ void loop() {
         throttle_input.processNewSignals();
         interrupts();
 
+        // Apply the moving average to smooth the signals
+        steering_sma.addDataPoint(steering_input.pwm_local);
+        throttle_sma.addDataPoint(throttle_input.pwm_local);
+
         // Map the pwm inputs to the [-1, 1] range for controls
-        cmd_execute.steering = -steering_pwm_map.mapToUnitOutput(steering_input.pwm_local);
-        cmd_execute.throttle = throttle_pwm_map.mapToUnitOutput(throttle_input.pwm_local);
+        cmd_execute.steering = -steering_pwm_map.mapToUnitOutput(steering_sma.getAverage());
+        cmd_execute.throttle = throttle_pwm_map.mapToUnitOutput(throttle_sma.getAverage());
     } else {
         // Check controller timeout
         if ((millis() - cmd_receive_time) > cmd_timout) {
diff --git a/arduino/simple_moving_average.h b/arduino/simple_moving_average.h
new file mode 100644
index 0000000..425b115
--- /dev/null
+++ b/arduino/simple_moving_average.h
@@ -0,0 +1,61 @@
+#pragma once
+
+/* SimpleMovingAverage
+ *
+ * Computes and stores a simple moving average over a fixed window size.
+ *
+ * The template parameter N is the window size.
+ */
+template <typename DataT, uint8_t N>
+class SimpleMovingAverage {
+  public:
+    /* SimpleMovingAverage
+     *
+     * @param init_value: Value to initialize the moving average to
+     */
+    SimpleMovingAverage(const DataT& init_value = 0) { reset(init_value); }
+
+    /* reset
+     *
+     * @param new_value: Value to assign to the current average and all values in the window
+     */
+    void reset(const DataT& new_value = 0) {
+        average_ = new_value;
+
+        index_ = 0;
+        for (uint8_t i = 0; i < N; ++i) {
+            data_[i] = new_value;
+        }
+    }
+
+    /* addDataPoint
+     *
+     * @param new_point: New data value to add to the moving average
+     */
+    void addDataPoint(const DataT& new_point) {
+        // Get the oldest data point in the history, replace it with the newest, and update the
+        // index for out position in the window
+        const DataT oldest = data_[index_];
+        data_[index_] = new_point;
+        index_++;
+        if (index_ == N) {
+            index_ = 0;
+        }
+
+        // Can perform an incremental update based on the new and oldest points in the window
+        average_ += (static_cast<float>(new_point) - static_cast<float>(oldest)) / N;
+    }
+
+    /* getAverage
+     *
+     * @return: Current value of the moving average
+     */
+    DataT getAverage() const { return static_cast<DataT>(average_); }
+
+  private:
+    // All data in the window
+    DataT data_[N];
+    uint8_t index_;
+    // Current average value
+    float average_;
+};  // end SimpleMovingAverage