fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 16:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_gps_position: untangle and remove unnecessary state

Browse Source
This commit is contained in:
Daniel Agar
2021-01-13 20:17:49 -05:00
parent bc8ec5d1f5
commit a0d8d5ac74
2 changed files with 178 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
+165 -193
View File
@@ -83,6 +83,15 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
_parameter_update_sub.copy(&param_update); _parameter_update_sub.copy(&param_update);
updateParams(); updateParams();
if (_param_sens_gps_mask.get() == 0) {
_sensor_gps_sub[0].registerCallback();
} else {
for (auto &sub : _sensor_gps_sub) {
sub.registerCallback();
}
}
} }
} }
@@ -91,8 +100,20 @@ void VehicleGPSPosition::Run()
perf_begin(_cycle_perf); perf_begin(_cycle_perf);
ParametersUpdate(); ParametersUpdate();
// backup schedule // GPS blending
ScheduleDelayed(500_ms); ScheduleDelayed(500_ms); // backup schedule
// if disabled simply republish the first sensor_gps as vehicle_gps_position and return immediately
if (_param_sens_gps_mask.get() == 0) {
sensor_gps_s gps;
if (_sensor_gps_sub[0].update(&gps)) {
Publish(gps, 0);
}
perf_end(_cycle_perf);
return;
}
// Check all GPS instance // Check all GPS instance
bool any_gps_updated = false; bool any_gps_updated = false;
@@ -111,18 +132,11 @@ void VehicleGPSPosition::Run()
} }
} }
if ((_param_sens_gps_mask.get() == 0) && gps_updated[0]) { if (any_gps_updated) {
// When GPS blending is disabled we always use the first receiver instance
Publish(_gps_state[0]);
} else if ((_param_sens_gps_mask.get() > 0) && any_gps_updated) {
// blend multiple receivers if available // blend multiple receivers if available
// calculate blending weights
if (!blend_gps_data()) { if (!blend_gps_data()) {
// Only use selected receiver data if it has been updated // Only use selected receiver data if it has been updated
_gps_new_output_data = false; uint8_t gps_select_index = 0;
_gps_select_index = 0;
// Find the single "best" GPS from the data we have // Find the single "best" GPS from the data we have
// First, find the GPS(s) with the best fix // First, find the GPS(s) with the best fix
@@ -140,35 +154,20 @@ void VehicleGPSPosition::Run()
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) { if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) {
max_sats = _gps_state[i].satellites_used; max_sats = _gps_state[i].satellites_used;
_gps_select_index = i; gps_select_index = i;
} }
} }
// Check for new data on selected GPS, and clear blend offsets // Check for new data on selected GPS, and clear blend offsets
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (gps_updated[i] && _gps_select_index == i) {
_gps_new_output_data = true;
}
_NE_pos_offset_m[i].zero(); _NE_pos_offset_m[i].zero();
_hgt_offset_mm[i] = 0.0f; _hgt_offset_mm[i] = 0.0f;
} }
}
if (_gps_new_output_data) { // re-publish current best GPS
// correct the _gps_state data for steady state offsets and write to _gps_output if (gps_updated[gps_select_index]) {
apply_gps_offsets(); Publish(_gps_state[gps_select_index], gps_select_index);
// calculate a blended output from the offset corrected receiver data
if (_gps_select_index == GPS_MAX_RECEIVERS) {
calc_gps_blend_output();
} }
// write selected GPS to EKF
Publish(_gps_output[_gps_select_index]);
// clear flag to avoid re-use of the same data
_gps_new_output_data = false;
} }
} }
@@ -177,9 +176,6 @@ void VehicleGPSPosition::Run()
bool VehicleGPSPosition::blend_gps_data() bool VehicleGPSPosition::blend_gps_data()
{ {
// zero the blend weights
memset(&_blend_weights, 0, sizeof(_blend_weights));
/* /*
* If both receivers have the same update rate, use the oldest non-zero time. * If both receivers have the same update rate, use the oldest non-zero time.
* If two receivers with different update rates are used, use the slowest. * If two receivers with different update rates are used, use the slowest.
@@ -241,7 +237,6 @@ bool VehicleGPSPosition::blend_gps_data()
if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) { if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) {
min_us = _gps_state[i].timestamp; min_us = _gps_state[i].timestamp;
_gps_oldest_index = i;
} }
} }
@@ -258,13 +253,14 @@ bool VehicleGPSPosition::blend_gps_data()
* Else we have two receivers at different update rates and use the slowest receiver * Else we have two receivers at different update rates and use the slowest receiver
* as the timing reference. * as the timing reference.
*/ */
bool gps_new_output_data = false;
if ((dt_max - dt_min) < 0.2f * dt_min) { if ((dt_max - dt_min) < 0.2f * dt_min) {
// both receivers assumed to be running at the same rate // both receivers assumed to be running at the same rate
if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) { if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
// data arrival within a short time window enables the two measurements to be blended // data arrival within a short time window enables the two measurements to be blended
_gps_time_ref_index = _gps_newest_index; _gps_time_ref_index = _gps_newest_index;
_gps_new_output_data = true; gps_new_output_data = true;
} }
} else { } else {
@@ -273,13 +269,11 @@ bool VehicleGPSPosition::blend_gps_data()
if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) { if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) {
// blend data at the rate of the slower receiver // blend data at the rate of the slower receiver
_gps_new_output_data = true; gps_new_output_data = true;
} }
} }
if (_gps_new_output_data) { if (gps_new_output_data) {
_gps_blended_state.timestamp = _gps_state[_gps_time_ref_index].timestamp;
// calculate the sum squared speed accuracy across all GPS sensors // calculate the sum squared speed accuracy across all GPS sensors
float speed_accuracy_sum_sq = 0.0f; float speed_accuracy_sum_sq = 0.0f;
@@ -325,7 +319,7 @@ bool VehicleGPSPosition::blend_gps_data()
float sum_of_all_weights = 0.0f; float sum_of_all_weights = 0.0f;
// calculate a weighting using the reported speed accuracy // calculate a weighting using the reported speed accuracy
float spd_blend_weights[GPS_MAX_RECEIVERS] = {}; float spd_blend_weights[GPS_MAX_RECEIVERS] {};
if (speed_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) { if (speed_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) {
// calculate the weights using the inverse of the variances // calculate the weights using the inverse of the variances
@@ -349,7 +343,7 @@ bool VehicleGPSPosition::blend_gps_data()
} }
// calculate a weighting using the reported horizontal position // calculate a weighting using the reported horizontal position
float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; float hpos_blend_weights[GPS_MAX_RECEIVERS] {};
if (horizontal_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) { if (horizontal_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) {
// calculate the weights using the inverse of the variances // calculate the weights using the inverse of the variances
@@ -396,87 +390,99 @@ bool VehicleGPSPosition::blend_gps_data()
}; };
} }
// blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
float blend_weights[GPS_MAX_RECEIVERS];
// calculate an overall weight // calculate an overall weight
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
_blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
} }
// With updated weights we can calculate a blended GPS solution and // With updated weights we can calculate a blended GPS solution and
// offsets for each physical receiver // offsets for each physical receiver
update_gps_blend_states(); sensor_gps_s gps_blended_state = gps_blend_states(blend_weights);
update_gps_offsets();
_gps_select_index = GPS_MAX_RECEIVERS; update_gps_offsets(gps_blended_state);
// calculate a blended output from the offset corrected receiver data
// publish if blending was successful
calc_gps_blend_output(gps_blended_state, blend_weights);
Publish(gps_blended_state, GPS_MAX_RECEIVERS);
} }
return true; return true;
} }
void VehicleGPSPosition::update_gps_blend_states() sensor_gps_s VehicleGPSPosition::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS]) const
{ {
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
_gps_blended_state = sensor_gps_s{}; sensor_gps_s gps_blended_state{};
_gps_blended_state.eph = FLT_MAX; gps_blended_state.eph = FLT_MAX;
_gps_blended_state.epv = FLT_MAX; gps_blended_state.epv = FLT_MAX;
_gps_blended_state.s_variance_m_s = FLT_MAX; gps_blended_state.s_variance_m_s = FLT_MAX;
_gps_blended_state.vel_ned_valid = true; gps_blended_state.vel_ned_valid = true;
//_gps_blended_state.gdop = FLT_MAX; // TODO: add gdop gps_blended_state.hdop = FLT_MAX;
gps_blended_state.vdop = FLT_MAX;
_blended_antenna_offset.zero();
// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() // 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; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
// blend the timing data // blend the timing data
_gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)_blend_weights[i]); gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
// use the highest status // use the highest status
if (_gps_state[i].fix_type > _gps_blended_state.fix_type) { if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
_gps_blended_state.fix_type = _gps_state[i].fix_type; gps_blended_state.fix_type = _gps_state[i].fix_type;
} }
// 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];
// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers // 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 any receiver contributing has an invalid velocity, then report blended velocity as invalid
if (_blend_weights[i] > 0.0f) { if (blend_weights[i] > 0.0f) {
// 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];
if (_gps_state[i].eph > 0.0f if (_gps_state[i].eph > 0.0f
&& _gps_state[i].eph < _gps_blended_state.eph) { && _gps_state[i].eph < gps_blended_state.eph) {
_gps_blended_state.eph = _gps_state[i].eph; gps_blended_state.eph = _gps_state[i].eph;
} }
if (_gps_state[i].epv > 0.0f if (_gps_state[i].epv > 0.0f
&& _gps_state[i].epv < _gps_blended_state.epv) { && _gps_state[i].epv < gps_blended_state.epv) {
_gps_blended_state.epv = _gps_state[i].epv; gps_blended_state.epv = _gps_state[i].epv;
} }
if (_gps_state[i].s_variance_m_s > 0.0f if (_gps_state[i].s_variance_m_s > 0.0f
&& _gps_state[i].s_variance_m_s < _gps_blended_state.s_variance_m_s) { && _gps_state[i].s_variance_m_s < gps_blended_state.s_variance_m_s) {
_gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s; gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s;
} }
// TODO: add gdop if (_gps_state[i].hdop > 0
// if (_gps_state[i].gdop > 0 && _gps_state[i].hdop < gps_blended_state.hdop) {
// && _gps_state[i].gdop < _gps_blended_state.gdop) { gps_blended_state.hdop = _gps_state[i].hdop;
// _gps_blended_state.gdop = _gps_state[i].gdop; }
// }
if (_gps_state[i].vdop > 0
&& _gps_state[i].vdop < gps_blended_state.vdop) {
gps_blended_state.vdop = _gps_state[i].vdop;
}
if (_gps_state[i].satellites_used > 0 if (_gps_state[i].satellites_used > 0
&& _gps_state[i].satellites_used > _gps_blended_state.satellites_used) { && _gps_state[i].satellites_used > gps_blended_state.satellites_used) {
_gps_blended_state.satellites_used = _gps_state[i].satellites_used; gps_blended_state.satellites_used = _gps_state[i].satellites_used;
} }
if (!_gps_state[i].vel_ned_valid) { if (!_gps_state[i].vel_ned_valid) {
_gps_blended_state.vel_ned_valid = false; gps_blended_state.vel_ned_valid = false;
} }
} }
// TODO read parameters for individual GPS antenna positions and blend // TODO read parameters for individual GPS antenna positions and blend
// Vector3f temp_antenna_offset = _antenna_offset[i]; // Vector3f temp_antenna_offset = _antenna_offset[i];
// temp_antenna_offset *= _blend_weights[i]; // temp_antenna_offset *= blend_weights[i];
// _blended_antenna_offset += temp_antenna_offset; // _blended_antenna_offset += temp_antenna_offset;
} }
@@ -488,86 +494,83 @@ void VehicleGPSPosition::update_gps_blend_states()
// Use the GPS with the highest weighting as the reference position // Use the GPS with the highest weighting as the reference position
float best_weight = 0.0f; float best_weight = 0.0f;
// index of the physical receiver with the lowest reported error
uint8_t gps_best_index = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > best_weight) { if (blend_weights[i] > best_weight) {
best_weight = _blend_weights[i]; best_weight = blend_weights[i];
_gps_best_index = i; gps_best_index = i;
_gps_blended_state.lat = _gps_state[i].lat; gps_blended_state.lat = _gps_state[i].lat;
_gps_blended_state.lon = _gps_state[i].lon; gps_blended_state.lon = _gps_state[i].lon;
_gps_blended_state.alt = _gps_state[i].alt; gps_blended_state.alt = _gps_state[i].alt;
} }
} }
// Convert each GPS position to a local NEU offset relative to the reference position // Convert each GPS position to a local NEU offset relative to the reference position
Vector2f blended_NE_offset_m; Vector2f blended_NE_offset_m{0, 0};
blended_NE_offset_m.zero();
float blended_alt_offset_mm = 0.0f; float blended_alt_offset_mm = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) { if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) {
// calculate the horizontal offset // calculate the horizontal offset
Vector2f horiz_offset{}; Vector2f horiz_offset{};
get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
(_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
&horiz_offset(0), &horiz_offset(1)); &horiz_offset(0), &horiz_offset(1));
// sum weighted offsets // sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i]; blended_NE_offset_m += horiz_offset * blend_weights[i];
// calculate vertical offset // calculate vertical offset
float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt); float vert_offset = (float)(_gps_state[i].alt - gps_blended_state.alt);
// sum weighted offsets // sum weighted offsets
blended_alt_offset_mm += vert_offset * _blend_weights[i]; blended_alt_offset_mm += vert_offset * blend_weights[i];
} }
} }
// Add the sum of weighted offsets to the reference position to obtain the blended position // Add the sum of weighted offsets to the reference position to obtain the blended position
double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
double lat_deg_res, lon_deg_res; double lat_deg_res = 0;
add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, double lon_deg_res = 0;
&lon_deg_res); add_vector_to_global_position(lat_deg_now, lon_deg_now,
_gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); blended_NE_offset_m(0), blended_NE_offset_m(1),
_gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); &lat_deg_res, &lon_deg_res);
_gps_blended_state.alt += (int32_t)blended_alt_offset_mm; gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
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 // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
uint8_t gps_best_yaw_index = 0; uint8_t gps_best_yaw_index = 0;
best_weight = 0.0f; float best_yaw_weight = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (PX4_ISFINITE(_gps_state[i].heading) && (_blend_weights[i] > best_weight)) { if (PX4_ISFINITE(_gps_state[i].heading) && (blend_weights[i] > best_yaw_weight)) {
best_weight = _blend_weights[i]; best_yaw_weight = blend_weights[i];
gps_best_yaw_index = i; gps_best_yaw_index = i;
} }
} }
_gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading; 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_offset = _gps_state[gps_best_yaw_index].heading_offset;
return gps_blended_state;
} }
void VehicleGPSPosition::update_gps_offsets() void VehicleGPSPosition::update_gps_offsets(const sensor_gps_s &gps_blended_state)
{ {
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// Increase the filter time constant proportional to the inverse of the weighting
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
float alpha[GPS_MAX_RECEIVERS] = {}; float alpha[GPS_MAX_RECEIVERS] {};
float omega_lpf = 1.0f / fmaxf(_param_sens_gps_tau.get(), 1.0f); float omega_lpf = 1.0f / fmaxf(_param_sens_gps_tau.get(), 1.0f);
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].timestamp - _time_prev_us[i] > 0) { if (_gps_state[i].timestamp - _time_prev_us[i] > 0) {
// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
0.0f, 1.0f); 0.0f, 1.0f);
// scale the filter coefficient so that time constant is inversely proprtional to weighting
if (_blend_weights[i] > min_alpha) {
alpha[i] = min_alpha / _blend_weights[i];
} else {
alpha[i] = 1.0f;
}
_time_prev_us[i] = _gps_state[i].timestamp; _time_prev_us[i] = _gps_state[i].timestamp;
} }
@@ -577,9 +580,12 @@ void VehicleGPSPosition::update_gps_offsets()
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
Vector2f offset; Vector2f offset;
get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
(_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1)); (gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
&offset(0), &offset(1));
_NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
_hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) * alpha[i] +
_hgt_offset_mm[i] = (float)(gps_blended_state.alt - _gps_state[i].alt) * alpha[i] +
_hgt_offset_mm[i] * (1.0f - alpha[i]); _hgt_offset_mm[i] * (1.0f - alpha[i]);
} }
@@ -592,7 +598,8 @@ void VehicleGPSPosition::update_gps_offsets()
if (i != j) { if (i != j) {
Vector2f offset; Vector2f offset;
get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
(_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1)); (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7),
&offset(0), &offset(1));
max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0)));
max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1)));
max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt)));
@@ -608,96 +615,61 @@ void VehicleGPSPosition::update_gps_offsets()
} }
} }
void VehicleGPSPosition::apply_gps_offsets() void VehicleGPSPosition::calc_gps_blend_output(sensor_gps_s &gps_blended_state,
{ float blend_weights[GPS_MAX_RECEIVERS]) const
// calculate offset corrected output for each physical GPS.
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
// Add the sum of weighted offsets to the reference position to obtain the blended position
double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7;
double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7;
double lat_deg_res, lon_deg_res;
add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res,
&lon_deg_res);
_gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res);
_gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res);
_gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i];
// other receiver data is used uncorrected
_gps_output[i].timestamp = _gps_state[i].timestamp;
_gps_output[i].fix_type = _gps_state[i].fix_type;
_gps_output[i].vel_m_s = _gps_state[i].vel_m_s;
_gps_output[i].vel_n_m_s = _gps_state[i].vel_n_m_s;
_gps_output[i].vel_e_m_s = _gps_state[i].vel_e_m_s;
_gps_output[i].vel_d_m_s = _gps_state[i].vel_d_m_s;
_gps_output[i].eph = _gps_state[i].eph;
_gps_output[i].epv = _gps_state[i].epv;
_gps_output[i].s_variance_m_s = _gps_state[i].s_variance_m_s;
//_gps_output[i].gdop = _gps_state[i].gdop; // TODO: add gdop
_gps_output[i].satellites_used = _gps_state[i].satellites_used;
_gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid;
_gps_output[i].heading = _gps_state[i].heading;
_gps_output[i].heading_offset = _gps_state[i].heading_offset;
}
}
void VehicleGPSPosition::calc_gps_blend_output()
{ {
// Convert each GPS position to a local NEU offset relative to the reference position // Convert each GPS position to a local NEU offset relative to the reference position
// which is defined as the positon of the blended solution calculated from non offset corrected data // which is defined as the positon of the blended solution calculated from non offset corrected data
Vector2f blended_NE_offset_m; Vector2f blended_NE_offset_m{0, 0};
blended_NE_offset_m.zero();
float blended_alt_offset_mm = 0.0f; float blended_alt_offset_mm = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) { if (blend_weights[i] > 0.0f) {
// Add the sum of weighted offsets to the reference position to obtain the blended position
const double lat_deg_orig = (double)_gps_state[i].lat * 1.0e-7;
const double lon_deg_orig = (double)_gps_state[i].lon * 1.0e-7;
double lat_deg_offset_res = 0;
double lon_deg_offset_res = 0;
add_vector_to_global_position(lat_deg_orig, lon_deg_orig,
_NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1),
&lat_deg_offset_res, &lon_deg_offset_res);
float alt_offset = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i];
// calculate the horizontal offset // calculate the horizontal offset
Vector2f horiz_offset{}; Vector2f horiz_offset{};
get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
(_gps_blended_state.lon / 1.0e7), lat_deg_offset_res, lon_deg_offset_res,
(_gps_output[i].lat / 1.0e7), &horiz_offset(0), &horiz_offset(1));
(_gps_output[i].lon / 1.0e7),
&horiz_offset(0),
&horiz_offset(1));
// sum weighted offsets // sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i]; blended_NE_offset_m += horiz_offset * blend_weights[i];
// calculate vertical offset // calculate vertical offset
float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt); float vert_offset = alt_offset - gps_blended_state.alt;
// sum weighted offsets // sum weighted offsets
blended_alt_offset_mm += vert_offset * _blend_weights[i]; blended_alt_offset_mm += vert_offset * blend_weights[i];
} }
} }
// Add the sum of weighted offsets to the reference position to obtain the blended position // Add the sum of weighted offsets to the reference position to obtain the blended position
double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
double lat_deg_res, lon_deg_res; double lat_deg_res = 0;
add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, double lon_deg_res = 0;
&lon_deg_res); add_vector_to_global_position(lat_deg_now, lon_deg_now,
_gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res); blended_NE_offset_m(0), blended_NE_offset_m(1),
_gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res); &lat_deg_res, &lon_deg_res);
_gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
// Copy remaining data from internal states to output gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
_gps_output[GPS_BLENDED_INSTANCE].timestamp = _gps_blended_state.timestamp; gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
_gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type; gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
_gps_output[GPS_BLENDED_INSTANCE].vel_m_s = _gps_blended_state.vel_m_s;
_gps_output[GPS_BLENDED_INSTANCE].vel_n_m_s = _gps_blended_state.vel_n_m_s;
_gps_output[GPS_BLENDED_INSTANCE].vel_e_m_s = _gps_blended_state.vel_e_m_s;
_gps_output[GPS_BLENDED_INSTANCE].vel_d_m_s = _gps_blended_state.vel_d_m_s;
_gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph;
_gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv;
_gps_output[GPS_BLENDED_INSTANCE].s_variance_m_s = _gps_blended_state.s_variance_m_s;
//_gps_output[GPS_BLENDED_INSTANCE].gdop = _gps_blended_state.gdop; // TODO: add gdop
_gps_output[GPS_BLENDED_INSTANCE].satellites_used = _gps_blended_state.satellites_used;
_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid;
_gps_output[GPS_BLENDED_INSTANCE].heading = _gps_blended_state.heading;
_gps_output[GPS_BLENDED_INSTANCE].heading_offset = _gps_blended_state.heading_offset;
} }
void VehicleGPSPosition::Publish(const sensor_gps_s &gps) void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
{ {
vehicle_gps_position_s gps_output{}; vehicle_gps_position_s gps_output{};
@@ -727,14 +699,14 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps)
gps_output.vel_ned_valid = gps.vel_ned_valid; gps_output.vel_ned_valid = gps.vel_ned_valid;
gps_output.satellites_used = gps.satellites_used; gps_output.satellites_used = gps.satellites_used;
gps_output.selected = _gps_select_index; gps_output.selected = selected;
_vehicle_gps_position_pub.publish(gps_output); _vehicle_gps_position_pub.publish(gps_output);
} }
void VehicleGPSPosition::PrintStatus() void VehicleGPSPosition::PrintStatus()
{ {
PX4_INFO("selected GPS: %d", _gps_select_index); //PX4_INFO("selected GPS: %d", _gps_select_index);
} }
}; // namespace sensors }; // namespace sensors
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
+13 -29
View File
@@ -64,10 +64,14 @@ public:
void PrintStatus(); void PrintStatus();
private: private:
// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
static constexpr int GPS_MAX_RECEIVERS = 2;
static constexpr int GPS_BLENDED_INSTANCE = GPS_MAX_RECEIVERS;
void Run() override; void Run() override;
void ParametersUpdate(bool force = false); void ParametersUpdate(bool force = false);
void Publish(const sensor_gps_s &gps); void Publish(const sensor_gps_s &gps, uint8_t selected);
/* /*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
@@ -80,36 +84,26 @@ private:
/* /*
* Calculate internal states used to blend GPS data from multiple receivers using weightings calculated * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
* by calc_blend_weights() * by calc_blend_weights()
* States are written to _gps_state and _gps_blended_state class variables
*/ */
void update_gps_blend_states(); sensor_gps_s gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS]) const;
/* /*
* The location in _gps_blended_state will move around as the relative accuracy changes. * The location in gps_blended_state will move around as the relative accuracy changes.
* To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
* calculated. * calculated.
*/ */
void update_gps_offsets(); void update_gps_offsets(const sensor_gps_s &gps_blended_state);
/*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/
void apply_gps_offsets();
/* /*
Calculate GPS output that is a blend of the offset corrected physical receiver data Calculate GPS output that is a blend of the offset corrected physical receiver data
*/ */
void calc_gps_blend_output(); void calc_gps_blend_output(sensor_gps_s &gps_blended_state, float blend_weights[GPS_MAX_RECEIVERS]) const;
// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1; static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1;
static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2; static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2;
static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4; static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4;
// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
static constexpr int GPS_MAX_RECEIVERS = 2;
static constexpr int GPS_BLENDED_INSTANCE = GPS_MAX_RECEIVERS;
// Set the GPS timeout to 2s, after which a receiver will be ignored // Set the GPS timeout to 2s, after which a receiver will be ignored
static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s; static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s;
static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f); static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f);
@@ -125,30 +119,20 @@ private:
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
// GPS blending and switching
sensor_gps_s _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS sensor_gps_s _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
sensor_gps_s _gps_blended_state{}; ///< internal state data for the blended GPS
sensor_gps_s _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
float _hgt_offset_mm[GPS_MAX_RECEIVERS] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) float _hgt_offset_mm[GPS_MAX_RECEIVERS] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
matrix::Vector3f _blended_antenna_offset{}; ///< blended antenna offset
float _blend_weights[GPS_MAX_RECEIVERS] {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
uint64_t _time_prev_us[GPS_MAX_RECEIVERS] {}; ///< the previous value of time_us for that GPS instance - used to detect new data. uint64_t _time_prev_us[GPS_MAX_RECEIVERS] {}; ///< the previous value of time_us for that GPS instance - used to detect new data.
uint8_t _gps_best_index{0}; ///< index of the physical receiver with the lowest reported error
uint8_t _gps_select_index{0}; ///< 0 = GPS1, 1 = GPS2, 2 = blended
uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update
uint8_t _gps_oldest_index{0}; ///< index of the physical receiver with the oldest data
uint8_t _gps_newest_index{0}; ///< index of the physical receiver with the newest data uint8_t _gps_newest_index{0}; ///< index of the physical receiver with the newest data
uint8_t _gps_slowest_index{0}; ///< index of the physical receiver with the slowest update rate uint8_t _gps_slowest_index{0}; ///< index of the physical receiver with the slowest update rate
float _gps_dt[GPS_MAX_RECEIVERS] {}; ///< average time step in seconds. float _gps_dt[GPS_MAX_RECEIVERS] {}; ///< average time step in seconds.
bool _gps_new_output_data{false}; ///< true if there is new output data for the EKF
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
// GPS blending (ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
(ParamInt<px4::params::SENS_GPS_MASK>) (ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
_param_sens_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::SENS_GPS_TAU>)
_param_sens_gps_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
) )
}; };
}; // namespace sensors }; // namespace sensors