aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/airspeed_calibration.cpp108
-rw-r--r--src/modules/commander/state_machine_helper.cpp26
-rw-r--r--src/modules/sensors/sensor_params.c13
-rw-r--r--src/modules/sensors/sensors.cpp20
-rw-r--r--src/modules/uORB/topics/differential_pressure.h1
5 files changed, 125 insertions, 43 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 5d21d89d0..421e53ae0 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -51,6 +51,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
+#include <systemlib/airspeed.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
- mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
- const int calibration_count = 2000;
+ const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
- int calibration_counter = 0;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
- 0.0f,
+ diff_pres_offset,
1.0f,
};
@@ -95,12 +94,21 @@ int do_airspeed_calibration(int mavlink_fd)
}
if (!paramreset_successful) {
- warn("FAILED to set scale / offsets for airspeed");
- mavlink_log_critical(mavlink_fd, "dpress reset failed");
- mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
- return ERROR;
+ warn("FAILED to reset - assuming analog");
+ mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+
+ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+ close(diff_pres_sub);
+ return ERROR;
+ }
}
+ unsigned calibration_counter = 0;
+
+ mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+ usleep(500 * 1000);
+
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -112,11 +120,25 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+ 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");
+ usleep(5000 * 1000);
+ continue;
+ }
+
+ /* do not log negative values in the second go */
+ if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) {
+ mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart");
+ close(diff_pres_sub);
+ return ERROR;
+ }
+
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
- mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2);
}
} else if (poll_ret == 0) {
@@ -147,14 +169,72 @@ int do_airspeed_calibration(int mavlink_fd)
return ERROR;
}
- mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
- tune_neutral(true);
- close(diff_pres_sub);
- return OK;
-
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
+
+ /* wait 500 ms to ensure parameter propagated through the system */
+ usleep(500 * 1000);
+
+ calibration_counter = 0;
+ diff_pres_offset = 0.0f;
+
+ /* just take a few samples and make sure pitot tubes are not reversed */
+ while (calibration_counter < 50) {
+
+ /* wait blocking for new data */
+ struct pollfd fds[1];
+ fds[0].fd = diff_pres_sub;
+ fds[0].events = POLLIN;
+
+ int poll_ret = poll(fds, 1, 1000);
+
+ if (poll_ret) {
+ orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+ diff_pres_offset += diff_pres.differential_pressure_raw_pa;
+ calibration_counter++;
+
+ float curr_avg = (diff_pres_offset / calibration_counter);
+
+ if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) {
+ mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot");
+ usleep(5000 * 1000);
+ continue;
+ }
+
+ /* do not log negative values in the second go */
+ if (curr_avg < calc_indicated_airspeed(-5.0f)) {
+ mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart");
+ close(diff_pres_sub);
+
+ /* the user setup is wrong, wipe the calibration to force a proper re-calibration */
+
+ diff_pres_offset = 0.0f;
+ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+ close(diff_pres_sub);
+ return ERROR;
+ }
+
+ /* save */
+ (void)param_save_default();
+
+ return ERROR;
+ }
+
+ } else if (poll_ret == 0) {
+ /* any poll failure for 1s is a reason to abort */
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+ close(diff_pres_sub);
+ return ERROR;
+ }
+ }
+
+ mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+ tune_neutral(true);
+ close(diff_pres_sub);
+ return OK;
}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 363f5e8eb..e90c77af4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -661,31 +662,22 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (!status->is_rotary_wing) {
-
/* accel done, close it */
close(fd);
- fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+ fd = orb_subscribe(ORB_ID(airspeed));
+
+ struct airspeed_s airspeed;
- if (fd <= 0) {
+ if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) ||
+ hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
failed = true;
goto system_eval;
}
- struct differential_pressure_s diff_pres;
-
- ret = read(fd, &diff_pres, sizeof(diff_pres));
-
- if (ret == sizeof(diff_pres)) {
- if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
- mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
- // XXX do not make this fatal yet
- }
- } else {
- mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
- /* this is frickin' fatal */
- failed = true;
- goto system_eval;
+ if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+ mavlink_log_critical(mavlink_fd, "#audio: WIND OR CALIBRATION MISSING");
+ // XXX do not make this fatal yet
}
}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 5deb471d6..38b190761 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -194,16 +194,25 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
/**
* Differential pressure sensor offset
*
+ * The offset (zero-reading) in Pascal
+ *
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
/**
- * Differential pressure sensor analog enabled
+ * Differential pressure sensor analog scaling
+ *
+ * Pick the appropriate scaling from the datasheet.
+ * this number defines the (linear) conversion from voltage
+ * to Pascal (pa). For the MPXV7002DP this is 1000.
+ *
+ * NOTE: If the sensor always registers zero, try switching
+ * the static and dynamic tubes.
*
* @group Sensor Calibration
*/
-PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
+PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
/**
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c40e52a0d..6bafb9ba6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -248,7 +248,7 @@ private:
float accel_offset[3];
float accel_scale[3];
float diff_pres_offset_pa;
- float diff_pres_analog_enabled;
+ float diff_pres_analog_scale;
int board_rotation;
int external_mag_rotation;
@@ -311,7 +311,7 @@ private:
param_t mag_offset[3];
param_t mag_scale[3];
param_t diff_pres_offset_pa;
- param_t diff_pres_analog_enabled;
+ param_t diff_pres_analog_scale;
param_t rc_map_roll;
param_t rc_map_pitch;
@@ -501,6 +501,7 @@ Sensors::Sensors() :
_battery_current_timestamp(0)
{
memset(&_rc, 0, sizeof(_rc));
+ memset(&_diff_pres, 0, sizeof(_diff_pres));
/* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -590,7 +591,7 @@ Sensors::Sensors() :
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
- _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
+ _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
@@ -798,7 +799,7 @@ Sensors::parameters_update()
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
- param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
+ param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -1323,22 +1324,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
- float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
+ float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
/**
* The voltage divider pulls the signal down, only act on
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
- if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) {
+ if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
- float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
+ float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
+ float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
- _diff_pres.differential_pressure_filtered_pa = diff_pres_pa;
+ _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
+ _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
_diff_pres.temperature = -1000.0f;
- _diff_pres.voltage = voltage;
/* announce the airspeed if needed, just publish else */
if (_diff_pres_pub > 0) {
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 01e14cda9..cd48d3cb2 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -58,7 +58,6 @@ struct differential_pressure_s {
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
- float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 19:22:29 +0000