diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 35e88c9964..0ecdc0a4e4 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -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); diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index 8d46073cb1..1baaf1da42 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -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)