Diff: main.cpp

Revision:

4:77914e52baf3

Parent:

3:e03714326b83

Child:

5:d97332d7f812

--- a/main.cpp	Thu Aug 22 14:22:58 2019 +0000
+++ b/main.cpp	Wed Aug 28 11:43:15 2019 +0000
@@ -1,8 +1,9 @@
 #include "mbed.h"
 #include "PX4Flow.h"
 #include "LSM9DS1_i2c.h"
-// #include "LSM9DS1_Registers.h"
-// #include "LSM9DS1_Types.h"
+#include "EncoderCounter.h"
+#include "DiffCounter.h"
+#define   pi 3.1415927f
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
@@ -18,6 +19,13 @@
 
 uint32_t counter;
 
+EncoderCounter encoder(PA_6, PC_7);
+short counts;
+float gain_encoder = 0.0000025927f;
+
+DiffCounter diffcounter(1/(2.0f*pi*30.0f), 0.005f);   // discrete differentiate, based on encoder data
+float velocity;
+
 int main()
 {
     i2c.frequency(400000);
@@ -31,6 +39,12 @@
     timer.start();
 
     counter = 0;
+    
+    encoder.reset();   // encoder reset
+    counts = 0;
+    
+    diffcounter.reset(0.0f,0.0f);
+    velocity = 0.0f;
 
     /**
     * uint16_t frame_count;       // counts created I2C frames 0
@@ -58,8 +72,13 @@
                 px4_gyro[0] = PX4.gyro_x_rate();
                 px4_gyro[1] = PX4.gyro_y_rate();
                 px4_gyro[2] = PX4.gyro_z_rate();
-
+                
+                counts = encoder;
+                velocity = diffcounter(counts)*gain_encoder;
+                
                 dt = timer.read();
+                timer.reset();
+                
                 /*pc.printf("Frame Count: %4.0d\t", PX4.frame_count());
                 pc.printf("Pixel Flow X Sum: %d\t", -PX4.pixel_flow_y_sum());
                 pc.printf("Pixel Flow Y Sum: %d\t", -PX4.pixel_flow_x_sum());
@@ -81,11 +100,14 @@
                 pc.printf("PX4 gyro z: %.3f\t", px4_gyro[2]);
                 pc.printf("Updaterate  px4flow in Hz: %.3f\t", 1.0f/dt);
                 pc.printf("\r\n");*/
-
-                pc.printf("%i, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f; \r\n", counter, lsm_gyro[0], lsm_gyro[1], lsm_gyro[2], px4_gyro[0], px4_gyro[1], px4_gyro[2], 1.0f/dt);
-                timer.reset();
+                
+                // compare gyros
+                // pc.printf("%i, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f; \r\n", counter, lsm_gyro[0], lsm_gyro[1], lsm_gyro[2], px4_gyro[0], px4_gyro[1], px4_gyro[2], 1.0f/dt);
+                // compare encoder and px4flow output
+                pc.printf("%i; %.3f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f \r\n", counter, velocity, -PX4.pixel_flow_y_sum(), -PX4.pixel_flow_x_sum(), -PX4.flow_comp_m_y(), -PX4.flow_comp_m_x(), PX4.qual(), 1.0f/dt);
+   
                 counter++;
-                wait_us(6800); // -> 100 Hz
+                wait_us(1600); // -> 200 Hz
             } else {
                 pc.printf("TimeOut\r\n");
             }