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

Update range finder interface

Browse Source
This commit is contained in:
kamilritz
2020-01-23 09:34:07 +11:00
committed by Paul Riseborough
parent 9f0d8ed59e
commit 4fb2d1b02e
4 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+6 -6
View File
@@ -320,7 +320,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
} }
} }
void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t quality) void EstimatorInterface::setRangeData(const rangeSample& range_sample)
{ {
if (!_initialised || _range_buffer_fail) { if (!_initialised || _range_buffer_fail) {
return; return;
@@ -338,12 +338,12 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
} }
// limit data rate to prevent data being lost // limit data rate to prevent data being lost
if ((time_usec - _time_last_range) > _min_obs_interval_us) { if ((range_sample.time_us - _time_last_range) > _min_obs_interval_us) {
_time_last_range = range_sample.time_us;
rangeSample range_sample_new; rangeSample range_sample_new;
range_sample_new.rng = data; range_sample_new.time_us -= _params.range_delay_ms * 1000;
range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000; range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
range_sample_new.quality = quality;
_time_last_range = time_usec;
_range_buffer.push(range_sample_new); _range_buffer.push(range_sample_new);
} }
EKF/estimator_interface.h
+1 -1
View File
@@ -185,7 +185,7 @@ public:
void setAirspeedData(const airspeedSample &airspeed_sample); void setAirspeedData(const airspeedSample &airspeed_sample);
void setRangeData(uint64_t time_usec, float data, int8_t quality); void setRangeData(const rangeSample& range_sample);
// if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead // if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
void setOpticalFlowData(uint64_t time_usec, flow_message *flow); void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
test/sensor_simulator/range_finder.cpp
+4 -3
View File
@@ -15,13 +15,14 @@ RangeFinder::~RangeFinder()
void RangeFinder::send(uint64_t time) void RangeFinder::send(uint64_t time)
{ {
_ekf->setRangeData(time, _range_data, _range_quality); _range_sample.time_us = time;
_ekf->setRangeData(_range_sample);
} }
void RangeFinder::setData(float range_data_meters, int8_t range_quality) void RangeFinder::setData(float range_data_meters, int8_t range_quality)
{ {
_range_data = range_data_meters; _range_sample.rng = range_data_meters;
_range_quality = range_quality; _range_sample.quality = range_quality;
} }
} // namespace sensor } // namespace sensor
test/sensor_simulator/range_finder.h
+1 -2
View File
@@ -54,8 +54,7 @@ public:
flow_message dataAtRest(); flow_message dataAtRest();
private: private:
float _range_data{0.0f}; rangeSample _range_sample {};
int8_t _range_quality;
void send(uint64_t time) override; void send(uint64_t time) override;