Diff: PX4Flow.h

Revision:

13:89b65bfe6dda

Parent:

12:19fe4f6a8b6b

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