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@7:692d01d007f9, 2019-09-05 (annotated)

Committer:
pmic
Date:
Thu Sep 05 15:41:17 2019 +0000
Revision:
7:692d01d007f9
Parent:
6:db20dd57a30a
Child:
8:cb6d8bc54aaf
Commit before revert.

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 4:77914e52baf3 4 #include "EncoderCounter.h"
pmic 4:77914e52baf3 5 #include "DiffCounter.h"
pmic 5:d97332d7f812 6 #include "TFmini.h"
pmic 4:77914e52baf3 7 #define pi 3.1415927f
altb2 0:4b02060af95b 8
pmic 3:e03714326b83 9 Serial pc(SERIAL_TX, SERIAL_RX);
altb2 0:4b02060af95b 10
altb2 1:562a583eb77c 11 I2C i2c(PC_9, PA_8);
altb2 0:4b02060af95b 12 PX4Flow PX4(i2c,pc);
pmic 3:e03714326b83 13 float px4_gyro[3];
altb2 0:4b02060af95b 14
pmic 3:e03714326b83 15 LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
pmic 3:e03714326b83 16 float lsm_gyro[3];
altb2 0:4b02060af95b 17
pmic 3:e03714326b83 18 Timer timer; // timer for time measurement
pmic 3:e03714326b83 19 float dt = 0.0f;
pmic 3:e03714326b83 20
pmic 3:e03714326b83 21 uint32_t counter;
altb2 0:4b02060af95b 22
pmic 4:77914e52baf3 23 EncoderCounter encoder(PA_6, PC_7);
pmic 4:77914e52baf3 24 short counts;
pmic 4:77914e52baf3 25 float gain_encoder = 0.0000025927f;
pmic 4:77914e52baf3 26
pmic 4:77914e52baf3 27 DiffCounter diffcounter(1/(2.0f*pi*30.0f), 0.005f); // discrete differentiate, based on encoder data
pmic 4:77914e52baf3 28 float velocity;
pmic 4:77914e52baf3 29
pmic 5:d97332d7f812 30 RawSerial uart(PC_10, PC_11);
pmic 5:d97332d7f812 31 TFmini tfmini(uart);
pmic 5:d97332d7f812 32 float dist;
pmic 5:d97332d7f812 33
altb2 0:4b02060af95b 34 int main()
altb2 0:4b02060af95b 35 {
altb2 0:4b02060af95b 36 i2c.frequency(400000);
altb2 0:4b02060af95b 37 pc.baud(2000000);
pmic 3:e03714326b83 38
pmic 3:e03714326b83 39 for(uint8_t i = 0; i < 3; i++) px4_gyro[i] = 0.0f;
pmic 3:e03714326b83 40
pmic 3:e03714326b83 41 for(uint8_t i = 0; i < 3; i++) lsm_gyro[i] = 0.0f;
pmic 3:e03714326b83 42 imu.begin();
pmic 3:e03714326b83 43
pmic 3:e03714326b83 44 timer.start();
pmic 3:e03714326b83 45
pmic 3:e03714326b83 46 counter = 0;
pmic 4:77914e52baf3 47
pmic 4:77914e52baf3 48 encoder.reset(); // encoder reset
pmic 4:77914e52baf3 49 counts = 0;
pmic 4:77914e52baf3 50
pmic 4:77914e52baf3 51 diffcounter.reset(0.0f,0.0f);
pmic 4:77914e52baf3 52 velocity = 0.0f;
pmic 5:d97332d7f812 53
pmic 5:d97332d7f812 54 dist = 0.0f;
pmic 3:e03714326b83 55
pmic 3:e03714326b83 56 /**
pmic 3:e03714326b83 57 * uint16_t frame_count; // counts created I2C frames 0
pmic 3:e03714326b83 58 * int16_t pixel_flow_x_sum; // accumulated x flow in pixels*10 since last I2C frame 2
pmic 3:e03714326b83 59 * int16_t pixel_flow_y_sum; // accumulated y flow in pixels*10 since last I2C frame 4
pmic 3:e03714326b83 60 * int16_t flow_comp_m_x; // x velocity*1000 in meters / timestep 6
pmic 3:e03714326b83 61 * int16_t flow_comp_m_y; // y velocity*1000 in meters / timestep 8
pmic 3:e03714326b83 62 * int16_t qual; // Optical flow quality / confidence 0: bad, 255: maximum quality 10
pmic 3:e03714326b83 63 * int16_t gyro_x_rate; // gyro x rate 12
pmic 3:e03714326b83 64 * int16_t gyro_y_rate; // gyro y rate 14
pmic 3:e03714326b83 65 * int16_t gyro_z_rate; // gyro z rate 16
pmic 3:e03714326b83 66 * uint8_t gyro_range; // gyro range 18
pmic 3:e03714326b83 67 * uint8_t sonar_timestamp; // timestep in milliseconds between I2C frames 19
pmic 3:e03714326b83 68 * int16_t ground_distance; // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
pmic 3:e03714326b83 69 */
altb2 0:4b02060af95b 70 while(1) {
pmic 7:692d01d007f9 71 if(1) {
pmic 3:e03714326b83 72 if(PX4.update()) {
pmic 3:e03714326b83 73
pmic 3:e03714326b83 74 imu.readGyro();
pmic 3:e03714326b83 75 lsm_gyro[0] = imu.gyroX;
pmic 3:e03714326b83 76 lsm_gyro[1] = imu.gyroY;
pmic 3:e03714326b83 77 lsm_gyro[2] = imu.gyroZ;
pmic 3:e03714326b83 78
pmic 3:e03714326b83 79 px4_gyro[0] = PX4.gyro_x_rate();
pmic 3:e03714326b83 80 px4_gyro[1] = PX4.gyro_y_rate();
pmic 3:e03714326b83 81 px4_gyro[2] = PX4.gyro_z_rate();
pmic 4:77914e52baf3 82
pmic 4:77914e52baf3 83 counts = encoder;
pmic 4:77914e52baf3 84 velocity = diffcounter(counts)*gain_encoder;
pmic 4:77914e52baf3 85
pmic 7:692d01d007f9 86 /*
pmic 5:d97332d7f812 87 dist = tfmini();
pmic 7:692d01d007f9 88 */
pmic 5:d97332d7f812 89
pmic 3:e03714326b83 90 dt = timer.read();
pmic 4:77914e52baf3 91 timer.reset();
pmic 4:77914e52baf3 92
pmic 6:db20dd57a30a 93 /*
pmic 7:692d01d007f9 94 pc.printf("Frame Count: %8d\t", PX4.frame_count());
pmic 7:692d01d007f9 95 // pc.printf("Flow X Sum: %d\t", PX4.pixel_flow_x_sum()); // acg flow
pmic 7:692d01d007f9 96 // pc.printf("Flow Y Sum: %d\t", PX4.pixel_flow_y_sum());
pmic 7:692d01d007f9 97 pc.printf("Acc Val Frames: %8d\t", PX4.flow_comp_m_x()); // valid frame count over batch
pmic 7:692d01d007f9 98 pc.printf("Acc Frames: %8d\t", PX4.flow_comp_m_y()); // total frame count over batch
pmic 7:692d01d007f9 99 pc.printf("Avg Quality: %8d\t", PX4.qual()); // avg quality
pmic 7:692d01d007f9 100 // pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
pmic 7:692d01d007f9 101 // pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
pmic 7:692d01d007f9 102 // pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
pmic 7:692d01d007f9 103 // pc.printf("Gyro Range: %d\t", PX4.gyro_range()); // temperature
pmic 7:692d01d007f9 104 pc.printf("fs Avg px: %8.3f\t", 1.0f/((float)PX4.sonar_timestamp()*50.0f*0.000001f));
pmic 7:692d01d007f9 105 pc.printf("fs Avg avg: %8.3f\t", 1.0f/((float)PX4.ground_distance()*0.001f));
pmic 7:692d01d007f9 106 pc.printf("fs Nuc: %8.3f\t", 1.0f/dt);
pmic 7:692d01d007f9 107 pc.printf("\r\n");
pmic 6:db20dd57a30a 108 */
pmic 4:77914e52baf3 109
pmic 4:77914e52baf3 110 // compare gyros
pmic 4:77914e52baf3 111 // 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 7:692d01d007f9 112 // compare encoder, lidar and px4flow output
pmic 7:692d01d007f9 113 // 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);
pmic 7:692d01d007f9 114 // compare encoder and px4flow output, branch tryAgain 2
pmic 7:692d01d007f9 115 // 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);
pmic 7:692d01d007f9 116 // compare encoder and px4flow output, branch tryAgain 3
pmic 7:692d01d007f9 117 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);
pmic 4:77914e52baf3 118
pmic 3:e03714326b83 119 counter++;
pmic 7:692d01d007f9 120 // wait_us(6500);
pmic 3:e03714326b83 121 } else {
pmic 3:e03714326b83 122 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 123 }
altb2 0:4b02060af95b 124 } else {
pmic 3:e03714326b83 125 if(PX4.update_integral()) {
pmic 6:db20dd57a30a 126
pmic 7:692d01d007f9 127 // PX4.update_integral is really buggy!!! Not useful.
pmic 7:692d01d007f9 128 /*
pmic 3:e03714326b83 129 pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
pmic 3:e03714326b83 130 pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
pmic 3:e03714326b83 131 pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
pmic 3:e03714326b83 132 pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
pmic 3:e03714326b83 133 pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
pmic 3:e03714326b83 134 pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
pmic 3:e03714326b83 135 pc.printf("Quality: %3.0d\t", PX4.quality_integral());
pmic 3:e03714326b83 136 pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
pmic 3:e03714326b83 137 pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
pmic 3:e03714326b83 138 pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
pmic 3:e03714326b83 139 pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
pmic 3:e03714326b83 140 pc.printf("\r\n");
pmic 7:692d01d007f9 141 */
pmic 7:692d01d007f9 142
pmic 3:e03714326b83 143 } else {
pmic 3:e03714326b83 144 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 145 }
altb2 0:4b02060af95b 146 }
pmic 7:692d01d007f9 147 wait(0.046);
altb2 0:4b02060af95b 148 }
altb2 0:4b02060af95b 149 }
altb2 0:4b02060af95b 150