Diff: main.cpp

Revision:

3:e03714326b83

Parent:

1:562a583eb77c

Child:

4:77914e52baf3

diff -r 562a583eb77c -r e03714326b83 main.cpp
--- a/main.cpp	Wed Aug 21 13:21:41 2019 +0000
+++ b/main.cpp	Thu Aug 22 14:22:58 2019 +0000
@@ -1,61 +1,114 @@
 #include "mbed.h"
 #include "PX4Flow.h"
+#include "LSM9DS1_i2c.h"
+// #include "LSM9DS1_Registers.h"
+// #include "LSM9DS1_Types.h"
 
-#define INTEGRAL false
+Serial pc(SERIAL_TX, SERIAL_RX);
 
-DigitalOut myled(LED1);
-//I2C i2c(PA_10,PA_9);
 I2C i2c(PC_9, PA_8);
-Serial pc(SERIAL_TX, SERIAL_RX);
 PX4Flow PX4(i2c,pc);
+float px4_gyro[3];
 
+LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
+float lsm_gyro[3];
 
+Timer timer; // timer for time measurement
+float dt = 0.0f;
+
+uint32_t counter;
 
 int main()
 {
     i2c.frequency(400000);
     pc.baud(2000000);
+
+    for(uint8_t i = 0; i < 3; i++) px4_gyro[i] = 0.0f;
+
+    for(uint8_t i = 0; i < 3; i++) lsm_gyro[i] = 0.0f;
+    imu.begin();
+
+    timer.start();
+
+    counter = 0;
+
+    /**
+    * uint16_t frame_count;       // counts created I2C frames 0
+    * int16_t pixel_flow_x_sum;   // accumulated x flow in pixels*10 since last I2C frame 2
+    * int16_t pixel_flow_y_sum;   // accumulated y flow in pixels*10 since last I2C frame 4
+    * int16_t flow_comp_m_x;      // x velocity*1000 in meters / timestep 6
+    * int16_t flow_comp_m_y;      // y velocity*1000 in meters / timestep 8
+    * int16_t qual;               // Optical flow quality / confidence 0: bad, 255: maximum quality 10
+    * int16_t gyro_x_rate;        // gyro x rate 12
+    * int16_t gyro_y_rate;        // gyro y rate 14
+    * int16_t gyro_z_rate;        // gyro z rate 16
+    * uint8_t gyro_range;         // gyro range 18
+    * uint8_t sonar_timestamp;    // timestep in milliseconds between I2C frames 19
+    * int16_t ground_distance;    // Ground distance in meters*1000. Positive value: distance known. Negative value: Unknown distance 20
+    */
     while(1) {
+        if(1) {
+            if(PX4.update()) {
+
+                imu.readGyro();
+                lsm_gyro[0] = imu.gyroX;
+                lsm_gyro[1] = imu.gyroY;
+                lsm_gyro[2] = imu.gyroZ;
+
+                px4_gyro[0] = PX4.gyro_x_rate();
+                px4_gyro[1] = PX4.gyro_y_rate();
+                px4_gyro[2] = PX4.gyro_z_rate();
 
-if(1){
-        if(PX4.update()) {
-            //pc.printf("Frame Count: %4.0d\t", PX4.frame_count());
- //           pc.printf("Pixel Flow X Sum: %d\t", -PX4.pixel_flow_y_sum());
- //           pc.printf("Pixel Flow Y Sum: %d\t", -PX4.pixel_flow_x_sum());
-            pc.printf("Flow Comp. M X.: %4d\t", -PX4.flow_comp_m_y());
-            pc.printf("Flow Comp. M Y.: %4d\t", -PX4.flow_comp_m_x());
-            pc.printf("Quality: %d\t", PX4.qual());
-            pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
-            pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
-            pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
-            //pc.printf("Sonar Timestamp: %3.d\t", PX4.sonar_timestamp());
-            pc.printf("z: %.3f\t", float(PX4.ground_distance())/1000);
-            pc.printf("\r\n");
+                dt = timer.read();
+                /*pc.printf("Frame Count: %4.0d\t", PX4.frame_count());
+                pc.printf("Pixel Flow X Sum: %d\t", -PX4.pixel_flow_y_sum());
+                pc.printf("Pixel Flow Y Sum: %d\t", -PX4.pixel_flow_x_sum());
+                pc.printf("Flow Comp. M X.: %4d\t", -PX4.flow_comp_m_y());
+                pc.printf("Flow Comp. M Y.: %4d\t", -PX4.flow_comp_m_x());
+                pc.printf("Quality: %d\t", PX4.qual());
+                pc.printf("Gyro X: %d\t", PX4.gyro_x_rate());
+                pc.printf("Gyro Y: %d\t", PX4.gyro_y_rate());
+                pc.printf("Gyro Z: %d\t", PX4.gyro_z_rate());
+                pc.printf("Gyro Range: %d\t", PX4.gyro_range());
+                pc.printf("Sonar Timestamp: %3.d\t", PX4.sonar_timestamp());
+                pc.printf("z: %.3f\t", (float)PX4.ground_distance()/1000);*/
+
+                /*pc.printf("LSM gyro x: %.3f\t", lsm_gyro[0]);
+                pc.printf("LSM gyro y: %.3f\t", lsm_gyro[1]);
+                pc.printf("LSM gyro z: %.3f\t", lsm_gyro[2]);
+                pc.printf("PX4 gyro x: %.3f\t", px4_gyro[0]);
+                pc.printf("PX4 gyro y: %.3f\t", px4_gyro[1]);
+                pc.printf("PX4 gyro z: %.3f\t", px4_gyro[2]);
+                pc.printf("Updaterate  px4flow in Hz: %.3f\t", 1.0f/dt);
+                pc.printf("\r\n");*/
+
+                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);
+                timer.reset();
+                counter++;
+                wait_us(6800); // -> 100 Hz
+            } else {
+                pc.printf("TimeOut\r\n");
+            }
         } else {
-            pc.printf("TimeOut\r\n");
+            if(PX4.update_integral()) {
+                pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
+                pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
+                pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
+                pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
+                pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
+                pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
+                pc.printf("Quality: %3.0d\t", PX4.quality_integral());
+                pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
+                pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
+                pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
+                pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
+                pc.printf("\r\n");
+
+            } else {
+                pc.printf("TimeOut\r\n");
+            }
         }
-}
-else
-{
-        if(PX4.update_integral()) {
-        //    pc.printf("Frame Count: %3.0d\t", PX4.frame_count_since_last_readout());
-            pc.printf("Pixel Flow X Integral: %3.0d\t", PX4.pixel_flow_x_integral());
-            pc.printf("Pixel Flow Y Integral: %3.0d\t", PX4.pixel_flow_y_integral());
-            pc.printf("Gyro X Rate: %3.0d\t", PX4.gyro_x_rate_integral());
-            pc.printf("Gyro Y Rate: %3.0d\t", PX4.gyro_y_rate_integral());
-            pc.printf("Gyro Z Rate: %3.0d\t", PX4.gyro_z_rate_integral());
-            pc.printf("Quality: %3.0d\t", PX4.quality_integral());
-            pc.printf("Sonar Timestamp: %10.d\t", PX4.sonar_timestamp_integral());
-            pc.printf("Ground Distance: %3.d\t", PX4.ground_distance_integral());
-            pc.printf("Gyro Temperature: %3.d\t", PX4.gyro_temperature());
-            pc.printf("Integration Timespan: %8.0d\t", PX4.integration_timespan());
-            pc.printf("\r\n");
-
-        } else {
-            pc.printf("TimeOut\r\n");
-        }
-}
-        wait(0.025  );
+        // wait(0.025);
     }
 }