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@8:cb6d8bc54aaf, 2019-09-06 (annotated)

Committer:
pmic
Date:
Fri Sep 06 09:32:37 2019 +0000
Revision:
8:cb6d8bc54aaf
Parent:
7:692d01d007f9
Child:
9:be40afb750d3
New integral frame is used according to px4flow software enhancements. Matching of the signals and scaling in the main file.

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 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 8:cb6d8bc54aaf 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 3:e03714326b83 77 } else {
pmic 3:e03714326b83 78 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 79 }
altb2 0:4b02060af95b 80 } else {
pmic 3:e03714326b83 81 if(PX4.update_integral()) {
pmic 8:cb6d8bc54aaf 82
pmic 8:cb6d8bc54aaf 83 imu.readGyro();
pmic 8:cb6d8bc54aaf 84 lsm_gyro[0] = imu.gyroX*1.17f;
pmic 8:cb6d8bc54aaf 85 lsm_gyro[1] = imu.gyroY*1.17f;
pmic 8:cb6d8bc54aaf 86 lsm_gyro[2] = imu.gyroZ*1.17f;
pmic 8:cb6d8bc54aaf 87
pmic 8:cb6d8bc54aaf 88 px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
pmic 8:cb6d8bc54aaf 89 px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
pmic 8:cb6d8bc54aaf 90 px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
pmic 6:db20dd57a30a 91
pmic 8:cb6d8bc54aaf 92 counts = encoder;
pmic 8:cb6d8bc54aaf 93 velocity = diffcounter(counts)*gain_encoder;
pmic 8:cb6d8bc54aaf 94
pmic 7:692d01d007f9 95 /*
pmic 8:cb6d8bc54aaf 96 dist = tfmini();
pmic 8:cb6d8bc54aaf 97 */
pmic 8:cb6d8bc54aaf 98
pmic 8:cb6d8bc54aaf 99 dt = timer.read();
pmic 8:cb6d8bc54aaf 100 timer.reset();
pmic 8:cb6d8bc54aaf 101
pmic 8:cb6d8bc54aaf 102 /*
pmic 8:cb6d8bc54aaf 103 pc.printf("Valid Framecount: %3.0d\t", PX4.frame_count_since_last_readout());
pmic 8:cb6d8bc54aaf 104 pc.printf("Flow X: %3.0d\t", PX4.pixel_flow_x_integral());
pmic 8:cb6d8bc54aaf 105 pc.printf("Flow Y: %3.0d\t", PX4.pixel_flow_y_integral());
pmic 8:cb6d8bc54aaf 106 pc.printf("Gyro X: %3.0d\t", PX4.gyro_x_rate_integral());
pmic 8:cb6d8bc54aaf 107 pc.printf("Gyro Y: %3.0d\t", PX4.gyro_y_rate_integral());
pmic 8:cb6d8bc54aaf 108 pc.printf("Gyro Z: %3.0d\t", PX4.gyro_z_rate_integral());
pmic 3:e03714326b83 109 pc.printf("Quality: %3.0d\t", PX4.quality_integral());
pmic 8:cb6d8bc54aaf 110 pc.printf("Framecount: %10.d\t", PX4.sonar_timestamp_integral());
pmic 8:cb6d8bc54aaf 111 pc.printf("Readout Hz*1000: %3.d\t", PX4.ground_distance_integral());
pmic 8:cb6d8bc54aaf 112 pc.printf("Gyro Temp: %3.d\t", PX4.gyro_temperature());
pmic 8:cb6d8bc54aaf 113 pc.printf("Update Hz*1000: %8.0d\t", PX4.integration_timespan());
pmic 3:e03714326b83 114 pc.printf("\r\n");
pmic 7:692d01d007f9 115 */
pmic 7:692d01d007f9 116
pmic 8:cb6d8bc54aaf 117
pmic 8:cb6d8bc54aaf 118 float flowx = (float)PX4.pixel_flow_x_integral()/3.0f; // avg flow x in mm/s
pmic 8:cb6d8bc54aaf 119 float flowy = (float)PX4.pixel_flow_y_integral()/3.0f; // avg flow y in mm/s
pmic 8:cb6d8bc54aaf 120 uint8_t valid_frame_count = (uint8_t)PX4.frame_count_since_last_readout(); // nr. of valid frames (qual > 0) between i2c readings
pmic 8:cb6d8bc54aaf 121 uint8_t frame_count = (uint8_t)PX4.sonar_timestamp_integral(); // nr. of frames between i2c readings
pmic 8:cb6d8bc54aaf 122 float update_fs = (float)PX4.integration_timespan()/1000.0f; // Hz
pmic 8:cb6d8bc54aaf 123 float readout_fs = (float)PX4.ground_distance_integral()/1000.0f; // Hz
pmic 8:cb6d8bc54aaf 124 uint8_t quality = PX4.quality_integral(); // avg quality 0...255
pmic 8:cb6d8bc54aaf 125 float gyro_temperature = (float)PX4.gyro_temperature()*0.01f; // avg temp deg
pmic 8:cb6d8bc54aaf 126
pmic 8:cb6d8bc54aaf 127 // compare encoder and px4flow output, branch IndNaV
pmic 8:cb6d8bc54aaf 128 pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
pmic 8:cb6d8bc54aaf 129 pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
pmic 8:cb6d8bc54aaf 130 pc.printf("%10.3f; %10.3f; %10.3f; %10.3f;", px4_gyro[0], px4_gyro[1], px4_gyro[2], gyro_temperature);
pmic 8:cb6d8bc54aaf 131 pc.printf("%10.3f; %10.3f; %10i;", flowx, flowy, quality);
pmic 8:cb6d8bc54aaf 132 pc.printf("%10.3f; %10.3f; %10d; %10i", update_fs, readout_fs, frame_count, valid_frame_count);
pmic 8:cb6d8bc54aaf 133 pc.printf("\r\n");
pmic 8:cb6d8bc54aaf 134
pmic 8:cb6d8bc54aaf 135
pmic 8:cb6d8bc54aaf 136 counter++;
pmic 8:cb6d8bc54aaf 137 wait(0.0455);
pmic 8:cb6d8bc54aaf 138
pmic 3:e03714326b83 139 } else {
pmic 3:e03714326b83 140 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 141 }
altb2 0:4b02060af95b 142 }
pmic 8:cb6d8bc54aaf 143 // wait(0.046);
altb2 0:4b02060af95b 144 }
altb2 0:4b02060af95b 145 }
altb2 0:4b02060af95b 146