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

Move GPS blending from ekf2 to sensors module

Browse Source 
- new sensors work item that subscribes to N x sensor_gps and publishes vehicle_gps_position
 - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

Co-authored-by: Jacob Crabill <jacob@volans-i.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl
2020-09-25 23:28:31 -04:00
committed by GitHub
co-authored by Jacob Crabill Jacob Dahl
parent e792c46f20
commit a24488328f
27 changed files with 1147 additions and 861 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+13 -633
View File
@@ -154,7 +154,6 @@ EKF2::EKF2(bool replay_mode):
// advertise immediately to ensure consistent uORB instance numbering
_att_pub.advertise();
_blended_gps_pub.advertise();
_ekf2_timestamps_pub.advertise();
_ekf_gps_drift_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
@@ -458,87 +457,18 @@ void EKF2::Run()
}
}
// read gps1 data if available
bool gps1_updated = _gps_subs[0].updated();
if (gps1_updated) {
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
if (_gps_subs[0].copy(&gps)) {
fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps);
_gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid;
}
}
if (_vehicle_gps_position_sub.copy(&gps)) {
gps_message gps_msg{};
// check for second GPS receiver data
bool gps2_updated = _gps_subs[1].updated();
fillGpsMsgWithVehicleGpsPosData(gps_msg, gps);
if (gps2_updated) {
vehicle_gps_position_s gps;
_ekf.setGpsData(gps_msg);
if (_gps_subs[1].copy(&gps)) {
fillGpsMsgWithVehicleGpsPosData(_gps_state[1], gps);
_gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid;
}
}
if ((_param_ekf2_gps_mask.get() == 0) && gps1_updated) {
// When GPS blending is disabled we always use the first receiver instance
_ekf.setGpsData(_gps_state[0]);
} else if ((_param_ekf2_gps_mask.get() > 0) && (gps1_updated || gps2_updated)) {
// blend dual receivers if available
// calculate blending weights
if (!blend_gps_data()) {
// handle case where the blended states cannot be updated
// Only use selected receiver data if it has been updated
_gps_new_output_data = (gps1_updated && _gps_select_index == 0) ||
(gps2_updated && _gps_select_index == 1);
// Reset relative position offsets to zero
_NE_pos_offset_m[0].zero();
_NE_pos_offset_m[1].zero();
_hgt_offset_mm[0] = _hgt_offset_mm[1] = 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 == 2) {
calc_gps_blend_output();
}
// write selected GPS to EKF
_ekf.setGpsData(_gps_output[_gps_select_index]);
// log blended solution as a third GPS instance
ekf_gps_position_s gps;
gps.timestamp = _gps_output[_gps_select_index].time_usec;
gps.lat = _gps_output[_gps_select_index].lat;
gps.lon = _gps_output[_gps_select_index].lon;
gps.alt = _gps_output[_gps_select_index].alt;
gps.fix_type = _gps_output[_gps_select_index].fix_type;
gps.eph = _gps_output[_gps_select_index].eph;
gps.epv = _gps_output[_gps_select_index].epv;
gps.s_variance_m_s = _gps_output[_gps_select_index].sacc;
gps.vel_m_s = _gps_output[_gps_select_index].vel_m_s;
gps.vel_n_m_s = _gps_output[_gps_select_index].vel_ned(0);
gps.vel_e_m_s = _gps_output[_gps_select_index].vel_ned(1);
gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned(2);
gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid;
gps.satellites_used = _gps_output[_gps_select_index].nsats;
gps.heading = _gps_output[_gps_select_index].yaw;
gps.heading_offset = _gps_output[_gps_select_index].yaw_offset;
gps.selected = _gps_select_index;
// Publish to the EKF blended GPS topic
_blended_gps_pub.publish(gps);
// clear flag to avoid re-use of the same data
_gps_new_output_data = false;
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = gps.alt_ellipsoid;
}
}
@@ -1400,570 +1330,20 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
}
}
bool EKF2::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.
* If time difference is excessive, use newest to prevent a disconnected receiver
* from blocking updates.
*/
// Calculate the time step for each receiver with some filtering to reduce the effects of jitter
// Find the largest and smallest time step.
float dt_max = 0.0f;
float dt_min = 0.3f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
float raw_dt = 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]);
if (raw_dt > 0.0f && raw_dt < 0.3f) {
_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
}
if (_gps_dt[i] > dt_max) {
dt_max = _gps_dt[i];
_gps_slowest_index = i;
}
if (_gps_dt[i] < dt_min) {
dt_min = _gps_dt[i];
}
}
// Find the receiver that is last be updated
uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
// Find largest and smallest times
if (_gps_state[i].time_usec > max_us) {
max_us = _gps_state[i].time_usec;
_gps_newest_index = i;
}
if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) {
min_us = _gps_state[i].time_usec;
_gps_oldest_index = i;
}
}
if ((max_us - min_us) > 300000) {
// A receiver has timed out so fall out of blending
if (_gps_state[0].time_usec > _gps_state[1].time_usec) {
_gps_select_index = 0;
} else {
_gps_select_index = 1;
}
return false;
}
// One receiver has lost 3D fix, fall out of blending
if (_gps_state[0].fix_type > 2 && _gps_state[1].fix_type < 3) {
_gps_select_index = 0;
return false;
} else if (_gps_state[1].fix_type > 2 && _gps_state[0].fix_type < 3) {
_gps_select_index = 1;
return false;
}
/*
* If the largest dt is less than 20% greater than the smallest, then we have receivers
* running at the same rate then we wait until we have two messages with an arrival time
* difference that is less than 50% of the smallest time step and use the time stamp from
* the newest data.
* Else we have two receivers at different update rates and use the slowest receiver
* as the timing reference.
*/
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;
}
} else {
// both receivers running at different rates
_gps_time_ref_index = _gps_slowest_index;
if (_gps_state[_gps_time_ref_index].time_usec > _time_prev_us[_gps_time_ref_index]) {
// blend data at the rate of the slower receiver
_gps_new_output_data = true;
}
}
if (_gps_new_output_data) {
_gps_blended_state.time_usec = _gps_state[_gps_time_ref_index].time_usec;
// calculate the sum squared speed accuracy across all GPS sensors
float speed_accuracy_sum_sq = 0.0f;
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc > 0.0f) {
speed_accuracy_sum_sq += _gps_state[i].sacc * _gps_state[i].sacc;
} else {
// not all receivers support this metric so set it to zero and don't use it
speed_accuracy_sum_sq = 0.0f;
break;
}
}
}
// calculate the sum squared horizontal position accuracy across all GPS sensors
float horizontal_accuracy_sum_sq = 0.0f;
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
} else {
// not all receivers support this metric so set it to zero and don't use it
horizontal_accuracy_sum_sq = 0.0f;
break;
}
}
}
// calculate the sum squared vertical position accuracy across all GPS sensors
float vertical_accuracy_sum_sq = 0.0f;
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
} else {
// not all receivers support this metric so set it to zero and don't use it
vertical_accuracy_sum_sq = 0.0f;
break;
}
}
}
// Check if we can do blending using reported accuracy
bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f
|| speed_accuracy_sum_sq > 0.0f);
// if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
if (!can_do_blending) {
return false;
}
float sum_of_all_weights = 0.0f;
// calculate a weighting using the reported speed accuracy
float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
if (speed_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_spd_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc >= 0.001f) {
spd_blend_weights[i] = 1.0f / (_gps_state[i].sacc * _gps_state[i].sacc);
sum_of_spd_weights += spd_blend_weights[i];
}
}
// normalise the weights
if (sum_of_spd_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
}
sum_of_all_weights += 1.0f;
}
}
// calculate a weighting using the reported horizontal position
float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
if (horizontal_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_hpos_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) {
hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph);
sum_of_hpos_weights += hpos_blend_weights[i];
}
}
// normalise the weights
if (sum_of_hpos_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
}
sum_of_all_weights += 1.0f;
}
}
// calculate a weighting using the reported vertical position accuracy
float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
if (vertical_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_vpos_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) {
vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv);
sum_of_vpos_weights += vpos_blend_weights[i];
}
}
// normalise the weights
if (sum_of_vpos_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
}
sum_of_all_weights += 1.0f;
};
}
// 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;
}
// 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 = 2;
}
return true;
}
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions
* with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators
* because if physical receivers have significant position differences, variation in receiver estimated accuracy will
* cause undesirable variation in the position solution.
*/
void EKF2::update_gps_blend_states()
{
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
_gps_blended_state.time_usec = 0;
_gps_blended_state.lat = 0;
_gps_blended_state.lon = 0;
_gps_blended_state.alt = 0;
_gps_blended_state.fix_type = 0;
_gps_blended_state.eph = FLT_MAX;
_gps_blended_state.epv = FLT_MAX;
_gps_blended_state.sacc = FLT_MAX;
_gps_blended_state.vel_m_s = 0.0f;
_gps_blended_state.vel_ned.setZero();
_gps_blended_state.vel_ned_valid = true;
_gps_blended_state.nsats = 0;
_gps_blended_state.pdop = FLT_MAX;
_blended_antenna_offset.zero();
// 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.time_usec += (uint64_t)((double)_gps_state[i].time_usec * (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;
}
// 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_ned += _gps_state[i].vel_ned * _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 (_gps_state[i].eph > 0.0f
&& _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;
}
if (_gps_state[i].sacc > 0.0f
&& _gps_state[i].sacc < _gps_blended_state.sacc) {
_gps_blended_state.sacc = _gps_state[i].sacc;
}
if (_gps_state[i].pdop > 0
&& _gps_state[i].pdop < _gps_blended_state.pdop) {
_gps_blended_state.pdop = _gps_state[i].pdop;
}
if (_gps_state[i].nsats > 0
&& _gps_state[i].nsats > _gps_blended_state.nsats) {
_gps_blended_state.nsats = _gps_state[i].nsats;
}
if (!_gps_state[i].vel_ned_valid) {
_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];
// _blended_antenna_offset += temp_antenna_offset;
}
/*
* Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state.
* This is statistically the most likely location, but may not be stable enough for direct use by the EKF.
*/
// Use the GPS with the highest weighting as the reference position
float best_weight = 0.0f;
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;
}
}
// Convert each GPS position to a local NEU offset relative to the reference position
Vector2f blended_NE_offset_m;
blended_NE_offset_m.zero();
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)) {
// 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),
&horiz_offset(0), &horiz_offset(1));
// sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i];
// calculate vertical offset
float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt);
// sum weighted offsets
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;
// 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;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) {
best_weight = _blend_weights[i];
gps_best_yaw_index = i;
}
}
_gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw;
_gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset;
}
/*
* 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 EKF2::update_gps_offsets()
{
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// 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 omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f);
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].time_usec - _time_prev_us[i] > 0) {
// 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].time_usec - _time_prev_us[i]),
0.0f, 1.0f);
_time_prev_us[i] = _gps_state[i].time_usec;
}
}
// Calculate a filtered position delta for each GPS relative to the blended solution state
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));
_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] * (1.0f - alpha[i]);
}
// calculate offset limits from the largest difference between receivers
Vector2f max_ne_offset{};
float max_alt_offset = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) {
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));
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)));
}
}
}
// apply offset limits
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
_NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
_NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1));
_hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset);
}
}
/*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/
void EKF2::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].time_usec = _gps_state[i].time_usec;
_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_ned = _gps_state[i].vel_ned;
_gps_output[i].eph = _gps_state[i].eph;
_gps_output[i].epv = _gps_state[i].epv;
_gps_output[i].sacc = _gps_state[i].sacc;
_gps_output[i].pdop = _gps_state[i].pdop;
_gps_output[i].nsats = _gps_state[i].nsats;
_gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid;
_gps_output[i].yaw = _gps_state[i].yaw;
_gps_output[i].yaw_offset = _gps_state[i].yaw_offset;
}
}
/*
Calculate GPS output that is a blend of the offset corrected physical receiver data
*/
void EKF2::calc_gps_blend_output()
{
// 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();
float blended_alt_offset_mm = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) {
// 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));
// sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i];
// calculate vertical offset
float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt);
// sum weighted offsets
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;
// Copy remaining data from internal states to output
_gps_output[GPS_BLENDED_INSTANCE].time_usec = _gps_blended_state.time_usec;
_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_ned = _gps_blended_state.vel_ned;
_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].sacc = _gps_blended_state.sacc;
_gps_output[GPS_BLENDED_INSTANCE].pdop = _gps_blended_state.pdop;
_gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats;
_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid;
_gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw;
_gps_output[GPS_BLENDED_INSTANCE].yaw_offset = _gps_blended_state.yaw_offset;
}
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt;
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
if (_gps_alttitude_ellipsoid_previous_timestamp[0] == 0) {
if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
_wgs84_hgt_offset = height_diff;
_gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
} else if (_gps_state[0].time_usec != _gps_alttitude_ellipsoid_previous_timestamp[0]) {
} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
// apply a 10 second first order low pass filter to baro offset
float dt = 1e-6f * static_cast<float>(_gps_state[0].time_usec - _gps_alttitude_ellipsoid_previous_timestamp[0]);
_gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
}
src/modules/ekf2/EKF2.hpp
+5 -79
View File
@@ -60,7 +60,6 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
@@ -85,15 +84,6 @@
#include "Utility/PreFlightChecker.hpp"
// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
#define BLEND_MASK_USE_SPD_ACC 1
#define BLEND_MASK_USE_HPOS_ACC 2
#define BLEND_MASK_USE_VPOS_ACC 4
// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
#define GPS_MAX_RECEIVERS 2
#define GPS_BLENDED_INSTANCE 2
class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@@ -135,38 +125,6 @@ private:
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
* receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
* have significant position differences, variation in receiver estimated accuracy will cause undesirable
* variation in the position solution.
*/
bool blend_gps_data();
/*
* 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();
/*
* 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();
/*
Calculate GPS output that is a blend of the offset corrected physical receiver data
*/
void calc_gps_blend_output();
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
@@ -197,34 +155,11 @@ private:
// Used to control saving of mag declination to be used on next startup
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
// TODO: the user should be allowed to set these values by a parameter
static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate
// GPS blending and switching
gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
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)
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
bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
uint64_t _gps_time_usec{0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
bool _imu_bias_reset_request{false};
@@ -242,6 +177,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@@ -255,16 +191,12 @@ private:
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
// because we can have multiple GPS instances
uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
sensor_selection_s _sensor_selection{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
@@ -516,12 +448,6 @@ private:
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
// GPS blending
(ParamInt<px4::params::EKF2_GPS_MASK>)
_param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
// Test used to determine if the vehicle is static or moving
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
src/modules/ekf2/ekf2_params.c
-31
View File
@@ -1375,37 +1375,6 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f);
/**
* Multi GPS Blending Control Mask.
*
* Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance.
* 0 : Set to true to use speed accuracy
* 1 : Set to true to use horizontal position accuracy
* 2 : Set to true to use vertical position accuracy
*
* @group EKF2
* @min 0
* @max 7
* @bit 0 use speed accuracy
* @bit 1 use hpos accuracy
* @bit 2 use vpos accuracy
*/
PARAM_DEFINE_INT32(EKF2_GPS_MASK, 0);
/**
* Multi GPS Blending Time Constant
*
* Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences.
*
*
* @group EKF2
* @min 1.0
* @max 100.0
* @unit s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_TAU, 10.0f);
/**
* Vehicle movement test threshold
*
src/modules/logger/logged_topics.cpp
+3 -3
View File
@@ -95,6 +95,7 @@ void LoggedTopics::add_default_topics()
add_topic("vehicle_command");
add_topic("vehicle_control_mode");
add_topic("vehicle_global_position", 200);
add_topic("vehicle_gps_position", 500);
add_topic("vehicle_land_detected");
add_topic("vehicle_local_position", 100);
add_topic("vehicle_local_position_setpoint", 100);
@@ -121,9 +122,9 @@ void LoggedTopics::add_default_topics()
add_topic_multi("optical_flow", 1000, 2);
add_topic_multi("sensor_accel", 1000, 3);
add_topic_multi("sensor_baro", 1000, 3);
add_topic_multi("sensor_gps", 1000, 3);
add_topic_multi("sensor_gyro", 1000, 3);
add_topic_multi("sensor_mag", 1000, 4);
add_topic_multi("vehicle_gps_position", 1000, 2);
add_topic_multi("vehicle_imu", 500, 3);
add_topic_multi("vehicle_imu_status", 1000, 3);
@@ -168,7 +169,6 @@ void LoggedTopics::add_estimator_replay_topics()
{
// for estimator replay (need to be at full rate)
add_topic("ekf2_timestamps");
add_topic("ekf_gps_position");
// current EKF2 subscriptions
add_topic("airspeed");
@@ -176,13 +176,13 @@ void LoggedTopics::add_estimator_replay_topics()
add_topic("sensor_combined");
add_topic("sensor_selection");
add_topic("vehicle_air_data");
add_topic("vehicle_gps_position");
add_topic("vehicle_land_detected");
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");
add_topic("vehicle_visual_odometry_aligned");
add_topic_multi("distance_sensor");
add_topic_multi("vehicle_gps_position", 0, 2);
}
void LoggedTopics::add_thermal_calibration_topics()
src/modules/mavlink/mavlink_messages.cpp
+6 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -93,6 +93,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/rpm.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/tecs_status.h>
@@ -105,7 +106,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -1792,7 +1792,7 @@ public:
}
private:
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _gps_sub{ORB_ID(sensor_gps), 0};
/* do not allow top copying this class */
MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &) = delete;
@@ -1804,7 +1804,7 @@ protected:
bool send(const hrt_abstime t) override
{
vehicle_gps_position_s gps;
sensor_gps_s gps;
if (_gps_sub.update(&gps)) {
mavlink_gps_raw_int_t msg{};
@@ -1868,7 +1868,7 @@ public:
}
private:
uORB::Subscription _gps2_sub{ORB_ID(vehicle_gps_position), 1};
uORB::Subscription _gps2_sub{ORB_ID(sensor_gps), 1};
/* do not allow top copying this class */
MavlinkStreamGPS2Raw(MavlinkStreamGPS2Raw &) = delete;
@@ -1880,7 +1880,7 @@ protected:
bool send(const hrt_abstime t) override
{
vehicle_gps_position_s gps;
sensor_gps_s gps;
if (_gps2_sub.update(&gps)) {
mavlink_gps2_raw_t msg = {};
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -2287,7 +2287,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
const uint64_t timestamp = hrt_absolute_time();
struct vehicle_gps_position_s hil_gps = {};
sensor_gps_s hil_gps{};
hil_gps.timestamp_time_relative = 0;
hil_gps.time_utc_usec = gps.time_usec;
src/modules/mavlink/mavlink_receiver.h
+2 -2
View File
@@ -85,6 +85,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/tune_control.h>
@@ -94,7 +95,6 @@
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -250,12 +250,12 @@ private:
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<sensor_gps_s> _gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_gps_position_s> _gps_pub{ORB_ID(vehicle_gps_position)};
uORB::Publication<vehicle_land_detected_s> _land_detector_pub{ORB_ID(vehicle_land_detected)};
uORB::Publication<vehicle_local_position_s> _local_pos_pub{ORB_ID(vehicle_local_position)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
src/modules/sensors/CMakeLists.txt
+2
View File
@@ -37,6 +37,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_air_data)
add_subdirectory(vehicle_gps_position)
add_subdirectory(vehicle_imu)
add_subdirectory(vehicle_magnetometer)
@@ -57,6 +58,7 @@ px4_add_module(
vehicle_acceleration
vehicle_angular_velocity
vehicle_air_data
vehicle_gps_position
vehicle_imu
vehicle_magnetometer
)
src/modules/sensors/sensors.cpp
+28
View File
@@ -73,6 +73,7 @@
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_gps_position/VehicleGPSPosition.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
@@ -172,6 +173,7 @@ private:
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData *_vehicle_air_data{nullptr};
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
VehicleGPSPosition *_vehicle_gps_position{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -204,6 +206,7 @@ private:
void adc_poll();
void InitializeVehicleAirData();
void InitializeVehicleGPSPosition();
void InitializeVehicleIMU();
void InitializeVehicleMagnetometer();
@@ -275,6 +278,11 @@ Sensors::~Sensors()
delete _vehicle_air_data;
}
if (_vehicle_gps_position) {
_vehicle_gps_position->Stop();
delete _vehicle_gps_position;
}
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Stop();
delete _vehicle_magnetometer;
@@ -483,6 +491,19 @@ void Sensors::InitializeVehicleAirData()
}
}
void Sensors::InitializeVehicleGPSPosition()
{
if (_vehicle_gps_position == nullptr) {
if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
_vehicle_gps_position = new VehicleGPSPosition();
if (_vehicle_gps_position) {
_vehicle_gps_position->Start();
}
}
}
}
void Sensors::InitializeVehicleIMU()
{
// create a VehicleIMU instance for each accel/gyro pair
@@ -553,6 +574,7 @@ void Sensors::Run()
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
@@ -592,6 +614,7 @@ void Sensors::Run()
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
_last_config_update = hrt_absolute_time();
@@ -678,6 +701,11 @@ int Sensors::print_status()
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
if (_vehicle_gps_position) {
PX4_INFO_RAW("\n");
_vehicle_gps_position->PrintStatus();
}
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {
src/modules/sensors/vehicle_gps_position/CMakeLists.txt
+38
View File
@@ -0,0 +1,38 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(vehicle_gps_position
VehicleGPSPosition.cpp
VehicleGPSPosition.hpp
)
target_link_libraries(vehicle_gps_position PRIVATE ecl_geo px4_work_queue)
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
+742
View File
@@ -0,0 +1,742 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "VehicleGPSPosition.hpp"
#include <px4_platform_common/log.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
namespace sensors
{
using namespace matrix;
using math::constrain;
VehicleGPSPosition::VehicleGPSPosition() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
}
VehicleGPSPosition::~VehicleGPSPosition()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleGPSPosition::Start()
{
// force initial updates
ParametersUpdate(true);
// needed to change the active sensor if the primary stops updating
bool anyGpsRegistered = false;
bool registered[GPS_MAX_RECEIVERS];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
registered[i] = _sensor_gps_sub[i].registerCallback();
anyGpsRegistered |= registered[i];
}
return anyGpsRegistered;
}
void VehicleGPSPosition::Stop()
{
Deinit();
// clear all registered callbacks
for (auto &sub : _sensor_gps_sub) {
sub.unregisterCallback();
}
}
void VehicleGPSPosition::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
}
}
void VehicleGPSPosition::Run()
{
perf_begin(_cycle_perf);
ParametersUpdate();
// Check all GPS instance
bool any_gps_updated = false;
bool gps_updated[GPS_MAX_RECEIVERS];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
gps_updated[i] = _sensor_gps_sub[i].updated();
if (gps_updated[i]) {
any_gps_updated = true;
_sensor_gps_sub[i].copy(&_gps_state[i]);
}
}
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) {
// 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;
// Find the single "best" GPS from the data we have
// First, find the GPS(s) with the best fix
uint8_t best_fix = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type > best_fix) {
best_fix = _gps_state[i].fix_type;
}
}
// Second, compare GPS's with best fix and take the one with most satellites
uint8_t max_sats = 0;
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;
}
}
// 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();
}
// 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;
}
}
perf_end(_cycle_perf);
}
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.
* If time difference is excessive, use newest to prevent a disconnected receiver
* from blocking updates.
*/
// Calculate the time step for each receiver with some filtering to reduce the effects of jitter
// 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();
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);
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) {
// 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;
continue;
}
// Only count GPSs with at least a 2D fix for blending purposes
if (_gps_state[i].fix_type < 2) {
continue;
}
if (_gps_dt[i] > dt_max) {
dt_max = _gps_dt[i];
_gps_slowest_index = i;
}
if (_gps_dt[i] < dt_min) {
dt_min = _gps_dt[i];
}
gps_count++;
}
// Find the receiver that is last be updated
uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
// Find largest and smallest times
if (_gps_state[i].timestamp > max_us) {
max_us = _gps_state[i].timestamp;
_gps_newest_index = i;
}
if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) {
min_us = _gps_state[i].timestamp;
_gps_oldest_index = i;
}
}
if (gps_count < 2) {
// Less than 2 receivers left, so fall out of blending
return false;
}
/*
* If the largest dt is less than 20% greater than the smallest, then we have receivers
* running at the same rate then we wait until we have two messages with an arrival time
* difference that is less than 50% of the smallest time step and use the time stamp from
* the newest data.
* Else we have two receivers at different update rates and use the slowest receiver
* as the timing reference.
*/
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;
}
} else {
// both receivers running at different rates
_gps_time_ref_index = _gps_slowest_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
_gps_new_output_data = true;
}
}
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
float speed_accuracy_sum_sq = 0.0f;
if (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) {
speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s;
}
}
}
// calculate the sum squared horizontal position accuracy across all GPS sensors
float horizontal_accuracy_sum_sq = 0.0f;
if (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
}
}
}
// calculate the sum squared vertical position accuracy across all GPS sensors
float vertical_accuracy_sum_sq = 0.0f;
if (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
}
}
}
// Check if we can do blending using reported accuracy
bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f
|| speed_accuracy_sum_sq > 0.0f);
// if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
if (!can_do_blending) {
return false;
}
float sum_of_all_weights = 0.0f;
// calculate a weighting using the reported speed accuracy
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
float sum_of_spd_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s >= 0.001f) {
spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s);
sum_of_spd_weights += spd_blend_weights[i];
}
}
// normalise the weights
if (sum_of_spd_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
}
sum_of_all_weights += 1.0f;
}
}
// calculate a weighting using the reported horizontal position
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
float sum_of_hpos_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) {
hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph);
sum_of_hpos_weights += hpos_blend_weights[i];
}
}
// normalise the weights
if (sum_of_hpos_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
}
sum_of_all_weights += 1.0f;
}
}
// calculate a weighting using the reported vertical position accuracy
float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
if (vertical_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_vpos_weights = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) {
vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv);
sum_of_vpos_weights += vpos_blend_weights[i];
}
}
// normalise the weights
if (sum_of_vpos_weights > 0.0f) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
}
sum_of_all_weights += 1.0f;
};
}
// 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;
}
// 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;
}
return true;
}
void VehicleGPSPosition::update_gps_blend_states()
{
// 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();
// 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]);
// 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;
}
// 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 (_gps_state[i].eph > 0.0f
&& _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;
}
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;
}
// 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].satellites_used > 0
&& _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;
}
}
// TODO read parameters for individual GPS antenna positions and blend
// Vector3f temp_antenna_offset = _antenna_offset[i];
// temp_antenna_offset *= _blend_weights[i];
// _blended_antenna_offset += temp_antenna_offset;
}
/*
* Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state.
* This is statistically the most likely location, but may not be stable enough for direct use by the EKF.
*/
// Use the GPS with the highest weighting as the reference position
float best_weight = 0.0f;
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;
}
}
// Convert each GPS position to a local NEU offset relative to the reference position
Vector2f blended_NE_offset_m;
blended_NE_offset_m.zero();
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)) {
// 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),
&horiz_offset(0), &horiz_offset(1));
// sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i];
// calculate vertical offset
float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt);
// sum weighted offsets
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;
// 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;
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];
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;
}
void VehicleGPSPosition::update_gps_offsets()
{
// 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 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;
}
_time_prev_us[i] = _gps_state[i].timestamp;
}
}
// Calculate a filtered position delta for each GPS relative to the blended solution state
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));
_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] * (1.0f - alpha[i]);
}
// calculate offset limits from the largest difference between receivers
Vector2f max_ne_offset{};
float max_alt_offset = 0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) {
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));
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)));
}
}
}
// apply offset limits
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
_NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
_NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1));
_hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset);
}
}
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()
{
// 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();
float blended_alt_offset_mm = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) {
// 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));
// sum weighted offsets
blended_NE_offset_m += horiz_offset * _blend_weights[i];
// calculate vertical offset
float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt);
// sum weighted offsets
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;
// 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;
}
void VehicleGPSPosition::Publish(const sensor_gps_s &gps)
{
vehicle_gps_position_s gps_output{};
gps_output.timestamp = gps.timestamp;
gps_output.time_utc_usec = gps.time_utc_usec;
gps_output.lat = gps.lat;
gps_output.lon = gps.lon;
gps_output.alt = gps.alt;
gps_output.alt_ellipsoid = gps.alt_ellipsoid;
gps_output.s_variance_m_s = gps.s_variance_m_s;
gps_output.c_variance_rad = gps.c_variance_rad;
gps_output.eph = gps.eph;
gps_output.epv = gps.epv;
gps_output.hdop = gps.hdop;
gps_output.vdop = gps.vdop;
gps_output.noise_per_ms = gps.noise_per_ms;
gps_output.jamming_indicator = gps.jamming_indicator;
gps_output.vel_m_s = gps.vel_m_s;
gps_output.vel_n_m_s = gps.vel_n_m_s;
gps_output.vel_e_m_s = gps.vel_e_m_s;
gps_output.vel_d_m_s = gps.vel_d_m_s;
gps_output.cog_rad = gps.cog_rad;
gps_output.timestamp_time_relative = gps.timestamp_time_relative;
gps_output.heading = gps.heading;
gps_output.heading_offset = gps.heading_offset;
gps_output.fix_type = gps.fix_type;
gps_output.vel_ned_valid = gps.vel_ned_valid;
gps_output.satellites_used = gps.satellites_used;
gps_output.selected = _gps_select_index;
_vehicle_gps_position_pub.publish(gps_output);
}
void VehicleGPSPosition::PrintStatus()
{
PX4_INFO("selected GPS: %d", _gps_select_index);
}
}; // namespace sensors
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
+154
View File
@@ -0,0 +1,154 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_gps_position.h>
using namespace time_literals;
namespace sensors
{
class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleGPSPosition();
~VehicleGPSPosition() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
void Publish(const sensor_gps_s &gps);
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
* receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
* have significant position differences, variation in receiver estimated accuracy will cause undesirable
* variation in the position solution.
*/
bool blend_gps_data();
/*
* 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();
/*
* 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();
/*
Calculate GPS output that is a blend of the offset corrected physical receiver data
*/
void calc_gps_blend_output();
// 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);
uORB::Publication<vehicle_gps_position_s> _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] { /**< sensor data subscription */
{this, ORB_ID(sensor_gps), 0},
{this, ORB_ID(sensor_gps), 1},
};
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)
)
};
}; // namespace sensors
src/modules/sensors/vehicle_gps_position/params.c
+63
View File
@@ -0,0 +1,63 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* Multi GPS Blending Control Mask.
*
* Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance.
* 0 : Set to true to use speed accuracy
* 1 : Set to true to use horizontal position accuracy
* 2 : Set to true to use vertical position accuracy
*
* @group Sensors
* @min 0
* @max 7
* @bit 0 use speed accuracy
* @bit 1 use hpos accuracy
* @bit 2 use vpos accuracy
*/
PARAM_DEFINE_INT32(SENS_GPS_MASK, 0);
/**
* Multi GPS Blending Time Constant
*
* Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences.
*
*
* @group Sensors
* @min 1.0
* @max 100.0
* @unit s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
src/modules/sih/sih.cpp
+24 -24
View File
@@ -229,16 +229,16 @@ void Sih::init_variables()
void Sih::init_sensors()
{
_vehicle_gps_pos.fix_type = 3; // 3D fix
_vehicle_gps_pos.satellites_used = 8;
_vehicle_gps_pos.heading = NAN;
_vehicle_gps_pos.heading_offset = NAN;
_vehicle_gps_pos.s_variance_m_s = 0.5f;
_vehicle_gps_pos.c_variance_rad = 0.1f;
_vehicle_gps_pos.eph = 0.9f;
_vehicle_gps_pos.epv = 1.78f;
_vehicle_gps_pos.hdop = 0.7f;
_vehicle_gps_pos.vdop = 1.1f;
_sensor_gps.fix_type = 3; // 3D fix
_sensor_gps.satellites_used = 8;
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = NAN;
_sensor_gps.s_variance_m_s = 0.5f;
_sensor_gps.c_variance_rad = 0.1f;
_sensor_gps.eph = 0.9f;
_sensor_gps.epv = 1.78f;
_sensor_gps.hdop = 0.7f;
_sensor_gps.vdop = 1.1f;
}
// read the motor signals outputted from the mixer
@@ -351,21 +351,21 @@ void Sih::send_IMU()
void Sih::send_gps()
{
_vehicle_gps_pos.timestamp = _now;
_vehicle_gps_pos.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees
_vehicle_gps_pos.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
_vehicle_gps_pos.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
_vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
_vehicle_gps_pos.vel_ned_valid = true; // True if NED velocity is valid
_vehicle_gps_pos.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
1)); // GPS ground speed, (metres/sec)
_vehicle_gps_pos.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
_vehicle_gps_pos.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
_vehicle_gps_pos.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec)
_vehicle_gps_pos.cog_rad = atan2(_gps_vel(1),
_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
_sensor_gps.timestamp = _now;
_sensor_gps.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees
_sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
_sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
_sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
_sensor_gps.vel_ned_valid = true; // True if NED velocity is valid
_sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
1)); // GPS ground speed, (metres/sec)
_sensor_gps.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
_sensor_gps.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
_sensor_gps.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec)
_sensor_gps.cog_rad = atan2(_gps_vel(1),
_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
_vehicle_gps_pos_pub.publish(_vehicle_gps_pos);
_sensor_gps_pub.publish(_sensor_gps);
}
void Sih::publish_sih()
src/modules/sih/sih.hpp
+3 -3
View File
@@ -50,10 +50,10 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/vehicle_gps_position.h>
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
@@ -104,8 +104,8 @@ private:
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
// to publish the gps position
vehicle_gps_position_s _vehicle_gps_pos{};
uORB::Publication<vehicle_gps_position_s> _vehicle_gps_pos_pub{ORB_ID(vehicle_gps_position)};
sensor_gps_s _sensor_gps{};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
// angular velocity groundtruth
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
src/modules/simulator/simulator.h
+2 -2
View File
@@ -65,10 +65,10 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
@@ -231,7 +231,7 @@ private:
// HIL GPS
static constexpr int MAX_GPS = 3;
uORB::PublicationMulti<vehicle_gps_position_s> *_vehicle_gps_position_pubs[MAX_GPS] {};
uORB::PublicationMulti<sensor_gps_s> *_sensor_gps_pubs[MAX_GPS] {};
uint8_t _gps_ids[MAX_GPS] {};
std::default_random_engine _gen{};
src/modules/simulator/simulator_mavlink.cpp
+6 -6
View File
@@ -336,7 +336,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
mavlink_msg_hil_gps_decode(msg, &hil_gps);
if (!_gps_blocked) {
vehicle_gps_position_s gps{};
sensor_gps_s gps{};
gps.timestamp = hrt_absolute_time();
gps.time_utc_usec = hil_gps.time_usec;
@@ -356,16 +356,16 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
// New publishers will be created based on the HIL_GPS ID's being different or not
for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
if (_vehicle_gps_position_pubs[i] && _gps_ids[i] == hil_gps.id) {
_vehicle_gps_position_pubs[i]->publish(gps);
if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
_sensor_gps_pubs[i]->publish(gps);
break;
}
if (_vehicle_gps_position_pubs[i] == nullptr) {
_vehicle_gps_position_pubs[i] = new uORB::PublicationMulti<vehicle_gps_position_s> {ORB_ID(vehicle_gps_position)};
if (_sensor_gps_pubs[i] == nullptr) {
_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
_gps_ids[i] = hil_gps.id;
_vehicle_gps_position_pubs[i]->publish(gps);
_sensor_gps_pubs[i]->publish(gps);
break;
}
}