File content as of revision 11:d367224f2194:

#include "PX4Flow.h"

PX4Flow::PX4Flow( I2C& i2c): i2c(i2c)
{
    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       = (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
    if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 ) == 0 ) {
        if(i2c.read(PX4FLOW_ADDRESS, bufferI, 26 ) == 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));
            return true;
        } else {
            return false;
        }
    } else {
        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;
}

/* 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
}
    
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()
{
    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...
}
    
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
}
    
float PX4Flow::avg_gyro_z()
{
    return (float)iframe.gyro_z_rate_integral*0.00064516132f; // avg gyro z in rad/s
}
    
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)
{
    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) );
}