Firmware SDK
twr_sam_m8q.c
1 #include <twr_sam_m8q.h>
2 #include <twr_gpio.h>
3 #include <minmea.h>
4 
5 static void _twr_sam_m8q_task(void *param);
6 static bool _twr_sam_m8q_parse(twr_sam_m8q_t *self, const char *line);
7 static bool _twr_sam_m8q_feed(twr_sam_m8q_t *self, char c);
8 static void _twr_sam_m8q_clear(twr_sam_m8q_t *self);
9 static bool _twr_sam_m8q_enable(twr_sam_m8q_t *self);
10 static bool _twr_sam_m8q_disable(twr_sam_m8q_t *self);
11 static bool _twr_sam_m8q_send_config(twr_sam_m8q_t *self);
12 
13 void twr_sam_m8q_init(twr_sam_m8q_t *self, twr_i2c_channel_t channel, uint8_t i2c_address, const twr_sam_m8q_driver_t *driver)
14 {
15  memset(self, 0, sizeof(twr_sam_m8q_t));
16 
18 
19  self->_i2c_channel = channel;
20  self->_i2c_address = i2c_address;
21  self->_driver = driver;
22 
23  self->_task_id = twr_scheduler_register(_twr_sam_m8q_task, self, TWR_TICK_INFINITY);
24 }
25 
26 void twr_sam_m8q_set_event_handler(twr_sam_m8q_t *self, twr_sam_m8q_event_handler_t event_handler, void *event_param)
27 {
28  self->_event_handler = event_handler;
29  self->_event_param = event_param;
30 }
31 
33 {
34  if (!self->_running)
35  {
36  self->_running = true;
37  self->_configured = false;
38 
39  twr_scheduler_plan_now(self->_task_id);
40  }
41 }
42 
44 {
45  if (self->_running)
46  {
47  self->_running = false;
48 
49  twr_scheduler_plan_now(self->_task_id);
50  }
51 }
52 
54 {
55  self->_rmc.valid = false;
56  self->_gga.valid = false;
57  self->_pubx.valid = false;
58 }
59 
61 {
62  memset(time, 0, sizeof(*time));
63 
64  if (!self->_rmc.valid)
65  {
66  return false;
67  }
68 
69  time->year = self->_rmc.date.year + 2000;
70  time->month = self->_rmc.date.month;
71  time->day = self->_rmc.date.day;
72  time->hours = self->_rmc.time.hours;
73  time->minutes = self->_rmc.time.minutes;
74  time->seconds = self->_rmc.time.seconds;
75 
76  return true;
77 }
78 
80 {
81  memset(position, 0, sizeof(*position));
82 
83  if (!self->_rmc.valid)
84  {
85  return false;
86  }
87 
88  position->latitude = self->_rmc.latitude;
89  position->longitude = self->_rmc.longitude;
90 
91  return true;
92 }
93 
95 {
96  memset(altitude, 0, sizeof(*altitude));
97 
98  if (!self->_gga.valid || self->_gga.fix_quality < 1)
99  {
100  return false;
101  }
102 
103  altitude->altitude = self->_gga.altitude;
104  altitude->units = self->_gga.altitude_units;
105 
106  return true;
107 }
108 
110 {
111  memset(quality, 0, sizeof(*quality));
112 
113  if (!self->_gga.valid || !self->_pubx.valid)
114  {
115  return false;
116  }
117 
118  quality->fix_quality = self->_gga.fix_quality;
119  quality->satellites_tracked = self->_pubx.satellites;
120 
121  return true;
122 }
123 
125 {
126  memset(accuracy, 0, sizeof(*accuracy));
127 
128  if (!self->_pubx.valid || self->_gga.fix_quality < 1)
129  {
130  return false;
131  }
132 
133  accuracy->horizontal = self->_pubx.h_accuracy;
134  accuracy->vertical = self->_pubx.v_accuracy;
135 
136  return true;
137 }
138 
139 static void _twr_sam_m8q_task(void *param)
140 {
141  twr_sam_m8q_t *self = param;
142 
143  if (!self->_running)
144  {
145  self->_state = TWR_SAM_M8Q_STATE_STOP;
146  }
147 
148  if (self->_running && self->_state == TWR_SAM_M8Q_STATE_STOP)
149  {
150  self->_state = TWR_SAM_M8Q_STATE_START;
151  }
152 
153 start:
154 
155  switch (self->_state)
156  {
157  case TWR_SAM_M8Q_STATE_ERROR:
158  {
159  if (self->_event_handler != NULL)
160  {
161  self->_event_handler(self, TWR_SAM_M8Q_EVENT_ERROR, self->_event_param);
162  }
163 
164  self->_state = TWR_SAM_M8Q_STATE_STOP;
165 
166  goto start;
167  }
168  case TWR_SAM_M8Q_STATE_START:
169  {
170  if (!_twr_sam_m8q_enable(self))
171  {
172  self->_state = TWR_SAM_M8Q_STATE_ERROR;
173 
174  goto start;
175  }
176 
177  _twr_sam_m8q_clear(self);
178 
179  if (self->_event_handler != NULL)
180  {
181  self->_event_handler(self, TWR_SAM_M8Q_EVENT_START, self->_event_param);
182  }
183 
184  self->_state = TWR_SAM_M8Q_STATE_READ;
185 
187 
188  break;
189  }
190  case TWR_SAM_M8Q_STATE_READ:
191  {
192  uint16_t bytes_available;
193 
194  if (!twr_i2c_memory_read_16b(self->_i2c_channel, self->_i2c_address, 0xfd, &bytes_available))
195  {
196  self->_state = TWR_SAM_M8Q_STATE_ERROR;
197 
198  goto start;
199  }
200 
201  while (bytes_available != 0)
202  {
203  self->_ddc_length = bytes_available;
204 
205  if (self->_ddc_length > sizeof(self->_ddc_buffer))
206  {
207  self->_ddc_length = sizeof(self->_ddc_buffer);
208  }
209 
210  memset(self->_ddc_buffer, 0, sizeof(self->_ddc_buffer));
211 
212  twr_i2c_memory_transfer_t transfer;
213 
214  transfer.device_address = self->_i2c_address;
215  transfer.memory_address = 0xff;
216  transfer.buffer = self->_ddc_buffer;
217  transfer.length = self->_ddc_length;
218 
219  if (!twr_i2c_memory_read(self->_i2c_channel, &transfer))
220  {
221  self->_state = TWR_SAM_M8Q_STATE_ERROR;
222 
223  goto start;
224  }
225 
226  for (size_t i = 0; i < self->_ddc_length; i++)
227  {
228  if (_twr_sam_m8q_feed(self, self->_ddc_buffer[i]))
229  {
230  self->_state = TWR_SAM_M8Q_STATE_UPDATE;
231  }
232  }
233 
234  bytes_available -= self->_ddc_length;
235  }
236 
237  if (self->_state == TWR_SAM_M8Q_STATE_UPDATE)
238  {
239  goto start;
240  }
241 
243 
244  break;
245  }
246  case TWR_SAM_M8Q_STATE_UPDATE:
247  {
248  self->_state = TWR_SAM_M8Q_STATE_READ;
249 
251 
252  if (self->_event_handler != NULL)
253  {
254  self->_event_handler(self, TWR_SAM_M8Q_EVENT_UPDATE, self->_event_param);
255  }
256 
257  goto start;
258  }
259  case TWR_SAM_M8Q_STATE_STOP:
260  {
261  self->_running = false;
262 
263  if (!_twr_sam_m8q_disable(self))
264  {
265  self->_state = TWR_SAM_M8Q_STATE_ERROR;
266 
267  goto start;
268  }
269 
270  if (self->_event_handler != NULL)
271  {
272  self->_event_handler(self, TWR_SAM_M8Q_EVENT_STOP, self->_event_param);
273  }
274 
275  break;
276  }
277  default:
278  {
279  self->_state = TWR_SAM_M8Q_STATE_ERROR;
280 
281  goto start;
282  }
283  }
284 }
285 
286 static bool _twr_sam_m8q_parse(twr_sam_m8q_t *self, const char *line)
287 {
288  bool ret = false;
289 
290  enum minmea_sentence_id id = minmea_sentence_id(line, true);
291 
292  if (id == MINMEA_SENTENCE_RMC)
293  {
294  struct minmea_sentence_rmc frame;
295 
296  if (minmea_parse_rmc(&frame, line))
297  {
298  if (frame.valid)
299  {
300  self->_rmc.date.year = frame.date.year;
301  self->_rmc.date.month = frame.date.month;
302  self->_rmc.date.day = frame.date.day;
303  self->_rmc.time.hours = frame.time.hours;
304  self->_rmc.time.minutes = frame.time.minutes;
305  self->_rmc.time.seconds = frame.time.seconds;
306  self->_rmc.latitude = minmea_tocoord(&frame.latitude);
307  self->_rmc.longitude = minmea_tocoord(&frame.longitude);
308  self->_rmc.valid = true;
309 
310  ret = true;
311  }
312  }
313  }
314  else if (id == MINMEA_SENTENCE_GGA)
315  {
316  struct minmea_sentence_gga frame;
317 
318  if (minmea_parse_gga(&frame, line))
319  {
320  self->_gga.fix_quality = frame.fix_quality;
321  self->_gga.altitude = minmea_tofloat(&frame.altitude);
322  self->_gga.altitude_units = frame.altitude_units;
323  self->_gga.valid = true;
324 
325  ret = true;
326  }
327  }
328  else if (id == MINMEA_SENTENCE_PUBX)
329  {
330  struct minmea_sentence_pubx frame;
331 
332  if (minmea_parse_pubx(&frame, line))
333  {
334  self->_pubx.h_accuracy = minmea_tofloat(&frame.h_accuracy);
335  self->_pubx.v_accuracy = minmea_tofloat(&frame.v_accuracy);
336  self->_pubx.speed = minmea_tofloat(&frame.speed);
337  self->_pubx.course = minmea_tofloat(&frame.course);
338  self->_pubx.satellites = frame.satellites;
339  self->_pubx.valid = true;
340 
341  ret = true;
342  }
343  }
344 
345  return ret;
346 }
347 
348 static bool _twr_sam_m8q_feed(twr_sam_m8q_t *self, char c)
349 {
350  bool ret = false;
351 
352  if (c == '\r' || c == '\n')
353  {
354  if (self->_line_length != 0)
355  {
356  if (!self->_line_clipped)
357  {
358  if (_twr_sam_m8q_parse(self, self->_line_buffer))
359  {
360  ret = true;
361  }
362  }
363 
364  _twr_sam_m8q_clear(self);
365 
366  if (!self->_configured)
367  {
368  if (_twr_sam_m8q_send_config(self))
369  {
370  self->_configured = true;
371  }
372  }
373  }
374  }
375  else
376  {
377  if (self->_line_length < sizeof(self->_line_buffer) - 1)
378  {
379  self->_line_buffer[self->_line_length++] = c;
380  }
381  else
382  {
383  self->_line_clipped = true;
384  }
385  }
386 
387  return ret;
388 }
389 
390 static void _twr_sam_m8q_clear(twr_sam_m8q_t *self)
391 {
392  memset(self->_line_buffer, 0, sizeof(self->_line_buffer));
393 
394  self->_line_clipped = false;
395  self->_line_length = 0;
396 }
397 
398 static bool _twr_sam_m8q_enable(twr_sam_m8q_t *self)
399 {
400  if (self->_driver != NULL)
401  {
402  if (!self->_driver->on(self))
403  {
404  return false;
405  }
406  }
407 
408  return true;
409 }
410 
411 static bool _twr_sam_m8q_disable(twr_sam_m8q_t *self)
412 {
413  if (self->_driver != NULL)
414  {
415  if (!self->_driver->off(self))
416  {
417  return false;
418  }
419  }
420 
421  return true;
422 }
423 
424 static bool _twr_sam_m8q_send_config(twr_sam_m8q_t *self)
425 {
426  // Enable PUBX POSITION message
427  uint8_t config_msg_pubx[] = {
428  0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xf1, 0x00,
429  0x01, 0x01, 0x00, 0x01, 0x01, 0x00, 0x04, 0x3b
430  };
431  twr_i2c_transfer_t transfer;
432 
433  transfer.device_address = self->_i2c_address;
434  transfer.buffer = config_msg_pubx;
435  transfer.length = sizeof(config_msg_pubx);
436 
437  if (!twr_i2c_write(self->_i2c_channel, &transfer))
438  {
439  return false;
440  }
441 
442  // Disable GSA message
443  uint8_t config_msg_gsa[] = {
444  0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xf0, 0x02,
445  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x31,
446  };
447 
448  transfer.device_address = self->_i2c_address;
449  transfer.buffer = config_msg_gsa;
450  transfer.length = sizeof(config_msg_gsa);
451 
452  if (!twr_i2c_write(self->_i2c_channel, &transfer))
453  {
454  return false;
455  }
456 
457  // Disable GSV message
458  uint8_t config_msg_gsv[] = {
459  0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xf0, 0x03,
460  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x38,
461  };
462 
463  transfer.device_address = self->_i2c_address;
464  transfer.buffer = config_msg_gsv;
465  transfer.length = sizeof(config_msg_gsv);
466 
467  if (!twr_i2c_write(self->_i2c_channel, &transfer))
468  {
469  return false;
470  }
471 
472  // Enable Galileo
473  uint8_t config_gnss[] = {
474  0xb5, 0x62, 0x06, 0x3e, 0x3c, 0x00, 0x00, 0x20,
475  0x20, 0x07, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00,
476  0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x01, 0x00,
477  0x01, 0x01, 0x02, 0x04, 0x08, 0x00, 0x01, 0x00,
478  0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00,
479  0x01, 0x01, 0x04, 0x00, 0x08, 0x00, 0x00, 0x00,
480  0x01, 0x03, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00,
481  0x01, 0x05, 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00,
482  0x01, 0x01, 0x55, 0x47,
483  };
484 
485  transfer.device_address = self->_i2c_address;
486  transfer.buffer = config_gnss;
487  transfer.length = sizeof(config_gnss);
488 
489  if (!twr_i2c_write(self->_i2c_channel, &transfer))
490  {
491  return false;
492  }
493 
494  // Set NMEA version to 4.1
495  uint8_t config_nmea[] = {
496  0xb5, 0x62, 0x06, 0x17, 0x14, 0x00, 0x00, 0x41,
497  0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
498  0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
499  0x00, 0x00, 0x75, 0x57,
500  };
501 
502  transfer.device_address = self->_i2c_address;
503  transfer.buffer = config_nmea;
504  transfer.length = sizeof(config_nmea);
505 
506  if (!twr_i2c_write(self->_i2c_channel, &transfer))
507  {
508  return false;
509  }
510 
511  return true;
512 }
void twr_i2c_init(twr_i2c_channel_t channel, twr_i2c_speed_t speed)
Initialize I2C channel.
Definition: twr_i2c.c:57
bool twr_i2c_memory_read(twr_i2c_channel_t channel, const twr_i2c_memory_transfer_t *transfer)
Memory read from I2C channel.
Definition: twr_i2c.c:371
bool twr_i2c_memory_read_16b(twr_i2c_channel_t channel, uint8_t device_address, uint32_t memory_address, uint16_t *data)
Memory read 2 bytes from I2C channel.
Definition: twr_i2c.c:449
bool twr_i2c_write(twr_i2c_channel_t channel, const twr_i2c_transfer_t *transfer)
Write to I2C channel.
Definition: twr_i2c.c:243
twr_i2c_channel_t
I2C channels.
Definition: twr_i2c.h:16
@ TWR_I2C_SPEED_100_KHZ
I2C communication speed is 100 kHz.
Definition: twr_i2c.h:33
@ TWR_I2C_I2C0
I2C channel I2C0.
Definition: twr_i2c.h:18
void twr_sam_m8q_stop(twr_sam_m8q_t *self)
Stop navigation module.
Definition: twr_sam_m8q.c:43
void twr_sam_m8q_invalidate(twr_sam_m8q_t *self)
Invalidate navigation data.
Definition: twr_sam_m8q.c:53
void twr_sam_m8q_start(twr_sam_m8q_t *self)
Start navigation module.
Definition: twr_sam_m8q.c:32
float longitude
Longitude.
Definition: twr_sam_m8q.h:77
void twr_sam_m8q_init(twr_sam_m8q_t *self, twr_i2c_channel_t channel, uint8_t i2c_address, const twr_sam_m8q_driver_t *driver)
Initialize SAM-M8Q module driver.
Definition: twr_sam_m8q.c:13
void twr_sam_m8q_set_event_handler(twr_sam_m8q_t *self, twr_sam_m8q_event_handler_t event_handler, void *event_param)
Set callback function.
Definition: twr_sam_m8q.c:26
int minutes
Minutes.
Definition: twr_sam_m8q.h:62
int seconds
Seconds.
Definition: twr_sam_m8q.h:65
float latitude
Latitude.
Definition: twr_sam_m8q.h:74
float horizontal
Horizontal accuracy estimate.
Definition: twr_sam_m8q.h:110
float altitude
Altitude.
Definition: twr_sam_m8q.h:86
int satellites_tracked
Number of satellites tracked.
Definition: twr_sam_m8q.h:101
bool twr_sam_m8q_get_quality(twr_sam_m8q_t *self, twr_sam_m8q_quality_t *quality)
Get quality.
Definition: twr_sam_m8q.c:109
bool twr_sam_m8q_get_accuracy(twr_sam_m8q_t *self, twr_sam_m8q_accuracy_t *accuracy)
Get accuracy.
Definition: twr_sam_m8q.c:124
struct twr_sam_m8q_t twr_sam_m8q_t
SAM-M8Q instance.
Definition: twr_sam_m8q.h:31
float vertical
Vertical accuracy estimate.
Definition: twr_sam_m8q.h:113
bool twr_sam_m8q_get_time(twr_sam_m8q_t *self, twr_sam_m8q_time_t *time)
Get time.
Definition: twr_sam_m8q.c:60
bool twr_sam_m8q_get_position(twr_sam_m8q_t *self, twr_sam_m8q_position_t *position)
Get position.
Definition: twr_sam_m8q.c:79
int fix_quality
Fix quality.
Definition: twr_sam_m8q.h:98
char units
Units of altitude.
Definition: twr_sam_m8q.h:89
bool twr_sam_m8q_get_altitude(twr_sam_m8q_t *self, twr_sam_m8q_altitude_t *altitude)
Get altitude.
Definition: twr_sam_m8q.c:94
@ TWR_SAM_M8Q_EVENT_ERROR
Error event.
Definition: twr_sam_m8q.h:16
@ TWR_SAM_M8Q_EVENT_STOP
Stop event.
Definition: twr_sam_m8q.h:25
@ TWR_SAM_M8Q_EVENT_START
Start event.
Definition: twr_sam_m8q.h:19
@ TWR_SAM_M8Q_EVENT_UPDATE
Update event.
Definition: twr_sam_m8q.h:22
void twr_scheduler_plan_current_relative(twr_tick_t tick)
Schedule current task to tick relative from current spin.
void twr_scheduler_plan_now(twr_scheduler_task_id_t task_id)
Schedule specified task for immediate execution.
twr_scheduler_task_id_t twr_scheduler_register(void(*task)(void *), void *param, twr_tick_t tick)
Register task in scheduler.
Definition: twr_scheduler.c:53
#define TWR_TICK_INFINITY
Maximum timestamp value.
Definition: twr_tick.h:12
I2C memory transfer parameters.
Definition: twr_i2c.h:58
uint32_t memory_address
8-bit I2C memory address (it can be extended to 16-bit format if OR-ed with TWR_I2C_MEMORY_ADDRESS_16...
Definition: twr_i2c.h:63
uint8_t device_address
7-bit I2C device address
Definition: twr_i2c.h:60
size_t length
Length of buffer which is being written or read.
Definition: twr_i2c.h:69
void * buffer
Pointer to buffer which is being written or read.
Definition: twr_i2c.h:66
I2C transfer parameters.
Definition: twr_i2c.h:43
void * buffer
Pointer to buffer which is being written or read.
Definition: twr_i2c.h:48
uint8_t device_address
7-bit I2C device address
Definition: twr_i2c.h:45
size_t length
Length of buffer which is being written or read.
Definition: twr_i2c.h:51
Accuracy data structure.
Definition: twr_sam_m8q.h:108
Altitude data structure.
Definition: twr_sam_m8q.h:84
SAM-M8Q driver.
Definition: twr_sam_m8q.h:36
Position data structure.
Definition: twr_sam_m8q.h:72
Quality data structure.
Definition: twr_sam_m8q.h:96
Time data structure.
Definition: twr_sam_m8q.h:48