Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
27 #include "subsystems/gps.h"
28 
29 #include "led.h"
30 
31 #ifdef GPS_POWER_GPIO
32 #include "mcu_periph/gpio.h"
33 
34 #ifndef GPS_POWER_GPIO_ON
35 #define GPS_POWER_GPIO_ON gpio_set
36 #endif
37 #endif
38 
39 #define MSEC_PER_WEEK (1000*60*60*24*7)
40 
41 struct GpsState gps;
42 
44 
45 #if PERIODIC_TELEMETRY
47 
48 static void send_gps(void) {
49  static uint8_t i;
50  int16_t climb = -gps.ned_vel.z;
51  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
52  DOWNLINK_SEND_GPS(DefaultChannel, DefaultDevice, &gps.fix,
54  &course, &gps.hmsl, &gps.gspeed, &climb,
55  &gps.week, &gps.tow, &gps.utm_pos.zone, &i);
56  if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;
57  if (i >= gps.nb_channels * 2) i = 0;
58  if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) {
59  DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i,
60  &gps.svinfos[i].svid, &gps.svinfos[i].flags,
61  &gps.svinfos[i].qi, &gps.svinfos[i].cno,
62  &gps.svinfos[i].elev, &gps.svinfos[i].azim);
63  }
64  i++;
65 }
66 
67 static void send_gps_int(void) {
68  static uint8_t i;
69  static uint8_t last_cnos[GPS_NB_CHANNELS];
70  DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice,
73  &gps.hmsl,
75  &gps.pacc, &gps.sacc,
76  &gps.tow,
77  &gps.pdop,
78  &gps.num_sv,
79  &gps.fix);
80  if (i == gps.nb_channels) i = 0;
81  if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) {
82  DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i,
83  &gps.svinfos[i].svid, &gps.svinfos[i].flags,
84  &gps.svinfos[i].qi, &gps.svinfos[i].cno,
85  &gps.svinfos[i].elev, &gps.svinfos[i].azim);
86  last_cnos[i] = gps.svinfos[i].cno;
87  }
88  i++;
89 }
90 
91 static void send_gps_lla(void) {
92  uint8_t err = 0;
93  int16_t climb = -gps.ned_vel.z;
94  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
95  DOWNLINK_SEND_GPS_LLA(DefaultChannel, DefaultDevice,
97  &course, &gps.gspeed, &climb,
98  &gps.week, &gps.tow,
99  &gps.fix, &err);
100 }
101 
102 static void send_gps_sol(void) {
103  DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
104 }
105 #endif
106 
107 void gps_init(void) {
108  gps.fix = GPS_FIX_NONE;
109  gps.week = 0;
110  gps.tow = 0;
111  gps.cacc = 0;
112 
113  gps.last_3dfix_ticks = 0;
114  gps.last_3dfix_time = 0;
115  gps.last_msg_ticks = 0;
116  gps.last_msg_time = 0;
117 #ifdef GPS_POWER_GPIO
119  GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
120 #endif
121 #ifdef GPS_LED
122  LED_OFF(GPS_LED);
123 #endif
124 #ifdef GPS_TYPE_H
125  gps_impl_init();
126 #endif
127 
128 #if PERIODIC_TELEMETRY
129  register_periodic_telemetry(DefaultPeriodic, "GPS", send_gps);
130  register_periodic_telemetry(DefaultPeriodic, "GPS_INT", send_gps_int);
131  register_periodic_telemetry(DefaultPeriodic, "GPS_LLA", send_gps_lla);
132  register_periodic_telemetry(DefaultPeriodic, "GPS_SOL", send_gps_sol);
133 #endif
134 }
135 
136 void gps_periodic_check(void) {
138  gps.fix = GPS_FIX_NONE;
139  }
140 }
141 
143 {
144  uint32_t clock_delta;
145  uint32_t time_delta;
146  uint32_t itow_now;
147 
148  if (sys_ticks < gps_time_sync.t0_ticks) {
149  clock_delta = (0xFFFFFFFF - sys_ticks) + gps_time_sync.t0_ticks + 1;
150  } else {
151  clock_delta = sys_ticks - gps_time_sync.t0_ticks;
152  }
153 
154  time_delta = msec_of_sys_time_ticks(clock_delta);
155 
156  itow_now = gps_time_sync.t0_tow + time_delta;
157  if (itow_now > MSEC_PER_WEEK) {
158  itow_now %= MSEC_PER_WEEK;
159  }
160 
161  return itow_now;
162 }
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:85
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:74
uint8_t zone
UTM zone number.
Periodic telemetry system header (includes downlink utility and generated code).
Some architecture independent helper functions for GPIOs.
static uint32_t msec_of_sys_time_ticks(uint32_t ticks)
Definition: sys_time.h:147
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:75
data structure for GPS time sync
Definition: gps.h:93
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
#define MSEC_PER_WEEK
Definition: gps.c:39
void gps_impl_init(void)
Definition: gps_ardrone2.c:38
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
#define GPS_NB_CHANNELS
Definition: gps_ardrone2.h:33
int16_t week
GPS week.
Definition: gps.h:79
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:56
data structure for GPS information
Definition: gps.h:63
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
int32_t z
Down.
int16_t azim
azimuth in deg
Definition: gps.h:59
uint32_t t0_ticks
hw clock ticks when GPS message is received
Definition: gps.h:96
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
#define GPS_FIX_NONE
Definition: gps.h:41
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t north
in centimeters
int32_t x
in centimeters
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:83
signed long int32_t
Definition: types.h:19
int32_t east
in centimeters
uint32_t pacc
position accuracy in cm
Definition: gps.h:73
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:55
unsigned char uint8_t
Definition: types.h:14
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:87
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
int8_t elev
elevation in deg
Definition: gps.h:58
int32_t lon
in degrees*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:54
void gps_periodic_check(void)
Periodic GPS check.
Definition: gps.c:136
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
struct GpsTimeSync gps_time_sync
Definition: gps.c:43
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:86
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:142
#define LED_OFF(i)
Definition: led_hw.h:29
arch independent LED (Light Emitting Diodes) API
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:88
#define GPS_TIMEOUT
GPS timeout in seconds.
Definition: gps.h:111
uint32_t t0_tow
GPS time of week in ms from last message.
Definition: gps.h:94
struct GpsState gps
global GPS state
Definition: gps.c:41
void gps_init(void)
initialize the global GPS state
Definition: gps.c:107
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:57
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:82
uint8_t num_sv
number of sat in fix
Definition: gps.h:77