diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index ab8a92873f..4be23df42e 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,5 +1,4 @@ # GPS position in WGS84 coordinates. -uint64 timestamp_position # Time of the position estimates since system start, (microseconds) int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 5c1ae95655..9e7ed8a5ea 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333 +Subproject commit 9e7ed8a5ea85c76c5d563413df051f138b9a0622 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3e29be8425..21950bbf4c 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -687,7 +687,7 @@ GPS::task_main() while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; @@ -757,7 +757,7 @@ GPS::task_main() _report_gps_pos.timestamp_time = hrt_absolute_time(); /* reset the timestamps for data, because we have no data yet */ - _report_gps_pos.timestamp_position = 0; + _report_gps_pos.timestamp = 0; _report_gps_pos.timestamp_variance = 0; _report_gps_pos.timestamp_velocity = 0; @@ -960,9 +960,9 @@ GPS::print_info() _report_gps_pos.noise_per_ms, _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); - if (_report_gps_pos.timestamp_position != 0) { + if (_report_gps_pos.timestamp != 0) { PX4_WARN("position lock: %d, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0); PX4_WARN("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_WARN("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 64a747f532..20bdabf6e5 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -153,7 +153,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl _distance_last = distance->current_distance; } - if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + if (gps->timestamp > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; C(0, 1) = 1; @@ -172,7 +172,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl _x += K * r; _P -= K * C * _P; - _time_last_gps = gps->timestamp_position; + _time_last_gps = gps->timestamp; } // reinitialise filter if we find bad data diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 273d159f2e..4aa6abca72 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -381,7 +381,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ @@ -537,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 3141d6d0b3..1f31586e19 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -366,7 +366,7 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) else { struct vehicle_gps_position_s gps; if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || - (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) { + (hrt_elapsed_time(&gps.timestamp) > 1000000)) { success = false; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c7521acd88..3752f56bf0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2167,7 +2167,7 @@ int commander_thread_main(int argc, char *argv[]) if (!map_projection_global_initialized() && (gps_position.eph < eph_threshold) && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); @@ -2189,7 +2189,7 @@ int commander_thread_main(int argc, char *argv[]) } } - if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where gps was regained */ if (status_flags.gps_failure && !gpsIsNoisy) { status_flags.gps_failure = false; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 274d924032..d482c4d8ac 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -517,7 +517,7 @@ void Ekf2::task_main() // read gps data if available if (gps_updated) { struct gps_message gps_msg = {}; - gps_msg.time_usec = gps.timestamp_position; + gps_msg.time_usec = gps.timestamp; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; gps_msg.alt = gps.alt; @@ -535,7 +535,7 @@ void Ekf2::task_main() //TODO add gdop to gps topic gps_msg.gdop = 0.0f; - _ekf.setGpsData(gps.timestamp_position, &gps_msg); + _ekf.setGpsData(gps.timestamp, &gps_msg); } // only set airspeed data if condition for airspeed fusion are met @@ -909,7 +909,7 @@ void Ekf2::task_main() // only write gps data if we had a gps update. if (gps_updated) { - replay.time_usec = gps.timestamp_position; + replay.time_usec = gps.timestamp; replay.time_usec_vel = gps.timestamp_velocity; replay.lat = gps.lat; replay.lon = gps.lon; diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index b8de2c70b1..823c60744b 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -396,7 +396,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) } else if (type == LOG_RPL2_MSG) { uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec; parseMessage(data, dest_ptr, type); - _gps.timestamp_position = replay_part2.time_pos_usec; + _gps.timestamp = replay_part2.time_pos_usec; _gps.timestamp_velocity = replay_part2.time_vel_usec; _gps.lat = replay_part2.lat; _gps.lon = replay_part2.lon; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5b75ab2b12..4fde0bea22 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -448,7 +448,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -821,7 +821,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -1068,7 +1068,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (!_local_pos.xy_global || !_local_pos.v_xy_valid || - _gps.timestamp_position == 0 || + _gps.timestamp == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; @@ -1584,7 +1584,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gpsIsGood) { //Calculate time since last good GPS fix - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(_gps.timestamp - _previousGPSTimestamp) / 1e6f; //Stop dead-reckoning mode if (_global_pos.dead_reckoning) { @@ -1643,7 +1643,7 @@ void AttitudePositionEstimatorEKF::pollData() // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); - _previousGPSTimestamp = _gps.timestamp_position; + _previousGPSTimestamp = _gps.timestamp; } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9cd58d66d8..125530fb98 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1066,7 +1066,7 @@ protected: if (_gps_sub->update(&_gps_time, &gps)) { mavlink_gps_raw_int_t msg = {}; - msg.time_usec = gps.timestamp_position; + msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; msg.lat = gps.lat; msg.lon = gps.lon; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c54c225d40..37530bb13a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1784,7 +1784,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_time = timestamp; hil_gps.time_utc_usec = gps.time_usec; - hil_gps.timestamp_position = timestamp; + hil_gps.timestamp = timestamp; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 1677a91d3c..79e070c614 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -944,7 +944,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { + if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index d80b4b425d..bf56539b7a 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -104,12 +104,12 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructuregetGPSSample((uint8_t *)&gps, sizeof(gps)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = gps.lat; _report_gps_pos.lon = gps.lon; _report_gps_pos.alt = gps.alt; - _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position; + _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp; _report_gps_pos.eph = (float)gps.eph * 1e-2f; _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position; + _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp; _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f; _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f; _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f; @@ -306,17 +306,17 @@ GPSSIM::task_main() while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position; + _report_gps_pos.timestamp_variance = _report_gps_pos.timestamp; _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; - _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position; + _report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp; _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; @@ -342,7 +342,7 @@ GPSSIM::task_main() //Publish initial report that we have access to a GPS //Make sure to clear any stale data in case driver is reset memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_time = hrt_absolute_time(); @@ -413,9 +413,9 @@ GPSSIM::print_info() _report_gps_pos.noise_per_ms, _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); - if (_report_gps_pos.timestamp_position != 0) { + if (_report_gps_pos.timestamp != 0) { PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0); PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);