File content as of revision 0:4b02060af95b:

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