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

Update airspeed interface

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/common.h
View File

@ -140,7 +140,7 @@ struct rangeSample {
struct airspeedSample {
float true_airspeed; ///< true airspeed measurement (m/sec)
float eas2tas; ///< equivalent to true airspeed factor
float eas2tas; ///< equivalent to true airspeed factor
uint64_t time_us; ///< timestamp of the measurement (uSec)
};

14
EKF/estimator_interface.cpp
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) {
return;
@ -309,13 +309,13 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
}
// limit data rate to prevent data being lost
if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) {
airspeedSample airspeed_sample_new;
airspeed_sample_new.true_airspeed = true_airspeed;
airspeed_sample_new.eas2tas = eas2tas;
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
if ((airspeed_sample.time_us - _time_last_airspeed) > _min_obs_interval_us) {
_time_last_airspeed = airspeed_sample.time_us;
airspeedSample airspeed_sample_new = airspeed_sample;
airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_airspeed = time_usec;
_airspeed_buffer.push(airspeed_sample_new);
}

2
EKF/estimator_interface.h
View File

@ -183,7 +183,7 @@ public:
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);

7
test/sensor_simulator/airspeed.cpp
View File

@ -17,8 +17,11 @@ void Airspeed::send(uint64_t time)
{
if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON)
{
float eas2tas = _true_airspeed_data / _indicated_airspeed_data;
_ekf->setAirspeedData(time, _true_airspeed_data, eas2tas);
airspeedSample airspeed_sample;
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);
}
}