Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
39
40#include "modules/core/abi.h"
41#include "modules/gps/gps.h"
42#include "led.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
52#endif
53
54#ifdef SECONDARY_GPS
55PRINT_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/* Maximum number of relative positions */
67#ifndef GPS_RELPOS_MAX
68#define GPS_RELPOS_MAX 3
69#endif
70
71#define MSEC_PER_WEEK (1000*60*60*24*7)
72#define TIME_TO_SWITCH 5000 //ten s in ms
73
74struct GpsState gps = {0};
76
77#ifdef SECONDARY_GPS
78static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
79#endif
80
83
85#if GPS_LOSS_TEST_TIMER
86 if (gps_disable_fix > 0) {
88 }
89#else
90 gps_disable_fix = 0; // reset disable fix counter
91#endif
92}
93
94#if PREFLIGHT_CHECKS
95/* Preflight checks */
97static struct preflight_check_t gps_pfc;
98
99static void gps_preflight(struct preflight_result_t *result) {
100 if(!gps_fix_valid()) {
101 preflight_error(result, "No valid GPS fix");
102 } else {
103 preflight_success(result, "GPS fix ok");
104 }
105}
106#endif // PREFLIGHT_CHECKS
107
108
109#if PERIODIC_TELEMETRY
111
112static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev,
113 uint8_t svid)
114{
115 if (svid < GPS_NB_CHANNELS) {
116 pprz_msg_send_SVINFO(trans, dev, AC_ID, &svid,
117 &gps.svinfos[svid].svid, &gps.svinfos[svid].flags,
118 &gps.svinfos[svid].qi, &gps.svinfos[svid].cno,
119 &gps.svinfos[svid].elev, &gps.svinfos[svid].azim);
120 }
121}
122
124static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
125{
126 static uint8_t i = 0;
127 if (i >= gps.nb_channels) { i = 0; }
128 send_svinfo_id(trans, dev, i);
129 i++;
130}
131
136static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
137{
138 static uint8_t i = 0;
139 static uint8_t last_cnos[GPS_NB_CHANNELS];
140 if (i >= gps.nb_channels) { i = 0; }
141 // send SVINFO for all satellites while no GPS fix,
142 // after 3D fix, send avialable sats if they were updated
143 if (gps.fix < GPS_FIX_3D) {
144 send_svinfo_id(trans, dev, i);
145 } else if (gps.svinfos[i].cno != last_cnos[i]) {
146 send_svinfo_id(trans, dev, i);
147 last_cnos[i] = gps.svinfos[i].cno;
148 }
149 i++;
150}
151
152static void send_gps(struct transport_tx *trans, struct link_device *dev)
153{
154 uint8_t zero = (gps_disable_fix <= 0)? 0 : gps_disable_fix;
155 int16_t climb = -gps.ned_vel.z;
156 int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
157 struct UtmCoor_i utm = utm_int_from_gps(&gps, 0);
158#if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
159 // broadcast GPS message
160 struct pprzlink_msg msg;
161 msg.trans = trans;
162 msg.dev = dev;
163 msg.sender_id = AC_ID;
164 msg.receiver_id = PPRZLINK_MSG_BROADCAST;
165 msg.component_id = 0;
166 pprzlink_msg_send_GPS(&msg,
167#else
168 pprz_msg_send_GPS(trans, dev, AC_ID,
169#endif
170 &gps.fix,
171 &utm.east, &utm.north,
172 &course, &gps.hmsl, &gps.gspeed, &climb,
173 &gps.week, &gps.tow, &utm.zone, &zero);
174 // send SVINFO for available satellites that have new data
176}
177
178static void send_gps_relpos(struct transport_tx *trans, struct link_device *dev)
179{
180 static uint8_t idx = 0;
181 if(gps_relposned[idx].tow == 0)
182 return;
183
184 pprz_msg_send_GPS_RELPOS(trans, dev, AC_ID,
185 &gps_relposned[idx].reference_id,
186 &gps_relposned[idx].tow,
187 &gps_relposned[idx].pos.x, &gps_relposned[idx].pos.y, &gps_relposned[idx].pos.z,
188 &gps_relposned[idx].distance,
189 &gps_relposned[idx].heading,
190 &gps_relposned[idx].pos_acc.x, &gps_relposned[idx].pos_acc.y, &gps_relposned[idx].pos_acc.z,
191 &gps_relposned[idx].distance_acc,
192 &gps_relposned[idx].heading_acc);
193
194 // Send the next index that is set
195 idx++;
196 if(idx >= GPS_RELPOS_MAX || gps_relposned[idx].tow == 0)
197 idx = 0;
198}
199
200static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
201{
202 // ECEF from the LLA position and NED velocity when the receiver doesn't send it
203 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
204 struct EcefCoor_i ecef_vel = ecef_vel_int_from_gps(&gps);
205#if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
206 // broadcast GPS message
207 struct pprzlink_msg msg;
208 msg.trans = trans;
209 msg.dev = dev;
210 msg.sender_id = AC_ID;
211 msg.receiver_id = PPRZLINK_MSG_BROADCAST;
212 msg.component_id = 0;
213 pprzlink_msg_send_GPS_INT(&msg,
214#else
215 pprz_msg_send_GPS_INT(trans, dev, AC_ID,
216#endif
217 &ecef_pos.x, &ecef_pos.y, &ecef_pos.z,
218 &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
219 &gps.hmsl,
220 &ecef_vel.x, &ecef_vel.y, &ecef_vel.z,
221 &gps.pacc, &gps.sacc,
222 &gps.tow,
223 &gps.pdop,
224 &gps.num_sv,
225 &gps.fix,
226 &gps.comp_id);
227 // send SVINFO for available satellites that have new data
229}
230
231static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
232{
233 uint8_t err = 0;
234 int16_t climb = -gps.ned_vel.z;
235 int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
236#if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST
237 // broadcast GPS message
238 struct pprzlink_msg msg;
239 msg.trans = trans;
240 msg.dev = dev;
241 msg.sender_id = AC_ID;
242 msg.receiver_id = PPRZLINK_MSG_BROADCAST;
243 msg.component_id = 0;
244 pprzlink_msg_send_GPS_LLA(&msg,
245#else
246 pprz_msg_send_GPS_LLA(trans, dev, AC_ID,
247#endif
248 &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
249 &gps.hmsl, &course, &gps.gspeed, &climb,
250 &gps.week, &gps.tow,
251 &gps.fix, &err);
252}
253
254static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
255{
256 pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
257}
258#endif
259
260
261#ifdef SECONDARY_GPS
262static uint8_t gps_multi_switch(struct GpsState *gps_s)
263{
264 static uint32_t time_since_last_gps_switch = 0;
265
267 return GpsId(PRIMARY_GPS);
268 } else if (multi_gps_mode == GPS_MODE_SECONDARY) {
269 return GpsId(SECONDARY_GPS);
270 } else {
271 if (gps_s->fix > gps.fix) {
272 return gps_s->comp_id;
273 } else if (gps.fix > gps_s->fix) {
274 return gps.comp_id;
275 } else {
276 if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) {
277 if (gps_s->num_sv > gps.num_sv) {
278 current_gps_id = gps_s->comp_id;
279 time_since_last_gps_switch = get_sys_time_msec();
280 } else if (gps.num_sv > gps_s->num_sv) {
281 current_gps_id = gps.comp_id;
282 time_since_last_gps_switch = get_sys_time_msec();
283 }
284 }
285 }
286 }
287 return current_gps_id;
288}
289#endif /*SECONDARY_GPS*/
290
291
292void gps_periodic_check(struct GpsState *gps_s)
293{
294 if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
295 gps_s->fix = GPS_FIX_NONE;
296 }
297
298#ifdef SECONDARY_GPS
299 current_gps_id = gps_multi_switch(gps_s);
300 if (gps_s->comp_id == current_gps_id) {
301 gps = *gps_s;
302 }
303#else
304 gps = *gps_s;
305#endif
306}
307
309{
310 bool gps_3d_timeout_valid = false;
311#ifdef GPS_FIX_TIMEOUT
312 if (get_sys_time_float() - gps_time_since_last_3dfix() < GPS_FIX_TIMEOUT) {
313 gps_3d_timeout_valid = true;
314 }
315#endif
316 return (gps.fix >= GPS_FIX_3D || gps_3d_timeout_valid);
317}
318
319
321static void gps_cb(uint8_t sender_id,
322 uint32_t stamp,
323 struct GpsState *gps_s)
324{
325 /* ignore callback from own AbiSendMsgGPS */
326 if (sender_id == GPS_MULTI_ID) {
327 return;
328 }
329
330 uint8_t old_fix = gps_s->fix;
331 uint32_t now_ts = get_sys_time_usec();
332#ifdef SECONDARY_GPS
333 current_gps_id = gps_multi_switch(gps_s);
334 if (gps_s->comp_id == current_gps_id) {
335 gps = *gps_s;
336#if GPS_LOSS_TEST_TIMER
337 if(gps_disable_fix > 0) {
338 gps_s->fix = GPS_FIX_NONE;
339 }
340#endif
341 AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
342 gps_s->fix = old_fix;
343 }
344#else
345 gps = *gps_s;
346#if GPS_LOSS_TEST_TIMER
347 if(gps_disable_fix > 0) {
348 gps_s->fix = GPS_FIX_NONE;
349 }
350#endif
351 AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
352 gps_s->fix = old_fix;
353#endif
354 // Sync the system time of week with the selected GPS (only with a valid time from a fix)
355 if (gps.comp_id == gps_s->comp_id && gps.fix >= GPS_FIX_3D && gps.tow != 0) {
356 sys_time_sync_gps(gps.week, gps.tow, stamp);
357 }
358}
359
361static void gps_relpos_cb(uint8_t sender_id __attribute__((unused)),
362 uint32_t stamp __attribute__((unused)),
363 struct RelPosNED *relpos)
364{
365 for(uint8_t i = 0; i < GPS_RELPOS_MAX; i++) {
366 // Find our index or a free index
367 if(gps_relposned[i].tow == 0 || gps_relposned[i].reference_id == relpos->reference_id)
368 {
369 // Copy and save result
370 gps_relposned[i] = *relpos;
371 break;
372 }
373 }
374}
375
376
377void gps_init(void)
378{
380
381 gps.valid_fields = 0;
382 gps.fix = GPS_FIX_NONE;
383 gps.week = 0;
384 gps.tow = 0;
385 gps.cacc = 0;
386 gps.hacc = 0;
387 gps.vacc = 0;
388
389 gps.last_3dfix_ticks = 0;
390 gps.last_3dfix_time = 0;
391 gps.last_msg_ticks = 0;
392 gps.last_msg_time = 0;
393 gps_disable_fix = 0;
394
395#ifdef GPS_POWER_GPIO
396 gpio_setup_output(GPS_POWER_GPIO);
397 GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
398#endif
399#ifdef GPS_LED
400 LED_OFF(GPS_LED);
401#endif
402
403 AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
404 AbiBindMsgRELPOS(ABI_BROADCAST, &gps_relpos_ev, gps_relpos_cb);
405
406#if PERIODIC_TELEMETRY
413#endif
414
415 /* Register preflight checks */
416#if PREFLIGHT_CHECKS
417 preflight_check_register(&gps_pfc, gps_preflight);
418#endif
419}
420
422{
423 // signed difference handles both timestamps in the past/future and tick counter wrap
424 int32_t dt_ticks = (int32_t)(sys_time.nb_tick - sys_ticks);
425 int32_t dt_ms = (int32_t)((int64_t)dt_ticks * 1000 / (int64_t)sys_time.ticks_per_sec);
427}
428
432void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)),
433 uint8_t *data __attribute__((unused)))
434{
435
436}
437
439{
440 // Check if the GPS is for this AC
441 if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { return; }
442
443 // GPS parse data
445 DL_GPS_INJECT_packet_id(buf),
446 DL_GPS_INJECT_data_length(buf),
447 DL_GPS_INJECT_data(buf)
448 );
449}
450
452{
453 // GPS parse data
454 gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
455 DL_RTCM_INJECT_data_length(buf),
456 DL_RTCM_INJECT_data(buf));
457}
458
465struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s) {
466 struct LlaCoor_i lla_i = lla_int_from_gps(gps_s);
467 struct LlaCoor_f lla_f;
468 LLA_FLOAT_OF_BFP(lla_f, lla_i);
469 return lla_f;
470}
471
478struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s) {
479 struct LlaCoor_i lla_i = { 0, 0, 0 };
480 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
481 return gps_s->lla_pos;
482 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
483 lla_of_ecef_i(&lla_i, &gps_s->ecef_pos);
484 }
485 return lla_i;
486}
487
495 struct EcefCoor_i ecef_i = ecef_int_from_gps(gps_s);
496 struct EcefCoor_f ecef_f;
497 ECEF_FLOAT_OF_BFP(ecef_f, ecef_i);
498 return ecef_f;
499}
500
507struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s) {
508 struct EcefCoor_i ecef_i = { 0, 0, 0 };
509 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) {
510 return gps_s->ecef_pos;
511 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
512 ecef_of_lla_i(&ecef_i, &gps_s->lla_pos);
513 }
514 return ecef_i;
515}
516
524 struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
525 struct EcefCoor_f ecef_vel_f;
526 ECEF_FLOAT_OF_BFP(ecef_vel_f, ecef_vel_i);
527 return ecef_vel_f;
528}
529
537 struct EcefCoor_i ecef_vel_i = { 0, 0, 0 };
538 if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
539 return gps_s->ecef_vel;
540 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
541 struct LtpDef_i def;
542 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
543 ltp_def_from_lla_i(&def, &gps_s->lla_pos);
544 } else { // assume ECEF
545 ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
546 }
547 ecef_of_ned_vect_i(&ecef_vel_i, &def, &gps_s->ned_vel);
548 }
549 return ecef_vel_i;
550}
551
559 struct NedCoor_i ned_vel_i = ned_vel_int_from_gps(gps_s);
560 struct NedCoor_f ned_vel_f;
561 VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel_i);
562 return ned_vel_f;
563}
564
572 struct NedCoor_i ned_vel_i = { 0, 0, 0 };
573 if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) {
574 return gps_s->ned_vel;
575 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) {
576 struct LtpDef_i def;
577 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
578 ltp_def_from_lla_i(&def, &gps_s->lla_pos);
579 } else { // assume ECEF
580 ltp_def_from_ecef_i(&def, &gps_s->ecef_pos);
581 }
582 ned_of_ecef_vect_i(&ned_vel_i, &def, &gps_s->ecef_vel);
583 }
584 return ned_vel_i;
585}
586
590#include "state.h"
592{
593 struct UtmCoor_f utm = {.east = 0., .north = 0., .alt = 0., .zone = zone};
594
595 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
596 /* A real UTM position is available, use the correct zone */
597 UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
598 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
599 /* Recompute UTM coordinates in this zone */
600 struct UtmCoor_i utm_i;
601 utm_i.zone = zone;
602 utm_of_lla_i(&utm_i, &gps_s->lla_pos);
603 UTM_FLOAT_OF_BFP(utm, utm_i);
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 / 1000.;
608 } else {
609 utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon) / 1000.;
610 }
611 }
612
613 return utm;
614}
615
617{
618 struct UtmCoor_i utm = {.east = 0, .north = 0, .alt = 0, .zone = zone};
619
620 if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
621 // A real UTM position is available, use the correct zone
622 UTM_COPY(utm, gps_s->utm_pos);
623 } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) {
624 /* Recompute UTM coordinates in zone */
625 utm_of_lla_i(&utm, &gps_s->lla_pos);
626
627 /* set utm.alt in hsml */
628 if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
629 utm.alt = gps_s->hmsl;
630 } else {
631 utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
632 }
633 }
634
635 return utm;
636}
637
641
642// known day_of_year for each month:
643// Major index 0 is for non-leap years, and 1 is for leap years
644// Minor index is for month number 1 .. 12, 0 at index 0 is number of days before January
645static const uint16_t month_days[2][13] = {
646 { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
647 { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
648};
649
650// Count the days since start of 1980, 6th of January
651// Counts year * 356 days + leap days + month lengths + days in month
652// The leap days counting needs the "+ 1" because GPS year 0 (i.e. 1980) was a leap year
654{
655 uint16_t gps_years = year - 1980;
656 uint16_t leap_year = (gps_years % 4 == 0) ? 1 : 0;
657 uint16_t day_of_year = month_days[leap_year][month - 1] + day;
658 if (gps_years == 0)
659 return day_of_year;
660 return gps_years * 365 + ((gps_years - 1) / 4) + 1 + day_of_year - 6;
661}
662
664{
665 return gps_day_number(year, month, day) / 7;
666}
667
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define GPS_MULTI_ID
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
static int16_t course[3]
#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.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
static struct uart_periph * dev
Some architecture independent helper functions for GPIOs.
uint8_t multi_gps_mode
Definition gps.c:81
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:591
int gps_disable_fix
Disable fix.
Definition gps.c:82
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition gps.c:321
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:558
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:523
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:616
static const uint16_t month_days[2][13]
GPS week number roll-over workaround application note.
Definition gps.c:645
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:663
void gps_parse_GPS_INJECT(uint8_t *buf)
Definition gps.c:438
struct GpsState gps
global GPS state
Definition gps.c:74
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:478
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:536
static abi_event gps_relpos_ev
Definition gps.c:360
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:494
static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
Definition gps.c:200
void gps_parse_RTCM_INJECT(uint8_t *buf)
Definition gps.c:451
static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
Definition gps.c:231
static void gps_relpos_cb(uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos)
Definition gps.c:361
static void send_svinfo_available(struct transport_tx *trans, struct link_device *dev)
send SVINFO message if updated.
Definition gps.c:136
static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev, uint8_t svid)
Definition gps.c:112
void WEAK gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Default parser for GPS injected data.
Definition gps.c:432
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:507
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:571
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:465
void gps_init(void)
initialize the global GPS state
Definition gps.c:377
#define TIME_TO_SWITCH
Definition gps.c:72
static void send_gps(struct transport_tx *trans, struct link_device *dev)
Definition gps.c:152
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:421
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:653
bool gps_fix_valid(void)
Check if GPS fix is valid.
Definition gps.c:308
static void send_svinfo(struct transport_tx *trans, struct link_device *dev)
send SVINFO message (regardless of state)
Definition gps.c:124
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition gps.c:292
#define GPS_RELPOS_MAX
Definition gps.c:68
void gps_periodic_fix_counter(void)
Increment periodic counter.
Definition gps.c:84
static void send_gps_relpos(struct transport_tx *trans, struct link_device *dev)
Definition gps.c:178
static struct RelPosNED gps_relposned[GPS_RELPOS_MAX]
Definition gps.c:75
#define MSEC_PER_WEEK
Definition gps.c:71
static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
Definition gps.c:254
Device independent GPS code (interface).
#define GPS_MODE_SECONDARY
Definition gps.h:62
static float gps_time_since_last_3dfix(void)
Returns the time since last 3D fix in seconds (float).
Definition gps.h:199
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:95
#define MULTI_GPS_MODE
Definition gps.h:65
#define GpsId(_x)
Definition gps.h:71
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_MODE_PRIMARY
Definition gps.h:61
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition gps.h:94
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition gps.h:98
#define GPS_NB_CHANNELS
Definition gps.h:57
#define GPS_TIMEOUT
GPS timeout in seconds in case of communication loss with GPS module.
Definition gps.h:180
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
struct NedCoor_i ned_vel
speed NED in cm/s
Definition gps.h:99
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition gps.h:120
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:91
uint8_t comp_id
id of current gps
Definition gps.h:92
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint16_t reference_id
Reference station identification.
Definition gps.h:132
#define GPS_VALID_POS_UTM_BIT
Definition gps.h:50
uint8_t num_sv
number of sat in fix
Definition gps.h:109
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
#define UTM_COPY(_u1, _u2)
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
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 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.
arch independent LED (Light Emitting Diodes) API
static uint32_t idx
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi generic macros for geodetic calculations.
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
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.
void sys_time_sync_gps(uint16_t week, uint32_t tow, uint32_t stamp)
Synchronize the time with a GPS measurement (has priority over the ground).
Definition sys_time.c:120
uint32_t get_sys_time_tow(void)
Get the GPS time of week in milliseconds.
Definition sys_time.c:72
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition sys_time.h:82
uint32_t ticks_per_sec
sys_time ticks per second (SYS_TIME_FREQUENCY)
Definition sys_time.h:86
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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
#define PRIMARY_GPS
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.