Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
gps_skytraq.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
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 #include "subsystems/gps.h"
23 #include "led.h"
24 
25 #if GPS_USE_LATLONG
26 /* currently needed to get nav_utm_zone0 */
29 #endif
30 
32 
33 /* parser status */
34 #define UNINIT 0
35 #define GOT_SYNC1 1
36 #define GOT_SYNC2 2
37 #define GOT_LEN1 3
38 #define GOT_LEN2 4
39 #define GOT_ID 5
40 #define GOT_PAYLOAD 6
41 #define GOT_CHECKSUM 7
42 #define GOT_SYNC3 8
43 
44 #define SKYTRAQ_FIX_NONE 0x00
45 #define SKYTRAQ_FIX_2D 0x01
46 #define SKYTRAQ_FIX_3D 0x02
47 #define SKYTRAQ_FIX_3D_DGPS 0x03
48 
49 // distance in cm (10km dist max any direction)
50 #define MAX_DISTANCE 1000000
51 
52 //#include "my_debug_servo.h"
53 
55 
56 static int distance_too_great( struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos );
57 
58 void gps_impl_init(void) {
59 
61 
62 
63  //DEBUG_SERVO1_INIT();
64 
65 }
66 
67 
69 
70  //DEBUG_S1_ON();
71 
83  // pacc;
84  // sacc;
85  // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
88 
91  case SKYTRAQ_FIX_3D:
92  gps.fix = GPS_FIX_3D;
93  break;
94  case SKYTRAQ_FIX_2D:
95  gps.fix = GPS_FIX_2D;
96  break;
97  default:
99  }
100 
101 #if GPS_USE_LATLONG
102  /* Computes from (lat, long) in the referenced UTM zone */
103  struct LlaCoor_f lla_f;
104  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
105  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
106  struct UtmCoor_f utm_f;
107  utm_f.zone = nav_utm_zone0;
108  /* convert to utm */
109  utm_of_lla_f(&utm_f, &lla_f);
110  /* copy results of utm conversion */
111  gps.utm_pos.east = utm_f.east*100;
112  gps.utm_pos.north = utm_f.north*100;
115 #endif
116 
117  if ( gps.fix == GPS_FIX_3D ) {
119  // just grab current ecef_pos as reference.
121  }
122  // convert ecef velocity vector to NED vector.
124 
125  // ground course in radians
126  gps.course = ( M_PI_4 + atan2( -gps.ned_vel.y, gps.ned_vel.x )) * 1e7;
127  // GT: gps.cacc = ... ? what should course accuracy be?
128 
129  // ground speed
130  gps.gspeed = sqrt( gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y );
132 
133  // vertical speed (climb)
134  // solved by gps.ned.z?
135  }
136 
137 
138 
139  //DEBUG_S2_TOGGLE();
140 
141 #ifdef GPS_LED
142  if (gps.fix == GPS_FIX_3D) {
143  LED_ON(GPS_LED);
144  }
145  else {
146  LED_TOGGLE(GPS_LED);
147  }
148 #endif
149  }
150 
151  //DEBUG_S1_OFF();
152 }
153 
157  switch (gps_skytraq.status) {
158  case UNINIT:
159  if (c == SKYTRAQ_SYNC1)
161  break;
162  case GOT_SYNC1:
163  if (c != SKYTRAQ_SYNC2) {
165  goto error;
166  }
168  break;
169  case GOT_SYNC2:
170  gps_skytraq.len = c<<8;
172  break;
173  case GOT_LEN1:
174  gps_skytraq.len += c;
178  goto error;
179  }
180  break;
181  case GOT_LEN2:
182  gps_skytraq.msg_id = c;
183  gps_skytraq.msg_idx = 0;
186  break;
187  case GOT_ID:
188  gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c;
190  if (gps_skytraq.msg_idx >= gps_skytraq.len-1) {
192  }
193  break;
194  case GOT_PAYLOAD:
195  if (c != gps_skytraq.checksum) {
197  goto error;
198  }
200  break;
201  case GOT_CHECKSUM:
202  if (c != SKYTRAQ_SYNC3) {
204  goto error;
205  }
207  break;
208  case GOT_SYNC3:
210  goto restart;
211  default:
213  goto error;
214  }
215  return;
216  error:
218  restart:
220  return;
221 }
222 
223 static int distance_too_great( struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos ) {
224  int32_t xdiff = abs(ecef_ref->x - ecef_pos->x);
225  if ( xdiff > MAX_DISTANCE ) return TRUE;
226  int32_t ydiff = abs(ecef_ref->y - ecef_pos->y);
227  if ( ydiff > MAX_DISTANCE ) return TRUE;
228  int32_t zdiff = abs(ecef_ref->z - ecef_pos->z);
229  if ( zdiff > MAX_DISTANCE ) return TRUE;
230 
231  return FALSE;
232 }
233 
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
#define GOT_CHECKSUM
Definition: gps_skytraq.c:41
#define GOT_SYNC3
Definition: gps_skytraq.c:42
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
int32_t lat
in radians*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:67
#define SKYTRAQ_FIX_3D
Definition: gps_skytraq.c:46
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
void gps_impl_init(void)
Definition: gps_skytraq.c:58
#define MAX_DISTANCE
Definition: gps_skytraq.c:50
uint8_t error_last
Definition: gps_skytraq.h:76
vector in EarthCenteredEarthFixed coordinates
uint8_t zone
UTM zone number.
uint8_t msg_id
Definition: gps_skytraq.h:69
struct LtpDef_i ref_ltp
Definition: gps_skytraq.c:54
#define SKYTRAQ_SYNC3
Definition: gps_skytraq.h:30
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.h:36
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition: gps_skytraq.h:50
uint8_t nav_utm_zone0
Definition: common_nav.c:43
#define LED_ON(i)
Definition: led_hw.h:28
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float lat
in radians
#define GOT_ID
Definition: gps_skytraq.c:39
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition: gps_skytraq.h:44
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
int32_t z
in centimeters
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition: gps_skytraq.h:48
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:63
vector in Latitude, Longitude and Altitude
#define GPS_SKYTRAQ_ERR_UNEXPECTED
Definition: gps_skytraq.h:63
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define GOT_SYNC2
Definition: gps_skytraq.c:36
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
uint8_t status
Definition: gps_skytraq.h:71
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition: gps_skytraq.h:37
definition of the local (flat earth) coordinate system
#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG
Definition: gps_skytraq.h:60
uint8_t zone
UTM zone number.
uint8_t msg_idx
Definition: gps_skytraq.h:73
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition: gps_skytraq.h:43
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
uint8_t error_cnt
Definition: gps_skytraq.h:75
#define SKYTRAQ_SYNC1
Definition: gps_skytraq.h:27
#define GOT_LEN1
Definition: gps_skytraq.c:37
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:40
#define GPS_FIX_2D
Definition: gps.h:41
#define GOT_PAYLOAD
Definition: gps_skytraq.c:40
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition: gps_skytraq.h:34
Device independent GPS code (interface)
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition: gps_skytraq.h:51
void gps_skytraq_read_message(void)
Definition: gps_skytraq.c:68
#define SKYTRAQ_FIX_3D_DGPS
Definition: gps_skytraq.c:47
position in UTM coordinates Units: meters
#define SKYTRAQ_SYNC2
Definition: gps_skytraq.h:28
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition: gps_skytraq.h:49
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition: gps_skytraq.h:53
#define SKYTRAQ_FIX_2D
Definition: gps_skytraq.c:45
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition: gps_skytraq.h:40
#define GPS_SKYTRAQ_ERR_CHECKSUM
Definition: gps_skytraq.h:61
int32_t north
in centimeters
int32_t x
in centimeters
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define GPS_SKYTRAQ_MAX_PAYLOAD
Definition: gps_skytraq.h:65
bool_t msg_available
Definition: gps_skytraq.h:68
signed long int32_t
Definition: types.h:19
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition: gps_skytraq.h:41
uint8_t checksum
Definition: gps_skytraq.h:74
#define TRUE
Definition: imu_chimu.h:144
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC
Definition: gps_skytraq.h:62
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
int32_t east
in centimeters
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
Definition: gps_skytraq.c:223
uint16_t len
Definition: gps_skytraq.h:72
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
#define GOT_SYNC1
Definition: gps_skytraq.c:35
struct EcefCoor_i ecef
Reference point in ecef.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:154
#define UNINIT
Definition: gps_skytraq.c:34
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:65
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition: gps_skytraq.h:42
struct GpsSkytraq gps_skytraq
Definition: gps_skytraq.c:31
#define GOT_LEN2
Definition: gps_skytraq.c:38
float lon
in radians
static struct point c
Definition: discsurvey.c:13
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:31
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition: gps_skytraq.h:52
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t num_sv
number of sat in fix
Definition: gps.h:76