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

mathlib: merge LowPassFilter2p, LowPassFilter2pArray, and LowPassFilter2pVector3f

Browse Source 
- fix reset for direct form 2
 - push NAN checks out to filter users
This commit is contained in:
Daniel Agar
2021-05-31 10:26:45 -04:00
parent 2a792ca201
commit 5493d96d17
23 changed files with 210 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
+11 -9
View File
@@ -92,7 +92,7 @@ protected:
int measure() override;
int collect() override;
math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
math::LowPassFilter2p<float> _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
};
/*
@@ -196,16 +196,18 @@ MEASAirspeed::collect()
and bottom port is used as the static port
*/
differential_pressure_s report{};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
_airspeed_pub.publish(report);
}
ret = OK;
src/drivers/differential_pressure/ms5525/MS5525.cpp
+11 -9
View File
@@ -250,16 +250,18 @@ MS5525::collect()
const float temperature_c = TEMP * 0.01f;
differential_pressure_s diff_pressure = {
.timestamp = hrt_absolute_time(),
.error_count = perf_event_count(_comms_errors),
.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset,
.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset,
.temperature = temperature_c,
.device_id = _device_id.devid
};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s diff_pressure{};
_airspeed_pub.publish(diff_pressure);
diff_pressure.error_count = perf_event_count(_comms_errors);
diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
diff_pressure.temperature = temperature_c;
diff_pressure.device_id = _device_id.devid;
diff_pressure.timestamp = hrt_absolute_time();
_airspeed_pub.publish(diff_pressure);
}
ret = OK;
src/drivers/differential_pressure/ms5525/MS5525.hpp
+1 -1
View File
@@ -71,7 +71,7 @@ private:
int collect() override;
// temperature is read once every 10 cycles
math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
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
+10 -8
View File
@@ -156,16 +156,18 @@ SDP3X::collect()
float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
differential_pressure_s report{};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
_airspeed_pub.publish(report);
}
perf_end(_sample_perf);
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
+1 -1
View File
@@ -100,7 +100,7 @@ private:
int configure();
int read_scale();
math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
bool init_sdp3x();
src/drivers/uavcan/sensors/differential_pressure.cpp
+10 -9
View File
@@ -75,14 +75,15 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
float diff_press_pa = msg.differential_pressure;
float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
differential_pressure_s report = {
.timestamp = hrt_absolute_time(),
.error_count = 0,
.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset,
.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter
.temperature = temperature_c,
.device_id = _device_id.devid
};
if (PX4_ISFINITE(diff_press_pa)) {
differential_pressure_s report{};
publish(msg.getSrcNodeID().get(), &report);
report.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset;
report.temperature = temperature_c;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
publish(msg.getSrcNodeID().get(), &report);
}
}
src/drivers/uavcan/sensors/differential_pressure.hpp
+1 -1
View File
@@ -59,7 +59,7 @@ public:
private:
float _diff_pres_offset{0.f};
math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
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);