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

sensors: add baro calibration and cleanup

Browse Source 
- sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting
This commit is contained in:
Daniel Agar
2022-03-04 09:51:02 -05:00
parent 5800c417c8
commit 0c31f63896
91 changed files with 1287 additions and 511 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/data_validator/DataValidator.cpp
+23 -22
View File
@@ -51,7 +51,6 @@ void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in,
void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t priority_in)
{
_event_count++;
if (error_count_in > _error_count) {
@@ -65,31 +64,33 @@ void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_
_priority = priority_in;
for (unsigned i = 0; i < dimensions; i++) {
if (_time_last == 0) {
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val - _mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
if (fabsf(_value[i] - val[i]) < 0.000001f) {
_value_equal_count++;
if (PX4_ISFINITE(val[i])) {
if (_time_last == 0) {
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
_value_equal_count = 0;
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val - _mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
if (fabsf(_value[i] - val[i]) < 0.000001f) {
_value_equal_count++;
} else {
_value_equal_count = 0;
}
}
// XXX replace with better filter, make it auto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
}
// XXX replace with better filter, make it auto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
}
_time_last = timestamp;