Diff: PX4Flow.cpp

Revision:

13:89b65bfe6dda

Parent:

12:19fe4f6a8b6b

--- 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)
 {