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_sirf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Freek van Tienen
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  */
22 
23 
24 #include "subsystems/gps.h"
25 
26 #include "led.h"
27 
28 #if GPS_USE_LATLONG
29 /* currently needed to get nav_utm_zone0 */
31 #endif
33 
34 #include <inttypes.h>
35 #include <math.h>
36 #include <stdio.h>
37 #include "gps_sirf.h"
38 
40 void sirf_parse_2(void);
41 void sirf_parse_41(void);
42 
43 void gps_impl_init( void ) {
46  gps_sirf.msg_len = 0;
47  gps_sirf.read_state = 0;
48 }
49 
51  switch(gps_sirf.read_state) {
52  case UNINIT:
53  if(c == 0xA0) {
54  gps_sirf.msg_len = 0;
56  gps_sirf.msg_len++;
58  }
59  break;
60  case GOT_A0:
61  if(c == 0xA2) {
63  gps_sirf.msg_len++;
65  }
66  else
67  goto restart;
68  break;
69  case GOT_A2:
71  gps_sirf.msg_len++;
72  if(c == 0xB0)
74  break;
75  case GOT_B0:
76  if(c == 0xB3) {
78  gps_sirf.msg_len++;
80  }
81  else
82  goto restart;
83  break;
84  default:
85  break;
86  }
87  return;
88 
89  restart:
91 }
92 
93 int start_time = 0;
94 int ticks = 0;
95 int start_time2 = 0;
96 int ticks2 = 0;
97 
98 void sirf_parse_41(void) {
99  struct sirf_msg_41* p = (struct sirf_msg_41*)&gps_sirf.msg_buf[4];
100 
101  gps.tow = Invert4Bytes(p->tow);
102  gps.hmsl = Invert4Bytes(p->alt_msl)*10;
103  gps.num_sv = p->num_sat;
104  gps.nb_channels = p ->num_sat;
105 
106  /* read latitude, longitude and altitude from packet */
110 
111 #if GPS_USE_LATLONG
112  /* convert to utm */
113  struct LlaCoor_f lla_f;
114  LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
115  struct UtmCoor_f utm_f;
116  utm_f.zone = nav_utm_zone0;
117  utm_of_lla_f(&utm_f, &lla_f);
118 
119  /* copy results of utm conversion */
120  gps.utm_pos.east = utm_f.east*100;
121  gps.utm_pos.north = utm_f.north*100;
124 #endif
125 
126  gps.sacc = (Invert2Bytes(p->ehve)>>16);
127  gps.course = RadOfDeg(Invert2Bytes(p->cog))*pow(10, 5);
128  gps.gspeed = RadOfDeg(Invert2Bytes(p->sog))*pow(10, 5);
129  gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err))*pow(10, 5);
130  gps.pacc = Invert4Bytes(p->ehpe);
131  gps.pdop = p->hdop * 20;
132 
133  if ((p->nav_type >> 8 & 0x7) >= 0x4)
134  gps.fix = GPS_FIX_3D;
135  else if((p->nav_type >> 8 & 0x7) >= 0x1)
136  gps.fix = GPS_FIX_2D;
137  else
138  gps.fix = GPS_FIX_NONE;
139 
140 
141  //Let gps_sirf know we have a position update
143 }
144 
145 void sirf_parse_2(void) {
146  struct sirf_msg_2* p = (struct sirf_msg_2*)&gps_sirf.msg_buf[4];
147 
148  gps.week = Invert2Bytes(p->week);
149 
150  gps.ecef_pos.x = Invert4Bytes(p->x_pos) * 100;
151  gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100;
152  gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100;
153 
154  gps.ecef_vel.x = (Invert2Bytes(p->vx)>>16)*100/8;
155  gps.ecef_vel.y = (Invert2Bytes(p->vy)>>16)*100/8;
156  gps.ecef_vel.z = (Invert2Bytes(p->vz)>>16)*100/8;
157 
158  if(gps.fix == GPS_FIX_3D) {
159  ticks++;
160 #if DEBUG_SIRF
161  printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2));
162 #endif
163  }
164  else if(sys_time.nb_sec - gps.last_3dfix_time > 10) {
166  ticks = 0;
167  }
168 
169 }
170 
171 void sirf_parse_msg(void) {
172  //Set position available to false and check if it is a valid message
174  if(gps_sirf.msg_len < 8)
175  return;
176 
177  if(start_time2 == 0)
179  ticks2++;
180 
181  //Check the message id and parse the message
182  uint8_t message_id = gps_sirf.msg_buf[4];
183  switch(message_id) {
184  case 0x29:
185  sirf_parse_41();
186  break;
187  case 0x02:
188  sirf_parse_2();
189  break;
190  }
191 
192 }
int16_t vy
y-velocity * 8 in m/s
Definition: gps_sirf.h:64
#define GOT_B0
Definition: gps_sirf.h:41
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
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
#define UNINIT
Definition: ins_xsens.c:170
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:74
uint8_t zone
UTM zone number.
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:75
Sirf protocol specific code.
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
Message ID 41 from GPS.
Definition: gps_sirf.h:88
uint8_t nav_utm_zone0
Definition: common_nav.c:44
uint16_t ehve
estimated horizontal velocity error in m/s * 10^2
Definition: gps_sirf.h:114
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
#define Invert4Bytes(x)
Definition: gps_sirf.h:55
int16_t vz
z-velocity * 8 in m/s
Definition: gps_sirf.h:65
void sirf_parse_msg(void)
Definition: gps_sirf.c:171
void gps_impl_init(void)
GPS initialization.
Definition: gps_sirf.c:43
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
Message ID 2 from GPS.
Definition: gps_sirf.h:58
int16_t week
GPS week.
Definition: gps.h:79
vector in Latitude, Longitude and Altitude
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint16_t sog
speed over ground, in m/s * 10^2
Definition: gps_sirf.h:106
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
uint32_t ehpe
estimated horizontal position error, in meters * 10^2
Definition: gps_sirf.h:111
uint8_t num_sat
Number of satellites used for solution.
Definition: gps_sirf.h:122
uint8_t zone
UTM zone number.
uint16_t week
Definition: gps_sirf.h:69
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
bool_t msg_available
Definition: gps_sirf.h:44
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:41
#define GPS_FIX_2D
Definition: gps.h:42
int32_t z_pos
z-position in m
Definition: gps_sirf.h:62
Device independent GPS code (interface)
position in UTM coordinates Units: meters
int32_t longitude
in degrees (+= East) *10*7
Definition: gps_sirf.h:102
void sirf_parse_2(void)
Definition: gps_sirf.c:145
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct GpsSirf gps_sirf
Definition: gps_sirf.c:39
char msg_buf[SIRF_MAXLEN]
buffer for storing one nmea-line
Definition: gps_sirf.h:46
int ticks
Definition: gps_sirf.c:94
int32_t north
in centimeters
int32_t x
in centimeters
int32_t alt_msl
in meters *10^2
Definition: gps_sirf.h:104
bool_t pos_available
Definition: gps_sirf.h:45
#define Invert2Bytes(x)
Definition: gps_sirf.h:54
#define TRUE
Definition: imu_chimu.h:144
uint32_t tow
time of week in seconds *10^3]
Definition: gps_sirf.h:93
int32_t east
in centimeters
uint32_t pacc
position accuracy in cm
Definition: gps.h:73
int start_time
Definition: gps_sirf.c:93
unsigned char uint8_t
Definition: types.h:14
int32_t x_pos
x-position in m
Definition: gps_sirf.h:60
int32_t lon
in degrees*1e7
#define LLA_FLOAT_OF_BFP(_o, _i)
int32_t latitude
in degrees (+= North) *10^7
Definition: gps_sirf.h:101
#define GOT_A0
Definition: gps_sirf.h:39
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
int16_t vx
x-velocity * 8 in m/s
Definition: gps_sirf.h:63
void sirf_parse_char(uint8_t c)
Definition: gps_sirf.c:50
int msg_len
Definition: gps_sirf.h:47
uint16_t nav_type
Definition: gps_sirf.h:91
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:86
uint16_t cog
course over ground, in degrees clockwise from true north * 10^2
Definition: gps_sirf.h:107
int read_state
Definition: gps_sirf.h:48
arch independent LED (Light Emitting Diodes) API
static float p[2][2]
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
int32_t y_pos
y-position in m
Definition: gps_sirf.h:61
uint16_t heading_err
in degrees * 10^2
Definition: gps_sirf.h:121
uint8_t hdop
Horizontal dilution of precision x 5 (0.2 precision)
Definition: gps_sirf.h:123
void sirf_parse_41(void)
Definition: gps_sirf.c:98
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:41
int ticks2
Definition: gps_sirf.c:96
int start_time2
Definition: gps_sirf.c:95
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:82
int32_t alt_ellipsoid
in meters *10^2
Definition: gps_sirf.h:103
uint8_t num_sv
number of sat in fix
Definition: gps.h:77
#define GOT_A2
Definition: gps_sirf.h:40