dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Daniel Agar 2021-08-26 11:09:45 -04:00
parent 258f558fea
commit d1d15a6f6d
20 changed files with 212 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
msg/airspeed.msg
View File

@ -1,9 +1,10 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
float32 indicated_airspeed_m_s # indicated airspeed in m/s
float32 indicated_airspeed_m_s # indicated airspeed in m/s
float32 true_airspeed_m_s # true filtered airspeed in m/s
float32 true_airspeed_m_s # true filtered airspeed in m/s
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
float32 confidence # confidence value from 0 to 1 for this sensor
float32 confidence # confidence value from 0 to 1 for this sensor

16
msg/differential_pressure.msg
View File

@ -1,6 +1,10 @@
uint64 timestamp # time since system start (microseconds)
uint64 error_count # Number of errors detected by driver
float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative)
float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading
float32 temperature # Temperature provided by sensor, -1000.0f if unknown
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 differential_pressure_pa # differential pressure reading in Pascals (may be negative)
float32 temperature # Temperature provided by sensor in celcius, NAN if unknown
uint32 error_count # Number of errors detected by driver

23
src/drivers/differential_pressure/ets/ets_airspeed.cpp
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);

35
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
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

23
src/drivers/differential_pressure/ms5525/MS5525.cpp
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;

5
src/drivers/differential_pressure/ms5525/MS5525.hpp
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

24
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
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);

4
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
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();
/**

19
src/drivers/uavcan/sensors/differential_pressure.cpp
View File

@ -65,21 +65,20 @@ int UavcanDifferentialPressureBridge::init()
void UavcanDifferentialPressureBridge::air_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg)
{
const hrt_abstime timestamp_sample = hrt_absolute_time();
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
float diff_press_pa = msg.differential_pressure;
float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
if (PX4_ISFINITE(diff_press_pa)) {
differential_pressure_s report{};
differential_pressure_s report{};
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id.devid;
report.differential_pressure_pa = diff_press_pa;
report.temperature = temperature_c;
report.timestamp = hrt_absolute_time();
report.differential_pressure_raw_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
report.temperature = temperature_c;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
publish(msg.getSrcNodeID().get(), &report);
}
publish(msg.getSrcNodeID().get(), &report);
}

3
src/drivers/uavcan/sensors/differential_pressure.hpp
View File

@ -37,7 +37,6 @@
#pragma once
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <uORB/topics/differential_pressure.h>
#include "sensor_bridge.hpp"
@ -56,8 +55,6 @@ public:
int init() override;
private:
math::LowPassFilter2p<float> _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);
typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *,

2
src/drivers/uavcannode/Publishers/RawAirData.hpp
View File

@ -76,7 +76,7 @@ public:
uavcan::equipment::air_data::RawAirData raw_air_data{};
// raw_air_data.static_pressure =
raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa;
raw_air_data.differential_pressure = diff_press.differential_pressure_pa;
// raw_air_data.static_pressure_sensor_temperature =
raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;

58
src/modules/commander/airspeed_calibration.cpp
View File

@ -76,38 +76,38 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
const unsigned calibration_count = (maxcount * 2) / 3;
uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
float diff_pres_offset = 0.0f;
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;
return PX4_ERROR;
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
px4_usleep(500 * 1000);
uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
goto error_return;
return PX4_ERROR;
}
differential_pressure_s diff_pres;
if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu32 ")",
diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
if (calibration_counter % (calibration_count / 20) == 0) {
@ -117,7 +117,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
} else {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
}
@ -134,12 +134,12 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
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;
return PX4_ERROR;
}
} else {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
@ -149,28 +149,35 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
float differential_pressure_sum = 0.f;
int differential_pressure_sum_count = 0;
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
goto error_return;
return PX4_ERROR;
}
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)",
(int)diff_pres.differential_pressure_filtered_pa);
differential_pressure_sum += diff_pres.differential_pressure_pa;
differential_pressure_sum_count++;
const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset;
if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) {
if (differential_pressure_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa);
break;
} else {
/* do not allow negative values */
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
(int)differential_pressure_pa);
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@ -178,7 +185,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
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;
return PX4_ERROR;
}
/* save */
@ -186,14 +193,18 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
param_save_default();
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
(int)differential_pressure_pa);
tune_neutral(true);
// reset average
differential_pressure_sum = 0.f;
differential_pressure_sum_count = 0;
}
calibration_counter++;
@ -201,13 +212,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
} else {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
return PX4_ERROR;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
@ -215,13 +226,8 @@ 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);
normal_return:
// This give a chance for the log messages to go out of the queue before someone else stomps on then
px4_sleep(1);
return result;
error_return:
result = PX4_ERROR;
goto normal_return;
}

2
src/modules/ekf2/EKF2.cpp
View File

@ -1483,7 +1483,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
&& (airspeed.indicated_airspeed_m_s > 0.f)
) {
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.time_us = airspeed.timestamp_sample,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};

8
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -2322,11 +2322,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
// differential pressure
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
differential_pressure_s report{};
report.timestamp = timestamp;
report.timestamp_sample = timestamp;
report.device_id = 0; // TODO
report.temperature = hil_sensor.temperature;
report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);
}

2
src/modules/mavlink/streams/HIGHRES_IMU.hpp
View File

@ -191,7 +191,7 @@ private:
msg.ymag = mag(1);
msg.zmag = mag(2);
msg.abs_pressure = air_data.baro_pressure_pa;
msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
msg.diff_pressure = differential_pressure.differential_pressure_pa;
msg.pressure_alt = air_data.baro_alt_meter;
msg.temperature = air_data.baro_temp_celcius;
msg.fields_updated = fields_updated;

2
src/modules/mavlink/streams/SCALED_PRESSURE.hpp
View File

@ -83,7 +83,7 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
}

2
src/modules/mavlink/streams/SCALED_PRESSURE2.hpp
View File

@ -83,7 +83,7 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
}

2
src/modules/mavlink/streams/SCALED_PRESSURE3.hpp
View File

@ -83,7 +83,7 @@ private:
msg.time_boot_ms = differential_pressure.timestamp / 1000;
}
msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
}

174
src/modules/sensors/sensors.cpp
View File

@ -139,13 +139,16 @@ private:
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
uint64_t _airspeed_last_publish{0};
uint64_t _diff_pres_timestamp_sum{0};
float _diff_pres_pressure_sum{0.f};
float _diff_pres_temperature_sum{0.f};
float _baro_pressure_sum{0.f};
int _diff_pres_count{0};
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */
uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; /**< adc_report sub */
differential_pressure_s _diff_pres {};
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)};
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@ -393,6 +396,18 @@ void Sensors::diff_pres_poll()
if (_diff_pres_sub.update(&diff_pres)) {
if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
// ignore invalid data and reset accumulated
// reset
_diff_pres_timestamp_sum = 0;
_diff_pres_pressure_sum = 0;
_diff_pres_temperature_sum = 0;
_baro_pressure_sum = 0;
_diff_pres_count = 0;
return;
}
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
@ -413,54 +428,71 @@ void Sensors::diff_pres_poll()
}
}
airspeed_s airspeed{};
airspeed.timestamp = diff_pres.timestamp;
// push raw data into validator
float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
// 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;
// accumulate average for publication
_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
_diff_pres_temperature_sum += air_temperature_celsius;
_baro_pressure_sum += air_data.baro_pressure_pa;
_diff_pres_count++;
/* push data into validator */
float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 100_ms) {
_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
// average data and apply calibration offset (SENS_DPRES_OFF)
const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa;
const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count;
const float temperature = _diff_pres_temperature_sum / _diff_pres_count;
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
// reset
_diff_pres_timestamp_sum = 0;
_diff_pres_pressure_sum = 0;
_diff_pres_temperature_sum = 0;
_baro_pressure_sum = 0;
_diff_pres_count = 0;
enum AIRSPEED_SENSOR_MODEL smodel;
switch ((diff_pres.device_id >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
enum AIRSPEED_SENSOR_MODEL smodel;
/* fallthrough */
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
switch ((diff_pres.device_id >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
/* fallthrough */
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
/* fallthrough */
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
break;
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
default:
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
break;
}
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
break;
/* don't risk to feed negative airspeed into the system */
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,
differential_pressure_filtered_pa, air_data.baro_pressure_pa,
air_temperature_celsius);
default:
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
break;
}
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
differential_pressure_pa, baro_pressure_pa, temperature);
airspeed.air_temperature_celsius = air_temperature_celsius;
// assume that CAS = IAS as we don't have an CAS-scale here
float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
_airspeed_pub.publish(airspeed);
if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
airspeed_s airspeed;
airspeed.timestamp_sample = timestamp_sample;
airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
airspeed.true_airspeed_m_s = true_airspeed_m_s;
airspeed.air_temperature_celsius = temperature;
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
airspeed.timestamp = hrt_absolute_time();
_airspeed_pub.publish(airspeed);
_airspeed_last_publish = airspeed.timestamp;
}
}
}
}
@ -490,49 +522,41 @@ void Sensors::adc_poll()
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
if (_parameters.diff_pres_analog_scale > 0.0f) {
adc_report_s adc;
hrt_abstime t = hrt_absolute_time();
if (_adc_report_sub.update(&adc)) {
/* Read add channels we got */
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
if (adc.channel_id[i] == -1) {
continue; // skip non-exist channels
}
/* rate limit to 100 Hz */
if (t - _last_adc >= 10000) {
adc_report_s adc;
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
if (_adc_report_sub.update(&adc)) {
/* Read add channels we got */
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
if (adc.channel_id[i] == -1) {
continue; // skip non-exist channels
}
/* calculate airspeed, raw is the difference from */
const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
/**
* 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.
*
* Notice: This won't work on devices which have PGA controlled
* vref. Those devices require no divider at all.
*/
if (voltage > 0.4f) {
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
/* calculate airspeed, raw is the difference from */
const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
differential_pressure_s diff_pres{};
diff_pres.timestamp_sample = adc.timestamp;
diff_pres.differential_pressure_pa = diff_pres_pa_raw;
diff_pres.temperature = NAN;
diff_pres.timestamp = hrt_absolute_time();
/**
* 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.
*
* Notice: This won't work on devices which have PGA controlled
* vref. Those devices require no divider at all.
*/
if (voltage > 0.4f) {
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;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
(diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
_diff_pres_pub.publish(_diff_pres);
}
_diff_pres_pub.publish(diff_pres);
}
}
}
_last_adc = t;
}
}

8
src/modules/simulator/simulator_mavlink.cpp
View File

@ -352,11 +352,11 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// differential pressure
if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
differential_pressure_s report{};
report.timestamp = time;
report.timestamp_sample = time;
report.device_id = 0; // TODO
report.differential_pressure_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
report.temperature = _sensors_temperature;
report.differential_pressure_filtered_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
report.differential_pressure_raw_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);
}
}