altb_pmic
/
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
Diff: main.cpp
- Revision:
- 13:89b65bfe6dda
- Parent:
- 12:19fe4f6a8b6b
diff -r 19fe4f6a8b6b -r 89b65bfe6dda main.cpp
--- a/main.cpp Sun Apr 05 08:47:25 2020 +0000
+++ b/main.cpp Mon Apr 06 05:57:56 2020 +0000
@@ -55,7 +55,7 @@
while(1) {
if(0) {
- if(PX4.update()) {
+ if(1) {// if(PX4.update()) {
/*
pc.printf("Frame Count: %8d\t", PX4.frame_count());
@@ -79,9 +79,9 @@
lsm_gyro[1] = imu.gyroY*1.17f;
lsm_gyro[2] = imu.gyroZ*1.17f;
- px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
- px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
- px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
counts = encoder;
velocity = diffcounter(counts)*gain_encoder;
@@ -94,11 +94,11 @@
timer.reset();
// compare encoder and px4flow output, branch OrgSoftwareNoSonar
- pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
- pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
- 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);
- pc.printf("%10.3f; %10.3f;", PX4.flow_comp_m_x()*0.3333333f, PX4.flow_comp_m_x()*0.3333333f);
- pc.printf("\r\n");
+ // pc.printf("%10i; %10.3f; %10.3f;", counter, velocity, 1.0f/dt);
+ // pc.printf("%10.3f; %10.3f; %10.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
+ // 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);
+ // pc.printf("%10.3f; %10.3f;", PX4.flow_comp_m_x()*0.3333333f, PX4.flow_comp_m_x()*0.3333333f);
+ // pc.printf("\r\n");
counter++;
wait(0.0455); // 20 Hz USE 20 Hz!!!
@@ -114,9 +114,9 @@
lsm_gyro[1] = imu.gyroY*1.17f;
lsm_gyro[2] = imu.gyroZ*1.17f;
- px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
- px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
- px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[0] = (float)PX4.gyro_x_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[1] = (float)PX4.gyro_y_rate_integral()*1.0f/155.0f/10.0f;
+ // px4_gyro[2] = (float)PX4.gyro_z_rate_integral()*1.0f/155.0f/10.0f;
counts = encoder;
velocity = diffcounter(counts)*gain_encoder;
@@ -143,6 +143,7 @@
pc.printf("\r\n");
*/
+ /*
// compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
pc.printf("%6i; %6.3f; %6.3f;", counter, velocity, 1.0f/dt);
pc.printf("%6.3f; %6.3f; %6.3f;", lsm_gyro[0], lsm_gyro[1], lsm_gyro[2]);
@@ -152,10 +153,24 @@
pc.printf("%6.3f; %6.3f; %4d; %4i;", PX4.update_fs(), PX4.readout_fs(), PX4.all_frame_count(), PX4.valid_frame_count());
pc.printf("%4i", PX4.avg_scaled_quality());
pc.printf("\r\n");
+ */
+
+ // float avg_flow_x(); // avg flow x in mm/s
+ // float avg_flow_y(); // avg flow y in mm/s
+ // uint8_t avg_qual(); // avg 0-255 linear quality measurement 0=bad, 255=best
+ // uint8_t valid_frame_count(); // nr. of valid frames (qual > 0) between i2c readings
+ // uint8_t frame_count(); // nr. of frames between i2c readings
+ // uint8_t avg_qual_scaled(); // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid_frame_count/N_frame_count
+
+ // compare encoder and px4flow output, branch IndNaV with new PX4Flow driver functionality
+ pc.printf("%6i; %6.3f;", counter, 1.0f/dt);
+ pc.printf("%6.3f; %6.3f; %4i;; %4i;", PX4.avg_flow_x(), PX4.avg_flow_y(), PX4.avg_qual_scaled(), PX4.avg_qual());
+ pc.printf("%4i; %4i;", PX4.valid_frame_count(), PX4.frame_count());
+ pc.printf("\r\n");
counter++;
// wait(0.0155); // 50 Hz
- wait(0.0455); // 20 Hz USE 20 Hz!!!
+ wait(0.0479); //: pmic, 2019 // wait(0.0455); //: pmic, int16_t for avg_flow, 2019 // 20 Hz USE 20 Hz!!!
// wait(0.0955); // 10 Hz
// wait(0.4955); // 2 Hz
// wait(0.9955); // 1 Hz