forked from Archive/PX4-Autopilot
airspeed calibration: Update and resolve compile errors
This commit is contained in:
parent
0332b79cdf
commit
0d1ac42354
|
@ -51,6 +51,7 @@
|
||||||
#include <mavlink/mavlink_log.h>
|
#include <mavlink/mavlink_log.h>
|
||||||
#include <systemlib/param/param.h>
|
#include <systemlib/param/param.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/airspeed.h>
|
||||||
|
|
||||||
/* oddly, ERROR is not defined for c++ */
|
/* oddly, ERROR is not defined for c++ */
|
||||||
#ifdef ERROR
|
#ifdef ERROR
|
||||||
|
@ -103,16 +104,10 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) {
|
|
||||||
|
|
||||||
unsigned calibration_counter = 0;
|
unsigned calibration_counter = 0;
|
||||||
|
|
||||||
if (i == 0) {
|
|
||||||
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
|
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
|
||||||
usleep(500 * 1000);
|
usleep(500 * 1000);
|
||||||
}
|
|
||||||
|
|
||||||
float diff_pres_offset = 0.0f;
|
|
||||||
|
|
||||||
while (calibration_counter < calibration_count) {
|
while (calibration_counter < calibration_count) {
|
||||||
|
|
||||||
|
@ -126,8 +121,6 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
if (poll_ret) {
|
if (poll_ret) {
|
||||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||||
|
|
||||||
if (i > 0) {
|
|
||||||
|
|
||||||
if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) {
|
if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) {
|
||||||
mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot");
|
mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot");
|
||||||
usleep(5000 * 1000);
|
usleep(5000 * 1000);
|
||||||
|
@ -140,7 +133,6 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
close(diff_pres_sub);
|
close(diff_pres_sub);
|
||||||
return ERROR;
|
return ERROR;
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
|
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
@ -156,7 +148,6 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||||
return ERROR;
|
return ERROR;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
diff_pres_offset = diff_pres_offset / calibration_count;
|
diff_pres_offset = diff_pres_offset / calibration_count;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue