LP-327 - implement some kind of pseudo variance to be used to determine if copter is steady

flight/libraries/math/mathmisc.c

@@ -28,5 +28,17 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include <mathmisc.h>
 
-// space deliberately left empty, any non inline misc math functions can go here
+void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size)
+{
+    variance->new_sma  = 0.0f;
+    variance->new_smsa = 0.0f;
+    variance->p1 = 1.0f / (float)window_size;
+    variance->p2 = 1.0f - variance->p1;
+}
+
+float pseudo_windowed_variance_get(pw_variance_t *variance)
+{
+    return variance->new_smsa - variance->new_sma * variance->new_sma;
+}

flight/libraries/math/mathmisc.h

@@ -34,6 +34,38 @@
 #include <math.h>
 #include <stdint.h>
 
+typedef struct {
+    float p1;
+    float p2;
+    float new_sma;
+    float new_smsa;
+} pw_variance_t;
+
+/***
+ * initialize pseudo windowed
+ * @param variance the instance to be initialized
+ * @param window_size size of the sample window
+ */
+void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size);
+
+/***
+ * Push a new sample
+ * @param variance the working instance
+ * @param sample the new sample
+ */
+static inline void pseudo_windowed_variance_push_sample(pw_variance_t *variance, float sample)
+{
+    variance->new_sma  = variance->new_sma * variance->p2 + sample * variance->p1;
+    variance->new_smsa = variance->new_smsa * variance->p2 + sample * sample * variance->p1;
+}
+
+/***
+ * Get the current variance value
+ * @param variance the working instance
+ * @return
+ */
+float pseudo_windowed_variance_get(pw_variance_t *variance);
+
 // returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
 // returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
 // returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)