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

gps blend: add failover logic and unit test

Browse Source 
also remove dependency to hrt library
This commit is contained in:
bresch
2021-01-30 11:32:47 +01:00
committed by Lorenz Meier
parent 9bff7a1c41
commit b4051b62a3
3 changed files with 176 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
+1 -1
View File
@@ -142,7 +142,7 @@ void VehicleGPSPosition::Run()
}
if (any_gps_updated) {
_gps_blending.update();
_gps_blending.update(hrt_absolute_time());
if (_gps_blending.isNewOutputDataAvailable()) {
Publish(_gps_blending.getOutputGpsData(), _gps_blending.getSelectedGps());
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
+33 -12
View File
@@ -72,10 +72,12 @@ public:
void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; }
void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; }
void setBlendingTimeConstant(float tau) { _blending_time_constant = tau; }
void setPrimaryInstance(int primary) { _primary_instance = primary; }
void update();
void update(uint64_t hrt_now_us);
bool isNewOutputDataAvailable() const { return _is_new_output_data_available; }
int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; }
const sensor_gps_s &getOutputGpsData() const
{
if (_selected_gps < GPS_MAX_RECEIVERS) {
@@ -94,7 +96,7 @@ private:
* have significant position differences, variation in receiver estimated accuracy will cause undesirable
* variation in the position solution.
*/
bool blend_gps_data();
bool blend_gps_data(uint64_t hrt_now_us);
/*
* Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
@@ -118,6 +120,9 @@ private:
sensor_gps_s _gps_blended_state {};
bool _gps_updated[GPS_MAX_RECEIVERS] {};
int _selected_gps{0};
int _np_gps_suitable_for_blending{0};
int _primary_instance{-1};
bool _fallback_allowed{false};
bool _is_new_output_data_available{false};
@@ -138,12 +143,12 @@ private:
};
template <int GPS_MAX_RECEIVERS>
void GpsBlending<GPS_MAX_RECEIVERS>::update()
void GpsBlending<GPS_MAX_RECEIVERS>::update(uint64_t hrt_now_us)
{
_is_new_output_data_available = false;
// blend multiple receivers if available
if (!blend_gps_data()) {
if (!blend_gps_data(hrt_now_us)) {
// Only use selected receiver data if it has been updated
uint8_t gps_select_index = 0;
@@ -173,14 +178,24 @@ void GpsBlending<GPS_MAX_RECEIVERS>::update()
_hgt_offset_mm[i] = 0.0f;
}
// re-publish current best GPS
// Only use a secondary instance if the fallback is allowed
if ((_primary_instance > -1)
&& (gps_select_index != _primary_instance)
&& !_fallback_allowed) {
gps_select_index = _primary_instance;
}
_selected_gps = gps_select_index;
_is_new_output_data_available = _gps_updated[gps_select_index];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
_gps_updated[gps_select_index] = false;
}
}
}
template <int GPS_MAX_RECEIVERS>
bool GpsBlending<GPS_MAX_RECEIVERS>::blend_gps_data()
bool GpsBlending<GPS_MAX_RECEIVERS>::blend_gps_data(uint64_t hrt_now_us)
{
/*
* If both receivers have the same update rate, use the oldest non-zero time.
@@ -193,23 +208,29 @@ bool GpsBlending<GPS_MAX_RECEIVERS>::blend_gps_data()
// Find the largest and smallest time step.
float dt_max = 0.0f;
float dt_min = GPS_TIMEOUT_S;
uint8_t gps_count = 0; // Count of receivers which have an active >=2D fix
const hrt_abstime hrt_now = hrt_absolute_time();
_np_gps_suitable_for_blending = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
const float raw_dt = 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]);
const float present_dt = 1e-6f * (float)(hrt_now - _gps_state[i].timestamp);
const float present_dt = 1e-6f * (float)(hrt_now_us - _gps_state[i].timestamp);
if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
} else if (present_dt >= GPS_TIMEOUT_S) {
} else if ((present_dt >= GPS_TIMEOUT_S)
&& (_gps_state[i].timestamp > 0)) {
// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
_gps_state[i].timestamp = 0;
_gps_state[i].fix_type = 0;
_gps_state[i].satellites_used = 0;
_gps_state[i].vel_ned_valid = 0;
if (i == _primary_instance) {
// Allow using a secondary instance when the primary
// receiver has timed out
_fallback_allowed = true;
}
continue;
}
@@ -227,7 +248,7 @@ bool GpsBlending<GPS_MAX_RECEIVERS>::blend_gps_data()
dt_min = _gps_dt[i];
}
gps_count++;
_np_gps_suitable_for_blending++;
}
// Find the receiver that is last be updated
@@ -246,7 +267,7 @@ bool GpsBlending<GPS_MAX_RECEIVERS>::blend_gps_data()
}
}
if (gps_count < 2) {
if (_np_gps_suitable_for_blending < 2) {
// Less than 2 receivers left, so fall out of blending
return false;
}
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
+142 -36
View File
@@ -49,14 +49,16 @@ class GpsBlendingTest : public ::testing::Test
{
public:
sensor_gps_s getDefaultGpsData();
void runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data, int instance);
void runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data0, sensor_gps_s &gps_data1);
GpsBlending<2> _gps_blending;
uint64_t _time_now_us{1000000};
};
sensor_gps_s GpsBlendingTest::getDefaultGpsData()
{
sensor_gps_s gps_data{};
gps_data.timestamp = hrt_absolute_time() - 10e3; // microseconds
gps_data.timestamp = _time_now_us - 10e3;
gps_data.time_utc_usec = 0;
gps_data.lat = 47e7;
gps_data.lon = 9e7;
@@ -85,76 +87,180 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData()
return gps_data;
}
void GpsBlendingTest::runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data, int instance)
{
const float dt = 0.1;
const uint64_t dt_us = static_cast<uint64_t>(dt * 1e6f);
for (int k = 0; k < static_cast<int>(duration_s / dt); k++) {
gps_blending.setGpsData(gps_data, instance);
gps_blending.update(_time_now_us);
_time_now_us += dt_us;
gps_data.timestamp += dt_us;
}
}
void GpsBlendingTest::runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data0,
sensor_gps_s &gps_data1)
{
const float dt = 0.1;
const uint64_t dt_us = static_cast<uint64_t>(dt * 1e6f);
for (int k = 0; k < static_cast<int>(duration_s / dt); k++) {
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
_time_now_us += dt_us;
gps_data0.timestamp += dt_us;
gps_data1.timestamp += dt_us;
}
}
TEST_F(GpsBlendingTest, noData)
{
EXPECT_EQ(_gps_blending.getSelectedGps(), 0);
EXPECT_FALSE(_gps_blending.isNewOutputDataAvailable());
GpsBlending<2> gps_blending;
_gps_blending.update();
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
EXPECT_EQ(_gps_blending.getSelectedGps(), 0);
EXPECT_FALSE(_gps_blending.isNewOutputDataAvailable());
gps_blending.update(_time_now_us);
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
}
TEST_F(GpsBlendingTest, singleReceiver)
{
GpsBlending<2> gps_blending;
sensor_gps_s gps_data = getDefaultGpsData();
_gps_blending.setGpsData(gps_data, 1);
_gps_blending.update();
gps_blending.setGpsData(gps_data, 1);
gps_blending.update(_time_now_us);
gps_data.timestamp = hrt_absolute_time() - 10e3;
_gps_blending.setGpsData(gps_data, 1);
_gps_blending.update();
_time_now_us += 200e3;
gps_data.timestamp = _time_now_us - 10e3;
gps_blending.setGpsData(gps_data, 1);
gps_blending.update(_time_now_us);
EXPECT_EQ(_gps_blending.getSelectedGps(), 1);
EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 1);
EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable());
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
// BUT IF: a second update is called without data
gps_blending.update(_time_now_us);
// THEN: no new data should be available
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1);
EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
}
TEST_F(GpsBlendingTest, dualReceiverNoBlending)
{
GpsBlending<2> gps_blending;
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
gps_data1.satellites_used = gps_data0.satellites_used + 2; // gps1 has more satellites than gps0
_gps_blending.setGpsData(gps_data0, 0);
_gps_blending.setGpsData(gps_data1, 1);
_gps_blending.update();
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
// THEN: gps1 should be selected because it has more satellites
EXPECT_EQ(_gps_blending.getSelectedGps(), 1);
EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable());
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
gps_data1.satellites_used = gps_data0.satellites_used - 2; // gps1 has less satellites than gps0
_gps_blending.setGpsData(gps_data0, 0);
_gps_blending.setGpsData(gps_data1, 1);
_gps_blending.update();
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
// THEN: gps0 should be selected because it has more satellites
EXPECT_EQ(_gps_blending.getSelectedGps(), 0);
EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable());
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
}
TEST_F(GpsBlendingTest, dualReceiverBlendingHPos)
{
GpsBlending<2> gps_blending;
sensor_gps_s gps_data0 = getDefaultGpsData();
sensor_gps_s gps_data1 = getDefaultGpsData();
_gps_blending.setBlendingUseHPosAccuracy(true);
gps_blending.setBlendingUseHPosAccuracy(true);
gps_data1.eph = gps_data0.eph / 2.f;
_gps_blending.setGpsData(gps_data0, 0);
_gps_blending.setGpsData(gps_data1, 1);
_gps_blending.update();
gps_blending.setGpsData(gps_data0, 0);
gps_blending.setGpsData(gps_data1, 1);
gps_blending.update(_time_now_us);
// THEN: the blended instance should be selected (2)
// and the eph should be adjusted
EXPECT_EQ(_gps_blending.getSelectedGps(), 2);
EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable());
EXPECT_LT(_gps_blending.getOutputGpsData().eph, gps_data0.eph);
EXPECT_FLOAT_EQ(_gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than
EXPECT_EQ(gps_blending.getSelectedGps(), 2);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph);
EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than
}
TEST_F(GpsBlendingTest, dualReceiverFailover)
{
GpsBlending<2> gps_blending;
sensor_gps_s gps_data1 = getDefaultGpsData();
gps_blending.setPrimaryInstance(0);
gps_blending.setBlendingUseSpeedAccuracy(0);
gps_blending.setBlendingUseHPosAccuracy(0);
gps_blending.setBlendingUseVPosAccuracy(0);
const float duration_s = 10.f;
runSeconds(duration_s, gps_blending, gps_data1, 1);
// THEN: the primary instance should be selected even if
// not available. No data is then available
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1);
EXPECT_FALSE(gps_blending.isNewOutputDataAvailable());
// BUT WHEN: the data of the primary receiver is avaialbe
sensor_gps_s gps_data0 = getDefaultGpsData();
gps_blending.setGpsData(gps_data0, 0);
gps_blending.update(_time_now_us);
// THEN: the primary instance is selected and the data
// is available
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
runSeconds(duration_s, gps_blending, gps_data0, gps_data1);
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
// BUT WHEN: the primary receiver isn't available anymore
runSeconds(duration_s, gps_blending, gps_data1, 1);
// THEN: the data of the secondary receiver can be used
EXPECT_EQ(gps_blending.getSelectedGps(), 1);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
// AND IF: the primary receiver is available again and has
// better metrics than the secondary one
gps_data0.timestamp = gps_data1.timestamp;
gps_data0.satellites_used = gps_data1.satellites_used + 2;
runSeconds(1.f, gps_blending, gps_data0, gps_data1);
// THEN: the primary receiver should be used again
EXPECT_EQ(gps_blending.getSelectedGps(), 0);
EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
}