Diff: PX4Flow.cpp

Revision:

0:4b02060af95b

Child:

9:be40afb750d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PX4Flow.cpp	Fri Aug 02 14:15:07 2019 +0000
@@ -0,0 +1,212 @@
+#include "PX4Flow.h"
+
+PX4Flow::PX4Flow( I2C& i2c, Serial& pc ): i2c(i2c), pc(pc)
+{
+    i2c_commands[0] = FRAME;
+    i2c_commands[1] = INTEGRAL_FRAME;
+}
+
+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       = static_cast<uint16_t>(read16(bufferS, FRAME_COUNT));
+            frame.pixel_flow_x_sum  = static_cast<int16_t>(read16(bufferS,  PIXEL_FLOW_X_SUM));
+            frame.pixel_flow_y_sum  = static_cast<int16_t>(read16(bufferS,  PIXEL_FLOW_Y_SUM));
+            frame.flow_comp_m_x     = static_cast<int16_t>(read16(bufferS,  FLOW_COMP_M_X));
+            frame.flow_comp_m_y     = static_cast<int16_t>(read16(bufferS,  FLOW_COMP_M_Y));
+            frame.qual              = static_cast<int16_t>(read16(bufferS,  QUAL));
+            frame.gyro_x_rate       = static_cast<int16_t>(read16(bufferS,  GYRO_X_RATE));
+            frame.gyro_y_rate       = static_cast<int16_t>(read16(bufferS,  GYRO_Y_RATE));
+            frame.gyro_z_rate       = static_cast<int16_t>(read16(bufferS,  GYRO_Z_RATE));
+            frame.gyro_range        = static_cast<uint8_t>(read8(bufferS,   GYRO_RANGE));
+            frame.sonar_timestamp   = static_cast<uint8_t>(read8(bufferS,   SONAR_TIMESTAMP));
+            frame.ground_distance   = static_cast<uint16_t>(read16(bufferS, GROUND_DISTANCE));
+
+            return true;
+
+        } else {
+            printf("Error: reading I2C\r\n");
+            return false;
+        }
+    } else  {
+        pc.printf("Error: writing I2C\r\n");
+        return false;
+    }
+
+}
+
+bool PX4Flow::update_integral()
+{  
+
+    //send 0x16 to PX4FLOW module and receive back 26 Bytes data
+    if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 ) == 0 ) {
+        
+        int ack = i2c.read(PX4FLOW_ADDRESS, bufferI, 26 );
+        if(  ack == 0 ) {
+
+            // assign the data
+            iframe.frame_count_since_last_readout   = static_cast<uint16_t>(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
+            iframe.pixel_flow_x_integral            = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
+            iframe.pixel_flow_y_integral            = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
+            iframe.gyro_x_rate_integral             = static_cast<int16_t> (read16(bufferI, GYRO_X_RATE_INTEGRAL));
+            iframe.gyro_y_rate_integral             = static_cast<int16_t> (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
+            iframe.gyro_z_rate_integral             = static_cast<int16_t> (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
+            iframe.integration_timespan             = static_cast<uint32_t>(read32(bufferI, INTEGRATION_TIMESPAN));
+            iframe.sonar_timestamp                  = static_cast<uint32_t>(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
+            iframe.ground_distance                  = static_cast<int16_t> (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
+            iframe.gyro_temperature                 = static_cast<int16_t> (read16(bufferI, GYRO_TEMPERATURE));
+            iframe.quality                          = static_cast<uint8_t> (read8(bufferI,  QUALITY_INTEGRAL));
+
+            return true;
+
+        } else {
+            pc.printf("Error: reading I2C\r\n");
+            return false;
+        }
+    } else {
+        pc.printf("Error: writing  I2C\r\n");
+        return false;
+    }
+}
+
+
+// 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()
+{
+    return frame.flow_comp_m_x;
+}
+
+int16_t PX4Flow::flow_comp_m_y()
+{
+    return frame.flow_comp_m_y;
+}
+
+int16_t PX4Flow::qual()
+{
+    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;
+}
+
+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()
+{
+    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;
+}
+
+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;
+}
+
+
+uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
+{
+    return uint8_t( buffer[idx] );
+}
+
+uint16_t PX4Flow::read16(char *buffer, const unsigned int& idx)
+{
+    return uint16_t( read8( buffer, idx ) | (read8(buffer, idx + 1 ) << 8 ) );
+}
+
+uint32_t PX4Flow::read32(char *buffer, const unsigned int& idx)
+{
+    return uint32_t( (buffer[idx] << 0) | (buffer[idx + 1] << 8) | (buffer[idx+2] << 16) | (buffer[idx+3] << 24) );
+}