File content as of revision 13:89b65bfe6dda:

#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);
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(1) {// 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");
                */
                
                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();
                                
                // compare encoder and px4flow output, branch OrgSoftwareNoSonar
                // 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;", PX4.gyro_x_rate()*0.00064516132f, PX4.gyro_y_rate()*0.00064516132f, PX4.gyro_z_rate()*0.00064516132f);
                // pc.printf("%10.3f; %10.3f;", PX4.flow_comp_m_x()*0.3333333f, PX4.flow_comp_m_x()*0.3333333f);
                // pc.printf("\r\n");
                
                counter++;
                wait(0.0455); // 20 Hz USE 20 Hz!!!
                
            } 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");
                */
                
                /*
                // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
                pc.printf("%6i; %6.3f; %6.3f;", counter, velocity, 1.0f/dt);
                pc.printf("%6.3f; %6.3f; %6.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
                // pc.printf("%6i; %6.3f; %6.3i;", counter, 1.0f/dt);
                pc.printf("%6.3f; %6.3f; %6.3f; %6.4f;", PX4.avg_gyro_x(), PX4.avg_gyro_y(), PX4.avg_gyro_z(), PX4.int_timespan());
                pc.printf("%6.3f; %6.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
                pc.printf("%6.3f; %6.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");
                */
                
                // 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
                
                // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
                pc.printf("%6i; %6.3f;", counter, 1.0f/dt);
                pc.printf("%6.3f; %6.3f; %4i;; %4i;", PX4.avg_flow_x(), PX4.avg_flow_y(), PX4.avg_qual_scaled(), PX4.avg_qual());
                pc.printf("%4i; %4i;", PX4.valid_frame_count(), PX4.frame_count());
                pc.printf("\r\n");
                
                counter++;
                // wait(0.0155); // 50 Hz
                wait(0.0479); //: pmic, 2019 // wait(0.0455); //: pmic, int16_t for avg_flow, 2019 // 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);
    }
}