Diff: main.cpp

Revision:

8:cb6d8bc54aaf

Parent:

7:692d01d007f9

Child:

9:be40afb750d3

--- a/main.cpp	Thu Sep 05 15:41:17 2019 +0000
+++ b/main.cpp	Fri Sep 06 09:32:37 2019 +0000
@@ -24,7 +24,7 @@
 short counts;
 float gain_encoder = 0.0000025927f;
 
-DiffCounter diffcounter(1/(2.0f*pi*30.0f), 0.005f);   // discrete differentiate, based on encoder data
+DiffCounter diffcounter(1/(2.0f*pi*9.0f), 0.05f);   // discrete differentiate, based on encoder data
 float velocity;
 
 RawSerial uart(PC_10, PC_11);
@@ -53,42 +53,9 @@
     
     dist = 0.0f;
 
-    /**
-    * uint16_t frame_count;       // counts created I2C frames 0
-    * int16_t pixel_flow_x_sum;   // accumulated x flow in pixels*10 since last I2C frame 2
-    * int16_t pixel_flow_y_sum;   // accumulated y flow in pixels*10 since last I2C frame 4
-    * int16_t flow_comp_m_x;      // x velocity*1000 in meters / timestep 6
-    * int16_t flow_comp_m_y;      // y velocity*1000 in meters / timestep 8
-    * int16_t qual;               // Optical flow quality / confidence 0: bad, 255: maximum quality 10
-    * int16_t gyro_x_rate;        // gyro x rate 12
-    * int16_t gyro_y_rate;        // gyro y rate 14
-    * int16_t gyro_z_rate;        // gyro z rate 16
-    * uint8_t gyro_range;         // gyro range 18
-    * uint8_t sonar_timestamp;    // timestep in milliseconds between I2C frames 19
-    * int16_t ground_distance;    // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
-    */
     while(1) {
-        if(1) {
+        if(0) {
             if(PX4.update()) {
-
-                imu.readGyro();
-                lsm_gyro[0] = imu.gyroX;
-                lsm_gyro[1] = imu.gyroY;
-                lsm_gyro[2] = imu.gyroZ;
-
-                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;
-                
-                /*
-                dist = tfmini();
-                */
-                
-                dt = timer.read();
-                timer.reset();
                 
                 /*
                 pc.printf("Frame Count: %8d\t", PX4.frame_count());
@@ -107,44 +74,73 @@
                 pc.printf("\r\n");
                 */
                 
-                // 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, lidar and px4flow output
-                // pc.printf("%i; %.3f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f; %.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(), dist, 1.0f/dt);
-                // compare encoder and px4flow output, branch tryAgain 2
-                // pc.printf("%i; %.4f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f; %.3f; %.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(), (float)(PX4.frame_count())*0.01f, (float)(PX4.ground_distance())*0.01f, 1.0f/dt);
-                // compare encoder and px4flow output, branch tryAgain 3
-                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(6500);
             } else {
                 pc.printf("TimeOut\r\n");
             }
         } else {
             if(PX4.update_integral()) {
+                                
+                imu.readGyro();
+                lsm_gyro[0] = imu.gyroX*1.17f;
+                lsm_gyro[1] = imu.gyroY*1.17f;
+                lsm_gyro[2] = imu.gyroZ*1.17f;
+
+                px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
+                px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
+                px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
                 
-                // PX4.update_integral is really buggy!!! Not useful.
+                counts = encoder;
+                velocity = diffcounter(counts)*gain_encoder;
+                
                 /*
-                pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
-                pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
-                pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
-                pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
-                pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
-                pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
+                dist = tfmini();
+                */
+                
+                dt = timer.read();
+                timer.reset();
+                
+                /*
+                pc.printf("Valid Framecount: %3.0d\t", PX4.frame_count_since_last_readout());
+                pc.printf("Flow X: %3.0d\t", PX4.pixel_flow_x_integral());
+                pc.printf("Flow Y: %3.0d\t", PX4.pixel_flow_y_integral());
+                pc.printf("Gyro X: %3.0d\t", PX4.gyro_x_rate_integral());
+                pc.printf("Gyro Y: %3.0d\t", PX4.gyro_y_rate_integral());
+                pc.printf("Gyro Z: %3.0d\t", PX4.gyro_z_rate_integral());
                 pc.printf("Quality: %3.0d\t", PX4.quality_integral());
-                pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
-                pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
-                pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
-                pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
+                pc.printf("Framecount: %10.d\t", PX4.sonar_timestamp_integral());
+                pc.printf("Readout Hz*1000: %3.d\t", PX4.ground_distance_integral());
+                pc.printf("Gyro Temp: %3.d\t", PX4.gyro_temperature());
+                pc.printf("Update Hz*1000: %8.0d\t", PX4.integration_timespan());
                 pc.printf("\r\n");
                 */
                 
+                            
+                float flowx = (float)PX4.pixel_flow_x_integral()/3.0f; // avg flow x in mm/s
+                float flowy = (float)PX4.pixel_flow_y_integral()/3.0f; // avg flow y in mm/s
+                uint8_t valid_frame_count = (uint8_t)PX4.frame_count_since_last_readout(); // nr. of valid frames (qual > 0) between i2c readings
+                uint8_t frame_count = (uint8_t)PX4.sonar_timestamp_integral(); // nr. of frames between i2c readings
+                float update_fs = (float)PX4.integration_timespan()/1000.0f; // Hz
+                float readout_fs = (float)PX4.ground_distance_integral()/1000.0f; // Hz
+                uint8_t quality = PX4.quality_integral(); // avg quality 0...255
+                float gyro_temperature = (float)PX4.gyro_temperature()*0.01f; // avg temp deg
+                
+                // compare encoder and px4flow output, branch IndNaV
+                pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
+                pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
+                pc.printf("%10.3f; %10.3f; %10.3f; %10.3f;", px4_gyro[0], px4_gyro[1], px4_gyro[2], gyro_temperature);
+                pc.printf("%10.3f; %10.3f; %10i;", flowx, flowy, quality);
+                pc.printf("%10.3f; %10.3f; %10d; %10i", update_fs, readout_fs, frame_count, valid_frame_count);
+                pc.printf("\r\n");
+                            
+   
+                counter++;
+                wait(0.0455);
+                
             } else {
                 pc.printf("TimeOut\r\n");
             }
         }
-        wait(0.046);
+        // wait(0.046);
     }
 }