1 | #include "gps.h"
|
2 |
|
3 | using namespace gps;
|
4 |
|
5 | Gps::Gps(USARTSerial* serial) {
|
6 | _gpsSerial = serial;
|
7 | _is_connected = false;
|
8 | _reading_status = false;
|
9 | };
|
10 |
|
11 | bool Gps::Connect(uint8_t baudrate) {
|
12 | //Log.info("Connect");
|
13 | //_gpsSerial->begin(baudrate);
|
14 | Serial1.begin(baudrate);
|
15 | _is_connected = true;
|
16 | return true;
|
17 | };
|
18 |
|
19 | void Gps::defaultMessageCallback(void (*messageCallback)(uint8_t message, size_t logID))
|
20 | {
|
21 | _messageCallback = messageCallback;
|
22 | };
|
23 |
|
24 | void Gps::StopReading() {
|
25 | Log.info("StopReading");
|
26 | _reading_status = false;
|
27 | };
|
28 |
|
29 | void Gps::ReadSerialPort() {
|
30 | Log.info("ReadSerialPort");
|
31 | _reading_status = true;
|
32 |
|
33 | uint8_t buffer[MAX_NOUT_SIZE];
|
34 | size_t len = 0;
|
35 | char incomingByte = 0;
|
36 |
|
37 | while (Serial1.available() && _reading_status) {
|
38 | //incomingByte = _gpsSerial->read();
|
39 | incomingByte = Serial1.read();
|
40 | buffer[len] = incomingByte;
|
41 | len++;
|
42 |
|
43 | if (len == MAX_NOUT_SIZE)
|
44 | break;
|
45 | }
|
46 |
|
47 | if (len > 0) {
|
48 | _BufferIncomingData(buffer, len);
|
49 | }
|
50 | else {
|
51 | Log.info("no data");
|
52 | };
|
53 |
|
54 | Serial.println("-----");
|
55 | return;
|
56 | };
|
57 |
|
58 | bool Gps::_SendMessage(uint8_t *msg_ptr, size_t length) {
|
59 | Log.info("_SendMessage");
|
60 |
|
61 | /*for (unsigned int i = 0; i < length; i++) {
|
62 | Serial.print(msg_ptr[i], HEX);
|
63 | Serial.print("-");
|
64 | };*/
|
65 |
|
66 | size_t bytes_written;
|
67 |
|
68 | //bytes_written=_gpsSerial->write(msg_ptr, length);
|
69 | bytes_written=Serial1.write(msg_ptr, length);
|
70 | //_gpsSerial->flush();
|
71 | Serial1.flush();
|
72 | //delay(50);
|
73 | // check that full message was sent to serial port
|
74 | if (bytes_written != length) {
|
75 | Log.error("Full message was not sent over serial port.");
|
76 | return false;
|
77 | }
|
78 | return true;
|
79 | };
|
80 |
|
81 | bool Gps::_ConfigureOutputFormat(OutputType output_type) {
|
82 | Log.info("_ConfigureOutputFormat");
|
83 | bool send_result;
|
84 |
|
85 | gps::ConfigureOutputFormat message;
|
86 | message.header.sync1 = SKYTRAQ_SYNC_BYTE_1;
|
87 | message.header.sync2 = SKYTRAQ_SYNC_BYTE_2;
|
88 | message.header.first_payload_byte = FIRST_PAYLOAD_BYTE;
|
89 | message.header.payload_length = CONFIGURE_OUTPUT_FORMAT_PAYLOAD_LENGTH;
|
90 | message.message_id = CFG_OUTPUT_FORMAT;
|
91 | message.type = output_type;
|
92 | message.footer.end1 = SKYTRAQ_END_BYTE_1;
|
93 | message.footer.end2 = SKYTRAQ_END_BYTE_2;
|
94 |
|
95 | //unsigned char* msg_ptr = (unsigned char*)&message;
|
96 | //_calculateCheckSum(msg_ptr+HEADER_LENGTH, CONFIGURE_OUTPUT_FORMAT_PAYLOAD_LENGTH,
|
97 | // &message.footer.checksum);
|
98 | uint8_t msg_ptr[] = {160, 161, 0, 2, 9, 2, 11, 13, 10};
|
99 | send_result = _SendMessage(msg_ptr,HEADER_LENGTH+CONFIGURE_OUTPUT_FORMAT_PAYLOAD_LENGTH+FOOTER_LENGTH);
|
100 | return send_result;
|
101 | };
|
102 |
|
103 | bool Gps::SetOutputFormatToBinary() {
|
104 | Log.info("SetOutputFormatToBinary");
|
105 |
|
106 | bool send_result;
|
107 | send_result = _ConfigureOutputFormat(BINARY_OUTPUT);
|
108 | return send_result;
|
109 | };
|
110 |
|
111 | bool Gps::QuerySoftwareVersion() {
|
112 | bool send_result;
|
113 |
|
114 | gps::QuerySoftwareVersion message;
|
115 | message.header.sync1 = SKYTRAQ_SYNC_BYTE_1;
|
116 | message.header.sync2 = SKYTRAQ_SYNC_BYTE_2;
|
117 | message.header.first_payload_byte = FIRST_PAYLOAD_BYTE;
|
118 | message.header.payload_length = QUERY_SOFTWARE_VERSION_PAYLOAD_LENGTH;
|
119 | message.message_id = QUERY_SOFTWARE_VERSION;
|
120 | message.software_type = SYSTEM_CODE;
|
121 | message.footer.end1 = SKYTRAQ_END_BYTE_1;
|
122 | message.footer.end2 = SKYTRAQ_END_BYTE_2;
|
123 |
|
124 | uint8_t msg_ptr[] = {160, 161, 0, 2, 2, 1, 3, 13, 10};
|
125 | //unsigned char* msg_ptr = (unsigned char*)&message;
|
126 | //_calculateCheckSum(msg_ptr+HEADER_LENGTH, QUERY_SOFTWARE_VERSION_PAYLOAD_LENGTH,
|
127 | // &message.footer.checksum);
|
128 |
|
129 | send_result = _SendMessage(msg_ptr,HEADER_LENGTH+QUERY_SOFTWARE_VERSION_PAYLOAD_LENGTH+FOOTER_LENGTH);
|
130 | return send_result;
|
131 | };
|
