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@12:19fe4f6a8b6b, 2020-04-05 (annotated)

Committer:
pmic
Date:
Sun Apr 05 08:47:25 2020 +0000
Revision:
12:19fe4f6a8b6b
Parent:
11:d367224f2194
Child:
13:89b65bfe6dda
Commit before implementing driver update (reduce ammount of data transmitted). i2c driver has to be updated on both software sides (F446RE and px4flow)!

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);
pmic 11:d367224f2194 12 PX4Flow PX4(i2c);
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 8:cb6d8bc54aaf 27 DiffCounter diffcounter(1/(2.0f*pi*9.0f), 0.05f); // 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
altb2 0:4b02060af95b 56 while(1) {
pmic 12:19fe4f6a8b6b 57 if(0) {
pmic 3:e03714326b83 58 if(PX4.update()) {
pmic 4:77914e52baf3 59
pmic 6:db20dd57a30a 60 /*
pmic 7:692d01d007f9 61 pc.printf("Frame Count: %8d\t", PX4.frame_count());
pmic 7:692d01d007f9 62 // pc.printf("Flow X Sum: %d\t", PX4.pixel_flow_x_sum()); // acg flow
pmic 7:692d01d007f9 63 // pc.printf("Flow Y Sum: %d\t", PX4.pixel_flow_y_sum());
pmic 7:692d01d007f9 64 pc.printf("Acc Val Frames: %8d\t", PX4.flow_comp_m_x()); // valid frame count over batch
pmic 7:692d01d007f9 65 pc.printf("Acc Frames: %8d\t", PX4.flow_comp_m_y()); // total frame count over batch
pmic 7:692d01d007f9 66 pc.printf("Avg Quality: %8d\t", PX4.qual()); // avg quality
pmic 7:692d01d007f9 67 // pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
pmic 7:692d01d007f9 68 // pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
pmic 7:692d01d007f9 69 // pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
pmic 7:692d01d007f9 70 // pc.printf("Gyro Range: %d\t", PX4.gyro_range()); // temperature
pmic 7:692d01d007f9 71 pc.printf("fs Avg px: %8.3f\t", 1.0f/((float)PX4.sonar_timestamp()*50.0f*0.000001f));
pmic 7:692d01d007f9 72 pc.printf("fs Avg avg: %8.3f\t", 1.0f/((float)PX4.ground_distance()*0.001f));
pmic 7:692d01d007f9 73 pc.printf("fs Nuc: %8.3f\t", 1.0f/dt);
pmic 7:692d01d007f9 74 pc.printf("\r\n");
pmic 6:db20dd57a30a 75 */
pmic 4:77914e52baf3 76
pmic 10:c654deb509e2 77 imu.readGyro();
pmic 10:c654deb509e2 78 lsm_gyro[0] = imu.gyroX*1.17f;
pmic 10:c654deb509e2 79 lsm_gyro[1] = imu.gyroY*1.17f;
pmic 10:c654deb509e2 80 lsm_gyro[2] = imu.gyroZ*1.17f;
pmic 10:c654deb509e2 81
pmic 10:c654deb509e2 82 px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
pmic 10:c654deb509e2 83 px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
pmic 10:c654deb509e2 84 px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
pmic 10:c654deb509e2 85
pmic 10:c654deb509e2 86 counts = encoder;
pmic 10:c654deb509e2 87 velocity = diffcounter(counts)*gain_encoder;
pmic 10:c654deb509e2 88
pmic 10:c654deb509e2 89 /*
pmic 10:c654deb509e2 90 dist = tfmini();
pmic 10:c654deb509e2 91 */
pmic 10:c654deb509e2 92
pmic 10:c654deb509e2 93 dt = timer.read();
pmic 10:c654deb509e2 94 timer.reset();
pmic 10:c654deb509e2 95
pmic 10:c654deb509e2 96 // compare encoder and px4flow output, branch OrgSoftwareNoSonar
pmic 10:c654deb509e2 97 pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
pmic 10:c654deb509e2 98 pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
pmic 10:c654deb509e2 99 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);
pmic 10:c654deb509e2 100 pc.printf("%10.3f; %10.3f;", PX4.flow_comp_m_x()*0.3333333f, PX4.flow_comp_m_x()*0.3333333f);
pmic 10:c654deb509e2 101 pc.printf("\r\n");
pmic 10:c654deb509e2 102
pmic 10:c654deb509e2 103 counter++;
pmic 10:c654deb509e2 104 wait(0.0455); // 20 Hz USE 20 Hz!!!
pmic 10:c654deb509e2 105
pmic 3:e03714326b83 106 } else {
pmic 3:e03714326b83 107 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 108 }
altb2 0:4b02060af95b 109 } else {
pmic 3:e03714326b83 110 if(PX4.update_integral()) {
pmic 8:cb6d8bc54aaf 111
pmic 8:cb6d8bc54aaf 112 imu.readGyro();
pmic 8:cb6d8bc54aaf 113 lsm_gyro[0] = imu.gyroX*1.17f;
pmic 8:cb6d8bc54aaf 114 lsm_gyro[1] = imu.gyroY*1.17f;
pmic 8:cb6d8bc54aaf 115 lsm_gyro[2] = imu.gyroZ*1.17f;
pmic 8:cb6d8bc54aaf 116
pmic 8:cb6d8bc54aaf 117 px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
pmic 8:cb6d8bc54aaf 118 px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
pmic 8:cb6d8bc54aaf 119 px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
pmic 6:db20dd57a30a 120
pmic 8:cb6d8bc54aaf 121 counts = encoder;
pmic 8:cb6d8bc54aaf 122 velocity = diffcounter(counts)*gain_encoder;
pmic 8:cb6d8bc54aaf 123
pmic 7:692d01d007f9 124 /*
pmic 8:cb6d8bc54aaf 125 dist = tfmini();
pmic 8:cb6d8bc54aaf 126 */
pmic 8:cb6d8bc54aaf 127
pmic 8:cb6d8bc54aaf 128 dt = timer.read();
pmic 8:cb6d8bc54aaf 129 timer.reset();
pmic 8:cb6d8bc54aaf 130
pmic 8:cb6d8bc54aaf 131 /*
pmic 8:cb6d8bc54aaf 132 pc.printf("Valid Framecount: %3.0d\t", PX4.frame_count_since_last_readout());
pmic 8:cb6d8bc54aaf 133 pc.printf("Flow X: %3.0d\t", PX4.pixel_flow_x_integral());
pmic 8:cb6d8bc54aaf 134 pc.printf("Flow Y: %3.0d\t", PX4.pixel_flow_y_integral());
pmic 8:cb6d8bc54aaf 135 pc.printf("Gyro X: %3.0d\t", PX4.gyro_x_rate_integral());
pmic 8:cb6d8bc54aaf 136 pc.printf("Gyro Y: %3.0d\t", PX4.gyro_y_rate_integral());
pmic 8:cb6d8bc54aaf 137 pc.printf("Gyro Z: %3.0d\t", PX4.gyro_z_rate_integral());
pmic 3:e03714326b83 138 pc.printf("Quality: %3.0d\t", PX4.quality_integral());
pmic 8:cb6d8bc54aaf 139 pc.printf("Framecount: %10.d\t", PX4.sonar_timestamp_integral());
pmic 8:cb6d8bc54aaf 140 pc.printf("Readout Hz*1000: %3.d\t", PX4.ground_distance_integral());
pmic 8:cb6d8bc54aaf 141 pc.printf("Gyro Temp: %3.d\t", PX4.gyro_temperature());
pmic 8:cb6d8bc54aaf 142 pc.printf("Update Hz*1000: %8.0d\t", PX4.integration_timespan());
pmic 3:e03714326b83 143 pc.printf("\r\n");
pmic 7:692d01d007f9 144 */
pmic 7:692d01d007f9 145
pmic 9:be40afb750d3 146 // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
pmic 12:19fe4f6a8b6b 147 pc.printf("%6i; %6.3f; %6.3f;", counter, velocity, 1.0f/dt);
pmic 12:19fe4f6a8b6b 148 pc.printf("%6.3f; %6.3f; %6.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
pmic 12:19fe4f6a8b6b 149 // pc.printf("%6i; %6.3f; %6.3i;", counter, 1.0f/dt);
pmic 12:19fe4f6a8b6b 150 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());
pmic 12:19fe4f6a8b6b 151 pc.printf("%6.3f; %6.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
pmic 12:19fe4f6a8b6b 152 pc.printf("%6.3f; %6.3f; %4d; %4i;", PX4.update_fs(), PX4.readout_fs(), PX4.all_frame_count(), PX4.valid_frame_count());
pmic 9:be40afb750d3 153 pc.printf("%4i", PX4.avg_scaled_quality());
pmic 10:c654deb509e2 154 pc.printf("\r\n");
pmic 10:c654deb509e2 155
pmic 8:cb6d8bc54aaf 156 counter++;
pmic 12:19fe4f6a8b6b 157 // wait(0.0155); // 50 Hz
pmic 9:be40afb750d3 158 wait(0.0455); // 20 Hz USE 20 Hz!!!
pmic 9:be40afb750d3 159 // wait(0.0955); // 10 Hz
pmic 9:be40afb750d3 160 // wait(0.4955); // 2 Hz
pmic 9:be40afb750d3 161 // wait(0.9955); // 1 Hz
pmic 8:cb6d8bc54aaf 162
pmic 3:e03714326b83 163 } else {
pmic 3:e03714326b83 164 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 165 }
altb2 0:4b02060af95b 166 }
pmic 8:cb6d8bc54aaf 167 // wait(0.046);
altb2 0:4b02060af95b 168 }
altb2 0:4b02060af95b 169 }
altb2 0:4b02060af95b 170