altb_pmic / Mbed 2 deprecated Test_optical_flow_PX4

Dependencies:   mbed

main.cpp

Committer:
pmic
Date:
2019-08-22
Revision:
3:e03714326b83
Parent:
1:562a583eb77c
Child:
4:77914e52baf3

File content as of revision 3:e03714326b83:

#include "mbed.h"
#include "PX4Flow.h"
#include "LSM9DS1_i2c.h"
// #include "LSM9DS1_Registers.h"
// #include "LSM9DS1_Types.h"

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;

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;

    /**
    * 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();

                dt = timer.read();
                /*pc.printf("Frame Count: %4.0d\t", PX4.frame_count());
                pc.printf("Pixel Flow X Sum: %d\t", -PX4.pixel_flow_y_sum());
                pc.printf("Pixel Flow Y Sum: %d\t", -PX4.pixel_flow_x_sum());
                pc.printf("Flow Comp. M X.: %4d\t", -PX4.flow_comp_m_y());
                pc.printf("Flow Comp. M Y.: %4d\t", -PX4.flow_comp_m_x());
                pc.printf("Quality: %d\t", PX4.qual());
                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());
                pc.printf("Sonar Timestamp: %3.d\t", PX4.sonar_timestamp());
                pc.printf("z: %.3f\t", (float)PX4.ground_distance()/1000);*/

                /*pc.printf("LSM gyro x: %.3f\t", lsm_gyro[0]);
                pc.printf("LSM gyro y: %.3f\t", lsm_gyro[1]);
                pc.printf("LSM gyro z: %.3f\t", lsm_gyro[2]);
                pc.printf("PX4 gyro x: %.3f\t", px4_gyro[0]);
                pc.printf("PX4 gyro y: %.3f\t", px4_gyro[1]);
                pc.printf("PX4 gyro z: %.3f\t", px4_gyro[2]);
                pc.printf("Updaterate  px4flow in Hz: %.3f\t", 1.0f/dt);
                pc.printf("\r\n");*/

                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);
                timer.reset();
                counter++;
                wait_us(6800); // -> 100 Hz
            } else {
                pc.printf("TimeOut\r\n");
            }
        } else {
            if(PX4.update_integral()) {
                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.025);
    }
}