altb_pmic / Mbed 2 deprecated Test_optical_flow_PX4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:e03714326b83, 2019-08-22 (annotated)

Committer:
pmic
Date:
Thu Aug 22 14:22:58 2019 +0000
Revision:
3:e03714326b83
Parent:
1:562a583eb77c
Child:
4:77914e52baf3
Gyro parameterization in Flow and evaluation/comparison with pesboard gyro.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:4b02060af95b 1 #include "mbed.h"
altb2 0:4b02060af95b 2 #include "PX4Flow.h"
pmic 3:e03714326b83 3 #include "LSM9DS1_i2c.h"
pmic 3:e03714326b83 4 // #include "LSM9DS1_Registers.h"
pmic 3:e03714326b83 5 // #include "LSM9DS1_Types.h"
altb2 0:4b02060af95b 6
pmic 3:e03714326b83 7 Serial pc(SERIAL_TX, SERIAL_RX);
altb2 0:4b02060af95b 8
altb2 1:562a583eb77c 9 I2C i2c(PC_9, PA_8);
altb2 0:4b02060af95b 10 PX4Flow PX4(i2c,pc);
pmic 3:e03714326b83 11 float px4_gyro[3];
altb2 0:4b02060af95b 12
pmic 3:e03714326b83 13 LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
pmic 3:e03714326b83 14 float lsm_gyro[3];
altb2 0:4b02060af95b 15
pmic 3:e03714326b83 16 Timer timer; // timer for time measurement
pmic 3:e03714326b83 17 float dt = 0.0f;
pmic 3:e03714326b83 18
pmic 3:e03714326b83 19 uint32_t counter;
altb2 0:4b02060af95b 20
altb2 0:4b02060af95b 21 int main()
altb2 0:4b02060af95b 22 {
altb2 0:4b02060af95b 23 i2c.frequency(400000);
altb2 0:4b02060af95b 24 pc.baud(2000000);
pmic 3:e03714326b83 25
pmic 3:e03714326b83 26 for(uint8_t i = 0; i < 3; i++) px4_gyro[i] = 0.0f;
pmic 3:e03714326b83 27
pmic 3:e03714326b83 28 for(uint8_t i = 0; i < 3; i++) lsm_gyro[i] = 0.0f;
pmic 3:e03714326b83 29 imu.begin();
pmic 3:e03714326b83 30
pmic 3:e03714326b83 31 timer.start();
pmic 3:e03714326b83 32
pmic 3:e03714326b83 33 counter = 0;
pmic 3:e03714326b83 34
pmic 3:e03714326b83 35 /**
pmic 3:e03714326b83 36 * uint16_t frame_count; // counts created I2C frames 0
pmic 3:e03714326b83 37 * int16_t pixel_flow_x_sum; // accumulated x flow in pixels*10 since last I2C frame 2
pmic 3:e03714326b83 38 * int16_t pixel_flow_y_sum; // accumulated y flow in pixels*10 since last I2C frame 4
pmic 3:e03714326b83 39 * int16_t flow_comp_m_x; // x velocity*1000 in meters / timestep 6
pmic 3:e03714326b83 40 * int16_t flow_comp_m_y; // y velocity*1000 in meters / timestep 8
pmic 3:e03714326b83 41 * int16_t qual; // Optical flow quality / confidence 0: bad, 255: maximum quality 10
pmic 3:e03714326b83 42 * int16_t gyro_x_rate; // gyro x rate 12
pmic 3:e03714326b83 43 * int16_t gyro_y_rate; // gyro y rate 14
pmic 3:e03714326b83 44 * int16_t gyro_z_rate; // gyro z rate 16
pmic 3:e03714326b83 45 * uint8_t gyro_range; // gyro range 18
pmic 3:e03714326b83 46 * uint8_t sonar_timestamp; // timestep in milliseconds between I2C frames 19
pmic 3:e03714326b83 47 * int16_t ground_distance; // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
pmic 3:e03714326b83 48 */
altb2 0:4b02060af95b 49 while(1) {
pmic 3:e03714326b83 50 if(1) {
pmic 3:e03714326b83 51 if(PX4.update()) {
pmic 3:e03714326b83 52
pmic 3:e03714326b83 53 imu.readGyro();
pmic 3:e03714326b83 54 lsm_gyro[0] = imu.gyroX;
pmic 3:e03714326b83 55 lsm_gyro[1] = imu.gyroY;
pmic 3:e03714326b83 56 lsm_gyro[2] = imu.gyroZ;
pmic 3:e03714326b83 57
pmic 3:e03714326b83 58 px4_gyro[0] = PX4.gyro_x_rate();
pmic 3:e03714326b83 59 px4_gyro[1] = PX4.gyro_y_rate();
pmic 3:e03714326b83 60 px4_gyro[2] = PX4.gyro_z_rate();
altb2 0:4b02060af95b 61
pmic 3:e03714326b83 62 dt = timer.read();
pmic 3:e03714326b83 63 /*pc.printf("Frame Count: %4.0d\t", PX4.frame_count());
pmic 3:e03714326b83 64 pc.printf("Pixel Flow X Sum: %d\t", -PX4.pixel_flow_y_sum());
pmic 3:e03714326b83 65 pc.printf("Pixel Flow Y Sum: %d\t", -PX4.pixel_flow_x_sum());
pmic 3:e03714326b83 66 pc.printf("Flow Comp. M X.: %4d\t", -PX4.flow_comp_m_y());
pmic 3:e03714326b83 67 pc.printf("Flow Comp. M Y.: %4d\t", -PX4.flow_comp_m_x());
pmic 3:e03714326b83 68 pc.printf("Quality: %d\t", PX4.qual());
pmic 3:e03714326b83 69 pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
pmic 3:e03714326b83 70 pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
pmic 3:e03714326b83 71 pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
pmic 3:e03714326b83 72 pc.printf("Gyro Range: %d\t", PX4.gyro_range());
pmic 3:e03714326b83 73 pc.printf("Sonar Timestamp: %3.d\t", PX4.sonar_timestamp());
pmic 3:e03714326b83 74 pc.printf("z: %.3f\t", (float)PX4.ground_distance()/1000);*/
pmic 3:e03714326b83 75
pmic 3:e03714326b83 76 /*pc.printf("LSM gyro x: %.3f\t", lsm_gyro[0]);
pmic 3:e03714326b83 77 pc.printf("LSM gyro y: %.3f\t", lsm_gyro[1]);
pmic 3:e03714326b83 78 pc.printf("LSM gyro z: %.3f\t", lsm_gyro[2]);
pmic 3:e03714326b83 79 pc.printf("PX4 gyro x: %.3f\t", px4_gyro[0]);
pmic 3:e03714326b83 80 pc.printf("PX4 gyro y: %.3f\t", px4_gyro[1]);
pmic 3:e03714326b83 81 pc.printf("PX4 gyro z: %.3f\t", px4_gyro[2]);
pmic 3:e03714326b83 82 pc.printf("Updaterate px4flow in Hz: %.3f\t", 1.0f/dt);
pmic 3:e03714326b83 83 pc.printf("\r\n");*/
pmic 3:e03714326b83 84
pmic 3:e03714326b83 85 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);
pmic 3:e03714326b83 86 timer.reset();
pmic 3:e03714326b83 87 counter++;
pmic 3:e03714326b83 88 wait_us(6800); // -> 100 Hz
pmic 3:e03714326b83 89 } else {
pmic 3:e03714326b83 90 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 91 }
altb2 0:4b02060af95b 92 } else {
pmic 3:e03714326b83 93 if(PX4.update_integral()) {
pmic 3:e03714326b83 94 pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
pmic 3:e03714326b83 95 pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
pmic 3:e03714326b83 96 pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
pmic 3:e03714326b83 97 pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
pmic 3:e03714326b83 98 pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
pmic 3:e03714326b83 99 pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
pmic 3:e03714326b83 100 pc.printf("Quality: %3.0d\t", PX4.quality_integral());
pmic 3:e03714326b83 101 pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
pmic 3:e03714326b83 102 pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
pmic 3:e03714326b83 103 pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
pmic 3:e03714326b83 104 pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
pmic 3:e03714326b83 105 pc.printf("\r\n");
pmic 3:e03714326b83 106
pmic 3:e03714326b83 107 } else {
pmic 3:e03714326b83 108 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 109 }
altb2 0:4b02060af95b 110 }
pmic 3:e03714326b83 111 // wait(0.025);
altb2 0:4b02060af95b 112 }
altb2 0:4b02060af95b 113 }
altb2 0:4b02060af95b 114