diff --git a/src/tracker.cpp b/src/tracker.cpp index c3b19599..65fda3c3 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -28,6 +28,8 @@ #define COURSE_SPEED 3.0 /* below this speed the direction is determined with sensor */ #define BASE_DIRECTORY "TRACKER" #define RECORD_INTERVAL 50.0 /* max distance interval between recorded positions */ +#define MIN_ELEVATION 10.0 /* threshold in meter */ +#define MAX_ACCEL 12.0 /* accel in meter per sec^2 */ #define TAG_TRKPT "TRKPT" #define TAG_WAYPT "WAYPT" @@ -228,6 +230,7 @@ TrackerModule::TrackerModule(QThread* thread, const QString& root) , m_state(osmscout::PositionAgent::PositionState::NoGpsSignal) , m_magneticDip(0.0) , m_azimuth(0.0) +, m_currentAlt(0) , m_currentSpeed(0) , m_maxSpeed(0) , m_lastPosition() @@ -279,53 +282,59 @@ void TrackerModule::onLocationChanged(bool positionValid, double lat, double lon osmscout::Timestamp now = std::chrono::system_clock::now(); osmscout::Bearing bearing; - if (m_lastPosition) + if (positionValid) { - double d = osmin::Utils::sphericalDistance(m_lastPosition.coord.GetLat(), m_lastPosition.coord.GetLon(), lat, lon); - auto sec = std::chrono::duration_cast >(now - m_lastPosition.time); - if (sec.count() > 0) + if (m_lastPosition) { - m_currentSpeed = 3.6 * d / sec.count(); - // update tracking data when moving - if (m_currentSpeed > MOVING_SPEED) + double d = osmin::Utils::sphericalDistance(m_lastPosition.coord.GetLat(), m_lastPosition.coord.GetLon(), lat, lon); + auto sec = std::chrono::duration_cast >(now - m_lastPosition.time); + if (sec.count() > 0) { - m_distance += d; - m_duration += sec.count(); - double a = alt - m_lastPosition.elevation; - if (a > 0) - m_ascent += a; + // use accel to validate this interval + double a0 = std::round((alt - m_currentAlt) / MIN_ELEVATION) * MIN_ELEVATION; + double s0 = 3.6 * d / sec.count(); + double s1 = std::sqrt(std::pow(d, 2) + std::pow(a0, 2)) / sec.count(); + double accel = std::fabs((s1 - (m_currentSpeed / 3.6)) / sec.count()); + if (accel > MAX_ACCEL) + { + // reset elevation + m_currentAlt = alt; + } else - m_descent -= a; - if (m_currentSpeed > m_maxSpeed) - m_maxSpeed = m_currentSpeed; - } - emit dataChanged(m_currentSpeed, m_distance, m_duration, m_ascent, m_descent, m_maxSpeed); - } - // when we are stationary the direction is calculated according to the azimuth of the device - // otherwise it is estimated according to the progress compared to the previous position - if (m_currentSpeed < COURSE_SPEED) - bearing = osmscout::Bearing::Degrees(m_azimuth); - else - bearing = osmscout::Bearing::Radians(osmin::Utils::sphericalBearingFinal(m_lastPosition.coord.GetLat(), m_lastPosition.coord.GetLon(), lat, lon)); - - if (m_recording) - { - if (!m_lastRecord) - { - record(); - m_lastRecord.time = m_lastPosition.time; - m_lastRecord.coord = m_lastPosition.coord; - m_lastRecord.bearing = m_lastPosition.bearing; - emit positionRecorded(m_lastRecord.coord); + { + // update tracking data when moving + if (s0 > MOVING_SPEED) + { + m_distance += d; + m_duration += sec.count(); + + if (a0 > 1.0) + { + m_ascent += a0; + m_currentAlt = alt; + } + else if (a0 < -1.0) + { + m_descent -= a0; + m_currentAlt = alt; + } + if (s0 > m_maxSpeed) + m_maxSpeed = s0; + } + m_currentSpeed = s0; + emit dataChanged(m_currentSpeed, m_distance, m_duration, m_ascent, m_descent, m_maxSpeed); + } } + // when we are stationary the direction is calculated according to the azimuth of the device + // otherwise it is estimated according to the progress compared to the previous position + if (m_currentSpeed < COURSE_SPEED) + bearing = osmscout::Bearing::Degrees(m_azimuth); else + bearing = osmscout::Bearing::Radians(osmin::Utils::sphericalBearingFinal(m_lastPosition.coord.GetLat(), m_lastPosition.coord.GetLon(), lat, lon)); + + if (m_recording) { - d = osmin::Utils::sphericalDistance(m_lastRecord.coord.GetLat(), m_lastRecord.coord.GetLon(), lat, lon); - double da = std::fabs(bearing.AsRadians() - m_lastRecord.bearing.AsRadians()); - if (da > M_PI_2) - da = fabs(da - M_PI); - // 0.17rad ~ 20deg - if (d >= RECORD_INTERVAL || (da > 0.17 && d > 5.0)) + if (!m_lastRecord) { record(); m_lastRecord.time = m_lastPosition.time; @@ -333,18 +342,42 @@ void TrackerModule::onLocationChanged(bool positionValid, double lat, double lon m_lastRecord.bearing = m_lastPosition.bearing; emit positionRecorded(m_lastRecord.coord); } + else + { + d = osmin::Utils::sphericalDistance(m_lastRecord.coord.GetLat(), m_lastRecord.coord.GetLon(), lat, lon); + double da = std::fabs(bearing.AsRadians() - m_lastRecord.bearing.AsRadians()); + if (da > M_PI_2) + da = std::fabs(da - M_PI); + // 0.17rad ~ 20deg + if (d >= RECORD_INTERVAL || (da > 0.17 && d > 5.0)) + { + record(); + m_lastRecord.time = m_lastPosition.time; + m_lastRecord.coord = m_lastPosition.coord; + m_lastRecord.bearing = m_lastPosition.bearing; + emit positionRecorded(m_lastRecord.coord); + } + } } } + else + { + bearing = osmscout::Bearing::Degrees(m_azimuth); + m_currentAlt = alt; + m_currentSpeed = 0.0; + } + + m_lastPosition.bearing = bearing; + m_lastPosition.coord.Set(lat, lon); + m_lastPosition.elevation = alt; + m_lastPosition.time = now; } else { - bearing = osmscout::Bearing::Degrees(m_azimuth); + // update azimuth only + m_lastPosition.bearing = osmscout::Bearing::Degrees(m_azimuth); } - m_lastPosition.bearing = bearing; - m_lastPosition.coord.Set(lat, lon); - m_lastPosition.elevation = alt; - m_lastPosition.time = now; emit positionChanged(m_state, m_lastPosition.coord, std::optional(m_lastPosition.bearing)); } diff --git a/src/tracker.h b/src/tracker.h index 725373c4..ea9b40fd 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -189,6 +189,7 @@ public slots: osmscout::PositionAgent::PositionState m_state; double m_magneticDip; double m_azimuth; + double m_currentAlt; double m_currentSpeed; double m_maxSpeed; struct position_t