File content as of revision 9:be40afb750d3:

#include "mbed.h"
#include "PX4Flow.h"
#include "LSM9DS1_i2c.h"
#include "EncoderCounter.h"
#include "DiffCounter.h"
#include "TFmini.h"
#define   pi 3.1415927f

Serial pc(SERIAL_TX, SERIAL_RX);

I2C i2c(PC_9, PA_8);
PX4Flow PX4(i2c,pc);
float px4_gyro[3];

LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
float lsm_gyro[3];

Timer timer; // timer for time measurement
float dt = 0.0f;

uint32_t counter;

EncoderCounter encoder(PA_6, PC_7);
short counts;
float gain_encoder = 0.0000025927f;

DiffCounter diffcounter(1/(2.0f*pi*9.0f), 0.05f);   // discrete differentiate, based on encoder data
float velocity;

RawSerial uart(PC_10, PC_11);
TFmini tfmini(uart);
float dist;

int main()
{
    i2c.frequency(400000);
    pc.baud(2000000);

    for(uint8_t i = 0; i < 3; i++) px4_gyro[i] = 0.0f;

    for(uint8_t i = 0; i < 3; i++) lsm_gyro[i] = 0.0f;
    imu.begin();

    timer.start();

    counter = 0;
    
    encoder.reset();   // encoder reset
    counts = 0;
    
    diffcounter.reset(0.0f,0.0f);
    velocity = 0.0f;
    
    dist = 0.0f;

    while(1) {
        if(0) {
            if(PX4.update()) {
                
                /*
                pc.printf("Frame Count: %8d\t", PX4.frame_count());
                // pc.printf("Flow X Sum: %d\t", PX4.pixel_flow_x_sum()); // acg flow
                // pc.printf("Flow Y Sum: %d\t", PX4.pixel_flow_y_sum());
                pc.printf("Acc Val Frames: %8d\t", PX4.flow_comp_m_x()); // valid frame count over batch
                pc.printf("Acc Frames: %8d\t", PX4.flow_comp_m_y()); // total frame count over batch
                pc.printf("Avg Quality: %8d\t", PX4.qual()); // avg quality
                // pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
                // pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
                // pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
                // pc.printf("Gyro Range: %d\t", PX4.gyro_range()); // temperature
                pc.printf("fs Avg px: %8.3f\t", 1.0f/((float)PX4.sonar_timestamp()*50.0f*0.000001f));
                pc.printf("fs Avg avg: %8.3f\t", 1.0f/((float)PX4.ground_distance()*0.001f));
                pc.printf("fs Nuc: %8.3f\t", 1.0f/dt);
                pc.printf("\r\n");
                */
                
            } else {
                pc.printf("TimeOut\r\n");
            }
        } else {
            if(PX4.update_integral()) {
                                
                imu.readGyro();
                lsm_gyro[0] = imu.gyroX*1.17f;
                lsm_gyro[1] = imu.gyroY*1.17f;
                lsm_gyro[2] = imu.gyroZ*1.17f;

                px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
                px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
                px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
                
                counts = encoder;
                velocity = diffcounter(counts)*gain_encoder;
                
                /*
                dist = tfmini();
                */
                
                dt = timer.read();
                timer.reset();
                
                /*
                pc.printf("Valid Framecount: %3.0d\t", PX4.frame_count_since_last_readout());
                pc.printf("Flow X: %3.0d\t", PX4.pixel_flow_x_integral());
                pc.printf("Flow Y: %3.0d\t", PX4.pixel_flow_y_integral());
                pc.printf("Gyro X: %3.0d\t", PX4.gyro_x_rate_integral());
                pc.printf("Gyro Y: %3.0d\t", PX4.gyro_y_rate_integral());
                pc.printf("Gyro Z: %3.0d\t", PX4.gyro_z_rate_integral());
                pc.printf("Quality: %3.0d\t", PX4.quality_integral());
                pc.printf("Framecount: %10.d\t", PX4.sonar_timestamp_integral());
                pc.printf("Readout Hz*1000: %3.d\t", PX4.ground_distance_integral());
                pc.printf("Gyro Temp: %3.d\t", PX4.gyro_temperature());
                pc.printf("Update Hz*1000: %8.0d\t", PX4.integration_timespan());
                pc.printf("\r\n");
                */
                
                /*
                float flowx = (float)PX4.pixel_flow_x_integral()/3.0f; // avg flow x in mm/s
                float flowy = (float)PX4.pixel_flow_y_integral()/3.0f; // avg flow y in mm/s
                uint8_t valid_frame_count = (uint8_t)PX4.frame_count_since_last_readout(); // nr. of valid frames (qual > 0) between i2c readings
                uint8_t frame_count = (uint8_t)PX4.sonar_timestamp_integral(); // nr. of frames between i2c readings
                float update_fs = (float)PX4.integration_timespan()/1000.0f; // Hz
                float readout_fs = (float)PX4.ground_distance_integral()/1000.0f; // Hz
                uint8_t quality = PX4.quality_integral(); // avg quality 0...255
                float integration_timespan = (float)PX4.gyro_temperature()*0.01f; // integration timespan in ms
                
                // compare encoder and px4flow output, branch IndNaV
                pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
                pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
                pc.printf("%10.3f; %10.3f; %10.3f; %10.3f;", px4_gyro[0], px4_gyro[1], px4_gyro[2], integration_timespan);
                pc.printf("%10.3f; %10.3f; %4i;", flowx, flowy, quality);
                pc.printf("%10.3f; %10.3f; %4d; %4i", update_fs, readout_fs, frame_count, valid_frame_count);
                pc.printf("\r\n");
                */
                
                // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
                pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
                pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
                pc.printf("%10.3f; %10.3f; %10.3f; %10.4f;", PX4.avg_gyro_x(), PX4.avg_gyro_y(), PX4.avg_gyro_z(), PX4.int_timespan());
                pc.printf("%10.3f; %10.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
                pc.printf("%10.3f; %10.3f; %4d; %4i;", PX4.update_fs(), PX4.readout_fs(), PX4.all_frame_count(), PX4.valid_frame_count());
                pc.printf("%4i", PX4.avg_scaled_quality());
                pc.printf("\r\n");           
   
                counter++;
                wait(0.0455); // 20 Hz USE 20 Hz!!!
                // wait(0.0955); // 10 Hz
                // wait(0.4955); // 2 Hz
                // wait(0.9955); // 1 Hz
                
            } else {
                pc.printf("TimeOut\r\n");
            }
        }
        // wait(0.046);
    }
}