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
- Committer:
- pmic
- Date:
- 2019-08-22
- Revision:
- 3:e03714326b83
- Parent:
- 1:562a583eb77c
- Child:
- 4:77914e52baf3
File content as of revision 3:e03714326b83:
#include "mbed.h" #include "PX4Flow.h" #include "LSM9DS1_i2c.h" // #include "LSM9DS1_Registers.h" // #include "LSM9DS1_Types.h" Serial pc(SERIAL_TX, SERIAL_RX); I2C i2c(PC_9, PA_8); 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(); 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 { 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); } }