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

vehicle_gps_position: use timestamp field instead of timestamp_position

Browse Source 
timestamp was unused. This allows to remove timestamp_position.
This commit is contained in:
Beat Küng
2016-06-15 12:59:39 +02:00
committed by Lorenz Meier
parent bf0b3c1585
commit 89f5bd27e8
15 changed files with 37 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+2 -2
View File
@@ -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));
}
src/modules/commander/PreflightCheck.cpp
+1 -1
View File
@@ -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;
}
}
src/modules/commander/commander.cpp
+2 -2
View File
@@ -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;
src/modules/ekf2/ekf2_main.cpp
+3 -3
View File
@@ -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;
src/modules/ekf2_replay/ekf2_replay_main.cpp
+1 -1
View File
@@ -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;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+5 -5
View File
@@ -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<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
const float dtLastGoodGPS = static_cast<float>(_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;
}
}
src/modules/mavlink/mavlink_messages.cpp
+1 -1
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -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;
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+1 -1
View File
@@ -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");
src/modules/uavcan/sensors/gnss.cpp
+4 -4
View File
@@ -104,12 +104,12 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
* to use an independent time source (based on hardware TIM5) instead of HRT.
* The proper solution is to be developed.
*/
report.timestamp_position = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
report.timestamp_variance = report.timestamp_position;
report.timestamp_variance = report.timestamp;
// Check if the msg contains valid covariance information
@@ -163,7 +163,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
report.fix_type = msg.status;
report.timestamp_velocity = report.timestamp_position;
report.timestamp_velocity = report.timestamp;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
@@ -172,7 +172,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
report.timestamp_time = report.timestamp_position;
report.timestamp_time = report.timestamp;
report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
report.satellites_used = msg.sats_used;