forked from Archive/PX4-Autopilot
sensors/vehicle_gps_position: untangle and remove unnecessary state
This commit is contained in:
Daniel Agar
parent
bc8ec5d1f5
commit
a0d8d5ac74
|
|
@ -83,6 +83,15 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
|
|||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
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);
|
||||
ParametersUpdate();
|
||||
|
||||
// backup schedule
|
||||
ScheduleDelayed(500_ms);
|
||||
// GPS blending
|
||||
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
|
||||
bool any_gps_updated = false;
|
||||
|
|
@ -111,18 +132,11 @@ void VehicleGPSPosition::Run()
|
|||
}
|
||||
}
|
||||
|
||||
if ((_param_sens_gps_mask.get() == 0) && gps_updated[0]) {
|
||||
// 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) {
|
||||
if (any_gps_updated) {
|
||||
// blend multiple receivers if available
|
||||
|
||||
// calculate blending weights
|
||||
if (!blend_gps_data()) {
|
||||
// Only use selected receiver data if it has been updated
|
||||
_gps_new_output_data = false;
|
||||
_gps_select_index = 0;
|
||||
uint8_t gps_select_index = 0;
|
||||
|
||||
// Find the single "best" GPS from the data we have
|
||||
// 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++) {
|
||||
if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) {
|
||||
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
|
||||
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();
|
||||
_hgt_offset_mm[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
if (_gps_new_output_data) {
|
||||
// correct the _gps_state data for steady state offsets and write to _gps_output
|
||||
apply_gps_offsets();
|
||||
|
||||
// calculate a blended output from the offset corrected receiver data
|
||||
if (_gps_select_index == GPS_MAX_RECEIVERS) {
|
||||
calc_gps_blend_output();
|
||||
// re-publish current best GPS
|
||||
if (gps_updated[gps_select_index]) {
|
||||
Publish(_gps_state[gps_select_index], gps_select_index);
|
||||
}
|
||||
|
||||
// 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()
|
||||
{
|
||||
// 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 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)) {
|
||||
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
|
||||
* as the timing reference.
|
||||
*/
|
||||
bool gps_new_output_data = false;
|
||||
|
||||
if ((dt_max - dt_min) < 0.2f * dt_min) {
|
||||
// both receivers assumed to be running at the same rate
|
||||
if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
|
||||
// data arrival within a short time window enables the two measurements to be blended
|
||||
_gps_time_ref_index = _gps_newest_index;
|
||||
_gps_new_output_data = true;
|
||||
gps_new_output_data = true;
|
||||
}
|
||||
|
||||
} 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]) {
|
||||
// blend data at the rate of the slower receiver
|
||||
_gps_new_output_data = true;
|
||||
gps_new_output_data = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_gps_new_output_data) {
|
||||
_gps_blended_state.timestamp = _gps_state[_gps_time_ref_index].timestamp;
|
||||
|
||||
if (gps_new_output_data) {
|
||||
// calculate the sum squared speed accuracy across all GPS sensors
|
||||
float speed_accuracy_sum_sq = 0.0f;
|
||||
|
||||
|
|
@ -325,7 +319,7 @@ bool VehicleGPSPosition::blend_gps_data()
|
|||
float sum_of_all_weights = 0.0f;
|
||||
|
||||
// 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)) {
|
||||
// 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
|
||||
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)) {
|
||||
// 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
|
||||
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
|
||||
// offsets for each physical receiver
|
||||
update_gps_blend_states();
|
||||
update_gps_offsets();
|
||||
_gps_select_index = GPS_MAX_RECEIVERS;
|
||||
sensor_gps_s gps_blended_state = gps_blend_states(blend_weights);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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.
|
||||
_gps_blended_state = sensor_gps_s{};
|
||||
_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.gdop = FLT_MAX; // TODO: add gdop
|
||||
|
||||
_blended_antenna_offset.zero();
|
||||
sensor_gps_s gps_blended_state{};
|
||||
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;
|
||||
|
||||
// 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++) {
|
||||
// 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
|
||||
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].fix_type > gps_blended_state.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
|
||||
// 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
|
||||
&& _gps_state[i].eph < _gps_blended_state.eph) {
|
||||
_gps_blended_state.eph = _gps_state[i].eph;
|
||||
&& _gps_state[i].eph < gps_blended_state.eph) {
|
||||
gps_blended_state.eph = _gps_state[i].eph;
|
||||
}
|
||||
|
||||
if (_gps_state[i].epv > 0.0f
|
||||
&& _gps_state[i].epv < _gps_blended_state.epv) {
|
||||
_gps_blended_state.epv = _gps_state[i].epv;
|
||||
&& _gps_state[i].epv < gps_blended_state.epv) {
|
||||
gps_blended_state.epv = _gps_state[i].epv;
|
||||
}
|
||||
|
||||
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_blended_state.s_variance_m_s = _gps_state[i].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;
|
||||
}
|
||||
|
||||
// TODO: add gdop
|
||||
// if (_gps_state[i].gdop > 0
|
||||
// && _gps_state[i].gdop < _gps_blended_state.gdop) {
|
||||
// _gps_blended_state.gdop = _gps_state[i].gdop;
|
||||
// }
|
||||
if (_gps_state[i].hdop > 0
|
||||
&& _gps_state[i].hdop < gps_blended_state.hdop) {
|
||||
gps_blended_state.hdop = _gps_state[i].hdop;
|
||||
}
|
||||
|
||||
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
|
||||
&& _gps_state[i].satellites_used > _gps_blended_state.satellites_used) {
|
||||
_gps_blended_state.satellites_used = _gps_state[i].satellites_used;
|
||||
&& _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;
|
||||
gps_blended_state.vel_ned_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO read parameters for individual GPS antenna positions and blend
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -488,86 +494,83 @@ void VehicleGPSPosition::update_gps_blend_states()
|
|||
// Use the GPS with the highest weighting as the reference position
|
||||
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++) {
|
||||
if (_blend_weights[i] > best_weight) {
|
||||
best_weight = _blend_weights[i];
|
||||
_gps_best_index = i;
|
||||
_gps_blended_state.lat = _gps_state[i].lat;
|
||||
_gps_blended_state.lon = _gps_state[i].lon;
|
||||
_gps_blended_state.alt = _gps_state[i].alt;
|
||||
if (blend_weights[i] > best_weight) {
|
||||
best_weight = blend_weights[i];
|
||||
gps_best_index = i;
|
||||
gps_blended_state.lat = _gps_state[i].lat;
|
||||
gps_blended_state.lon = _gps_state[i].lon;
|
||||
gps_blended_state.alt = _gps_state[i].alt;
|
||||
}
|
||||
}
|
||||
|
||||
// Convert each GPS position to a local NEU offset relative to the reference position
|
||||
Vector2f blended_NE_offset_m;
|
||||
blended_NE_offset_m.zero();
|
||||
Vector2f blended_NE_offset_m{0, 0};
|
||||
float blended_alt_offset_mm = 0.0f;
|
||||
|
||||
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
|
||||
Vector2f horiz_offset{};
|
||||
get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
|
||||
(_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
|
||||
get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
|
||||
(_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
|
||||
&horiz_offset(0), &horiz_offset(1));
|
||||
|
||||
// sum weighted offsets
|
||||
blended_NE_offset_m += horiz_offset * _blend_weights[i];
|
||||
blended_NE_offset_m += horiz_offset * blend_weights[i];
|
||||
|
||||
// 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
|
||||
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
|
||||
double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
|
||||
double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
|
||||
double lat_deg_res, lon_deg_res;
|
||||
add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
|
||||
&lon_deg_res);
|
||||
_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;
|
||||
const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
|
||||
const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
|
||||
double lat_deg_res = 0;
|
||||
double lon_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, &lon_deg_res);
|
||||
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
|
||||
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++) {
|
||||
if (PX4_ISFINITE(_gps_state[i].heading) && (_blend_weights[i] > best_weight)) {
|
||||
best_weight = _blend_weights[i];
|
||||
if (PX4_ISFINITE(_gps_state[i].heading) && (blend_weights[i] > best_yaw_weight)) {
|
||||
best_yaw_weight = blend_weights[i];
|
||||
gps_best_yaw_index = i;
|
||||
}
|
||||
}
|
||||
|
||||
_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 = _gps_state[gps_best_yaw_index].heading;
|
||||
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
|
||||
// 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
|
||||
float alpha[GPS_MAX_RECEIVERS] = {};
|
||||
float alpha[GPS_MAX_RECEIVERS] {};
|
||||
float omega_lpf = 1.0f / fmaxf(_param_sens_gps_tau.get(), 1.0f);
|
||||
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
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
|
||||
float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
|
||||
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;
|
||||
}
|
||||
alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
|
||||
0.0f, 1.0f);
|
||||
|
||||
_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++) {
|
||||
Vector2f offset;
|
||||
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]);
|
||||
_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]);
|
||||
}
|
||||
|
||||
|
|
@ -592,7 +598,8 @@ void VehicleGPSPosition::update_gps_offsets()
|
|||
if (i != j) {
|
||||
Vector2f offset;
|
||||
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(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)));
|
||||
|
|
@ -608,96 +615,61 @@ void VehicleGPSPosition::update_gps_offsets()
|
|||
}
|
||||
}
|
||||
|
||||
void VehicleGPSPosition::apply_gps_offsets()
|
||||
{
|
||||
// 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()
|
||||
void VehicleGPSPosition::calc_gps_blend_output(sensor_gps_s &gps_blended_state,
|
||||
float blend_weights[GPS_MAX_RECEIVERS]) const
|
||||
{
|
||||
// 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
|
||||
Vector2f blended_NE_offset_m;
|
||||
blended_NE_offset_m.zero();
|
||||
Vector2f blended_NE_offset_m{0, 0};
|
||||
float blended_alt_offset_mm = 0.0f;
|
||||
|
||||
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
|
||||
Vector2f horiz_offset{};
|
||||
get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
|
||||
(_gps_blended_state.lon / 1.0e7),
|
||||
(_gps_output[i].lat / 1.0e7),
|
||||
(_gps_output[i].lon / 1.0e7),
|
||||
&horiz_offset(0),
|
||||
&horiz_offset(1));
|
||||
get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
|
||||
lat_deg_offset_res, lon_deg_offset_res,
|
||||
&horiz_offset(0), &horiz_offset(1));
|
||||
|
||||
// sum weighted offsets
|
||||
blended_NE_offset_m += horiz_offset * _blend_weights[i];
|
||||
blended_NE_offset_m += horiz_offset * blend_weights[i];
|
||||
|
||||
// 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
|
||||
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
|
||||
double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
|
||||
double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
|
||||
double lat_deg_res, lon_deg_res;
|
||||
add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
|
||||
&lon_deg_res);
|
||||
_gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res);
|
||||
_gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res);
|
||||
_gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
|
||||
const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
|
||||
const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
|
||||
double lat_deg_res = 0;
|
||||
double lon_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, &lon_deg_res);
|
||||
|
||||
// Copy remaining data from internal states to output
|
||||
_gps_output[GPS_BLENDED_INSTANCE].timestamp = _gps_blended_state.timestamp;
|
||||
_gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type;
|
||||
_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;
|
||||
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 = gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
|
||||
}
|
||||
|
||||
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{};
|
||||
|
||||
|
|
@ -727,14 +699,14 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps)
|
|||
gps_output.vel_ned_valid = gps.vel_ned_valid;
|
||||
gps_output.satellites_used = gps.satellites_used;
|
||||
|
||||
gps_output.selected = _gps_select_index;
|
||||
gps_output.selected = selected;
|
||||
|
||||
_vehicle_gps_position_pub.publish(gps_output);
|
||||
}
|
||||
|
||||
void VehicleGPSPosition::PrintStatus()
|
||||
{
|
||||
PX4_INFO("selected GPS: %d", _gps_select_index);
|
||||
//PX4_INFO("selected GPS: %d", _gps_select_index);
|
||||
}
|
||||
|
||||
}; // namespace sensors
|
||||
|
|
|
|||
|
|
@ -64,10 +64,14 @@ public:
|
|||
void PrintStatus();
|
||||
|
||||
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 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
|
||||
|
|
@ -80,36 +84,26 @@ private:
|
|||
/*
|
||||
* Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
|
||||
* 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
|
||||
* calculated.
|
||||
*/
|
||||
void update_gps_offsets();
|
||||
|
||||
/*
|
||||
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
|
||||
*/
|
||||
void apply_gps_offsets();
|
||||
void update_gps_offsets(const sensor_gps_s &gps_blended_state);
|
||||
|
||||
/*
|
||||
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
|
||||
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_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
|
||||
static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s;
|
||||
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")};
|
||||
|
||||
// GPS blending and switching
|
||||
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)
|
||||
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.
|
||||
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_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_slowest_index{0}; ///< index of the physical receiver with the slowest update rate
|
||||
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(
|
||||
// GPS blending
|
||||
(ParamInt<px4::params::SENS_GPS_MASK>)
|
||||
_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)
|
||||
(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
|
||||
(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
|
||||
)
|
||||
};
|
||||
}; // namespace sensors
|
||||
|
|
|
|||
Loading…
Reference in New Issue