Skip to content

Commit

Permalink
trying to fix bad data accuracy
Browse files Browse the repository at this point in the history
  • Loading branch information
@janbar
janbar committed Aug 15, 2024
1 parent 22a3e1a commit a78d3a1
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 45 deletions.
123 changes: 78 additions & 45 deletions src/tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -279,72 +282,102 @@ 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<std::chrono::duration<double> >(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<std::chrono::duration<double> >(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;
m_lastRecord.coord = m_lastPosition.coord;
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<osmscout::Bearing>(m_lastPosition.bearing));
}

Expand Down
1 change: 1 addition & 0 deletions src/tracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit a78d3a1

Please sign in to comment.