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

Update baro interface

Browse Source
This commit is contained in:
kamilritz
2020-01-23 09:34:07 +11:00
committed by Paul Riseborough
parent 74ec80cdc7
commit b8a3ed5f09
5 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+2 -2
View File
@@ -128,8 +128,8 @@ struct magSample {
}; };
struct baroSample { struct baroSample {
float hgt{0.0f}; ///< pressure altitude above sea level (m) float hgt; ///< pressure altitude above sea level (m)
uint64_t time_us{0}; ///< timestamp of the measurement (uSec) uint64_t time_us; ///< timestamp of the measurement (uSec)
}; };
struct rangeSample { struct rangeSample {
EKF/estimator_interface.cpp
+5 -5
View File
@@ -245,7 +245,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
} }
} }
void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) void EstimatorInterface::setBaroData(const baroSample &baro_sample)
{ {
if (!_initialised || _baro_buffer_fail) { if (!_initialised || _baro_buffer_fail) {
return; return;
@@ -265,11 +265,12 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
// downsample to highest possible sensor rate // downsample to highest possible sensor rate
// by baro data by taking the average of incoming sample // by baro data by taking the average of incoming sample
_baro_sample_count++; _baro_sample_count++;
_baro_alt_sum += baro_alt_meter; _baro_alt_sum += baro_sample.hgt;
_baro_timestamp_sum += time_usec / 1000; // Dividing by 1000 to avoid overflow _baro_timestamp_sum += baro_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if ((time_usec - _time_last_baro) > _min_obs_interval_us) { if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) {
_time_last_baro = baro_sample.time_us;
const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count; const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count;
@@ -284,7 +285,6 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
_baro_buffer.push(baro_sample_new); _baro_buffer.push(baro_sample_new);
_time_last_baro = time_usec;
_baro_sample_count = 0; _baro_sample_count = 0;
_baro_alt_sum = 0.0f; _baro_alt_sum = 0.0f;
_baro_timestamp_sum = 0; _baro_timestamp_sum = 0;
EKF/estimator_interface.h
+1 -1
View File
@@ -181,7 +181,7 @@ public:
void setGpsData(const gps_message &gps); void setGpsData(const gps_message &gps);
void setBaroData(uint64_t time_usec, float data); void setBaroData(const baroSample &baro_sample);
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas); void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas);
test/sensor_simulator/baro.cpp
+2 -1
View File
@@ -15,7 +15,8 @@ Baro::~Baro()
void Baro::send(uint64_t time) void Baro::send(uint64_t time)
{ {
_ekf->setBaroData(time,_baro_data); const baroSample baro_sample {_baro_data, time};
_ekf->setBaroData(baro_sample);
} }
void Baro::setData(float baro) void Baro::setData(float baro)
test/test_EKF_measurementSampling.cpp
+2 -1
View File
@@ -66,7 +66,8 @@ TEST_F(EkfMeasurementSamplingTest, baroDownSampling)
imu_sample.time_us = time; imu_sample.time_us = time;
_ekf->setIMUData(imu_sample); _ekf->setIMUData(imu_sample);
} }
_ekf->setBaroData(time, baro_data); const baroSample baro_sample {baro_data, time};
_ekf->setBaroData(baro_sample);
baro_data *= -1.0f; baro_data *= -1.0f;
time += 1000000 / baro_rate_Hz; time += 1000000 / baro_rate_Hz;
} }