Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Update airspeed interface

Browse Source
This commit is contained in:
kamilritz
2020-01-21 14:48:37 +01:00
committed by Paul Riseborough
parent b8a3ed5f09
commit 856961ba85
4 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+7 -7
View File
@@ -291,7 +291,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
} }
} }
void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas) void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
{ {
if (!_initialised || _airspeed_buffer_fail) { if (!_initialised || _airspeed_buffer_fail) {
return; return;
@@ -309,13 +309,13 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) { if ((airspeed_sample.time_us - _time_last_airspeed) > _min_obs_interval_us) {
airspeedSample airspeed_sample_new; _time_last_airspeed = airspeed_sample.time_us;
airspeed_sample_new.true_airspeed = true_airspeed;
airspeed_sample_new.eas2tas = eas2tas; airspeedSample airspeed_sample_new = airspeed_sample;
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_airspeed = time_usec;
_airspeed_buffer.push(airspeed_sample_new); _airspeed_buffer.push(airspeed_sample_new);
} }
EKF/estimator_interface.h
+1 -1
View File
@@ -183,7 +183,7 @@ public:
void setBaroData(const baroSample &baro_sample); void setBaroData(const baroSample &baro_sample);
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas); void setAirspeedData(const airspeedSample &airspeed_sample);
void setRangeData(uint64_t time_usec, float data, int8_t quality); void setRangeData(uint64_t time_usec, float data, int8_t quality);
test/sensor_simulator/airspeed.cpp
+5 -2
View File
@@ -17,8 +17,11 @@ void Airspeed::send(uint64_t time)
{ {
if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON)
{ {
float eas2tas = _true_airspeed_data / _indicated_airspeed_data; airspeedSample airspeed_sample;
_ekf->setAirspeedData(time, _true_airspeed_data, eas2tas); airspeed_sample.time_us = time;
airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data;
airspeed_sample.true_airspeed = _true_airspeed_data;
_ekf->setAirspeedData(airspeed_sample);
} }
} }