Revision 13:89b65bfe6dda, committed 2020-04-06

Comitter:

pmic

Date:

Mon Apr 06 05:57:56 2020 +0000

Parent:

12:19fe4f6a8b6b

Commit message:

Change receiving driver and firmware. Data latency now 2.4 ms faster.

Changed in this revision

--- a/PX4Flow.cpp	Sun Apr 05 08:47:25 2020 +0000
+++ b/PX4Flow.cpp	Mon Apr 06 05:57:56 2020 +0000
@@ -2,60 +2,32 @@
 
 PX4Flow::PX4Flow( I2C& i2c): i2c(i2c), dout1(PA_10)
 {
-    i2c_commands[0] = FRAME;
-    i2c_commands[1] = INTEGRAL_FRAME;
+    i2c_commands = INTEGRAL_FRAME;
+    
+    iframe.avg_flow_x        = 0;
+    iframe.avg_flow_y        = 0;
+    iframe.avg_qual          = 0;
+    iframe.valid_frame_count = 0;
+    iframe.frame_count       = 0;
 }
 
 PX4Flow::~PX4Flow() {}
 
-bool PX4Flow::update()
-{
-    //send 0x0 to PX4FLOW module and receive back 22 Bytes data
-    if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[0], 1 ) == 0) {
-        if(i2c.read(PX4FLOW_ADDRESS, bufferS, 22 ) == 0 ) {
-            // assign the data
-            frame.frame_count       = (uint16_t)(read16(bufferS, FRAME_COUNT));
-            frame.pixel_flow_x_sum  = (int16_t) (read16(bufferS, PIXEL_FLOW_X_SUM));
-            frame.pixel_flow_y_sum  = (int16_t) (read16(bufferS, PIXEL_FLOW_Y_SUM));
-            frame.flow_comp_m_x     = (int16_t) (read16(bufferS, FLOW_COMP_M_X));
-            frame.flow_comp_m_y     = (int16_t) (read16(bufferS, FLOW_COMP_M_Y));
-            frame.qual              = (int16_t) (read16(bufferS, QUAL));
-            frame.gyro_x_rate       = (int16_t) (read16(bufferS, GYRO_X_RATE));
-            frame.gyro_y_rate       = (int16_t) (read16(bufferS, GYRO_Y_RATE));
-            frame.gyro_z_rate       = (int16_t) (read16(bufferS, GYRO_Z_RATE));
-            frame.gyro_range        = (uint8_t) (read8(bufferS,  GYRO_RANGE));
-            frame.sonar_timestamp   = (uint8_t) (read8(bufferS,  SONAR_TIMESTAMP));
-            frame.ground_distance   = (uint16_t)(read16(bufferS, GROUND_DISTANCE));
-            return true;
-        } else {
-            return false;
-        }
-    } else  {
-        return false;
-    }
-}
-
 bool PX4Flow::update_integral()
 {  
-    //send 0x16 to PX4FLOW module and receive back 26 Bytes data
-    int b1 = i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 );
+    // send 0x16 to PX4FLOW module and receive back 7 Bytes data
+    int b1 = i2c.write(PX4FLOW_ADDRESS, &i2c_commands, 1);
     if( b1 == 0 ) {
-//        dout1.write(1);
-        b1 = i2c.read(PX4FLOW_ADDRESS, bufferI, 26 );
-//        dout1.write(0);
+        // dout1.write(1);
+        b1 = i2c.read(PX4FLOW_ADDRESS, bufferI, 7);
+        // dout1.write(0);
         if(b1 == 0 ) {
             // assign the data
-            iframe.frame_count_since_last_readout   = (uint16_t)(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
-            iframe.pixel_flow_x_integral            = (int16_t) (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
-            iframe.pixel_flow_y_integral            = (int16_t) (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
-            iframe.gyro_x_rate_integral             = (int16_t) (read16(bufferI, GYRO_X_RATE_INTEGRAL));
-            iframe.gyro_y_rate_integral             = (int16_t) (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
-            iframe.gyro_z_rate_integral             = (int16_t) (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
-            iframe.integration_timespan             = (uint32_t)(read32(bufferI, INTEGRATION_TIMESPAN));
-            iframe.sonar_timestamp                  = (uint32_t)(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
-            iframe.ground_distance                  = (int16_t) (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
-            iframe.gyro_temperature                 = (int16_t) (read16(bufferI, GYRO_TEMPERATURE));
-            iframe.quality                          = (uint8_t) (read8(bufferI,  QUALITY_INTEGRAL));
+            iframe.avg_flow_x        = (int16_t)(read16(bufferI, AVG_FLOW_X       ));
+            iframe.avg_flow_y        = (int16_t)(read16(bufferI, AVG_FLOW_Y       ));
+            iframe.avg_qual          = (uint8_t)( read8(bufferI, AVG_QUAL         ));
+            iframe.valid_frame_count = (uint8_t)( read8(bufferI, VALID_FRAME_COUNT));
+            iframe.frame_count       = (uint8_t)( read8(bufferI, FRAME_COUNT_     ));
             return true;
         } else {
             return false;
@@ -65,191 +37,39 @@
     }
 }
 
-// Methods to return the sensordata from datastructure
-// Simple frame
-uint16_t PX4Flow::frame_count()
-{
-    return frame.frame_count;
-}
-
-int16_t PX4Flow::pixel_flow_x_sum()
-{
-    return frame.pixel_flow_x_sum;
-}
-
-int16_t PX4Flow::pixel_flow_y_sum()
-{
-    return frame.pixel_flow_y_sum;
-}
-
-int16_t PX4Flow::flow_comp_m_x()
+float PX4Flow::avg_flow_x()
 {
-    return frame.flow_comp_m_x;
+    return (float)iframe.avg_flow_x*0.3333333f;
 }
-
-int16_t PX4Flow::flow_comp_m_y()
-{
-    return frame.flow_comp_m_y;
-}
-
-int16_t PX4Flow::qual()
+    
+float PX4Flow::avg_flow_y()
 {
-    return frame.qual;
-}
-
-int16_t PX4Flow::gyro_x_rate()
-{
-    return frame.gyro_x_rate;
-}
-
-int16_t PX4Flow::gyro_y_rate()
-{
-    return frame.gyro_y_rate;
+    return (float)iframe.avg_flow_y*0.3333333f;
 }
 
-int16_t PX4Flow::gyro_z_rate()
-{
-    return frame.gyro_z_rate;
-}
-
-uint8_t PX4Flow::gyro_range()
-{
-    return frame.gyro_range;
-}
-
-uint8_t PX4Flow::sonar_timestamp()
-{
-    return frame.sonar_timestamp;
-}
-
-int16_t PX4Flow::ground_distance()
-{
-    return frame.ground_distance;
-}
-
-// Integral frame
-uint16_t PX4Flow::frame_count_since_last_readout()
+uint8_t PX4Flow::avg_qual()
 {
-    return iframe.frame_count_since_last_readout;
-}
-
-int16_t PX4Flow::pixel_flow_x_integral()
-{
-    return iframe.pixel_flow_x_integral;
-}
-
-int16_t PX4Flow::pixel_flow_y_integral()
-{
-    return iframe.pixel_flow_y_integral;
-}
-
-int16_t PX4Flow::gyro_x_rate_integral()
-{
-    return iframe.gyro_x_rate_integral;
-}
-
-int16_t PX4Flow::gyro_y_rate_integral()
-{
-    return iframe.gyro_y_rate_integral;
-}
-
-int16_t PX4Flow::gyro_z_rate_integral()
-{
-    return iframe.gyro_z_rate_integral;
+    return iframe.avg_qual;
 }
 
-uint32_t PX4Flow::integration_timespan()
-{
-    return iframe.integration_timespan;
-}
-
-uint32_t PX4Flow::sonar_timestamp_integral()
-{
-    return iframe.sonar_timestamp;
-}
-
-int16_t PX4Flow::ground_distance_integral()
-{
-    return iframe.ground_distance;
-}
-
-int16_t PX4Flow::gyro_temperature()
-{
-    return iframe.gyro_temperature;
-}
-
-uint8_t PX4Flow::quality_integral()
-{
-    return iframe.quality;
-}
-
-/* IndNav Git-branch modifications, start here */
-// Wrapper functions to read modified firmware data
-float PX4Flow::avg_flowx()
-{
-    return (float)iframe.pixel_flow_x_integral*0.3333333f;    // avg flow x in mm/s
-}
-    
-float PX4Flow::avg_flowy()
-{
-    return (float)iframe.pixel_flow_y_integral*0.3333333f;    // avg flow y in mm/s
-}
-    
 uint8_t PX4Flow::valid_frame_count()
 {
-    return (uint8_t)iframe.frame_count_since_last_readout;    // nr. of valid frames (qual > 0) between i2c readings
+    return iframe.valid_frame_count;
 }
     
-uint8_t PX4Flow::all_frame_count()
-{
-    return (uint8_t)iframe.sonar_timestamp;                   // nr. of frames between i2c readings
-}
-    
-float PX4Flow::update_fs()
-{
-    return (float)iframe.integration_timespan*0.001f;         // i2c averaging update rate in Hz
-}
-    
-float PX4Flow::readout_fs()
+uint8_t PX4Flow::frame_count()
 {
-    return (float)iframe.ground_distance*0.001f;              // i2c readout update rate in Hz
-}
-
-uint8_t PX4Flow::avg_quality()
-{
-    return iframe.quality;                                    // avg 0-255 linear quality measurement 0=bad, 255=best
-}
-    
-float PX4Flow::int_timespan()
-{
-    return (float)iframe.gyro_temperature*0.01f;              // integration timespan in ms, now you can integrate flow values, if valid...
+    return iframe.frame_count;
 }
     
-float PX4Flow::avg_gyro_x()
-{
-    return (float)iframe.gyro_x_rate_integral*0.00064516132f; // avg gyro x in rad/s
-}
-    
-float PX4Flow::avg_gyro_y()
-{
-    return (float)iframe.gyro_y_rate_integral*0.00064516132f; // avg gyro y in rad/s
-}
+uint8_t PX4Flow::avg_qual_scaled(){
     
-float PX4Flow::avg_gyro_z()
-{
-    return (float)iframe.gyro_z_rate_integral*0.00064516132f; // avg gyro z in rad/s
+    uint8_t avg_qual_scaled = 0;
+    if(iframe.frame_count > 0) {
+        avg_qual_scaled = (uint8_t)( (float)iframe.avg_qual * ( (float)iframe.valid_frame_count / (float)iframe.frame_count ) );
+    }        
+    return avg_qual_scaled;
 }
-    
-uint8_t PX4Flow::avg_scaled_quality(){
-    
-    uint8_t avg_scaled_quality = 0;
-    /* valid_frame_count := iframe.frame_count_since_last_readout */
-    if(iframe.frame_count_since_last_readout > 0) {
-        avg_scaled_quality = (uint8_t)( (float)iframe.quality * ( (float)iframe.frame_count_since_last_readout / (float)iframe.sonar_timestamp ) ); // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid/N_frames
-    }        
-    return avg_scaled_quality;
-}
-/* IndNav Git-branch modifications, end here */
 
 uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
 {

--- a/PX4Flow.h	Sun Apr 05 08:47:25 2020 +0000
+++ b/PX4Flow.h	Mon Apr 06 05:57:56 2020 +0000
@@ -8,137 +8,55 @@
 #define PX4FLOW_ADDRESS 0x42<<1
 
 // Commands
-#define    FRAME 0x00
 #define    INTEGRAL_FRAME 0x16
 
-// define buffer indicees
-// simple frame
-#define    FRAME_COUNT 0
-#define    PIXEL_FLOW_X_SUM 2
-#define    PIXEL_FLOW_Y_SUM 4
-#define    FLOW_COMP_M_X 6
-#define    FLOW_COMP_M_Y 8
-#define    QUAL 10
-#define    GYRO_X_RATE 12
-#define    GYRO_Y_RATE 14
-#define    GYRO_Z_RATE 16
-#define    GYRO_RANGE 18
-#define    SONAR_TIMESTAMP 19
-#define    GROUND_DISTANCE 20
-
 // integral frame
-#define    FRAME_COUNT_SINCE_LAST_READOUT 0
-#define    PIXEL_FLOW_X_INTEGRAL 2
-#define    PIXEL_FLOW_Y_INTEGRAL 4
-#define    GYRO_X_RATE_INTEGRAL 6
-#define    GYRO_Y_RATE_INTEGRAL 8
-#define    GYRO_Z_RATE_INTEGRAL 10
-#define    INTEGRATION_TIMESPAN 12
-#define    SONAR_TIMESTAMP_INTEGRAL 16
-#define    GROUND_DISTANCE_INTEGRAL 20
-#define    GYRO_TEMPERATURE 22
-#define    QUALITY_INTEGRAL 24
+#define    AVG_FLOW_X        0
+#define    AVG_FLOW_Y        2
+#define    AVG_QUAL          4
+#define    VALID_FRAME_COUNT 5
+#define    FRAME_COUNT_      6
 
-// valid with original and modified firmware, but different scaling with modified firmware, see Git-branch IndNav of the Firmware itselt, pmic 12.09.2019
-typedef struct i2c_frame {
-    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
-} i2c_frame;
-
-// only valid with original firmware, pmic 12.09.2019
+// Integral frame, only valid with modified firmware, see Git-branch IndNav from the Firmware
 typedef struct i2c_integral_frame {
-    uint16_t frame_count_since_last_readout; // number of flow measurements since last I2C readout [#frames] 22
-    int16_t pixel_flow_x_integral;           // accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000] 24
-    int16_t pixel_flow_y_integral;           // accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000] 26
-    int16_t gyro_x_rate_integral;            // accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000] 28
-    int16_t gyro_y_rate_integral;            // accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000] 30 
-    int16_t gyro_z_rate_integral;            // accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000] 32
-    uint32_t integration_timespan;           // accumulation timespan in microseconds since last I2C readout [microseconds] 34
-    uint32_t sonar_timestamp;                // time since last sonar update [microseconds] 38
-    int16_t ground_distance;                 // Ground distance in meters*1000 [meters*1000] 42
-    int16_t gyro_temperature;                // Temperature * 100 in centi-degrees Celsius [degcelsius*100] 44
-    uint8_t quality;                         // averaged quality of accumulated flow values [0:bad quality;255: max quality] 46
+    int16_t avg_flow_x;
+    int16_t avg_flow_y;
+    uint8_t avg_qual;
+    uint8_t valid_frame_count;
+    uint8_t frame_count;
 } i2c_integral_frame;
 
 
 class PX4Flow
 {
 public:
-    // Constructor
+    
     PX4Flow(I2C& i2c);
     
-    // Deconstructor
     virtual ~PX4Flow();
 
-    // Methods
-    bool update();
     bool update_integral();
-
-    // Simple frame, valid with original and modified firmware, but different scaling with modified firmware, see Git-branch IndNav of the Firmware itselt, pmic 12.09.2019
-    uint16_t frame_count();
-    int16_t pixel_flow_x_sum();
-    int16_t pixel_flow_y_sum();
-    int16_t flow_comp_m_x();
-    int16_t flow_comp_m_y();
-    int16_t qual();
-    int16_t gyro_x_rate();
-    int16_t gyro_y_rate();
-    int16_t gyro_z_rate();
-    uint8_t gyro_range();
-    uint8_t sonar_timestamp();
-    int16_t ground_distance();
+    
+    float   avg_flow_x();        // avg flow x in mm/s
+    float   avg_flow_y();        // avg flow y in mm/s
+    uint8_t avg_qual();          // avg 0-255 linear quality measurement 0=bad, 255=best
+    uint8_t valid_frame_count(); // nr. of valid frames (qual > 0) between i2c readings
+    uint8_t frame_count();       // nr. of frames between i2c readings    
+    uint8_t avg_qual_scaled();   // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid_frame_count/N_frame_count
+    
+private:
 
-    // Integral frame, only valid with original firmware, pmic 12.09.2019
-    uint16_t frame_count_since_last_readout();
-    int16_t  pixel_flow_x_integral();
-    int16_t  pixel_flow_y_integral();
-    int16_t  gyro_x_rate_integral();
-    int16_t  gyro_y_rate_integral();
-    int16_t  gyro_z_rate_integral();
-    uint32_t integration_timespan();
-    uint32_t sonar_timestamp_integral();
-    int16_t  ground_distance_integral();
-    int16_t  gyro_temperature();
-    uint8_t  quality_integral();
-
-    /* IndNav Git-branch modifications, start here */
-    // Integral frame, only valid with modified firmware, see Git-branch IndNav of the Firmware itselt, pmic 12.09.2019
-    float avg_flowx();              // avg flow x in mm/s
-    float avg_flowy();              // avg flow y in mm/s
-    uint8_t valid_frame_count();    // nr. of valid frames (qual > 0) between i2c readings
-    uint8_t all_frame_count();      // nr. of frames between i2c readings
-    float update_fs();              // i2c averaging update rate in Hz
-    float readout_fs();             // i2c readout update rate in Hz
-    uint8_t avg_quality();          // avg 0-255 linear quality measurement 0=bad, 255=best
-    float int_timespan();           // integration timespan in ms, now you can integrate flow values, if valid...
-    float avg_gyro_x();             // avg gyro x in rad/s
-    float avg_gyro_y();             // avg gyro y in rad/s
-    float avg_gyro_z();             // avg gyro z in rad/s
-    uint8_t avg_scaled_quality();   // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid/N_frames
-    /* IndNav Git-branch modifications, end here */
-private:
-       DigitalOut dout1;
+    DigitalOut dout1;
 
 protected:
+
     I2C i2c;
     
-    // Buffer to read in the sensordata
-    char bufferS[22];
-    char bufferI[26];
+    // storage for sensordata
+    char bufferI[7];
 
-    char i2c_commands[2];
+    char i2c_commands;
     
-    struct i2c_frame frame;
     struct i2c_integral_frame iframe;
 
     uint8_t  read8(char *buffer, const unsigned int& idx = 0);

--- a/main.cpp	Sun Apr 05 08:47:25 2020 +0000
+++ b/main.cpp	Mon Apr 06 05:57:56 2020 +0000
@@ -55,7 +55,7 @@
 
     while(1) {
         if(0) {
-            if(PX4.update()) {
+            if(1) {// if(PX4.update()) {
                 
                 /*
                 pc.printf("Frame Count: %8d\t", PX4.frame_count());
@@ -79,9 +79,9 @@
                 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_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;
@@ -94,11 +94,11 @@
                 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");
+                // 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!!!
@@ -114,9 +114,9 @@
                 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_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;
@@ -143,6 +143,7 @@
                 pc.printf("\r\n");
                 */
                 
+                /*
                 // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
                 pc.printf("%6i; %6.3f; %6.3f;", counter, velocity, 1.0f/dt);
                 pc.printf("%6.3f; %6.3f; %6.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
@@ -152,10 +153,24 @@
                 pc.printf("%6.3f; %6.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");
+                */
+                
+                // float   avg_flow_x();        // avg flow x in mm/s
+                // float   avg_flow_y();        // avg flow y in mm/s
+                // uint8_t avg_qual();          // avg 0-255 linear quality measurement 0=bad, 255=best
+                // uint8_t valid_frame_count(); // nr. of valid frames (qual > 0) between i2c readings
+                // uint8_t frame_count();       // nr. of frames between i2c readings    
+                // uint8_t avg_qual_scaled();   // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid_frame_count/N_frame_count
+                
+                // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
+                pc.printf("%6i; %6.3f;", counter, 1.0f/dt);
+                pc.printf("%6.3f; %6.3f; %4i;; %4i;", PX4.avg_flow_x(), PX4.avg_flow_y(), PX4.avg_qual_scaled(), PX4.avg_qual());
+                pc.printf("%4i; %4i;", PX4.valid_frame_count(), PX4.frame_count());
+                pc.printf("\r\n");
                 
                 counter++;
                 // wait(0.0155); // 50 Hz
-                wait(0.0455); // 20 Hz USE 20 Hz!!!
+                wait(0.0479); //: pmic, 2019 // wait(0.0455); //: pmic, int16_t for avg_flow, 2019 // 20 Hz USE 20 Hz!!!
                 // wait(0.0955); // 10 Hz
                 // wait(0.4955); // 2 Hz
                 // wait(0.9955); // 1 Hz