mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
abstracted booz_gps to support different protocols - added support for skytraq binary protocol
This commit is contained in:
Antoine Drouin
@@ -1,180 +0,0 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* 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 "booz2_gps.h"
|
||||
|
||||
#include "led.h"
|
||||
|
||||
struct Booz_gps_state booz_gps_state;
|
||||
|
||||
uint8_t booz_gps_lost_counter; /* updated at 4Hz, reset on gps event */
|
||||
|
||||
|
||||
#ifdef SITL
|
||||
bool_t booz_gps_available;
|
||||
#endif
|
||||
|
||||
/* misc */
|
||||
volatile uint8_t booz2_gps_nb_ovrn;
|
||||
|
||||
void booz2_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
|
||||
ubx_init();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void booz2_gps_read_ubx_message(void) {
|
||||
|
||||
if (ubx_class == UBX_NAV_ID) {
|
||||
if (ubx_id == UBX_NAV_SOL_ID) {
|
||||
booz_gps_state.fix = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
|
||||
booz_gps_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(ubx_msg_buf);
|
||||
booz_gps_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(ubx_msg_buf);
|
||||
booz_gps_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(ubx_msg_buf);
|
||||
booz_gps_state.pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
|
||||
booz_gps_state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(ubx_msg_buf);
|
||||
booz_gps_state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(ubx_msg_buf);
|
||||
booz_gps_state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
|
||||
booz_gps_state.sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
|
||||
booz_gps_state.pdop = UBX_NAV_SOL_PDOP(ubx_msg_buf);
|
||||
booz_gps_state.num_sv = UBX_NAV_SOL_numSV(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 (ubx_id == UBX_NAV_POSLLH_ID) {
|
||||
booz_gps_state.lla_pos.lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
|
||||
booz_gps_state.lla_pos.lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
|
||||
booz_gps_state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(ubx_msg_buf) / 10;
|
||||
booz_gps_state.hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf) / 10;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* UBX parsing */
|
||||
|
||||
bool_t ubx_msg_available;
|
||||
#define UBX_MAX_PAYLOAD 255
|
||||
uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD] __attribute__ ((aligned));
|
||||
uint8_t ubx_id;
|
||||
uint8_t ubx_class;
|
||||
|
||||
#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
|
||||
|
||||
static uint8_t ubx_status;
|
||||
static uint16_t ubx_len;
|
||||
static uint8_t ubx_msg_idx;
|
||||
static uint8_t ck_a, ck_b;
|
||||
|
||||
|
||||
void ubx_init(void) {
|
||||
ubx_status = UNINIT;
|
||||
ubx_msg_available = FALSE;
|
||||
}
|
||||
|
||||
void ubx_parse( uint8_t c ) {
|
||||
if (ubx_status < GOT_PAYLOAD) {
|
||||
ck_a += c;
|
||||
ck_b += ck_a;
|
||||
}
|
||||
switch (ubx_status) {
|
||||
case UNINIT:
|
||||
if (c == UBX_SYNC1)
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_SYNC1:
|
||||
if (c != UBX_SYNC2)
|
||||
goto error;
|
||||
ck_a = 0;
|
||||
ck_b = 0;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_SYNC2:
|
||||
if (ubx_msg_available) {
|
||||
/* Previous message has not yet been parsed: discard this one */
|
||||
booz2_gps_nb_ovrn++;
|
||||
goto error;
|
||||
}
|
||||
ubx_class = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_CLASS:
|
||||
ubx_id = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_ID:
|
||||
ubx_len = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_LEN1:
|
||||
ubx_len |= (c<<8);
|
||||
if (ubx_len > UBX_MAX_PAYLOAD)
|
||||
goto error;
|
||||
ubx_msg_idx = 0;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_LEN2:
|
||||
ubx_msg_buf[ubx_msg_idx] = c;
|
||||
ubx_msg_idx++;
|
||||
if (ubx_msg_idx >= ubx_len) {
|
||||
ubx_status++;
|
||||
}
|
||||
break;
|
||||
case GOT_PAYLOAD:
|
||||
if (c != ck_a)
|
||||
goto error;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_CHECKSUM1:
|
||||
if (c != ck_b)
|
||||
goto error;
|
||||
ubx_msg_available = TRUE;
|
||||
goto restart;
|
||||
break;
|
||||
default: break;
|
||||
}
|
||||
return;
|
||||
error:
|
||||
restart:
|
||||
ubx_status = UNINIT;
|
||||
return;
|
||||
}
|
||||
Reference in New Issue
Block a user