Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/gps/gps.h"
23#include "modules/core/abi.h"
24
25#include "fms/fms_network.h"
26#include <string.h>
27#include <stdio.h>
28
29//Check if variables are set and else define them
30#ifndef GPS_UDP_HOST
31#define GPS_UDP_HOST 192.168.1.2
32#endif
33
34#define GPS_UDP_MSG_LEN (11*4)
35
36//Define the buffer, check bytes and FmsNetwork
37unsigned char gps_udp_read_buffer[256];
38struct FmsNetwork *gps_network = NULL;
39
40struct GpsState gps_udp = {0};
41
42void gps_udp_init(void)
43{
45 gps_network = network_new(STRINGIFY(GPS_UDP_HOST), 6000 /*out*/, 7000 /*in*/, TRUE);
46}
47
48#define STX 99
49
50#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)
51
52void gps_udp_parse(void)
53{
54 if (gps_network == NULL) { return; }
55
56 //Read from the network
57 int size = network_read(gps_network, &gps_udp_read_buffer[0], 256);
58
59 if (size > 0) {
60 // Correct MSG
61 if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) {
62 gps_udp.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4);
63 gps_udp.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8);
64 gps_udp.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12);
65 SetBit(gps_udp.valid_fields, GPS_VALID_POS_LLA_BIT);
66
68 SetBit(gps_udp.valid_fields, GPS_VALID_HMSL_BIT);
69
70 gps_udp.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20);
71 gps_udp.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24);
72 gps_udp.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28);
73 SetBit(gps_udp.valid_fields, GPS_VALID_POS_ECEF_BIT);
74
75 gps_udp.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32);
76 gps_udp.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36);
77 gps_udp.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
78 SetBit(gps_udp.valid_fields, GPS_VALID_VEL_ECEF_BIT);
79
80 gps_udp.fix = GPS_FIX_3D;
81
82 // publish new GPS data
83 uint32_t now_ts = get_sys_time_usec();
84 gps_udp.last_msg_ticks = sys_time.nb_sec_rem;
85 gps_udp.last_msg_time = sys_time.nb_sec;
86 if (gps_udp.fix == GPS_FIX_3D) {
87 gps_udp.last_3dfix_ticks = sys_time.nb_sec_rem;
88 gps_udp.last_3dfix_time = sys_time.nb_sec;
89 }
90 AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps_udp);
91
92 } else {
93 printf("gps_udp error: msg len invalid %d bytes\n", size);
94 }
95 memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
96 }
97}
Main include for ABI (AirBorneInterface).
#define GPS_UDP_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define STX
Definition protocol.h:99
Device independent GPS code (interface).
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#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
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:90
struct FmsNetwork * gps_network
Definition gps_udp.c:38
#define GPS_UDP_MSG_LEN
Definition gps_udp.c:34
#define GPS_UDP_HOST
Definition gps_udp.c:31
struct GpsState gps_udp
Definition gps_udp.c:40
#define UDP_GPS_INT(_udp_gps_payload)
Definition gps_udp.c:50
void gps_udp_init(void)
Definition gps_udp.c:42
void gps_udp_parse(void)
Definition gps_udp.c:52
unsigned char gps_udp_read_buffer[256]
Definition gps_udp.c:37
#define TRUE
Definition std.h:4
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.