fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

apply differential pressure calibration (SENS_DPRES_OFF) centrally

Browse Source 
- remove drv_airspeed.h and ioctls
This commit is contained in:
Daniel Agar
2022-04-26 08:39:19 -04:00
parent c6ab4c466e
commit 258f558fea
12 changed files with 29 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+11 -76
View File
@@ -49,7 +49,7 @@
#include <fcntl.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
@@ -76,47 +76,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
const unsigned calibration_count = (maxcount * 2) / 3;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd >= 0) {
if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
@@ -128,15 +94,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
differential_pressure_s diff_pres;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
@@ -154,7 +114,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
} else {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
@@ -164,18 +124,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale >= 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
// Prevent a completely zero param
// since this is used to detect a missing calibration
// This value is numerically down in the noise and has
@@ -210,16 +158,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
differential_pressure_s diff_pres;
if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
@@ -257,7 +198,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_counter++;
} else if (poll_ret == 0) {
} else {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
@@ -274,13 +215,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
* the followup preflight checks might fail. */
px4_usleep(2e6);
normal_return:
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
px4_sleep(1);
src/modules/sensors/sensors.cpp
+9 -21
View File
@@ -42,8 +42,8 @@
*/
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_sensor.h>
#include <lib/airspeed/airspeed.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
@@ -416,8 +416,13 @@ void Sensors::diff_pres_poll()
airspeed_s airspeed{};
airspeed.timestamp = diff_pres.timestamp;
// apply calibration offset (SENS_DPRES_OFF)
const float differential_pressure_raw_pa = diff_pres.differential_pressure_raw_pa - _parameters.diff_pres_offset_pa;
const float differential_pressure_filtered_pa = diff_pres.differential_pressure_filtered_pa -
_parameters.diff_pres_offset_pa;
/* push data into validator */
float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
@@ -446,7 +451,7 @@ void Sensors::diff_pres_poll()
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
@@ -472,23 +477,6 @@ Sensors::parameter_update_poll(bool forced)
// update parameters from storage
parameters_update();
updateParams();
/* update airspeed scale */
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
/* this sensor is optional, abort without error */
if (fd >= 0) {
struct airspeed_scale airscale = {
_parameters.diff_pres_offset_pa,
1.0f,
};
if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
}
px4_close(fd);
}
}
}
@@ -530,7 +518,7 @@ void Sensors::adc_poll()
* vref. Those devices require no divider at all.
*/
if (voltage > 0.4f) {
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;