Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
gps.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
40 #include "modules/core/abi.h"
41 #include "modules/gps/gps.h"
42 #include "led.h"
43 #include "modules/core/settings.h"
44 #include "generated/settings.h"
46 #include "math/pprz_geodetic.h"
47 
48 #ifndef PRIMARY_GPS
49 #error "PRIMARY_GPS not set!"
50 #else
51 PRINT_CONFIG_VAR(PRIMARY_GPS)
52 #endif
53 
54 #ifdef SECONDARY_GPS
55 PRINT_CONFIG_VAR(SECONDARY_GPS)
56 #endif
57 
58 #ifdef GPS_POWER_GPIO
59 #include "mcu_periph/gpio.h"
60 
61 #ifndef GPS_POWER_GPIO_ON
62 #define GPS_POWER_GPIO_ON gpio_set
63 #endif
64 #endif
65 
66 #define MSEC_PER_WEEK (1000*60*60*24*7)
67 #define TIME_TO_SWITCH 5000 //ten s in ms
68 
69 struct GpsState gps;
72 struct RtcmMan rtcm_man;
73 
74 #ifdef SECONDARY_GPS
75 static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
76 #endif
77 
79 
80 #if PREFLIGHT_CHECKS
81 /* Preflight checks */
83 static struct preflight_check_t gps_pfc;
84 
85 static void gps_preflight(struct preflight_result_t *result) {
86  if(!gps_fix_valid()) {
87  preflight_error(result, "No valid GPS fix");
88  } else {
89  preflight_success(result, "GPS fix ok");
90  }
91 }
92 #endif // PREFLIGHT_CHECKS
93 
94 
95 #if PERIODIC_TELEMETRY
97 
98 static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev,
99  uint8_t svid)
100 {
101  if (svid < GPS_NB_CHANNELS) {
102  pprz_msg_send_SVINFO(trans, dev, AC_ID, &svid,
103  &gps.svinfos[svid].svid, &gps.svinfos[svid].flags,
104  &gps.svinfos[svid].qi, &gps.svinfos[svid].cno,
105  &gps.svinfos[svid].elev, &gps.svinfos[svid].azim);
106  }
107 }
108 
110 static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
111 {
112  static uint8_t i = 0;
113  if (i >= gps.nb_channels) { i = 0; }
114  send_svinfo_id(trans, dev, i);
115  i++;
116 }
117 
122 static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
123 {
124  static uint8_t i = 0;
125  static uint8_t last_cnos[GPS_NB_CHANNELS];
126  if (i >= gps.nb_channels) { i = 0; }
127  // send SVINFO for all satellites while no GPS fix,
128  // after 3D fix, send avialable sats if they were updated
129  if (gps.fix < GPS_FIX_3D) {
130  send_svinfo_id(trans, dev, i);
131  } else if (gps.svinfos[i].cno != last_cnos[i]) {
132  send_svinfo_id(trans, dev, i);
133  last_cnos[i] = gps.svinfos[i].cno;
134  }
135  i++;
136 }
137 
138 static void send_gps(struct transport_tx *trans, struct link_device *dev)
139 {
140  uint8_t zero = 0;
141  int16_t climb = -gps.ned_vel.z;
142  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
143  struct UtmCoor_i utm = utm_int_from_gps(&gps, 0);
144 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
145  // broadcast GPS message
146  struct pprzlink_msg msg;
147  msg.trans = trans;
148  msg.dev = dev;
149  msg.sender_id = AC_ID;
150  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
151  msg.component_id = 0;
152  pprzlink_msg_send_GPS(&msg,
153 #else
154  pprz_msg_send_GPS(trans, dev, AC_ID,
155 #endif
156  &gps.fix,
157  &utm.east, &utm.north,
158  &course, &gps.hmsl, &gps.gspeed, &climb,
159  &gps.week, &gps.tow, &utm.zone, &zero);
160  // send SVINFO for available satellites that have new data
161  send_svinfo_available(trans, dev);
162 }
163 
164 static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
165 {
166  pprz_msg_send_GPS_RTK(trans, dev, AC_ID,
176 }
177 
178 static void send_gps_rxmrtcm(struct transport_tx *trans, struct link_device *dev)
179 {
180  pprz_msg_send_GPS_RXMRTCM(trans, dev, AC_ID,
181  &rtcm_man.Cnt105,
182  &rtcm_man.Cnt177,
183  &rtcm_man.Cnt187,
184  &rtcm_man.Crc105,
185  &rtcm_man.Crc177,
186  &rtcm_man.Crc187);
187 }
188 
189 static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
190 {
191 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
192  // broadcast GPS message
193  struct pprzlink_msg msg;
194  msg.trans = trans;
195  msg.dev = dev;
196  msg.sender_id = AC_ID;
197  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
198  msg.component_id = 0;
199  pprzlink_msg_send_GPS_INT(&msg,
200 #else
201  pprz_msg_send_GPS_INT(trans, dev, AC_ID,
202 #endif
205  &gps.hmsl,
207  &gps.pacc, &gps.sacc,
208  &gps.tow,
209  &gps.pdop,
210  &gps.num_sv,
211  &gps.fix,
212  &gps.comp_id);
213  // send SVINFO for available satellites that have new data
214  send_svinfo_available(trans, dev);
215 }
216 
217 static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
218 {
219  uint8_t err = 0;
220  int16_t climb = -gps.ned_vel.z;
221  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
222 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
223  // broadcast GPS message
224  struct pprzlink_msg msg;
225  msg.trans = trans;
226  msg.dev = dev;
227  msg.sender_id = AC_ID;
228  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
229  msg.component_id = 0;
230  pprzlink_msg_send_GPS_LLA(&msg,
231 #else
232  pprz_msg_send_GPS_LLA(trans, dev, AC_ID,
233 #endif
235  &gps.hmsl, &course, &gps.gspeed, &climb,
236  &gps.week, &gps.tow,
237  &gps.fix, &err);
238 }
239 
240 static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
241 {
242  pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
243 }
244 #endif
245 
246 
247 #ifdef SECONDARY_GPS
248 static uint8_t gps_multi_switch(struct GpsState *gps_s)
249 {
250  static uint32_t time_since_last_gps_switch = 0;
251 
253  return GpsId(PRIMARY_GPS);
254  } else if (multi_gps_mode == GPS_MODE_SECONDARY) {
255  return GpsId(SECONDARY_GPS);
256  } else {
257  if (gps_s->fix > gps.fix) {
258  return gps_s->comp_id;
259  } else if (gps.fix > gps_s->fix) {
260  return gps.comp_id;
261  } else {
262  if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) {
263  if (gps_s->num_sv > gps.num_sv) {
264  current_gps_id = gps_s->comp_id;
265  time_since_last_gps_switch = get_sys_time_msec();
266  } else if (gps.num_sv > gps_s->num_sv) {
267  current_gps_id = gps.comp_id;
268  time_since_last_gps_switch = get_sys_time_msec();
269  }
270  }
271  }
272  }
273  return current_gps_id;
274 }
275 #endif /*SECONDARY_GPS*/
276 
277 
278 void gps_periodic_check(struct GpsState *gps_s)
279 {
280  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
281  gps_s->fix = GPS_FIX_NONE;
282  }
283 
284 #ifdef SECONDARY_GPS
285  current_gps_id = gps_multi_switch(gps_s);
286  if (gps_s->comp_id == current_gps_id) {
287  gps = *gps_s;
288  }
289 #else
290  gps = *gps_s;
291 #endif
292 }
293 
294 bool gps_fix_valid(void)
295 {
296  bool gps_3d_timeout_valid = false;
297 #ifdef GPS_FIX_TIMEOUT
298  if (get_sys_time_float() - gps_time_since_last_3dfix() < GPS_FIX_TIMEOUT) {
299  gps_3d_timeout_valid = true;
300  }
301 #endif
302  return (gps.fix >= GPS_FIX_3D || gps_3d_timeout_valid);
303 }
304 
305 
307 static void gps_cb(uint8_t sender_id,
308  uint32_t stamp __attribute__((unused)),
309  struct GpsState *gps_s)
310 {
311  /* ignore callback from own AbiSendMsgGPS */
312  if (sender_id == GPS_MULTI_ID) {
313  return;
314  }
315  uint32_t now_ts = get_sys_time_usec();
316 #ifdef SECONDARY_GPS
317  current_gps_id = gps_multi_switch(gps_s);
318  if (gps_s->comp_id == current_gps_id) {
319  gps = *gps_s;
320  AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
321  }
322 #else
323  gps = *gps_s;
324  AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
325 #endif
326  if (gps.tow != gps_time_sync.t0_tow) {
329  }
330 }
331 
332 
333 void gps_init(void)
334 {
336 
337  gps.valid_fields = 0;
338  gps.fix = GPS_FIX_NONE;
339  gps.week = 0;
340  gps.tow = 0;
341  gps.cacc = 0;
342  gps.hacc = 0;
343  gps.vacc = 0;
344 
345  gps.last_3dfix_ticks = 0;
346  gps.last_3dfix_time = 0;
347  gps.last_msg_ticks = 0;
348  gps.last_msg_time = 0;
349 
350 #ifdef GPS_POWER_GPIO
352  GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
353 #endif
354 #ifdef GPS_LED
355  LED_OFF(GPS_LED);
356 #endif
357 
358  AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
359 
360 #if PERIODIC_TELEMETRY
368 #endif
369 
370  /* Register preflight checks */
371 #if PREFLIGHT_CHECKS
372  preflight_check_register(&gps_pfc, gps_preflight);
373 #endif
374 
375  // Initializing counter variables to count the number of Rtcm msgs in the input stream(for each msg type)
376  rtcm_man.Cnt105 = 0;
377  rtcm_man.Cnt177 = 0;
378  rtcm_man.Cnt187 = 0;
379  // Initializing counter variables to count the number of messages that failed Crc Check
380  rtcm_man.Crc105 = 0;
381  rtcm_man.Crc177 = 0;
382  rtcm_man.Crc187 = 0;
383 }
384 
386 {
387  uint32_t clock_delta;
388  uint32_t time_delta;
389  uint32_t itow_now;
390 
391  if (sys_ticks < gps_time_sync.t0_ticks) {
392  clock_delta = (0xFFFFFFFF - sys_ticks) + gps_time_sync.t0_ticks + 1;
393  } else {
394  clock_delta = sys_ticks - gps_time_sync.t0_ticks;
395  }
396 
397  time_delta = msec_of_sys_time_ticks(clock_delta);
398 
399  itow_now = gps_time_sync.t0_tow + time_delta;
400  if (itow_now > MSEC_PER_WEEK) {
401  itow_now %= MSEC_PER_WEEK;
402  }
403 
404  return itow_now;
405 }
406 
410 void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)),
411  uint8_t *data __attribute__((unused)))
412 {
413 
414 }
415 
417 {
418  // Check if the GPS is for this AC
419  if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { return; }
420 
421  // GPS parse data
423  DL_GPS_INJECT_packet_id(buf),
424  DL_GPS_INJECT_data_length(buf),
425  DL_GPS_INJECT_data(buf)
426  );
427 }
428 
430 {
431  // GPS parse data
432  gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
433  DL_RTCM_INJECT_data_length(buf),
434  DL_RTCM_INJECT_data(buf));
435 }
436 
443 struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s) {
444  struct LlaCoor_i lla_i = lla_int_from_gps(gps_s);
445  struct LlaCoor_f lla_f;
446  LLA_FLOAT_OF_BFP(lla_f, lla_i);
447  return lla_f;
448 }
449 
456 struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s) {
457  struct LlaCoor_i lla_i = { 0, 0, 0 };
458  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
459  return gps_s->lla_pos;
460  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
461  lla_of_ecef_i(&lla_i, &gps_s->ecef_pos);
462  }
463  return lla_i;
464 }
465 
472 struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s) {
473  struct EcefCoor_i ecef_i = ecef_int_from_gps(gps_s);
474  struct EcefCoor_f ecef_f;
475  ECEF_FLOAT_OF_BFP(ecef_f, ecef_i);
476  return ecef_f;
477 }
478 
485 struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s) {
486  struct EcefCoor_i ecef_i = { 0, 0, 0 };
487  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
488  return gps_s->ecef_pos;
489  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
490  ecef_of_lla_i(&ecef_i, &gps_s->lla_pos);
491  }
492  return ecef_i;
493 }
494 
501 struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s) {
502  struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
503  struct EcefCoor_f ecef_vel_f;
504  ECEF_FLOAT_OF_BFP(ecef_vel_f, ecef_vel_i);
505  return ecef_vel_f;
506 }
507 
514 struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s) {
515  struct EcefCoor_i ecef_vel_i = { 0, 0, 0 };
516  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
517  return gps_s->ecef_vel;
518  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
519  struct LtpDef_i def;
520  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
521  ltp_def_from_lla_i(&def, &gps_s->lla_pos);
522  } else { // assume ECEF
523  ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
524  }
525  ecef_of_ned_vect_i(&ecef_vel_i, &def, &gps_s->ned_vel);
526  }
527  return ecef_vel_i;
528 }
529 
536 struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s) {
537  struct NedCoor_i ned_vel_i = ned_vel_int_from_gps(gps_s);
538  struct NedCoor_f ned_vel_f;
539  VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel_i);
540  return ned_vel_f;
541 }
542 
549 struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s) {
550  struct NedCoor_i ned_vel_i = { 0, 0, 0 };
551  if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
552  return gps_s->ned_vel;
553  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
554  struct LtpDef_i def;
555  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
556  ltp_def_from_lla_i(&def, &gps_s->lla_pos);
557  } else { // assume ECEF
558  ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
559  }
560  ned_of_ecef_vect_i(&ned_vel_i, &def, &gps_s->ecef_vel);
561  }
562  return ned_vel_i;
563 }
564 
568 #include "state.h"
569 struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
570 {
571  struct UtmCoor_f utm = {.east = 0., .north = 0., .alt = 0., .zone = zone};
572 
573  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
574  /* A real UTM position is available, use the correct zone */
575  UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
576  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
577  /* Recompute UTM coordinates in this zone */
578  struct UtmCoor_i utm_i;
579  utm_i.zone = zone;
580  utm_of_lla_i(&utm_i, &gps_s->lla_pos);
581  UTM_FLOAT_OF_BFP(utm, utm_i);
582 
583  /* set utm.alt in hsml */
584  if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
585  utm.alt = gps_s->hmsl / 1000.;
586  } else {
587  utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon) / 1000.;
588  }
589  }
590 
591  return utm;
592 }
593 
594 struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
595 {
596  struct UtmCoor_i utm = {.east = 0, .north = 0, .alt = 0, .zone = zone};
597 
598  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
599  // A real UTM position is available, use the correct zone
600  UTM_COPY(utm, gps_s->utm_pos);
601  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
602  /* Recompute UTM coordinates in zone */
603  utm_of_lla_i(&utm, &gps_s->lla_pos);
604 
605  /* set utm.alt in hsml */
606  if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
607  utm.alt = gps_s->hmsl;
608  } else {
609  utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
610  }
611  }
612 
613  return utm;
614 }
615 
620 // known day_of_year for each month:
621 // Major index 0 is for non-leap years, and 1 is for leap years
622 // Minor index is for month number 1 .. 12, 0 at index 0 is number of days before January
623 static const uint16_t month_days[2][13] = {
624  { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
625  { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
626 };
627 
628 // Count the days since start of 1980, 6th of January
629 // Counts year * 356 days + leap days + month lengths + days in month
630 // The leap days counting needs the "+ 1" because GPS year 0 (i.e. 1980) was a leap year
632 {
633  uint16_t gps_years = year - 1980;
634  uint16_t leap_year = (gps_years % 4 == 0) ? 1 : 0;
635  uint16_t day_of_year = month_days[leap_year][month - 1] + day;
636  if (gps_years == 0)
637  return day_of_year;
638  return gps_years * 365 + ((gps_years - 1) / 4) + 1 + day_of_year - 6;
639 }
640 
642 {
643  return gps_day_number(year, month, day) / 7;
644 }
645 
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define GPS_MULTI_ID
static int16_t course[3]
Definition: airspeed_uADC.c:58
#define LED_OFF(i)
Definition: led_hw.h:52
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
#define PRIMARY_GPS
Some architecture independent helper functions for GPIOs.
uint8_t multi_gps_mode
Definition: gps.c:78
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:569
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: gps.c:307
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition: gps.c:536
struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (float) Converted on the fly if not available.
Definition: gps.c:501
struct GpsTimeSync gps_time_sync
Definition: gps.c:70
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience function to get utm position in int from GPS structure.
Definition: gps.c:594
static const uint16_t month_days[2][13]
GPS week number roll-over workaround application note.
Definition: gps.c:623
uint16_t gps_week_number(uint16_t year, uint8_t month, uint8_t day)
Number of weeks since navigation epoch (6 January 1980)
Definition: gps.c:641
void gps_parse_GPS_INJECT(uint8_t *buf)
Definition: gps.c:416
struct GpsState gps
global GPS state
Definition: gps.c:69
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition: gps.c:456
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
Definition: gps.c:514
struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (float) Converted on the fly if not available.
Definition: gps.c:472
static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:189
void gps_parse_RTCM_INJECT(uint8_t *buf)
Definition: gps.c:429
static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:217
static void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
send SVINFO message if updated.
Definition: gps.c:122
static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev, uint8_t svid)
Definition: gps.c:98
void WEAK gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Default parser for GPS injected data.
Definition: gps.c:410
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition: gps.c:485
struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (integer) Converted on the fly if not available.
Definition: gps.c:549
struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s)
Get GPS lla (float) Converted on the fly if not available.
Definition: gps.c:443
static void send_gps_rxmrtcm(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:178
void gps_init(void)
initialize the global GPS state
Definition: gps.c:333
#define TIME_TO_SWITCH
Definition: gps.c:67
static void send_gps(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:138
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
Convert time in sys_time ticks to GPS time of week.
Definition: gps.c:385
uint16_t gps_day_number(uint16_t year, uint8_t month, uint8_t day)
Number of days since navigation epoch (6 January 1980)
Definition: gps.c:631
bool gps_fix_valid(void)
Check if GPS fix is valid.
Definition: gps.c:294
static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
send SVINFO message (regardless of state)
Definition: gps.c:110
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: gps.c:278
struct RtcmMan rtcm_man
Definition: gps.c:72
struct GpsRelposNED gps_relposned
Definition: gps.c:71
static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:164
static abi_event gps_ev
Definition: gps.c:306
#define MSEC_PER_WEEK
Definition: gps.c:66
static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:240
Device independent GPS code (interface)
int16_t azim
azimuth in deg
Definition: gps.h:81
uint32_t tow
GPS time of week in ms.
Definition: gps.h:107
#define GPS_MODE_SECONDARY
Definition: gps.h:60
uint32_t Cnt105
Definition: gps.h:148
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:78
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
uint32_t Cnt187
Definition: gps.h:150
static float gps_time_since_last_3dfix(void)
Returns the time since last 3D fix in seconds (float)
Definition: gps.h:211
uint16_t refStationId
Definition: gps.h:129
uint32_t iTOW
Definition: gps.h:128
int8_t elev
elevation in deg
Definition: gps.h:80
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
uint32_t Crc177
Definition: gps.h:152
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:101
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:102
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
uint8_t gnssFixOK
Definition: gps.h:142
#define MULTI_GPS_MODE
Definition: gps.h:63
uint32_t accD
Definition: gps.h:138
uint32_t Crc187
Definition: gps.h:153
#define GpsId(_x)
Definition: gps.h:69
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:49
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:50
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:79
uint32_t accE
Definition: gps.h:137
#define GPS_MODE_PRIMARY
Definition: gps.h:59
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:89
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:99
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:47
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:112
int32_t relPosN
Definition: gps.h:130
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:93
uint32_t t0_ticks
hw clock ticks when GPS message is received
Definition: gps.h:123
#define GPS_NB_CHANNELS
Definition: gps.h:55
uint32_t Crc105
Definition: gps.h:151
#define GPS_TIMEOUT
GPS timeout in seconds in case of communication loss with GPS module.
Definition: gps.h:192
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:103
uint8_t diffSoln
Definition: gps.h:141
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:40
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:46
#define GPS_VALID_HMSL_BIT
Definition: gps.h:51
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:94
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:115
uint32_t t0_tow
GPS time of week in ms from last message.
Definition: gps.h:121
uint8_t svid
Satellite ID.
Definition: gps.h:76
int8_t relPosHPN
Definition: gps.h:133
uint32_t Cnt177
Definition: gps.h:149
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:109
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:113
uint32_t pacc
position accuracy in cm
Definition: gps.h:98
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:86
uint8_t comp_id
id of current gps
Definition: gps.h:87
uint8_t carrSoln
Definition: gps.h:139
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:100
uint8_t relPosValid
Definition: gps.h:140
int32_t relPosE
Definition: gps.h:131
int32_t relPosD
Definition: gps.h:132
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
int8_t relPosHPD
Definition: gps.h:135
uint32_t accN
Definition: gps.h:136
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:110
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:77
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:114
int8_t relPosHPE
Definition: gps.h:134
#define GPS_VALID_POS_UTM_BIT
Definition: gps.h:48
uint8_t num_sv
number of sat in fix
Definition: gps.h:104
uint16_t week
GPS week.
Definition: gps.h:106
uint8_t fix
status of fix
Definition: gps.h:105
data structures for GPS with RTK capabilities
Definition: gps.h:127
data structure for GPS information
Definition: gps.h:85
data structure for GPS time sync
Definition: gps.h:120
Definition: gps.h:145
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:71
int32_t lat
in degrees*1e7
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t z
in centimeters
uint8_t zone
UTM zone number.
int32_t x
in centimeters
int32_t east
in centimeters
int32_t y
in centimeters
int32_t lon
in degrees*1e7
int32_t north
in centimeters
#define ECEF_FLOAT_OF_BFP(_o, _i)
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
#define VECT3_FLOAT_OF_CM(_o, _i)
#define LLA_FLOAT_OF_BFP(_o, _i)
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
#define UTM_FLOAT_OF_BFP(_o, _i)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
position in UTM coordinates
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
arch independent LED (Light Emitting Diodes) API
#define GPS_POWER_GPIO
Definition: navstik_1.0.h:58
Paparazzi generic macros for geodetic calculations.
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float east
in meters
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
WGS-84 Geoid Heights.
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Persistent settings interface.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
static uint32_t msec_of_sys_time_ticks(uint32_t ticks)
Definition: sys_time.h:167
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98