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@9:be40afb750d3, 2019-09-13 (annotated)

Committer:
pmic
Date:
Fri Sep 13 10:43:57 2019 +0000
Revision:
9:be40afb750d3
Parent:
8:cb6d8bc54aaf
Child:
10:c654deb509e2
Implement wrapper functions for PX4Flow class according to adjusted PX4Flow firmware (Git-branch: IndNav). Use the PX4Flow Sensor not with a update frequency < 10 Hz. 20 Hz is recommended.

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 9:be40afb750d3 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 9:be40afb750d3 125 float integration_timespan = (float)PX4.gyro_temperature()*0.01f; // integration timespan in ms
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 9:be40afb750d3 130 pc.printf("%10.3f; %10.3f; %10.3f; %10.3f;", px4_gyro[0], px4_gyro[1], px4_gyro[2], integration_timespan);
pmic 9:be40afb750d3 131 pc.printf("%10.3f; %10.3f; %4i;", flowx, flowy, quality);
pmic 9:be40afb750d3 132 pc.printf("%10.3f; %10.3f; %4d; %4i", update_fs, readout_fs, frame_count, valid_frame_count);
pmic 8:cb6d8bc54aaf 133 pc.printf("\r\n");
pmic 9:be40afb750d3 134 */
pmic 9:be40afb750d3 135
pmic 9:be40afb750d3 136 // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
pmic 9:be40afb750d3 137 pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
pmic 9:be40afb750d3 138 pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
pmic 9:be40afb750d3 139 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 140 pc.printf("%10.3f; %10.3f; %4i;", PX4.avg_flowx(), PX4.avg_flowy(), PX4.avg_quality());
pmic 9:be40afb750d3 141 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 142 pc.printf("%4i", PX4.avg_scaled_quality());
pmic 9:be40afb750d3 143 pc.printf("\r\n");
pmic 8:cb6d8bc54aaf 144
pmic 8:cb6d8bc54aaf 145 counter++;
pmic 9:be40afb750d3 146 wait(0.0455); // 20 Hz USE 20 Hz!!!
pmic 9:be40afb750d3 147 // wait(0.0955); // 10 Hz
pmic 9:be40afb750d3 148 // wait(0.4955); // 2 Hz
pmic 9:be40afb750d3 149 // wait(0.9955); // 1 Hz
pmic 8:cb6d8bc54aaf 150
pmic 3:e03714326b83 151 } else {
pmic 3:e03714326b83 152 pc.printf("TimeOut\r\n");
pmic 3:e03714326b83 153 }
altb2 0:4b02060af95b 154 }
pmic 8:cb6d8bc54aaf 155 // wait(0.046);
altb2 0:4b02060af95b 156 }
altb2 0:4b02060af95b 157 }
altb2 0:4b02060af95b 158