Signed-off-by: Ferry Huberts <ferry.huberts@pelagic.nl>
/* System includes */
#include <assert.h>
/* System includes */
#include <assert.h>
+#include <math.h>
+#include <string.h>
#include <nmea/info.h>
#include <nmea/sentence.h>
#include <nmea/info.h>
#include <nmea/sentence.h>
sizeof(positionAverageList->counters));
nmea_zero_INFO(&positionAverageList->positionAverageCumulative.nmeaInfo);
sizeof(positionAverageList->counters));
nmea_zero_INFO(&positionAverageList->positionAverageCumulative.nmeaInfo);
+ memset(&positionAverageList->positionAverageCumulative.track, 0, sizeof(positionAverageList->positionAverageCumulative.track));
+ memset(&positionAverageList->positionAverageCumulative.mtrack, 0, sizeof(positionAverageList->positionAverageCumulative.mtrack));
+ memset(&positionAverageList->positionAverageCumulative.magvar, 0, sizeof(positionAverageList->positionAverageCumulative.magvar));
+
nmea_zero_INFO(&positionAverageList->positionAverage.nmeaInfo);
}
nmea_zero_INFO(&positionAverageList->positionAverage.nmeaInfo);
}
- * Adjust the range of the track so that we can correctly average it:
- * <pre>
- * [ 0, 180) --> [ 0, 180)
- * [ 180, 360) --> [-180, 0)
- * </pre>
- * @param track the track to adjust
- * @return the adjusted track
+ * Calculate angle components
+ *
+ * @param components a pointer to the components structure
+ * @param angle a pointer to the angle (in degrees) from which to calculate the components
-static double getAdjustedTrackForAveraging(double track) {
- assert(track >= (double)0.0);
- assert(track < (double)360.0);
-
- if (track >= (double)180.0) {
- return (track - (double)360.0);
- }
+static void calculateAngleComponents(AngleComponents * components, double * angle) {
+ if (!components || !angle)
+ return;
+ components->x = cos(nmea_degree2radian(*angle));
+ components->y = sin(nmea_degree2radian(*angle));
- * Adjust the range of the track after averaging: the reverse of getAdjustedTrackForAveraging
- * <pre>
- * [-180, 0) --> [ 180, 360)
- * [ 0, 180) --> [ 0, 180)
- * </pre>
- * @param track the track to adjust
- * @return the adjusted track
+ * Calculate angle from its components
+ *
+ * @param components a pointer to the components structure
+ * @return angle the angle (in degrees)
-static double getAdjustedTrackAfterAveraging(double track) {
- assert(track >= (double)-180.0);
- assert(track < (double)180.0);
+static double calculateAngle(AngleComponents * components) {
+ if (!components)
+ return 0;
- if (track < (double)0.0) {
- return (track + (double)360.0);
- }
+ return nmea_radian2degree(atan2(components->y, components->x));
+}
+/**
+ * Add the src angle components to the dst angle components (accumulate)
+ *
+ * @param dst a pointer to the destination components structure
+ * @param src a pointer to the source components structure
+ * @param add true to add, false to subtract
+ */
+static void addAngleComponents(AngleComponents * dst, AngleComponents * src, bool add) {
+ if (!dst || !src)
+ return;
+
+ dst->x += add ? src->x : -src->x;
+ src->y += add ? src->y : -src->y;
bool add) {
PositionUpdateEntry * cumulative =
&positionAverageList->positionAverageCumulative;
bool add) {
PositionUpdateEntry * cumulative =
&positionAverageList->positionAverageCumulative;
- double adjustedTrack = getAdjustedTrackForAveraging(entry->nmeaInfo.track);
- double adjustedMTrack = getAdjustedTrackForAveraging(entry->nmeaInfo.mtrack);
if (!add) {
assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
if (!add) {
assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
cumulative->nmeaInfo.lon += add ? entry->nmeaInfo.lon
: -entry->nmeaInfo.lon;
cumulative->nmeaInfo.lon += add ? entry->nmeaInfo.lon
: -entry->nmeaInfo.lon;
- /* elv, speed, track, mtrack, magvar */
cumulative->nmeaInfo.elv += add ? entry->nmeaInfo.elv
: -entry->nmeaInfo.elv;
cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
: -entry->nmeaInfo.speed;
cumulative->nmeaInfo.elv += add ? entry->nmeaInfo.elv
: -entry->nmeaInfo.elv;
cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
: -entry->nmeaInfo.speed;
- cumulative->nmeaInfo.track += add ? adjustedTrack
- : -adjustedTrack;
- cumulative->nmeaInfo.mtrack += add ? adjustedMTrack
- : -adjustedMTrack;
- cumulative->nmeaInfo.magvar += add ? entry->nmeaInfo.magvar
- : -entry->nmeaInfo.magvar;
+ /* track, mtrack, magvar */
+ addAngleComponents(&cumulative->track, &entry->track, add);
+ addAngleComponents(&cumulative->mtrack, &entry->mtrack, add);
+ addAngleComponents(&cumulative->magvar, &entry->magvar, add);
+
+ /* adjust list count */
positionAverageList->entriesCount += (add ? 1 : -1);
updateCounters(positionAverageList, entry, add);
positionAverageList->entriesCount += (add ? 1 : -1);
updateCounters(positionAverageList, entry, add);
PositionAverageList * positionAverageList) {
double divider = positionAverageList->entriesCount;
PositionAverageList * positionAverageList) {
double divider = positionAverageList->entriesCount;
+ double avgTrack = calculateAngle(&positionAverageList->positionAverageCumulative.track);
+ double avgMTrack = calculateAngle(&positionAverageList->positionAverageCumulative.mtrack);
+ double avgMagvar = calculateAngle(&positionAverageList->positionAverageCumulative.magvar);
+
positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
/* smask: use from cumulative average */
positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
/* smask: use from cumulative average */
positionAverageList->positionAverage.nmeaInfo.elv /= divider;
positionAverageList->positionAverage.nmeaInfo.speed /= divider;
positionAverageList->positionAverage.nmeaInfo.elv /= divider;
positionAverageList->positionAverage.nmeaInfo.speed /= divider;
- positionAverageList->positionAverage.nmeaInfo.track /= divider;
- positionAverageList->positionAverage.nmeaInfo.mtrack /= divider;
- positionAverageList->positionAverage.nmeaInfo.magvar /= divider;
- }
- positionAverageList->positionAverage.nmeaInfo.track = getAdjustedTrackAfterAveraging(positionAverageList->positionAverage.nmeaInfo.track);
- positionAverageList->positionAverage.nmeaInfo.mtrack = getAdjustedTrackAfterAveraging(positionAverageList->positionAverage.nmeaInfo.mtrack);
+ positionAverageList->positionAverage.nmeaInfo.track = avgTrack;
+ positionAverageList->positionAverage.nmeaInfo.mtrack = avgMTrack;
+ positionAverageList->positionAverage.nmeaInfo.magvar = avgMagvar;
+ }
/* satinfo: use from average */
}
/* satinfo: use from average */
}
getPositionAverageEntry(positionAverageList, OLDEST), false);
}
getPositionAverageEntry(positionAverageList, OLDEST), false);
}
+ /* calculate the angle components */
+ calculateAngleComponents(&newEntry->track, &newEntry->nmeaInfo.track);
+ calculateAngleComponents(&newEntry->mtrack, &newEntry->nmeaInfo.mtrack);
+ calculateAngleComponents(&newEntry->magvar, &newEntry->nmeaInfo.magvar);
+
/* now just add the new position */
addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
/* now just add the new position */
addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
#include <nmea/info.h>
#include <stdbool.h>
#include <nmea/info.h>
#include <stdbool.h>
+/** Stores angle components */
+typedef struct _AngleComponents {
+ double x; /**< cos of the angle (in radians) */
+ double y; /**< sin of the angle (in radians) */
+} AngleComponents;
+
/** Stores an nmeaINFO entry, used in the averaging */
typedef struct _PositionUpdateEntry {
nmeaINFO nmeaInfo; /**< the position information */
/** Stores an nmeaINFO entry, used in the averaging */
typedef struct _PositionUpdateEntry {
nmeaINFO nmeaInfo; /**< the position information */
+
+ /* used for averaging of angles */
+ AngleComponents track; /**< the track angle components */
+ AngleComponents mtrack; /**< the mtrack angle components */
+ AngleComponents magvar; /**< the magvar angle components */
} PositionUpdateEntry;
/**
} PositionUpdateEntry;
/**