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

gps_blending: fix blending init to best instance 

- output GPS publication defaults to best input instance
 - blended states are explicitly cleared and then populated with
weighted blend
This commit is contained in:
Daniel Agar 2022-09-30 13:38:34 -04:00
parent f79dad1e63
commit 5f0a539633
2 changed files with 36 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

@ -362,35 +362,34 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
}
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
sensor_gps_s gps_blended_state{_gps_state[0]}; // start with best GPS for all other misc fields
gps_blended_state.eph = FLT_MAX;
gps_blended_state.epv = FLT_MAX;
gps_blended_state.s_variance_m_s = FLT_MAX;
gps_blended_state.vel_ned_valid = true;
gps_blended_state.hdop = FLT_MAX;
gps_blended_state.vdop = FLT_MAX;
sensor_gps_s gps_blended_state{_gps_state[gps_best_index]}; // start with best GPS for all other misc fields
// zerp all fields that are an accumulated blend below
gps_blended_state.timestamp = 0;
gps_blended_state.timestamp_sample = 0;
gps_blended_state.vel_m_s = 0;
gps_blended_state.vel_n_m_s = 0;
gps_blended_state.vel_e_m_s = 0;
gps_blended_state.vel_d_m_s = 0;
// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
// blend the timing data
gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]);
// use the highest status
if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
gps_blended_state.fix_type = _gps_state[i].fix_type;
}
// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
// If any receiver contributing has an invalid velocity, then report blended velocity as invalid
if (blend_weights[i] > 0.0f) {
// blend the timing data
gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]);
// calculate a blended average speed and velocity vector
gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * blend_weights[i];
gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * blend_weights[i];
gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * blend_weights[i];
gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * blend_weights[i];
// use the lowest value
if (_gps_state[i].eph > 0.0f
&& _gps_state[i].eph < gps_blended_state.eph) {
gps_blended_state.eph = _gps_state[i].eph;
@ -416,13 +415,18 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
gps_blended_state.vdop = _gps_state[i].vdop;
}
if (_gps_state[i].satellites_used > 0
&& _gps_state[i].satellites_used > gps_blended_state.satellites_used) {
// use the highest status
if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
gps_blended_state.fix_type = _gps_state[i].fix_type;
}
if (_gps_state[i].satellites_used > gps_blended_state.satellites_used) {
gps_blended_state.satellites_used = _gps_state[i].satellites_used;
}
if (!_gps_state[i].vel_ned_valid) {
gps_blended_state.vel_ned_valid = false;
if (_gps_state[i].vel_ned_valid) {
gps_blended_state.vel_ned_valid = true;
}
}
@ -473,7 +477,7 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
gps_blended_state.alt += (int32_t)blended_alt_offset_mm;
// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
uint8_t gps_best_yaw_index = 0;
int8_t gps_best_yaw_index = -1;
float best_yaw_weight = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
@ -483,8 +487,11 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
}
}
gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading;
gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset;
if (gps_best_yaw_index >= 0) {
gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading;
gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset;
gps_blended_state.heading_accuracy = _gps_state[gps_best_yaw_index].heading_accuracy;
}
return gps_blended_state;
}
@ -497,7 +504,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f);
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if (_gps_state[i].timestamp - _time_prev_us[i] > 0) {
if (_gps_state[i].timestamp > _time_prev_us[i]) {
// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
0.0f, 1.0f);

5
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

@ -211,6 +211,11 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos)
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.getOutputGpsData().timestamp, gps_data0.timestamp);
EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample);
EXPECT_EQ(gps_blending.getOutputGpsData().lat, gps_data0.lat);
EXPECT_EQ(gps_blending.getOutputGpsData().lon, gps_data0.lon);
EXPECT_EQ(gps_blending.getOutputGpsData().alt, gps_data0.alt);
}
TEST_F(GpsBlendingTest, dualReceiverFailover)