44 #include <px4_platform_common/defines.h> 45 #include <px4_platform_common/posix.h> 46 #include <px4_platform_common/time.h> 70 unsigned calibration_counter = 0;
71 const unsigned maxcount = 2400;
76 const unsigned calibration_count = (maxcount * 2) / 3;
81 float diff_pres_offset = 0.0f;
89 bool paramreset_successful =
false;
94 paramreset_successful =
true;
105 if (!paramreset_successful) {
108 float analog_scaling = 0.0f;
111 if (fabsf(analog_scaling) < 0.1
f) {
124 px4_usleep(500 * 1000);
126 while (calibration_counter < calibration_count) {
133 px4_pollfd_struct_t fds[1];
134 fds[0].fd = diff_pres_sub;
135 fds[0].events = POLLIN;
137 int poll_ret =
px4_poll(fds, 1, 1000);
140 orb_copy(
ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
143 calibration_counter++;
154 if (calibration_counter % (calibration_count / 20) == 0) {
158 }
else if (poll_ret == 0) {
165 diff_pres_offset = diff_pres_offset / calibration_count;
167 if (PX4_ISFINITE(diff_pres_offset)) {
184 if (fabsf(diff_pres_offset) < 0.00000001
f) {
185 diff_pres_offset = 0.00000001f;
201 px4_usleep(500 * 1000);
205 calibration_counter = 0;
208 while (calibration_counter < maxcount) {
215 px4_pollfd_struct_t fds[1];
216 fds[0].fd = diff_pres_sub;
217 fds[0].events = POLLIN;
219 int poll_ret =
px4_poll(fds, 1, 1000);
222 orb_copy(
ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
237 diff_pres_offset = 0.0f;
253 if (calibration_counter % 500 == 0) {
259 calibration_counter++;
261 }
else if (poll_ret == 0) {
268 if (calibration_counter == maxcount) {
void tune_neutral(bool use_buzzer)
Blink white LED and play neutral tune (if use_buzzer == true).
static orb_advert_t * mavlink_log_pub
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
float differential_pressure_raw_pa
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
#define AIRSPEED0_DEVICE_PATH
Common calibration messages.
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
High-resolution timer with callouts and timekeeping.
Global flash based parameter store.
void calibrate_cancel_unsubscribe(int cmd_sub)
Called to cancel the subscription to the cancel command.
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Commander helper functions definitions.
airspeed scaling factors; out = (in * Vscale) + offset
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
#define CAL_QGC_STARTED_MSG
#define CAL_ERROR_SET_PARAMS_MSG
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__EXPORT int param_save_default(void)
Save parameters to the default file.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
#define AIRSPEEDIOCSSCALE
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
Used to periodically check for a cancel command.
#define calibration_log_critical(_pub, _text,...)
static const char * sensor_name
#define CAL_QGC_PROGRESS_MSG
#define CAL_QGC_FAILED_MSG
Airspeed driver interface.
int calibrate_cancel_subscribe()
Called at the beginning of calibration in order to subscribe to the cancel command.
float differential_pressure_filtered_pa
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
#define calibration_log_info(_pub, _text,...)