dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vehicle_gps_position: use timestamp field instead of timestamp_position 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_gps_position.msg
View File

@ -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)

2
src/drivers/gps/devices

@ -1 +1 @@
Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333
Subproject commit 9e7ed8a5ea85c76c5d563413df051f138b9a0622

8
src/drivers/gps/gps.cpp
View File

@ -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);

4
src/lib/terrain_estimation/terrain_estimator.cpp
View File

@ -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<float, 1, n_x> 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

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
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));
}

2
src/modules/commander/PreflightCheck.cpp
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;
}
}

4
src/modules/commander/commander.cpp
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;

6
src/modules/ekf2/ekf2_main.cpp
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;

2
src/modules/ekf2_replay/ekf2_replay_main.cpp
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;

10
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
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;
}
}

2
src/modules/mavlink/mavlink_messages.cpp
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;

2
src/modules/mavlink/mavlink_receiver.cpp
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;

2
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
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");

8
src/modules/uavcan/sensors/gnss.cpp
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;

18
src/platforms/posix/drivers/gpssim/gpssim.cpp
View File

@ -278,14 +278,14 @@ GPSSIM::receive(int timeout)
simulator::RawGPSData gps;
sim->getGPSSample((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);