Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

add gpsd to ivy forwarder

Browse Source
This commit is contained in:
Martin Mueller
2009-05-14 09:15:07 +00:00
parent 53b0439c4a
commit b044e74096
2 changed files with 167 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/tmtc/Makefile
+2
View File
@@ -135,6 +135,8 @@ GLIB_LDFLAGS = `pkg-config glib-2.0 --libs` -lglibivy -lpcre
GTK_CFLAGS = -Wall `pkg-config gtk+-2.0 --cflags`
GTK_LDFLAGS = `pkg-config gtk+-2.0 --libs` -lglibivy -lpcre
gpsd2ivy: gpsd2ivy.c
$(CC) $(GLIB_CFLAGS) -o $@ $< $(GLIB_LDFLAGS) -lgps
c_ivy_client_example_1: c_ivy_client_example_1.c
$(CC) $(GLIB_CFLAGS) -o $@ $< $(GLIB_LDFLAGS)
sw/ground_segment/tmtc/gpsd2ivy.c
+165
View File
@@ -0,0 +1,165 @@
/*
* $Id$
*
* Copyright (C) 2009 Martin Mueller
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file gpsd_ivy.c
* \brief GPSd forwarder
*
* This receives position information through gpsd and forwards it
* through the ivy bus to the gcs to display the ground station location
* on the map
*/
/*
<message name="FLIGHT_PARAM" id="11">
<field name="ac_id" type="string"/>
<field name="roll" type="float" unit="deg"/>
<field name="pitch" type="float" unit="deg"/>
<field name="heading" type="float" unit="deg"/>
<field name="lat" type="float" unit="deg"/>
<field name="long" type="float" unit="deg"/>
<field name="speed" type="float" unit="m/s"/>
<field name="course" type="float" unit="deg" format="%.1f"/>
<field name="alt" type="float" unit="m"/>
<field name="climb" type="float" unit="m/s"/>
<field name="agl" type="float" unit="m"/>
<field name="unix_time" type="float" unit="s (Unix time)"/>
<field name="itow" type="uint32" unit="ms"/>
</message>
*/
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <math.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <glib.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include "gps.h"
#define MSG_DEST "ground"
#define MSG_NAME "FLIGHT_PARAM"
#define MSG_ID "GCS"
#define TIMEOUT_PERIOD 200
static struct gps_data_t *gpsdata;
static void update_gps(struct gps_data_t *gpsdata,
char *message,
size_t len,
int level)
{
static double fix_time = 0;
double fix_track = 0;
double fix_speed = 0;
double fix_altitude = 0;
double fix_climb = 0;
if ((isnan(gpsdata->fix.latitude) == 0) &&
(isnan(gpsdata->fix.longitude) == 0) &&
(isnan(gpsdata->fix.time) == 0) &&
(gpsdata->fix.mode >= MODE_2D) &&
(gpsdata->fix.time != fix_time))
{
if (isnan(gpsdata->fix.track) != 0) fix_track = gpsdata->fix.track;
if (isnan(gpsdata->fix.speed) != 0) fix_speed = gpsdata->fix.speed;
if (gpsdata->fix.mode >= MODE_3D)
{
if (isnan(gpsdata->fix.altitude) != 0) fix_altitude = gpsdata->fix.altitude;
if (isnan(gpsdata->fix.climb) != 0) fix_climb = gpsdata->fix.climb;
}
IvySendMsg("%s %s %s %f %f %f %f %f %f %f %f %f %f %f %d",
MSG_DEST,
MSG_NAME,
MSG_ID, // ac_id
0.0, // roll,
0.0, // pitch,
0.0, // heading
gpsdata->fix.latitude,
gpsdata->fix.longitude,
fix_speed,
fix_track, // course
fix_altitude,
fix_climb,
0.0, // agl
gpsdata->fix.time,
0); // itow
fix_time = gpsdata->fix.time;
}
}
static gboolean gps_periodic(gpointer data __attribute__ ((unused)))
{
struct timeval timeout;
int ret;
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(gpsdata->gps_fd, &rfds);
timeout.tv_sec = 0;
timeout.tv_usec = 100000;
ret = select(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &timeout);
if (ret == -1)
{
perror("socket error\n");
exit(2);
}
else if (ret) gps_poll(gpsdata);
return 1;
}
int main(int argc, char *argv[])
{
char *server = NULL, *port = DEFAULT_GPSD_PORT;
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
gpsdata = gps_open(server, port);
if (!gpsdata) perror("error connecting to gpsd");
gps_set_raw_hook(gpsdata, update_gps);
gps_query(gpsdata, "w+x\n");
gps_query(gpsdata, "j=1\n");
IvyInit ("GPSd2Ivy", "GPSd2Ivy READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, gps_periodic, NULL);
g_main_loop_run(ml);
return 0;
}