added responsiveanalogread smoothing

include/imu.h

@@ -8,6 +8,7 @@
 #include <BMM150class.h>
 #include <utility/quaternionFilters.h>
 
+#include <ResponsiveAnalogRead.h>
 #include <Smoothed.h>
 
 #include <ArduinoJson.h>
@@ -32,6 +33,10 @@ extern Smoothed<float> yaw_smooth;
 extern Smoothed<float> pitch_smooth;
 extern Smoothed<float> roll_smooth;
 
+extern ResponsiveAnalogRead yaw_rar;
+extern ResponsiveAnalogRead pitch_rar;
+extern ResponsiveAnalogRead roll_rar;
+
 void initGyro();
 void calibGyro();
 void setup_imu();

lib/ResponsiveAnalogRead/LICENSE

@@ -0,0 +1,22 @@
+
+MIT License
+
+Copyright (c) 2016 Damien Clarke
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

lib/ResponsiveAnalogRead/README.md

@@ -0,0 +1,172 @@
+# ResponsiveAnalogRead
+
+![ResponsiveAnalogRead](https://user-images.githubusercontent.com/345320/50956817-c4631a80-1510-11e9-806a-27583707ca91.jpg)
+
+ResponsiveAnalogRead is an Arduino library for eliminating noise in analogRead inputs without decreasing responsiveness. It sets out to achieve the following:
+
+1. Be able to reduce large amounts of noise when reading a signal. So if a voltage is unchanging aside from noise, the values returned should never change due to noise alone.
+2. Be extremely responsive (i.e. not sluggish) when the voltage changes quickly.
+3. Have the option to be responsive when a voltage *stops* changing - when enabled the values returned must stop changing almost immediately after. When this option is enabled, a very small sacrifice in accuracy is permitted.
+4. The returned values must avoid 'jumping' up several numbers at once, especially when the input signal changes very slowly. It's better to transition smoothly as long as that smooth transition is short.
+
+You can preview the way the algorithm works with [sleep enabled](http://codepen.io/dxinteractive/pen/zBEbpP) (minimising the time spend transitioning between values) and with [sleep disabled](http://codepen.io/dxinteractive/pen/ezdJxL) (transitioning responsively and accurately but smooth).
+
+An article discussing the design of the algorithm can be found [here](http://damienclarke.me/code/posts/writing-a-better-noise-reducing-analogread).
+
+## How to use
+
+Here's a basic example:
+
+```Arduino
+// include the ResponsiveAnalogRead library
+#include <ResponsiveAnalogRead.h>
+
+// define the pin you want to use
+const int ANALOG_PIN = A0;
+
+// make a ResponsiveAnalogRead object, pass in the pin, and either true or false depending on if you want sleep enabled
+// enabling sleep will cause values to take less time to stop changing and potentially stop changing more abruptly,
+// where as disabling sleep will cause values to ease into their correct position smoothly and with slightly greater accuracy
+ResponsiveAnalogRead analog(ANALOG_PIN, true);
+
+// the next optional argument is snapMultiplier, which is set to 0.01 by default
+// you can pass it a value from 0 to 1 that controls the amount of easing
+// increase this to lessen the amount of easing (such as 0.1) and make the responsive values more responsive
+// but doing so may cause more noise to seep through if sleep is not enabled
+
+void setup() {
+  // begin serial so we can see analog read values through the serial monitor
+  Serial.begin(9600);
+}
+
+void loop() {
+  // update the ResponsiveAnalogRead object every loop
+  analog.update();
+
+  Serial.print(analog.getRawValue());
+  Serial.print("\t");
+  Serial.print(analog.getValue());
+  
+  // if the responsive value has change, print out 'changed'
+  if(analog.hasChanged()) {
+    Serial.print("\tchanged");
+  }
+  
+  Serial.println("");
+  delay(20);
+}
+```
+
+### Using your own ADC
+
+```Arduino
+#include <ResponsiveAnalogRead.h>
+
+ResponsiveAnalogRead analog(0, true);
+
+void setup() {
+  // begin serial so we can see analog read values through the serial monitor
+  Serial.begin(9600);
+}
+
+void loop() {
+  // read from your ADC
+  // update the ResponsiveAnalogRead object every loop
+  int reading = YourADCReadMethod();
+  analog.update(reading);
+  Serial.print(analog.getValue());
+  
+  Serial.println("");
+  delay(20);
+}
+```
+
+### Smoothing multiple inputs
+
+```Arduino
+#include <ResponsiveAnalogRead.h>
+
+ResponsiveAnalogRead analogOne(A1, true);
+ResponsiveAnalogRead analogTwo(A2, true);
+
+void setup() {
+  // begin serial so we can see analog read values through the serial monitor
+  Serial.begin(9600);
+}
+
+void loop() {
+  // update the ResponsiveAnalogRead objects every loop
+  analogOne.update();
+  analogTwo.update();
+  
+  Serial.print(analogOne.getValue());
+  Serial.print(analogTwo.getValue());
+  
+  Serial.println("");
+  delay(20);
+}
+```
+
+## How to install
+
+In the Arduino IDE, go to Sketch > Include libraries > Manage libraries, and search for ResponsiveAnalogRead.
+You can also just use the files directly from the src folder.
+
+Look at the example in the examples folder for an idea on how to use it in your own projects.
+The source files are also heavily commented, so check those out if you want fine control of the library's behaviour.
+
+## Constructor arguments
+
+- `pin` - int, the pin to read (e.g. A0).
+- `sleepEnable` - boolean, sets whether sleep is enabled. Defaults to true. Enabling sleep will cause values to take less time to stop changing and potentially stop changing more abruptly, where as disabling sleep will cause values to ease into their correct position smoothly.
+- `snapMultiplier` - float, a value from 0 to 1 that controls the amount of easing. Defaults to 0.01. Increase this to lessen the amount of easing (such as 0.1) and make the responsive values more responsive, but doing so may cause more noise to seep through if sleep is not enabled.
+
+## Basic methods
+
+- `int getValue() // get the responsive value from last update`
+- `int getRawValue() // get the raw analogRead() value from last update`
+- `bool hasChanged() // returns true if the responsive value has changed during the last update`
+- `void update(); // updates the value by performing an analogRead() and calculating a responsive value based off it`
+- `void update(int rawValue); // updates the value by accepting a raw value and calculating a responsive value based off it (version 1.1.0+)`
+- `bool isSleeping() // returns true if the algorithm is in sleep mode (version 1.1.0+)`
+
+## Other methods (settings)
+
+### Sleep
+
+- `void enableSleep()`
+- `void disableSleep()`
+
+Sleep allows you to minimise the amount of responsive value changes over time. Increasingly small changes in the output value to be ignored, so instead of having the responsiveValue slide into position over a couple of seconds, it stops when it's "close enough". It's enabled by default. Here's a summary of how it works:
+
+1. "Sleep" is when the output value decides to ignore increasingly small changes.
+2. When it sleeps, it is less likely to start moving again, but a large enough nudge will wake it up and begin responding as normal.
+3. It classifies changes in the input voltage as being "active" or not. A lack of activity tells it to sleep.
+
+### Activity threshold
+- `void setActivityThreshold(float newThreshold) // the amount of movement that must take place for it to register as activity and start moving the output value. Defaults to 4.0. (version 1.1+)`
+
+### Snap multiplier
+- `void setSnapMultiplier(float newMultiplier)`
+
+SnapMultiplier is a value from 0 to 1 that controls the amount of easing. Increase this to lessen the amount of easing (such as 0.1) and make the responsive values more responsive, but doing so may cause more noise to seep through when sleep is not enabled.
+
+### Edge snapping
+- `void enableEdgeSnap() // edge snap ensures that values at the edges of the spectrum (0 and 1023) can be easily reached when sleep is enabled`
+
+### Analog resolution
+- `void setAnalogResolution(int resolution)`
+
+If your ADC is something other than 10bit (1024), set that using this.
+
+## License
+
+Licensed under the MIT License (MIT)
+
+Copyright (c) 2016, Damien Clarke
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

lib/ResponsiveAnalogRead/examples/ResponsiveAnalogRead/ResponsiveAnalogRead.ino

@@ -0,0 +1,37 @@
+// include the ResponsiveAnalogRead library
+#include <ResponsiveAnalogRead.h>
+
+// define the pin you want to use
+const int ANALOG_PIN = A0;
+
+// make a ResponsiveAnalogRead object, pass in the pin, and either true or false depending on if you want sleep enabled
+// enabling sleep will cause values to take less time to stop changing and potentially stop changing more abruptly,
+//   where as disabling sleep will cause values to ease into their correct position smoothly and more accurately
+ResponsiveAnalogRead analog(ANALOG_PIN, true);
+
+// the next optional argument is snapMultiplier, which is set to 0.01 by default
+// you can pass it a value from 0 to 1 that controls the amount of easing
+// increase this to lessen the amount of easing (such as 0.1) and make the responsive values more responsive
+// but doing so may cause more noise to seep through if sleep is not enabled
+
+void setup() {
+  // begin serial so we can see analog read values through the serial monitor
+  Serial.begin(9600);
+}
+
+void loop() {
+  // update the ResponsiveAnalogRead object every loop
+  analog.update();
+
+  Serial.print(analog.getRawValue());
+  Serial.print("\t");
+  Serial.print(analog.getValue());
+  
+  // if the repsonsive value has change, print out 'changed'
+  if(analog.hasChanged()) {
+    Serial.print("\tchanged");
+  }
+  
+  Serial.println("");
+  delay(20);
+}

lib/ResponsiveAnalogRead/keywords.txt

@@ -0,0 +1,25 @@
+#######################################
+# Syntax Coloring Map
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+ResponsiveAnalogRead	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+getValue	KEYWORD2
+getRawValue	KEYWORD2
+hasChanged	KEYWORD2
+update	KEYWORD2
+setSnapMultiplier	KEYWORD2
+enableSleep	KEYWORD2
+disableSleep	KEYWORD2
+isSleeping	KEYWORD2
+setActivityThreshold	KEYWORD2
+setAnalogResolution	KEYWORD2
+enableEdgeSnap	KEYWORD2
+begin	KEYWORD2

lib/ResponsiveAnalogRead/library.properties

@@ -0,0 +1,9 @@
+name=ResponsiveAnalogRead
+version=1.2.1
+author=Damien Clarke <dxinteractive@gmail.com>
+maintainer=Damien Clarke <dxinteractive@gmail.com>
+sentence=Arduino library for eliminating noise in analogRead inputs without decreasing responsiveness
+paragraph=Arduino library for eliminating noise in analogRead inputs without decreasing responsiveness
+category=Signal Input/Output
+url=http://damienclarke.me/code/responsive-analog-read
+architectures=*

lib/ResponsiveAnalogRead/src/ResponsiveAnalogRead.cpp

@@ -0,0 +1,140 @@
+/*
+ * ResponsiveAnalogRead.cpp
+ * Arduino library for eliminating noise in analogRead inputs without decreasing responsiveness
+ *
+ * Copyright (c) 2016 Damien Clarke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <Arduino.h>
+#include "ResponsiveAnalogRead.h"
+
+void ResponsiveAnalogRead::begin(int pin, bool sleepEnable, float snapMultiplier){
+    pinMode(pin, INPUT ); // ensure button pin is an input
+    digitalWrite(pin, LOW ); // ensure pullup is off on button pin
+    
+    this->pin = pin;
+    this->sleepEnable = sleepEnable;
+    setSnapMultiplier(snapMultiplier);
+    
+    
+}
+
+
+void ResponsiveAnalogRead::update()
+{
+  rawValue = analogRead(pin);
+  this->update(rawValue);
+}
+
+void ResponsiveAnalogRead::update(int rawValueRead)
+{
+  rawValue = rawValueRead;
+  prevResponsiveValue = responsiveValue;
+  responsiveValue = getResponsiveValue(rawValue);
+  responsiveValueHasChanged = responsiveValue != prevResponsiveValue;
+}
+
+int ResponsiveAnalogRead::getResponsiveValue(int newValue)
+{
+  // if sleep and edge snap are enabled and the new value is very close to an edge, drag it a little closer to the edges
+  // This'll make it easier to pull the output values right to the extremes without sleeping,
+  // and it'll make movements right near the edge appear larger, making it easier to wake up
+  if(sleepEnable && edgeSnapEnable) {
+    if(newValue < activityThreshold) {
+      newValue = (newValue * 2) - activityThreshold;
+    } else if(newValue > analogResolution - activityThreshold) {
+      newValue = (newValue * 2) - analogResolution + activityThreshold;
+    }
+  }
+
+  // get difference between new input value and current smooth value
+  unsigned int diff = abs(newValue - smoothValue);
+
+  // measure the difference between the new value and current value
+  // and use another exponential moving average to work out what
+  // the current margin of error is
+  errorEMA += ((newValue - smoothValue) - errorEMA) * 0.4;
+
+  // if sleep has been enabled, sleep when the amount of error is below the activity threshold
+  if(sleepEnable) {
+    // recalculate sleeping status
+    sleeping = abs(errorEMA) < activityThreshold;
+  }
+
+  // if we're allowed to sleep, and we're sleeping
+  // then don't update responsiveValue this loop
+  // just output the existing responsiveValue
+  if(sleepEnable && sleeping) {
+    return (int)smoothValue;
+  }
+
+  // use a 'snap curve' function, where we pass in the diff (x) and get back a number from 0-1.
+  // We want small values of x to result in an output close to zero, so when the smooth value is close to the input value
+  // it'll smooth out noise aggressively by responding slowly to sudden changes.
+  // We want a small increase in x to result in a much higher output value, so medium and large movements are snappy and responsive,
+  // and aren't made sluggish by unnecessarily filtering out noise. A hyperbola (f(x) = 1/x) curve is used.
+  // First x has an offset of 1 applied, so x = 0 now results in a value of 1 from the hyperbola function.
+  // High values of x tend toward 0, but we want an output that begins at 0 and tends toward 1, so 1-y flips this up the right way.
+  // Finally the result is multiplied by 2 and capped at a maximum of one, which means that at a certain point all larger movements are maximally snappy
+
+  // then multiply the input by SNAP_MULTIPLER so input values fit the snap curve better.
+  float snap = snapCurve(diff * snapMultiplier);
+
+  // when sleep is enabled, the emphasis is stopping on a responsiveValue quickly, and it's less about easing into position.
+  // If sleep is enabled, add a small amount to snap so it'll tend to snap into a more accurate position before sleeping starts.
+  if(sleepEnable) {
+    snap *= 0.5 + 0.5;
+  }
+
+  // calculate the exponential moving average based on the snap
+  smoothValue += (newValue - smoothValue) * snap;
+
+  // ensure output is in bounds
+  if(smoothValue < 0.0) {
+    smoothValue = 0.0;
+  } else if(smoothValue > analogResolution - 1) {
+    smoothValue = analogResolution - 1;
+  }
+
+  // expected output is an integer
+  return (int)smoothValue;
+}
+
+float ResponsiveAnalogRead::snapCurve(float x)
+{
+  float y = 1.0 / (x + 1.0);
+  y = (1.0 - y) * 2.0;
+  if(y > 1.0) {
+    return 1.0;
+  }
+  return y;
+}
+
+void ResponsiveAnalogRead::setSnapMultiplier(float newMultiplier)
+{
+  if(newMultiplier > 1.0) {
+    newMultiplier = 1.0;
+  }
+  if(newMultiplier < 0.0) {
+    newMultiplier = 0.0;
+  }
+  snapMultiplier = newMultiplier;
+}

lib/ResponsiveAnalogRead/src/ResponsiveAnalogRead.h

@@ -0,0 +1,89 @@
+/*
+ * ResponsiveAnalogRead.h
+ * Arduino library for eliminating noise in analogRead inputs without decreasing responsiveness
+ *
+ * Copyright (c) 2016 Damien Clarke
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.  
+ */
+ 
+#ifndef RESPONSIVE_ANALOG_READ_H
+#define RESPONSIVE_ANALOG_READ_H
+
+#include <Arduino.h>
+
+class ResponsiveAnalogRead
+{
+  public:
+
+    // pin - the pin to read
+    // sleepEnable - enabling sleep will cause values to take less time to stop changing and potentially stop changing more abruptly,
+    //   where as disabling sleep will cause values to ease into their correct position smoothly
+    // snapMultiplier - a value from 0 to 1 that controls the amount of easing
+    //   increase this to lessen the amount of easing (such as 0.1) and make the responsive values more responsive
+    //   but doing so may cause more noise to seep through if sleep is not enabled
+    
+    ResponsiveAnalogRead(){};  //default constructor must be followed by call to begin function
+    ResponsiveAnalogRead(int pin, bool sleepEnable, float snapMultiplier = 0.01){
+        begin(pin, sleepEnable, snapMultiplier);
+    };
+
+    void begin(int pin, bool sleepEnable, float snapMultiplier = 0.01);  // use with default constructor to initialize 
+    
+    inline int getValue() { return responsiveValue; } // get the responsive value from last update
+    inline int getRawValue() { return rawValue; } // get the raw analogRead() value from last update
+    inline bool hasChanged() { return responsiveValueHasChanged; } // returns true if the responsive value has changed during the last update
+    inline bool isSleeping() { return sleeping; } // returns true if the algorithm is currently in sleeping mode
+    void update(); // updates the value by performing an analogRead() and calculating a responsive value based off it
+    void update(int rawValueRead); // updates the value accepting a value and calculating a responsive value based off it
+
+    void setSnapMultiplier(float newMultiplier);
+    inline void enableSleep() { sleepEnable = true; }
+    inline void disableSleep() { sleepEnable = false; }
+    inline void enableEdgeSnap() { edgeSnapEnable = true; }
+    // edge snap ensures that values at the edges of the spectrum (0 and 1023) can be easily reached when sleep is enabled
+    inline void disableEdgeSnap() { edgeSnapEnable = false; }
+    inline void setActivityThreshold(float newThreshold) { activityThreshold = newThreshold; }
+    // the amount of movement that must take place to register as activity and start moving the output value. Defaults to 4.0
+    inline void setAnalogResolution(int resolution) { analogResolution = resolution; }
+    // if your ADC is something other than 10bit (1024), set that here
+
+  private:
+    int pin;
+    int analogResolution = 1024;
+    float snapMultiplier;
+    bool sleepEnable;
+    float activityThreshold = 4.0;
+    bool edgeSnapEnable = true;
+
+    float smoothValue;
+    unsigned long lastActivityMS;
+    float errorEMA = 0.0;
+    bool sleeping = false;
+
+    int rawValue;
+    int responsiveValue;
+    int prevResponsiveValue;
+    bool responsiveValueHasChanged;
+
+    int getResponsiveValue(int newValue);
+    float snapCurve(float x);
+};
+
+#endif

src/imu.cpp

@@ -67,6 +67,10 @@ Smoothed<float> yaw_smooth;
 Smoothed<float> pitch_smooth;
 Smoothed<float> roll_smooth;
 
+ResponsiveAnalogRead yaw_rar(-1, true, 0.001);
+ResponsiveAnalogRead pitch_rar(-1, true);
+ResponsiveAnalogRead roll_rar(-1, true);
+
 void loadCalibration(const char *calibFile, Calib &calib) {
   // Open file for reading
   SPIFFS.begin();
@@ -205,6 +209,14 @@ void setup_imu()
     pitch_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
     roll_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
 
+    yaw_rar.setAnalogResolution(3600);
+    pitch_rar.setAnalogResolution(3600);
+    roll_rar.setAnalogResolution(3600);
+
+    yaw_rar.setActivityThreshold(30);
+    pitch_rar.setActivityThreshold(30);
+    roll_rar.setActivityThreshold(30);
+
     M5.IMU.Init();
     initGyro();
     bmm150.Init();
@@ -273,5 +285,8 @@ void handle_imu()
     pitch_smooth.add(pitch);
     roll_smooth.add(roll);
 
-    
+
+    yaw_rar.update(10.*yaw);
+    pitch_rar.update((int)10*pitch);
+    roll_rar.update((int)10*roll);
 }

src/main.cpp

@@ -30,7 +30,7 @@ extern void lcd_wakeup();
 #include "imu.h"
 
 
-int portal_rate = 40;
+int portal_rate = 30;
 long int portal_last_sent;
 
 ///////////////////////   SENSOR   ////////////////////////////////////
@@ -238,9 +238,10 @@ void loop() {
     sendOSC_3f("/ahrs", yaw_ahrs, pitch_ahrs, roll_ahrs);
     sendOSC_3f("/ahrs_fixed", yaw, pitch, roll);
     sendOSC_3f("/ahrs_smooth", yaw_smooth.get(), pitch_smooth.get(), roll_smooth.get());
+    sendOSC_3f("/ahrs_rar",yaw_rar.getValue()/10., pitch_rar.getValue()/10., roll_rar.getValue()/10.);
   }
 
-//update CD
+//update LCD
 //send sensor data 
   if(current_time-lcd_last_sent>lcd_rate){
     lcd_last_sent = current_time;