forked from cgwood/ArdustationMega
-
Notifications
You must be signed in to change notification settings - Fork 0
/
GPS.ino
176 lines (150 loc) · 4.68 KB
/
GPS.ino
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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//#include <FastSerial.h>
#define GPS_DEBUGGING 0
#if GPS_DEBUGGING
//#include <FastSerial.h>
# define Debug(fmt, args ...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); delay(0); } while(0)
#else
# define Debug(fmt, args ...)
#endif
// #include <AP_Common.h>
// #include <AP_Math.h>
#include "GPS.h"
//#if defined(ARDUINO) && ARDUINO >= 100
// #include "Arduino.h"
//#else
// #include "WProgram.h"
//#endif
void
GPS::update(void)
{
bool result;
uint32_t tnow;
// call the GPS driver to process incoming data
result = read();
tnow = millis();
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((tnow - _idleTimer) > idleTimeout) {
// Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
Serial.print("gps read timeout ");
Serial.print((unsigned long)tnow);
Serial.print(", ");
Serial.println((unsigned long)_idleTimer);
_status = NO_GPS;
init(_nav_setting);
// reset the idle timer
_idleTimer = tnow;
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
valid_read = true;
new_data = true;
// reset the idle timer
_idleTimer = tnow;
if (_status == GPS_OK) {
last_fix_time = _idleTimer;
_last_ground_speed_cm = ground_speed;
if (_have_raw_velocity) {
// the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01;
_velocity_east = _vel_east * 0.01;
_velocity_down = _vel_down * 0.01;
} else {
float gps_heading = (ground_course * 0.01)*3.14159/180.0;
float gps_speed = ground_speed * 0.01;
float sin_heading, cos_heading;
cos_heading = cos(gps_heading);
sin_heading = sin(gps_heading);
_velocity_north = gps_speed * cos_heading;
_velocity_east = gps_speed * sin_heading;
// no good way to get descent rate
_velocity_down = 0;
}
}
// Update the ASM GPS data
ASM.lat = (float)g_gps->latitude / T7;
ASM.lon = (float)g_gps->longitude / T7;
ASM.num_sats = g_gps->num_sats;
ASM.altitude = g_gps->altitude;
ASM.time = g_gps->time + nvram.nv.gpsTimezone*3600000;
// Serial.print(g_gps->time);
// Serial.print(" ");
// Serial.print(nvram.nv.gpsTimezone*3600000);
// Serial.print(" ");
// Serial.println(ASM.time);
ASM.gps_status = _status;
}
}
void
GPS::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{
}
// XXX this is probably the wrong way to do it, too
void
GPS::_error(const char *msg)
{
Serial.println(msg);
}
///
/// write a block of configuration data to a GPS
///
void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size)
{
while (size--) {
_fs->write(pgm_read_byte(pstr++));
}
}
/*
a prog_char block queue, used to send out config commands to a GPS
in 16 byte chunks. This saves us having to have a 128 byte GPS send
buffer, while allowing us to avoid a long delay in sending GPS init
strings while waiting for the GPS auto detection to happen
*/
// maximum number of pending progstrings
#define PROGSTR_QUEUE_SIZE 3
struct progstr_queue {
const prog_char *pstr;
uint8_t ofs, size;
};
static struct {
Stream *fs;
uint8_t queue_size;
uint8_t idx, next_idx;
struct progstr_queue queue[PROGSTR_QUEUE_SIZE];
} progstr_state;
void GPS::_send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size)
{
progstr_state.fs = (Stream *)_fs;
struct progstr_queue *q = &progstr_state.queue[progstr_state.next_idx];
q->pstr = pstr;
q->size = size;
q->ofs = 0;
progstr_state.next_idx++;
if (progstr_state.next_idx == PROGSTR_QUEUE_SIZE) {
progstr_state.next_idx = 0;
}
}
void GPS::_update_progstr(void)
{
struct progstr_queue *q = &progstr_state.queue[progstr_state.idx];
// quick return if nothing to do
// if (q->size == 0 || progstr_state.fs->available() == 0) { //->tx_pending()) {
// return;
// }
uint8_t nbytes = q->size - q->ofs;
if (nbytes > 16) {
nbytes = 16;
}
_write_progstr_block(progstr_state.fs, q->pstr+q->ofs, nbytes);
q->ofs += nbytes;
if (q->ofs == q->size) {
q->size = 0;
progstr_state.idx++;
if (progstr_state.idx == PROGSTR_QUEUE_SIZE) {
progstr_state.idx = 0;
}
}
}