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@10:c654deb509e2, 2019-09-20 (annotated)

Committer:
pmic
Date:
Fri Sep 20 12:55:45 2019 +0000
Revision:
10:c654deb509e2
Parent:
9:be40afb750d3
Child:
11:d367224f2194
Latest adjustment for testing.

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 10:c654deb509e2 57 if(1) {
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 9:be40afb750d3 147 pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
pmic 9:be40afb750d3 148 pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
pmic 9:be40afb750d3 149 pc.printf("%10.3f; %10.3f; %10.3f; %10.4f;", PX4.avg_gyro_x(), PX4.avg_gyro_y(), PX4.avg_gyro_z(), PX4.int_timespan());
pmic 9:be40afb750d3 150 pc.printf("%10.3f; %10.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
pmic 9:be40afb750d3 151 pc.printf("%10.3f; %10.3f; %4d; %4i;", PX4.update_fs(), PX4.readout_fs(), PX4.all_frame_count(), PX4.valid_frame_count());
pmic 9:be40afb750d3 152 pc.printf("%4i", PX4.avg_scaled_quality());
pmic 10:c654deb509e2 153 pc.printf("\r\n");
pmic 10:c654deb509e2 154
pmic 8:cb6d8bc54aaf 155 counter++;
pmic 9:be40afb750d3 156 wait(0.0455); // 20 Hz USE 20 Hz!!!
pmic 9:be40afb750d3 157 // wait(0.0955); // 10 Hz
pmic 9:be40afb750d3 158 // wait(0.4955); // 2 Hz
pmic 9:be40afb750d3 159 // wait(0.9955); // 1 Hz
pmic 8:cb6d8bc54aaf 160
pmic 3:e03714326b83 161 } else {
pmic 3:e03714326b83 162 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 163 }
altb2 0:4b02060af95b 164 }
pmic 8:cb6d8bc54aaf 165 // wait(0.046);
altb2 0:4b02060af95b 166 }
altb2 0:4b02060af95b 167 }
altb2 0:4b02060af95b 168