Paparazzi UAS  v5.18.0_stable
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 "subsystems/abi.h"
41 #include "subsystems/gps.h"
42 #include "led.h"
43 #include "subsystems/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;
73 
74 #ifdef SECONDARY_GPS
75 static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
76 #endif
77 
79 
80 
81 #if PERIODIC_TELEMETRY
83 
84 static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev,
85  uint8_t svid)
86 {
87  if (svid < GPS_NB_CHANNELS) {
88  pprz_msg_send_SVINFO(trans, dev, AC_ID, &svid,
89  &gps.svinfos[svid].svid, &gps.svinfos[svid].flags,
90  &gps.svinfos[svid].qi, &gps.svinfos[svid].cno,
91  &gps.svinfos[svid].elev, &gps.svinfos[svid].azim);
92  }
93 }
94 
96 static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
97 {
98  static uint8_t i = 0;
99  if (i >= gps.nb_channels) { i = 0; }
100  send_svinfo_id(trans, dev, i);
101  i++;
102 }
103 
108 static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
109 {
110  static uint8_t i = 0;
111  static uint8_t last_cnos[GPS_NB_CHANNELS];
112  if (i >= gps.nb_channels) { i = 0; }
113  // send SVINFO for all satellites while no GPS fix,
114  // after 3D fix, send avialable sats if they were updated
115  if (gps.fix < GPS_FIX_3D) {
116  send_svinfo_id(trans, dev, i);
117  } else if (gps.svinfos[i].cno != last_cnos[i]) {
118  send_svinfo_id(trans, dev, i);
119  last_cnos[i] = gps.svinfos[i].cno;
120  }
121  i++;
122 }
123 
124 static void send_gps(struct transport_tx *trans, struct link_device *dev)
125 {
126  uint8_t zero = 0;
127  int16_t climb = -gps.ned_vel.z;
128  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
129  struct UtmCoor_i utm = utm_int_from_gps(&gps, 0);
130 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
131  // broadcast GPS message
132  struct pprzlink_msg msg;
133  msg.trans = trans;
134  msg.dev = dev;
135  msg.sender_id = AC_ID;
136  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
137  msg.component_id = 0;
138  pprzlink_msg_send_GPS(&msg,
139 #else
140  pprz_msg_send_GPS(trans, dev, AC_ID,
141 #endif
142  &gps.fix,
143  &utm.east, &utm.north,
144  &course, &gps.hmsl, &gps.gspeed, &climb,
145  &gps.week, &gps.tow, &utm.zone, &zero);
146  // send SVINFO for available satellites that have new data
147  send_svinfo_available(trans, dev);
148 }
149 
150 static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
151 {
152  pprz_msg_send_GPS_RTK(trans, dev, AC_ID,
162 }
163 
164 static void send_gps_rxmrtcm(struct transport_tx *trans, struct link_device *dev)
165 {
166  pprz_msg_send_GPS_RXMRTCM(trans, dev, AC_ID,
167  &rtcm_man.Cnt105,
168  &rtcm_man.Cnt177,
169  &rtcm_man.Cnt187,
170  &rtcm_man.Crc105,
171  &rtcm_man.Crc177,
172  &rtcm_man.Crc187);
173 }
174 
175 static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
176 {
177 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
178  // broadcast GPS message
179  struct pprzlink_msg msg;
180  msg.trans = trans;
181  msg.dev = dev;
182  msg.sender_id = AC_ID;
183  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
184  msg.component_id = 0;
185  pprzlink_msg_send_GPS_INT(&msg,
186 #else
187  pprz_msg_send_GPS_INT(trans, dev, AC_ID,
188 #endif
191  &gps.hmsl,
193  &gps.pacc, &gps.sacc,
194  &gps.tow,
195  &gps.pdop,
196  &gps.num_sv,
197  &gps.fix,
198  &gps.comp_id);
199  // send SVINFO for available satellites that have new data
200  send_svinfo_available(trans, dev);
201 }
202 
203 static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
204 {
205  uint8_t err = 0;
206  int16_t climb = -gps.ned_vel.z;
207  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
208 #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
209  // broadcast GPS message
210  struct pprzlink_msg msg;
211  msg.trans = trans;
212  msg.dev = dev;
213  msg.sender_id = AC_ID;
214  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
215  msg.component_id = 0;
216  pprzlink_msg_send_GPS_LLA(&msg,
217 #else
218  pprz_msg_send_GPS_LLA(trans, dev, AC_ID,
219 #endif
221  &gps.hmsl, &course, &gps.gspeed, &climb,
222  &gps.week, &gps.tow,
223  &gps.fix, &err);
224 }
225 
226 static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
227 {
228  pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
229 }
230 #endif
231 
232 
233 #ifdef SECONDARY_GPS
234 static uint8_t gps_multi_switch(struct GpsState *gps_s)
235 {
236  static uint32_t time_since_last_gps_switch = 0;
237 
239  return GpsId(PRIMARY_GPS);
240  } else if (multi_gps_mode == GPS_MODE_SECONDARY) {
241  return GpsId(SECONDARY_GPS);
242  } else {
243  if (gps_s->fix > gps.fix) {
244  return gps_s->comp_id;
245  } else if (gps.fix > gps_s->fix) {
246  return gps.comp_id;
247  } else {
248  if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) {
249  if (gps_s->num_sv > gps.num_sv) {
250  current_gps_id = gps_s->comp_id;
251  time_since_last_gps_switch = get_sys_time_msec();
252  } else if (gps.num_sv > gps_s->num_sv) {
253  current_gps_id = gps.comp_id;
254  time_since_last_gps_switch = get_sys_time_msec();
255  }
256  }
257  }
258  }
259  return current_gps_id;
260 }
261 #endif /*SECONDARY_GPS*/
262 
263 
264 void gps_periodic_check(struct GpsState *gps_s)
265 {
266  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
267  gps_s->fix = GPS_FIX_NONE;
268  }
269 
270 #ifdef SECONDARY_GPS
271  current_gps_id = gps_multi_switch(gps_s);
272  if (gps_s->comp_id == current_gps_id) {
273  gps = *gps_s;
274  }
275 #else
276  gps = *gps_s;
277 #endif
278 }
279 
280 
282 static void gps_cb(uint8_t sender_id,
283  uint32_t stamp __attribute__((unused)),
284  struct GpsState *gps_s)
285 {
286  /* ignore callback from own AbiSendMsgGPS */
287  if (sender_id == GPS_MULTI_ID) {
288  return;
289  }
290  uint32_t now_ts = get_sys_time_usec();
291 #ifdef SECONDARY_GPS
292  current_gps_id = gps_multi_switch(gps_s);
293  if (gps_s->comp_id == current_gps_id) {
294  gps = *gps_s;
295  AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
296  }
297 #else
298  gps = *gps_s;
299  AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
300 #endif
301  if (gps.tow != gps_time_sync.t0_tow) {
304  }
305 }
306 
307 
308 void gps_init(void)
309 {
311 
312  gps.valid_fields = 0;
313  gps.fix = GPS_FIX_NONE;
314  gps.week = 0;
315  gps.tow = 0;
316  gps.cacc = 0;
317  gps.hacc = 0;
318  gps.vacc = 0;
319 
320  gps.last_3dfix_ticks = 0;
321  gps.last_3dfix_time = 0;
322  gps.last_msg_ticks = 0;
323  gps.last_msg_time = 0;
324 
325 #ifdef GPS_POWER_GPIO
327  GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
328 #endif
329 #ifdef GPS_LED
330  LED_OFF(GPS_LED);
331 #endif
332 
333  AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
334 
335 #if PERIODIC_TELEMETRY
343 #endif
344 
345  // Initializing counter variables to count the number of Rtcm msgs in the input stream(for each msg type)
346  rtcm_man.Cnt105 = 0;
347  rtcm_man.Cnt177 = 0;
348  rtcm_man.Cnt187 = 0;
349  // Initializing counter variables to count the number of messages that failed Crc Check
350  rtcm_man.Crc105 = 0;
351  rtcm_man.Crc177 = 0;
352  rtcm_man.Crc187 = 0;
353 }
354 
356 {
357  uint32_t clock_delta;
358  uint32_t time_delta;
359  uint32_t itow_now;
360 
361  if (sys_ticks < gps_time_sync.t0_ticks) {
362  clock_delta = (0xFFFFFFFF - sys_ticks) + gps_time_sync.t0_ticks + 1;
363  } else {
364  clock_delta = sys_ticks - gps_time_sync.t0_ticks;
365  }
366 
367  time_delta = msec_of_sys_time_ticks(clock_delta);
368 
369  itow_now = gps_time_sync.t0_tow + time_delta;
370  if (itow_now > MSEC_PER_WEEK) {
371  itow_now %= MSEC_PER_WEEK;
372  }
373 
374  return itow_now;
375 }
376 
380 void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)),
381  uint8_t *data __attribute__((unused)))
382 {
383 
384 }
385 
389 #include "state.h"
391 {
392  struct UtmCoor_f utm = {.east = 0., .north = 0., .alt = 0., .zone = zone};
393 
394  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
395  /* A real UTM position is available, use the correct zone */
396  UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
397  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
398  /* Recompute UTM coordinates in this zone */
399  struct UtmCoor_i utm_i;
400  utm_i.zone = zone;
401  utm_of_lla_i(&utm_i, &gps_s->lla_pos);
402  UTM_FLOAT_OF_BFP(utm, utm_i);
403 
404  /* set utm.alt in hsml */
405  if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
406  utm.alt = gps_s->hmsl / 1000.;
407  } else {
408  utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon) / 1000.;
409  }
410  }
411 
412  return utm;
413 }
414 
416 {
417  struct UtmCoor_i utm = {.east = 0, .north = 0, .alt = 0, .zone = zone};
418 
419  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
420  // A real UTM position is available, use the correct zone
421  UTM_COPY(utm, gps_s->utm_pos);
422  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
423  /* Recompute UTM coordinates in zone */
424  utm_of_lla_i(&utm, &gps_s->lla_pos);
425 
426  /* set utm.alt in hsml */
427  if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
428  utm.alt = gps_s->hmsl;
429  } else {
430  utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
431  }
432  }
433 
434  return utm;
435 }
436 
441 // known day_of_year for each month:
442 // Major index 0 is for non-leap years, and 1 is for leap years
443 // Minor index is for month number 1 .. 12, 0 at index 0 is number of days before January
444 static const uint16_t month_days[2][13] = {
445  { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
446  { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
447 };
448 
449 // Count the days since start of 1980
450 // Counts year * 356 days + leap days + month lengths + days in month
451 // The leap days counting needs the "+ 1" because GPS year 0 (i.e. 1980) was a leap year
453 {
454  uint16_t gps_years = year - 1980;
455  uint16_t leap_year = (gps_years % 4 == 0) ? 1 : 0;
456  uint16_t day_of_year = month_days[leap_year][month - 1] + day;
457  if (gps_years == 0)
458  return day_of_year;
459  return gps_years * 365 + ((gps_years - 1) / 4) + 1 + day_of_year;
460 }
461 
463 {
464  return gps_day_number(year, month, day) / 7;
465 }
466 
GpsRelposNED::relPosHPE
int8_t relPosHPE
Definition: gps.h:136
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint16_t
unsigned short uint16_t
Definition: types.h:16
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
month_days
static const uint16_t month_days[2][13]
GPS week number roll-over workaround application note.
Definition: gps.c:444
SVinfo::azim
int16_t azim
azimuth in deg
Definition: gps.h:83
GpsRelposNED::gnssFixOK
uint8_t gnssFixOK
Definition: gps.h:144
pprz_geodetic.h
Paparazzi generic macros for geodetic calculations.
send_gps_rxmrtcm
static void send_gps_rxmrtcm(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:164
UtmCoor_i::north
int32_t north
in centimeters
Definition: pprz_geodetic_int.h:87
settings.h
gps_init
void gps_init(void)
initialize the global GPS state
Definition: gps.c:308
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
SVinfo::flags
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:79
abi.h
PRIMARY_GPS
#define PRIMARY_GPS
Definition: dw1000_arduino.h:55
GpsRelposNED::accE
uint32_t accE
Definition: gps.h:139
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
utm_of_lla_i
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
Definition: pprz_geodetic_int.c:423
RtcmMan::Cnt105
uint32_t Cnt105
Definition: gps.h:150
GpsRelposNED::relPosD
int32_t relPosD
Definition: gps.h:134
SVinfo::qi
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:80
GpsTimeSync::t0_tow
uint32_t t0_tow
GPS time of week in ms from last message.
Definition: gps.h:123
RtcmMan::Cnt177
uint32_t Cnt177
Definition: gps.h:151
send_gps_rtk
static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:150
GpsState::nb_channels
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:111
wgs84_ellipsoid_to_geoid_i
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
Definition: pprz_geodetic_wgs84.h:81
GpsState
data structure for GPS information
Definition: gps.h:87
send_svinfo
static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
send SVINFO message (regardless of state)
Definition: gps.c:96
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
utm_int_from_gps
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:415
gpio_setup_output
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
UtmCoor_f::east
float east
in meters
Definition: pprz_geodetic_float.h:83
MSEC_PER_WEEK
#define MSEC_PER_WEEK
Definition: gps.c:66
GpsRelposNED::diffSoln
uint8_t diffSoln
Definition: gps.h:143
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
GpsRelposNED::iTOW
uint32_t iTOW
Definition: gps.h:130
GpsState::vacc
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
UTM_COPY
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:71
MULTI_GPS_MODE
#define MULTI_GPS_MODE
Definition: gps.h:65
GPS_TIMEOUT
#define GPS_TIMEOUT
GPS timeout in seconds.
Definition: gps.h:176
GpsRelposNED::relPosHPD
int8_t relPosHPD
Definition: gps.h:137
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
GpsId
#define GpsId(_x)
Definition: gps.h:71
send_gps_lla
static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:203
GpsRelposNED::relPosE
int32_t relPosE
Definition: gps.h:133
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
msg
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Definition: high_speed_logger_direct_memory.c:134
RtcmMan::Cnt187
uint32_t Cnt187
Definition: gps.h:152
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
UtmCoor_i::east
int32_t east
in centimeters
Definition: pprz_geodetic_int.h:88
gps_ev
static abi_event gps_ev
Definition: gps.c:281
GpsRelposNED::relPosN
int32_t relPosN
Definition: gps.h:132
SVinfo::cno
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:81
GpsState::hacc
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
gps_week_number
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:462
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
send_svinfo_id
static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev, uint8_t svid)
Definition: gps.c:84
sys_time::nb_tick
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
GPS_MODE_PRIMARY
#define GPS_MODE_PRIMARY
Definition: gps.h:61
GpsTimeSync
data structure for GPS time sync
Definition: gps.h:122
GpsState::week
uint16_t week
GPS week.
Definition: gps.h:108
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
GPS_VALID_POS_UTM_BIT
#define GPS_VALID_POS_UTM_BIT
Definition: gps.h:50
SVinfo::svid
uint8_t svid
Satellite ID.
Definition: gps.h:78
UtmCoor_i::alt
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_int.h:89
int16_t
signed short int16_t
Definition: types.h:17
get_sys_time_msec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t
unsigned char uint8_t
Definition: types.h:14
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
GpsRelposNED
data structures for GPS with RTK capabilities
Definition: gps.h:129
GpsRelposNED::refStationId
uint16_t refStationId
Definition: gps.h:131
send_gps
static void send_gps(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:124
GPS_NB_CHANNELS
#define GPS_NB_CHANNELS
Definition: gps.h:57
RtcmMan::Crc177
uint32_t Crc177
Definition: gps.h:154
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
gps_periodic_check
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: gps.c:264
RtcmMan::Crc105
uint32_t Crc105
Definition: gps.h:153
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
GpsState::comp_id
uint8_t comp_id
id of current gps
Definition: gps.h:89
led.h
arch independent LED (Light Emitting Diodes) API
send_gps_int
static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:175
GpsRelposNED::accD
uint32_t accD
Definition: gps.h:140
send_svinfo_available
static void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
send SVINFO message if updated.
Definition: gps.c:108
send_gps_sol
static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
Definition: gps.c:226
gps_day_number
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:452
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
utm_float_from_gps
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:390
gps_tow_from_sys_ticks
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:355
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
gps_relposned
struct GpsRelposNED gps_relposned
Definition: gps.c:71
gpio.h
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
int32_t
signed long int32_t
Definition: types.h:19
gps_inject_data
void WEAK gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Default parser for GPS injected data.
Definition: gps.c:380
sys_time
Definition: sys_time.h:71
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
GPS_MULTI_ID
#define GPS_MULTI_ID
Definition: abi_sender_ids.h:256
GpsRelposNED::accN
uint32_t accN
Definition: gps.h:138
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
multi_gps_mode
uint8_t multi_gps_mode
Definition: gps.c:78
GpsTimeSync::t0_ticks
uint32_t t0_ticks
hw clock ticks when GPS message is received
Definition: gps.h:125
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: gps.c:282
GPS_MODE_SECONDARY
#define GPS_MODE_SECONDARY
Definition: gps.h:62
SVinfo::elev
int8_t elev
elevation in deg
Definition: gps.h:82
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
UtmCoor_i
position in UTM coordinates
Definition: pprz_geodetic_int.h:86
GpsRelposNED::carrSoln
uint8_t carrSoln
Definition: gps.h:141
GpsRelposNED::relPosValid
uint8_t relPosValid
Definition: gps.h:142
GpsState::cacc
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
state.h
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
GPS_POWER_GPIO
#define GPS_POWER_GPIO
Definition: navstik_1.0.h:58
rtcm_man
struct RtcmMan rtcm_man
Definition: gps.c:72
GpsRelposNED::relPosHPN
int8_t relPosHPN
Definition: gps.h:135
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
GpsState::svinfos
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:112
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
RtcmMan::Crc187
uint32_t Crc187
Definition: gps.h:155
RtcmMan
Definition: gps.h:147
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
msec_of_sys_time_ticks
static uint32_t msec_of_sys_time_ticks(uint32_t ticks)
Definition: sys_time.h:158
gps_time_sync
struct GpsTimeSync gps_time_sync
Definition: gps.c:70
UTM_FLOAT_OF_BFP
#define UTM_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:247
pprz_geodetic_wgs84.h
WGS-84 Geoid Heights.
UtmCoor_i::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_int.h:90
TIME_TO_SWITCH
#define TIME_TO_SWITCH
Definition: gps.c:67