[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5245] abstracted booz_gps to support different prot
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [5245] abstracted booz_gps to support different protocols - added support for skytraq binary protocol |
|
Date: |
Fri, 06 Aug 2010 08:38:29 +0000 |
Revision: 5245
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5245
Author: poine
Date: 2010-08-06 08:38:28 +0000 (Fri, 06 Aug 2010)
Log Message:
-----------
abstracted booz_gps to support different protocols - added support for skytraq
binary protocol
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
paparazzi3/trunk/sw/airborne/booz/test/booz2_test_gps.c
Added Paths:
-----------
paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_skytraq.makefile
paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_ublox.makefile
paparazzi3/trunk/sw/airborne/booz/booz_gps.c
paparazzi3/trunk/sw/airborne/booz/booz_gps.h
paparazzi3/trunk/sw/airborne/booz/gps/
paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.c
paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.h
paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.c
paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.h
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml 2010-08-06 08:06:27 UTC
(rev 5244)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml 2010-08-06 08:38:28 UTC
(rev 5245)
@@ -225,7 +225,7 @@
include $(CFG_BOOZ)/subsystems/booz2_radio_control_ppm.makefile
include $(CFG_BOOZ)/subsystems/booz2_actuators_mkk.makefile
include $(CFG_BOOZ)/subsystems/booz2_imu_b2v1.makefile
-include $(CFG_BOOZ)/subsystems/booz2_gps.makefile
+include $(CFG_BOOZ)/subsystems/booz_gps_ublox.makefile
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-08-06 08:06:27 UTC
(rev 5244)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-08-06 08:38:28 UTC
(rev 5245)
@@ -192,7 +192,7 @@
include $(CFG_BOOZ)/subsystems/booz2_radio_control_spektrum.makefile
include $(CFG_BOOZ)/subsystems/booz2_actuators_asctec_v2.makefile
include $(CFG_BOOZ)/subsystems/booz2_imu_b2v1_1.makefile
-#include $(CFG_BOOZ)/subsystems/booz2_gps.makefile
+include $(CFG_BOOZ)/subsystems/booz_gps_ublox.makefile
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
Added: paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_skytraq.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_skytraq.makefile
(rev 0)
+++ paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_skytraq.makefile
2010-08-06 08:38:28 UTC (rev 5245)
@@ -0,0 +1,14 @@
+
+ap.srcs += $(SRC_BOOZ)/booz_gps.c
+ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_skytraq.h\"
+ap.srcs += $(SRC_BOOZ)/gps/booz_gps_skytraq.c
+ifeq ($(ARCHI), arm7)
+ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5
+ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart0 -DGPS_LED=4
+else ifeq ($(ARCHI), stm32)
+ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
+ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart1 -DGPS_LED=3
+endif
+
+sim.CFLAGS += -DUSE_GPS
+sim.srcs += $(SRC_BOOZ)/booz_gps.c
Copied: paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_ublox.makefile
(from rev 5233, paparazzi3/trunk/conf/autopilot/subsystems/booz2_gps.makefile)
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_ublox.makefile
(rev 0)
+++ paparazzi3/trunk/conf/autopilot/subsystems/booz_gps_ublox.makefile
2010-08-06 08:38:28 UTC (rev 5245)
@@ -0,0 +1,13 @@
+
+ap.srcs += $(SRC_BOOZ)/booz_gps.c
+ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_ubx.h\"
+ap.srcs += $(SRC_BOOZ)/gps/booz_gps_ubx.c
+ifeq ($(ARCHI), arm7)
+ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5
+ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart0 -DGPS_LED=4
+else ifeq ($(ARCHI), stm32)
+ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
+ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart1 -DGPS_LED=3
+endif
+sim.CFLAGS += -DUSE_GPS
+sim.srcs += $(SRC_BOOZ)/booz_gps.c
Copied: paparazzi3/trunk/sw/airborne/booz/booz_gps.c (from rev 5221,
paparazzi3/trunk/sw/airborne/booz/booz2_gps.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_gps.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/booz_gps.c 2010-08-06 08:38:28 UTC
(rev 5245)
@@ -0,0 +1,52 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_gps.h"
+
+#include "led.h"
+
+struct Booz_gps_state booz_gps_state;
+
+
+#ifdef SITL
+bool_t booz_gps_available;
+#endif
+
+
+void booz_gps_init(void) {
+#ifdef SITL
+ booz_gps_available = FALSE;
+#endif
+ booz_gps_state.fix = BOOZ2_GPS_FIX_NONE;
+#ifdef GPS_LED
+ LED_OFF(GPS_LED);
+#endif
+ booz_gps_impl_init();
+
+}
+
+
+
+
+
+
Copied: paparazzi3/trunk/sw/airborne/booz/booz_gps.h (from rev 5221,
paparazzi3/trunk/sw/airborne/booz/booz2_gps.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_gps.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/booz_gps.h 2010-08-06 08:38:28 UTC
(rev 5245)
@@ -0,0 +1,127 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_GPS_H
+#define BOOZ2_GPS_H
+
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "uart.h"
+
+struct Booz_gps_state {
+ struct EcefCoor_i ecef_pos; /* pos ECEF in cm */
+ struct EcefCoor_i ecef_vel; /* speed ECEF in cm/s */
+ struct LlaCoor_i lla_pos; /* pos LLA */
+ int32_t hmsl; /* above mean sea level */
+ uint32_t pacc; /* position accuracy */
+ uint32_t sacc; /* speed accuracy */
+ uint16_t pdop; /* dilution of precision */
+ uint8_t num_sv; /* number of sat in fix */
+ uint8_t fix; /* status of fix */
+ uint32_t tow; /* time of week in 1e-2s */
+
+ uint8_t lost_counter; /* updated at 4Hz */
+};
+
+extern struct Booz_gps_state booz_gps_state;
+
+
+/* GPS model specific implementation */
+
+#include BOOZ_GPS_TYPE_H
+extern void booz_gps_impl_init(void);
+
+
+
+/* UBX NAV SOL */
+#define BOOZ2_GPS_FIX_NONE 0x00
+#define BOOZ2_GPS_FIX_3D 0x03
+
+
+#include "ubx_protocol.h"
+
+#define GpsFixValid() (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)
+
+/*
+ * This part is used by the simulator to feed simulated data
+ *
+ */
+#ifdef SITL
+
+extern bool_t booz_gps_available;
+#define GPS_LINKChAvailable() (FALSE)
+#define GPS_LINKGetch() (TRUE)
+#include "nps_sensors.h"
+#include "flight_plan.h"
+
+static inline void booz_gps_feed_value() {
+ booz_gps_state.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
+ booz_gps_state.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
+ booz_gps_state.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
+ booz_gps_state.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
+ booz_gps_state.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
+ booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
+ booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
+ booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
+ booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100.;
+ booz_gps_state.hmsl = sensors.gps.hmsl * 100.;
+ booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
+ booz_gps_available = TRUE;
+}
+
+#define BoozGpsEvent(_sol_available_callback) { \
+ if (booz_gps_available) { \
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) \
+ booz_gps_lost_counter = 0; \
+ _sol_available_callback(); \
+ booz_gps_available = FALSE; \
+ } \
+ }
+#else /* ! SITL */
+/*
+ * This part is used by the autopilot to read data from a uart
+ *
+ */
+
+
+extern void booz_gps_init(void);
+
+static inline void booz_gps_periodic( void ) {
+ RunOnceEvery(128, booz_gps_state.lost_counter++; );
+}
+
+#define GpsIsLost() (booz_gps_state.lost_counter > 20) /* 4Hz -> 5s */
+
+
+#define __GpsLink(dev, _x) dev##_x
+#define _GpsLink(dev, _x) __GpsLink(dev, _x)
+#define GpsLink(_x) _GpsLink(GPS_LINK, _x)
+
+#define GpsBuffer() GpsLink(ChAvailable())
+
+
+
+
+#endif /* !SITL */
+
+#endif /* BOOZ2_GPS_H */
Added: paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.c 2010-08-06
08:38:28 UTC (rev 5245)
@@ -0,0 +1,156 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_gps.h"
+
+struct BoozGpsSkytraq booz_gps_skytraq;
+
+/* parser status */
+#define UNINIT 0
+#define GOT_SYNC1 1
+#define GOT_SYNC2 2
+#define GOT_LEN1 3
+#define GOT_LEN2 4
+#define GOT_ID 5
+#define GOT_PAYLOAD 6
+#define GOT_CHECKSUM 7
+#define GOT_SYNC3 8
+
+#include "my_debug_servo.h"
+#include "led.h"
+
+void booz_gps_impl_init(void) {
+
+ booz_gps_skytraq.status = UNINIT;
+
+
+ DEBUG_SERVO1_INIT();
+
+}
+
+
+void booz_gps_skytraq_read_message(void) {
+
+ DEBUG_S1_ON();
+
+ if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
+ booz_gps_state.ecef_pos.x =
SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf);
+ booz_gps_state.ecef_pos.y =
SKYTRAQ_NAVIGATION_DATA_ECEFY(booz_gps_skytraq.msg_buf);
+ booz_gps_state.ecef_pos.z =
SKYTRAQ_NAVIGATION_DATA_ECEFZ(booz_gps_skytraq.msg_buf);
+ booz_gps_state.ecef_vel.x =
SKYTRAQ_NAVIGATION_DATA_ECEFVX(booz_gps_skytraq.msg_buf);
+ booz_gps_state.ecef_vel.y =
SKYTRAQ_NAVIGATION_DATA_ECEFVY(booz_gps_skytraq.msg_buf);
+ booz_gps_state.ecef_vel.z =
SKYTRAQ_NAVIGATION_DATA_ECEFVZ(booz_gps_skytraq.msg_buf);
+ booz_gps_state.lla_pos.lat =
SKYTRAQ_NAVIGATION_DATA_LAT(booz_gps_skytraq.msg_buf);
+ booz_gps_state.lla_pos.lon =
SKYTRAQ_NAVIGATION_DATA_LON(booz_gps_skytraq.msg_buf);
+ booz_gps_state.lla_pos.alt =
SKYTRAQ_NAVIGATION_DATA_AEL(booz_gps_skytraq.msg_buf);
+ booz_gps_state.hmsl =
SKYTRAQ_NAVIGATION_DATA_ASL(booz_gps_skytraq.msg_buf);
+ // pacc;
+ // sacc;
+ // booz_gps_state.pdop =
SKYTRAQ_NAVIGATION_DATA_PDOP(booz_gps_skytraq.msg_buf);
+ booz_gps_state.num_sv =
SKYTRAQ_NAVIGATION_DATA_NumSV(booz_gps_skytraq.msg_buf);
+ booz_gps_state.fix =
SKYTRAQ_NAVIGATION_DATA_FixMode(booz_gps_skytraq.msg_buf);
+ booz_gps_state.tow =
SKYTRAQ_NAVIGATION_DATA_TOW(booz_gps_skytraq.msg_buf);
+ DEBUG_S2_TOGGLE();
+
+#ifdef GPS_LED
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
+ LED_ON(GPS_LED);
+ }
+ else {
+ LED_TOGGLE(GPS_LED);
+ }
+#endif
+ }
+
+ DEBUG_S1_OFF();
+}
+
+void booz_gps_skytraq_parse(uint8_t c) {
+ if (booz_gps_skytraq.status < GOT_PAYLOAD)
+ booz_gps_skytraq.checksum ^= c;
+ switch (booz_gps_skytraq.status) {
+ case UNINIT:
+ if (c == SKYTRAQ_SYNC1)
+ booz_gps_skytraq.status = GOT_SYNC1;
+ break;
+ case GOT_SYNC1:
+ if (c != SKYTRAQ_SYNC2) {
+ booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ booz_gps_skytraq.status = GOT_SYNC2;
+ break;
+ case GOT_SYNC2:
+ booz_gps_skytraq.len = c<<8;
+ booz_gps_skytraq.status = GOT_LEN1;
+ break;
+ case GOT_LEN1:
+ booz_gps_skytraq.len += c;
+ booz_gps_skytraq.status = GOT_LEN2;
+ if (booz_gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) {
+ booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG;
+ goto error;
+ }
+ break;
+ case GOT_LEN2:
+ booz_gps_skytraq.msg_id = c;
+ booz_gps_skytraq.msg_idx = 0;
+ booz_gps_skytraq.checksum = c;
+ booz_gps_skytraq.status = GOT_ID;
+ break;
+ case GOT_ID:
+ booz_gps_skytraq.msg_buf[booz_gps_skytraq.msg_idx] = c;
+ booz_gps_skytraq.msg_idx++;
+ if (booz_gps_skytraq.msg_idx >= booz_gps_skytraq.len-1) {
+ booz_gps_skytraq.status = GOT_PAYLOAD;
+ }
+ break;
+ case GOT_PAYLOAD:
+ if (c != booz_gps_skytraq.checksum) {
+ booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM;
+ goto error;
+ }
+ booz_gps_skytraq.status = GOT_CHECKSUM;
+ break;
+ case GOT_CHECKSUM:
+ if (c != SKYTRAQ_SYNC3) {
+ booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ booz_gps_skytraq.status = GOT_SYNC3;
+ break;
+ case GOT_SYNC3:
+ booz_gps_skytraq.msg_available = TRUE;
+ goto restart;
+ default:
+ booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED;
+ goto error;
+ }
+ return;
+ error:
+ booz_gps_skytraq.error_cnt++;
+ restart:
+ booz_gps_skytraq.status = UNINIT;
+ return;
+}
+
Added: paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_skytraq.h 2010-08-06
08:38:28 UTC (rev 5245)
@@ -0,0 +1,88 @@
+#ifndef BOOZ_GPS_SKYTRAQ_H
+#define BOOZ_GPS_SKYTRAQ_H
+
+/*
+ *
+*
+*/
+
+#define SKYTRAQ_SYNC1 0xA0
+#define SKYTRAQ_SYNC2 0xA1
+
+#define SKYTRAQ_SYNC3 0x0D
+#define SKYTRAQ_SYNC4 0x0A
+
+
+#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8
+
+#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t)
(*((uint8_t*)_payload+2-2))
+#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t)
(*((uint8_t*)_payload+3-2))
+
+//#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) (uint32_t)(_payload[7] +
(((uint32_t)_payload[6])<<8) + (((uint32_t)_payload[5])<<16) +
(((uint32_t)_payload[4])<<24))
+#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
__builtin_bswap32(*(uint32_t*)&_payload[ 6-2])
+#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) __builtin_bswap32(*(
int32_t*)&_payload[10-2])
+#define SKYTRAQ_NAVIGATION_DATA_LON(_payload) __builtin_bswap32(*(
int32_t*)&_payload[14-2])
+#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
__builtin_bswap32(*(uint32_t*)&_payload[18-2])
+#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
__builtin_bswap32(*(uint32_t*)&_payload[22-2])
+//#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload)
__builtin_bswap16(*(uint16_t*)&_payload[26-2])
+//#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)
__builtin_bswap(*(uint16_t*)&_payload[28-2])
+
+#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) __builtin_bswap32(*(
int32_t*)&_payload[36-2])
+#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) __builtin_bswap32(*(
int32_t*)&_payload[40-2])
+#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) __builtin_bswap32(*(
int32_t*)&_payload[44-2])
+#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) __builtin_bswap32(*(
int32_t*)&_payload[48-2])
+#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) __builtin_bswap32(*(
int32_t*)&_payload[52-2])
+#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) __builtin_bswap32(*(
int32_t*)&_payload[56-2])
+
+
+
+/* last error type */
+#define GPS_SKYTRAQ_ERR_NONE 0
+#define GPS_SKYTRAQ_ERR_OVERRUN 1
+#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG 2
+#define GPS_SKYTRAQ_ERR_CHECKSUM 3
+#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC 4
+#define GPS_SKYTRAQ_ERR_UNEXPECTED 5
+
+#define GPS_SKYTRAQ_MAX_PAYLOAD 255
+struct BoozGpsSkytraq {
+ uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD] __attribute__ ((aligned));
+ bool_t msg_available;
+ uint8_t msg_id;
+
+ uint8_t status;
+ uint16_t len;
+ uint8_t msg_idx;
+ uint8_t checksum;
+ uint8_t error_cnt;
+ uint8_t error_last;
+};
+
+extern struct BoozGpsSkytraq booz_gps_skytraq;
+
+extern void booz_gps_skytraq_read_message(void);
+extern void booz_gps_skytraq_parse(uint8_t c);
+
+#include "my_debug_servo.h"
+
+#define BoozGpsEvent(_sol_available_callback) {
\
+ if (GpsBuffer()) { \
+ ReadGpsBuffer(); \
+ } \
+ if (booz_gps_skytraq.msg_available) { \
+ booz_gps_skytraq_read_message(); \
+ if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) \
+ booz_gps_state.lost_counter = 0; \
+ _sol_available_callback(); \
+ }
\
+ booz_gps_skytraq.msg_available = FALSE; \
+ } \
+ }
+
+#define ReadGpsBuffer() { \
+ while (GpsLink(ChAvailable())&&!booz_gps_skytraq.msg_available) \
+ booz_gps_skytraq_parse(GpsLink(Getch()));
\
+ }
+#endif /* BOOZ_GPS_SKYTRAQ_H */
+
Added: paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.c 2010-08-06
08:38:28 UTC (rev 5245)
@@ -0,0 +1,171 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+
+#include "booz_gps.h"
+
+#include "led.h"
+
+/* parser status */
+#define UNINIT 0
+#define GOT_SYNC1 1
+#define GOT_SYNC2 2
+#define GOT_CLASS 3
+#define GOT_ID 4
+#define GOT_LEN1 5
+#define GOT_LEN2 6
+#define GOT_PAYLOAD 7
+#define GOT_CHECKSUM1 8
+
+/* last error type */
+#define GPS_UBX_ERR_NONE 0
+#define GPS_UBX_ERR_OVERRUN 1
+#define GPS_UBX_ERR_MSG_TOO_LONG 2
+#define GPS_UBX_ERR_CHECKSUM 3
+#define GPS_UBX_ERR_UNEXPECTED 4
+#define GPS_UBX_ERR_OUT_OF_SYNC 5
+
+struct BoosGpsUbx booz_gps_ubx;
+
+void booz_gps_impl_init(void) {
+ booz_gps_ubx.status = UNINIT;
+ booz_gps_ubx.msg_available = FALSE;
+ booz_gps_ubx.error_cnt = 0;
+ booz_gps_ubx.error_last = GPS_UBX_ERR_NONE;
+}
+
+
+void booz_gps_ubx_read_message(void) {
+
+ if (booz_gps_ubx.msg_class == UBX_NAV_ID) {
+ if (booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) {
+ booz_gps_state.fix = UBX_NAV_SOL_GPSfix(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(booz_gps_ubx.msg_buf);
+ booz_gps_state.pacc = UBX_NAV_SOL_Pacc(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(booz_gps_ubx.msg_buf);
+ booz_gps_state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(booz_gps_ubx.msg_buf);
+ booz_gps_state.sacc = UBX_NAV_SOL_Sacc(booz_gps_ubx.msg_buf);
+ booz_gps_state.pdop = UBX_NAV_SOL_PDOP(booz_gps_ubx.msg_buf);
+ booz_gps_state.num_sv = UBX_NAV_SOL_numSV(booz_gps_ubx.msg_buf);
+#ifdef GPS_LED
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
+ LED_ON(GPS_LED);
+ }
+ else {
+ LED_TOGGLE(GPS_LED);
+ }
+#endif
+ } else if (booz_gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
+ booz_gps_state.lla_pos.lat = UBX_NAV_POSLLH_LAT(booz_gps_ubx.msg_buf);
+ booz_gps_state.lla_pos.lon = UBX_NAV_POSLLH_LON(booz_gps_ubx.msg_buf);
+ booz_gps_state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(booz_gps_ubx.msg_buf)
/ 10;
+ booz_gps_state.hmsl = UBX_NAV_POSLLH_HMSL(booz_gps_ubx.msg_buf) /
10;
+ }
+ }
+}
+
+
+
+/* UBX parsing */
+
+void booz_gps_ubx_parse( uint8_t c ) {
+ if (booz_gps_ubx.status < GOT_PAYLOAD) {
+ booz_gps_ubx.ck_a += c;
+ booz_gps_ubx.ck_b += booz_gps_ubx.ck_a;
+ }
+ switch (booz_gps_ubx.status) {
+ case UNINIT:
+ if (c == UBX_SYNC1)
+ booz_gps_ubx.status++;
+ break;
+ case GOT_SYNC1:
+ if (c != UBX_SYNC2) {
+ booz_gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ booz_gps_ubx.ck_a = 0;
+ booz_gps_ubx.ck_b = 0;
+ booz_gps_ubx.status++;
+ break;
+ case GOT_SYNC2:
+ if (booz_gps_ubx.msg_available) {
+ /* Previous message has not yet been parsed: discard this one */
+ booz_gps_ubx.error_last = GPS_UBX_ERR_OVERRUN;
+ goto error;
+ }
+ booz_gps_ubx.msg_class = c;
+ booz_gps_ubx.status++;
+ break;
+ case GOT_CLASS:
+ booz_gps_ubx.msg_id = c;
+ booz_gps_ubx.status++;
+ break;
+ case GOT_ID:
+ booz_gps_ubx.len = c;
+ booz_gps_ubx.status++;
+ break;
+ case GOT_LEN1:
+ booz_gps_ubx.len |= (c<<8);
+ if (booz_gps_ubx.len > GPS_UBX_MAX_PAYLOAD) {
+ booz_gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG;
+ goto error;
+ }
+ booz_gps_ubx.msg_idx = 0;
+ booz_gps_ubx.status++;
+ break;
+ case GOT_LEN2:
+ booz_gps_ubx.msg_buf[booz_gps_ubx.msg_idx] = c;
+ booz_gps_ubx.msg_idx++;
+ if (booz_gps_ubx.msg_idx >= booz_gps_ubx.len) {
+ booz_gps_ubx.status++;
+ }
+ break;
+ case GOT_PAYLOAD:
+ if (c != booz_gps_ubx.ck_a) {
+ booz_gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM;
+ goto error;
+ }
+ booz_gps_ubx.status++;
+ break;
+ case GOT_CHECKSUM1:
+ if (c != booz_gps_ubx.ck_b) {
+ booz_gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM;
+ goto error;
+ }
+ booz_gps_ubx.msg_available = TRUE;
+ goto restart;
+ break;
+ default:
+ booz_gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED;
+ goto error;
+ }
+ return;
+ error:
+ booz_gps_ubx.error_cnt++;
+ restart:
+ booz_gps_ubx.status = UNINIT;
+ return;
+}
Added: paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/gps/booz_gps_ubx.h 2010-08-06
08:38:28 UTC (rev 5245)
@@ -0,0 +1,45 @@
+#ifndef BOOZ_GPS_UBX_H
+#define BOOZ_GPS_UBX_H
+
+#define GPS_UBX_MAX_PAYLOAD 255
+struct BoosGpsUbx {
+ bool_t msg_available;
+ uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned));
+ uint8_t msg_id;
+ uint8_t msg_class;
+
+ uint8_t status;
+ uint16_t len;
+ uint8_t msg_idx;
+ uint8_t ck_a, ck_b;
+ uint8_t error_cnt;
+ uint8_t error_last;
+};
+
+extern struct BoosGpsUbx booz_gps_ubx;
+
+extern void booz_gps_ubx_read_message(void);
+extern void booz_gps_ubx_parse(uint8_t c);
+
+#define BoozGpsEvent(_sol_available_callback) {
\
+ if (GpsBuffer()) { \
+ ReadGpsBuffer(); \
+ } \
+ if (booz_gps_ubx.msg_available) { \
+ booz_gps_ubx_read_message(); \
+ if (booz_gps_ubx.msg_class == UBX_NAV_ID && \
+ booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) { \
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) \
+ booz_gps_state.lost_counter = 0; \
+ _sol_available_callback(); \
+ }
\
+ booz_gps_ubx.msg_available = FALSE; \
+ } \
+ }
+
+#define ReadGpsBuffer() { \
+ while (GpsLink(ChAvailable())&&!booz_gps_ubx.msg_available) \
+ booz_gps_ubx_parse(GpsLink(Getch())); \
+ }
+
+#endif /* BOOZ_GPS_UBX_H */
Modified: paparazzi3/trunk/sw/airborne/booz/test/booz2_test_gps.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/booz2_test_gps.c 2010-08-06
08:06:27 UTC (rev 5244)
+++ paparazzi3/trunk/sw/airborne/booz/test/booz2_test_gps.c 2010-08-06
08:38:28 UTC (rev 5245)
@@ -26,11 +26,8 @@
#include "std.h"
#include "init_hw.h"
#include "sys_time.h"
-#include "led.h"
-#include "uart.h"
-#include "messages.h"
#include "downlink.h"
-#include "booz2_gps.h"
+#include "booz_gps.h"
#include "interrupt_hw.h"
static inline void main_init( void );
@@ -53,25 +50,38 @@
hw_init();
sys_time_init();
led_init();
- uart0_init();
- uart1_init();
- booz2_gps_init();
+ booz_gps_init();
int_enable();
}
static inline void main_periodic_task( void ) {
-
-
+ RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+ RunOnceEvery(128, { LED_PERIODIC();});
}
static inline void main_event_task( void ) {
- Booz2GpsEvent(on_gps_sol);
+ BoozGpsEvent(on_gps_sol);
}
static void on_gps_sol(void) {
- uint16_t foo = booz_gps_state.num_sv;
- DOWNLINK_SEND_BOOT(&foo);
+
+ DOWNLINK_SEND_BOOZ2_GPS( DefaultChannel,
+ &booz_gps_state.ecef_pos.x,
+ &booz_gps_state.ecef_pos.y,
+ &booz_gps_state.ecef_pos.z,
+ &booz_gps_state.lla_pos.lat,
+ &booz_gps_state.lla_pos.lon,
+ &booz_gps_state.lla_pos.alt,
+ &booz_gps_state.ecef_vel.x,
+ &booz_gps_state.ecef_vel.y,
+ &booz_gps_state.ecef_vel.z,
+ &booz_gps_state.pacc,
+ &booz_gps_state.sacc,
+ &booz_gps_state.tow,
+ &booz_gps_state.pdop,
+ &booz_gps_state.num_sv,
+ &booz_gps_state.fix);
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5245] abstracted booz_gps to support different protocols - added support for skytraq binary protocol,
antoine drouin <=