Diff: main.cpp

Revision:

10:c654deb509e2

Parent:

9:be40afb750d3

Child:

11:d367224f2194

diff -r be40afb750d3 -r c654deb509e2 main.cpp
--- a/main.cpp	Fri Sep 13 10:43:57 2019 +0000
+++ b/main.cpp	Fri Sep 20 12:55:45 2019 +0000
@@ -54,7 +54,7 @@
     dist = 0.0f;
 
     while(1) {
-        if(0) {
+        if(1) {
             if(PX4.update()) {
                 
                 /*
@@ -74,6 +74,35 @@
                 pc.printf("\r\n");
                 */
                 
+                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;
+                
+                counts = encoder;
+                velocity = diffcounter(counts)*gain_encoder;
+                
+                /*
+                dist = tfmini();
+                */
+                
+                dt = timer.read();
+                timer.reset();
+                                
+                // compare encoder and px4flow output, branch OrgSoftwareNoSonar
+                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;", PX4.gyro_x_rate()*0.00064516132f, PX4.gyro_y_rate()*0.00064516132f, PX4.gyro_z_rate()*0.00064516132f);
+                pc.printf("%10.3f; %10.3f;", PX4.flow_comp_m_x()*0.3333333f, PX4.flow_comp_m_x()*0.3333333f);
+                pc.printf("\r\n");
+                
+                counter++;
+                wait(0.0455); // 20 Hz USE 20 Hz!!!
+                
             } else {
                 pc.printf("TimeOut\r\n");
             }
@@ -114,25 +143,6 @@
                 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 integration_timespan = (float)PX4.gyro_temperature()*0.01f; // integration timespan in ms
-                
-                // 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], integration_timespan);
-                pc.printf("%10.3f; %10.3f; %4i;", flowx, flowy, quality);
-                pc.printf("%10.3f; %10.3f; %4d; %4i", update_fs, readout_fs, frame_count, valid_frame_count);
-                pc.printf("\r\n");
-                */
-                
                 // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
                 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]);
@@ -140,8 +150,8 @@
                 pc.printf("%10.3f; %10.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
                 pc.printf("%10.3f; %10.3f; %4d; %4i;", PX4.update_fs(), PX4.readout_fs(), PX4.all_frame_count(), PX4.valid_frame_count());
                 pc.printf("%4i", PX4.avg_scaled_quality());
-                pc.printf("\r\n");           
-   
+                pc.printf("\r\n");
+                
                 counter++;
                 wait(0.0455); // 20 Hz USE 20 Hz!!!
                 // wait(0.0955); // 10 Hz