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

differential pressure remove filters from drivers and average in sensors/airspeed

Browse Source
This commit is contained in:
Daniel Agar
2022-04-26 08:39:19 -04:00
parent 258f558fea
commit d1d15a6f6d
20 changed files with 212 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/differential_pressure/ets/ets_airspeed.cpp
+9 -14
View File
@@ -122,14 +122,12 @@ ETSAirspeed::measure()
int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
perf_count(_comms_errors);
@@ -138,25 +136,22 @@ ETSAirspeed::collect()
float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]);
differential_pressure_s report{};
report.timestamp = hrt_absolute_time();
if (diff_pres_pa_raw < FLT_EPSILON) {
// a zero value indicates no measurement
// since the noise floor has been arbitrarily killed
// it defeats our stuck sensor detection - the best we
// can do is to output some numerical noise to show
// that we are still correctly sampling.
diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
diff_pres_pa_raw = 0.001f * (timestamp_sample & 0x01);
}
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
differential_pressure_s report{};
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id.devid;
report.differential_pressure_pa = diff_pres_pa_raw;
report.temperature = NAN;
report.error_count = perf_event_count(_comms_errors);
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
+12 -23
View File
@@ -49,7 +49,6 @@
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
*/
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -70,7 +69,6 @@ enum MS_DEVICE_TYPE {
/* Measurement rate is 100Hz */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
@@ -91,8 +89,6 @@ protected:
int measure() override;
int collect() override;
math::LowPassFilter2p<float> _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
};
/*
@@ -136,10 +132,10 @@ int
MEASAirspeed::collect()
{
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
const hrt_abstime timestamp_sample = hrt_absolute_time();
uint8_t val[4] = {0, 0, 0, 0};
int ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
@@ -199,32 +195,25 @@ MEASAirspeed::collect()
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
float diff_press_pa = diff_press_PSI * PSI_to_Pa;
/*
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
*/
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
}
ret = OK;
differential_pressure_s report;
report.timestamp_sample = timestamp_sample;
report.device_id = get_device_id();
report.differential_pressure_pa = diff_press_pa;
report.temperature = temperature;
report.error_count = perf_event_count(_comms_errors);
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
perf_end(_sample_perf);
return ret;
return PX4_OK;
}
void
src/drivers/differential_pressure/ms5525/MS5525.cpp
+10 -13
View File
@@ -184,6 +184,7 @@ MS5525::collect()
// read ADC
uint8_t cmd = CMD_ADC_READ;
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = transfer(&cmd, 1, nullptr, 0);
if (ret != PX4_OK) {
@@ -257,22 +258,18 @@ MS5525::collect()
// 1 PSI = 6894.76 Pascals
static constexpr float PSI_to_Pa = 6894.757f;
const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
const float diff_press_pa = diff_press_PSI * PSI_to_Pa;
const float temperature_c = TEMP * 0.01f;
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s diff_pressure{};
diff_pressure.error_count = perf_event_count(_comms_errors);
diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw;
diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
diff_pressure.temperature = temperature_c;
diff_pressure.device_id = _device_id.devid;
diff_pressure.timestamp = hrt_absolute_time();
_airspeed_pub.publish(diff_pressure);
}
differential_pressure_s diff_pressure;
diff_pressure.timestamp_sample = timestamp_sample;
diff_pressure.device_id = get_device_id();
diff_pressure.differential_pressure_pa = diff_press_pa;
diff_pressure.temperature = temperature_c;
diff_pressure.error_count = perf_event_count(_comms_errors);
diff_pressure.timestamp = hrt_absolute_time();
_airspeed_pub.publish(diff_pressure);
ret = OK;
src/drivers/differential_pressure/ms5525/MS5525.hpp
-5
View File
@@ -35,7 +35,6 @@
#include <drivers/airspeed/airspeed.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -45,7 +44,6 @@ static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76;
/* Measurement rate is 100Hz */
static constexpr unsigned MEAS_RATE = 100;
static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f;
static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
class MS5525 : public Airspeed, public I2CSPIDriver<MS5525>
@@ -70,9 +68,6 @@ private:
int measure() override;
int collect() override;
// temperature is read once every 10 cycles
math::LowPassFilter2p<float> _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
+11 -13
View File
@@ -146,6 +146,8 @@ SDP3X::collect()
{
perf_begin(_sample_perf);
const hrt_abstime timestamp_sample = hrt_absolute_time();
// read 6 bytes from the sensor
uint8_t val[6];
int ret = transfer(nullptr, 0, &val[0], sizeof(val));
@@ -164,21 +166,17 @@ SDP3X::collect()
int16_t P = (((int16_t)val[0]) << 8) | val[1];
int16_t temp = (((int16_t)val[3]) << 8) | val[4];
float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
float diff_press_pa = static_cast<float>(P) / static_cast<float>(_scale);
float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
}
differential_pressure_s report;
report.device_id = get_device_id();
report.timestamp_sample = timestamp_sample;
report.differential_pressure_pa = diff_press_pa;
report.temperature = temperature_c;
report.error_count = perf_event_count(_comms_errors);
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
perf_end(_sample_perf);
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
-4
View File
@@ -43,7 +43,6 @@
#include <drivers/airspeed/airspeed.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -64,7 +63,6 @@
// Measurement rate is 20Hz
#define SPD3X_MEAS_RATE 100
#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */
class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
@@ -98,8 +96,6 @@ private:
int configure();
int read_scale();
math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
bool init_sdp3x();
/**