File content as of revision 7:692d01d007f9:

#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*30.0f), 0.005f);   // 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;

    /**
    * uint16_t frame_count;       // counts created I2C frames 0
    * int16_t pixel_flow_x_sum;   // accumulated x flow in pixels*10 since last I2C frame 2
    * int16_t pixel_flow_y_sum;   // accumulated y flow in pixels*10 since last I2C frame 4
    * int16_t flow_comp_m_x;      // x velocity*1000 in meters / timestep 6
    * int16_t flow_comp_m_y;      // y velocity*1000 in meters / timestep 8
    * int16_t qual;               // Optical flow quality / confidence 0: bad, 255: maximum quality 10
    * int16_t gyro_x_rate;        // gyro x rate 12
    * int16_t gyro_y_rate;        // gyro y rate 14
    * int16_t gyro_z_rate;        // gyro z rate 16
    * uint8_t gyro_range;         // gyro range 18
    * uint8_t sonar_timestamp;    // timestep in milliseconds between I2C frames 19
    * int16_t ground_distance;    // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
    */
    while(1) {
        if(1) {
            if(PX4.update()) {

                imu.readGyro();
                lsm_gyro[0] = imu.gyroX;
                lsm_gyro[1] = imu.gyroY;
                lsm_gyro[2] = imu.gyroZ;

                px4_gyro[0] = PX4.gyro_x_rate();
                px4_gyro[1] = PX4.gyro_y_rate();
                px4_gyro[2] = PX4.gyro_z_rate();
                
                counts = encoder;
                velocity = diffcounter(counts)*gain_encoder;
                
                /*
                dist = tfmini();
                */
                
                dt = timer.read();
                timer.reset();
                
                /*
                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");
                */
                
                // compare gyros
                // pc.printf("%i, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f; \r\n", counter, lsm_gyro[0], lsm_gyro[1], lsm_gyro[2], px4_gyro[0], px4_gyro[1], px4_gyro[2], 1.0f/dt);
                // compare encoder, lidar and px4flow output
                // pc.printf("%i; %.3f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f; %.3f \r\n", counter, velocity, -PX4.pixel_flow_y_sum(), -PX4.pixel_flow_x_sum(), -PX4.flow_comp_m_y(), -PX4.flow_comp_m_x(), PX4.qual(), dist, 1.0f/dt);
                // compare encoder and px4flow output, branch tryAgain 2
                // pc.printf("%i; %.4f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f; %.3f; %.3f \r\n", counter, velocity, -PX4.pixel_flow_y_sum(), -PX4.pixel_flow_x_sum(), PX4.flow_comp_m_y(), PX4.flow_comp_m_x(), PX4.qual(), (float)(PX4.frame_count())*0.01f, (float)(PX4.ground_distance())*0.01f, 1.0f/dt);
                // compare encoder and px4flow output, branch tryAgain 3
                pc.printf("%i; %.3f; %.3i; %.3i; %.3i; %.3i; %.3i; %.3f \r\n", counter, velocity, -PX4.pixel_flow_y_sum(), -PX4.pixel_flow_x_sum(), -PX4.flow_comp_m_y(), -PX4.flow_comp_m_x(), PX4.qual(), 1.0f/dt);
   
                counter++;
                // wait_us(6500);
            } else {
                pc.printf("TimeOut\r\n");
            }
        } else {
            if(PX4.update_integral()) {
                
                // PX4.update_integral is really buggy!!! Not useful.
                /*
                pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
                pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
                pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
                pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
                pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
                pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
                pc.printf("Quality: %3.0d\t", PX4.quality_integral());
                pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
                pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
                pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
                pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
                pc.printf("\r\n");
                */
                
            } else {
                pc.printf("TimeOut\r\n");
            }
        }
        wait(0.046);
    }
}