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@4:77914e52baf3, 2019-08-28 (annotated)

Committer:
pmic
Date:
Wed Aug 28 11:43:15 2019 +0000
Revision:
4:77914e52baf3
Parent:
3:e03714326b83
Child:
5:d97332d7f812
Save actual working status

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