LPMStat/NmeaReaderTask.cpp at master · a-tk-by/LPMStat · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
#include "NmeaReaderTask.h"
#include "Arduino.h"

#include "ExpressTask.h"
#include "StaticAllocActivator.h"
#include "Globals.h"
#include "Critical.h"
#include "libs/GPS/TinyGPS-mod.h"
#include "Globals.h"

#include "LcdIo.h"
#include <SoftwareSerial.h>
#include "LcdIoTask.h"
static TinyGPS_mod gps;

//static SoftwareSerial NMEA = SoftwareSerial((byte)Pins::NMEA_RX, (byte)Pins::NMEA_TX);
#define NMEA Serial

void UpdateVersion(byte& version);

TASK_BEGIN(NmeaReaderTask,
{
	bool newData;
	int c;
    byte i;
})

NMEA.begin(9600);

for (;;)
{
	TASK_YIELD_WHILE(!NMEA.available());
	while (NMEA.available())
	{
		c = NMEA.read();
#ifdef DISPLAY_NMEA
        Serial.write(c);
#endif
		if (gps.encode(c))
		{
			newData = true;
		}
	}

	if (newData)
	{
		newData = false;

		int year;
		byte month, day, hour, minute, second, hundredths;
		unsigned long age;

		gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
		year -= 2000;

		long lon, lat;
		gps.get_position(&lat, &lon, &age);
		int alt = (gps.altitude() + 50) / 100;

		NmeaData::DateTime = { (byte)year, month, day, hour, minute, second };
		NmeaData::Location = { Angle(lat), Angle(lon), alt };
        UpdateVersion(::NmeaData::Version);
	}
}

TASK_END;

inline void UpdateVersion(byte& version)
{
    if (!++version)
    {
        ++version;
    }
}

bool RegisterNmeaReaderTask(Scheduler& scheduler)
{
	return scheduler.Register(InstanceOnce<NmeaReaderTask>(), TaskPriority::SensorPoll);
}