Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_udp.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 
22 #include "subsystems/gps.h"
23 #include "subsystems/abi.h"
24 
25 #include "fms/fms_network.h"
26 #include <string.h>
27 #include <stdio.h>
28 
29 #if GPS_USE_LATLONG
32 #endif
33 
34 //Check if variables are set and else define them
35 #ifndef GPS_UDP_HOST
36 #define GPS_UDP_HOST 192.168.1.2
37 #endif
38 
39 #define GPS_UDP_MSG_LEN (11*4)
40 
41 //Define the buffer, check bytes and FmsNetwork
42 unsigned char gps_udp_read_buffer[256];
43 struct FmsNetwork *gps_network = NULL;
44 
45 void gps_impl_init(void)
46 {
48  gps_network = network_new(STRINGIFY(GPS_UDP_HOST), 6000 /*out*/, 7000 /*in*/, TRUE);
49 }
50 
51 #define STX 99
52 
53 #define UDP_GPS_INT(_udp_gps_payload) (int32_t)(*((uint8_t*)_udp_gps_payload)|*((uint8_t*)_udp_gps_payload+1)<<8|((int32_t)*((uint8_t*)_udp_gps_payload+2))<<16|((int32_t)*((uint8_t*)_udp_gps_payload+3))<<24)
54 
55 void gps_parse(void)
56 {
57  if (gps_network == NULL) { return; }
58 
59  //Read from the network
60  int size = network_read(gps_network, &gps_udp_read_buffer[0], 256);
61 
62  if (size > 0) {
63  // Correct MSG
64  if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) {
69 
73 
77 
78  gps.fix = GPS_FIX_3D;
79 
80 #if GPS_USE_LATLONG
81  // Computes from (lat, long) in the referenced UTM zone
82  struct LlaCoor_f lla_f;
84  struct UtmCoor_f utm_f;
85  utm_f.zone = nav_utm_zone0;
86  // convert to utm
87  utm_of_lla_f(&utm_f, &lla_f);
88  // copy results of utm conversion
89  gps.utm_pos.east = utm_f.east * 100;
90  gps.utm_pos.north = utm_f.north * 100;
93 #endif
94 
95  // publish new GPS data
96  uint32_t now_ts = get_sys_time_usec();
99  if (gps.fix == GPS_FIX_3D) {
102  }
103  AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps);
104 
105  } else {
106  printf("gps_udp error: msg len invalid %d bytes\n", size);
107  }
108  memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
109  }
110 }
111 
int32_t z
in centimeters
int32_t north
in centimeters
float east
in meters
float north
in meters
int32_t y
in centimeters
Main include for ABI (AirBorneInterface).
uint8_t nav_utm_zone0
Definition: common_nav.c:44
position in UTM coordinates Units: meters
int32_t east
in centimeters
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:70
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:89
int32_t alt
in millimeters above WGS84 reference ellipsoid
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:92
uint8_t zone
UTM zone number.
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
#define TRUE
Definition: std.h:4
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:71
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:41
#define STX
Definition: gps_udp.c:51
Device independent GPS code (interface)
#define GPS_UDP_ID
void gps_impl_init(void)
GPS initialization.
Definition: gps_udp.c:45
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:68
#define GPS_UDP_HOST
Definition: gps_udp.c:36
#define UDP_GPS_INT(_udp_gps_payload)
Definition: gps_udp.c:53
int32_t lon
in degrees*1e7
uint8_t zone
UTM zone number.
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:70
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:90
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define GPS_UDP_MSG_LEN
Definition: gps_udp.c:39
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:91
int32_t x
in centimeters
struct FmsNetwork * gps_network
Definition: gps_udp.c:43
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:72
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:69
int32_t lat
in degrees*1e7
unsigned char gps_udp_read_buffer[256]
Definition: gps_udp.c:42
uint8_t fix
status of fix
Definition: gps.h:82
struct GpsState gps
global GPS state
Definition: gps.c:41
void gps_parse(void)
Definition: gps_udp.c:55
#define LLA_FLOAT_OF_BFP(_o, _i)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)