Diff: main.cpp

Revision:

7:692d01d007f9

Parent:

6:db20dd57a30a

Child:

8:cb6d8bc54aaf

--- a/main.cpp	Tue Sep 03 10:40:40 2019 +0000
+++ b/main.cpp	Thu Sep 05 15:41:17 2019 +0000
@@ -68,7 +68,7 @@
     * int16_t ground_distance;    // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
     */
     while(1) {
-        if(0) {
+        if(1) {
             if(PX4.update()) {
 
                 imu.readGyro();
@@ -83,40 +83,49 @@
                 counts = encoder;
                 velocity = diffcounter(counts)*gain_encoder;
                 
+                /*
                 dist = tfmini();
+                */
                 
                 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());
-                pc.printf("Flow Comp. M X.: %4d\t", -PX4.flow_comp_m_y());
-                pc.printf("Flow Comp. M Y.: %4d\t", -PX4.flow_comp_m_x());
-                pc.printf("Quality: %d\t", PX4.qual());
-                pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
-                pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
-                pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
-                pc.printf("Gyro Range: %d\t", PX4.gyro_range());
-                pc.printf("Sonar Timestamp: %3.d\t", PX4.sonar_timestamp());
-                pc.printf("z: %.3f\t", (float)PX4.ground_distance()/1000);
+                pc.printf("Frame Count: %8d\t", PX4.frame_count());
+                // pc.printf("Flow X Sum: %d\t", PX4.pixel_flow_x_sum()); // acg flow
+                // pc.printf("Flow Y Sum: %d\t", PX4.pixel_flow_y_sum());
+                pc.printf("Acc Val Frames: %8d\t", PX4.flow_comp_m_x()); // valid frame count over batch
+                pc.printf("Acc Frames: %8d\t", PX4.flow_comp_m_y()); // total frame count over batch
+                pc.printf("Avg Quality: %8d\t", PX4.qual()); // avg quality
+                // pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
+                // pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
+                // pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
+                // pc.printf("Gyro Range: %d\t", PX4.gyro_range()); // temperature
+                pc.printf("fs Avg px: %8.3f\t", 1.0f/((float)PX4.sonar_timestamp()*50.0f*0.000001f));
+                pc.printf("fs Avg avg: %8.3f\t", 1.0f/((float)PX4.ground_distance()*0.001f));
+                pc.printf("fs Nuc: %8.3f\t", 1.0f/dt);
+                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 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, 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(1600); // -> 200 Hz
+                // wait_us(6500);
             } else {
                 pc.printf("TimeOut\r\n");
             }
         } else {
             if(PX4.update_integral()) {
                 
-                ///*
+                // PX4.update_integral is really buggy!!! Not useful.
+                /*
                 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());
@@ -129,16 +138,13 @@
                 pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
                 pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
                 pc.printf("\r\n");
-                //*/
-                   
-                counter++;
-                // wait_us(1600); // -> 200 Hz
-
+                */
+                
             } else {
                 pc.printf("TimeOut\r\n");
             }
         }
-        // wait(0.025);
+        wait(0.046);
     }
 }