diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-07 15:07:37 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-07 15:07:37 +0400 |
commit | 72a531b018f0089c89ed40261969555fd282e459 (patch) | |
tree | 2c7f51e4ac2e430500477f76b0252d09bc8a594a | |
parent | dae9b48462bc851ee61d6d18ff2b5697dddf620b (diff) | |
download | px4-firmware-72a531b018f0089c89ed40261969555fd282e459.tar.gz px4-firmware-72a531b018f0089c89ed40261969555fd282e459.tar.bz2 px4-firmware-72a531b018f0089c89ed40261969555fd282e459.zip |
Fixed UAVCAN GNSS bridge - EPV computation, catching up with the new GPS ORB topic
-rw-r--r-- | src/modules/uavcan/gnss_receiver.cpp | 28 |
1 files changed, 14 insertions, 14 deletions
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 3e98bdf14..debba9fee 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -85,13 +85,18 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); if (valid_position_covariance) { - float pos_cov[9]; - msg.position_covariance.unpackSquareMatrix(pos_cov); - _report.p_variance_m = math::max(pos_cov[0], pos_cov[4]); - _report.eph_m = sqrtf(_report.p_variance_m); + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + + // Horizontal position uncertainty + const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); + _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.p_variance_m = -1.0; - _report.eph_m = -1.0; + _report.eph = -1.0F; + _report.epv = -1.0F; } if (valid_velocity_covariance) { @@ -118,12 +123,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); - _report.epv_m = sqrtf(msg.position_covariance[8]); - } else { - _report.s_variance_m_s = -1.0; - _report.c_variance_rad = -1.0; - _report.epv_m = -1.0; + _report.s_variance_m_s = -1.0F; + _report.c_variance_rad = -1.0F; } _report.fix_type = msg.status; @@ -139,9 +141,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav _report.timestamp_time = _report.timestamp_position; _report.time_gps_usec = msg.timestamp.husec * msg.timestamp.USEC_PER_LSB; // Convert to microseconds - _report.timestamp_satellites = _report.timestamp_position; - _report.satellites_visible = msg.sats_used; - _report.satellite_info_available = 0; // Set to 0 for no info available + _report.satellites_used = msg.sats_used; if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); |