mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 19:17:28 +08:00
Merge branch 'gps_ucenter' into dev
This commit is contained in:
Felix Ruess
@@ -0,0 +1,29 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<!--
|
||||
|
||||
Automatically configure any Ublox GPS for paparazzi
|
||||
|
||||
-configures all the messages, and the rates
|
||||
-automatic baudrate detection
|
||||
|
||||
Warning: you still need to tell the driver
|
||||
a) which paparazzi uart you use
|
||||
b) inside the ublox gps there are also many ports. the tiny/ppzgps use ublox_internal_port1 but if for instance you use a LS-SAM or I2C device you need to configure:
|
||||
|
||||
<define name="GPS_PORT_ID" value="GPS_PORT_UART2" />
|
||||
|
||||
-->
|
||||
|
||||
<module name="gps_ubx" dir="gps">
|
||||
<header>
|
||||
<file name="gps_ubx_ucenter.h"/>
|
||||
</header>
|
||||
<init fun="gps_ubx_ucenter_init()"/>
|
||||
<periodic fun="gps_ubx_ucenter_periodic()" start="gps_ubx_ucenter_init()" freq="4." autorun="TRUE"/>
|
||||
<makefile target="ap">
|
||||
<define name="GPS_UBX_UCENTER" value="\\\"modules/gps/gps_ubx_ucenter.c\\\"" />
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
<!DOCTYPE settings SYSTEM "settings.dtd">
|
||||
|
||||
<settings>
|
||||
<dl_settings NAME="ublox">
|
||||
|
||||
<dl_settings name="ucenter">
|
||||
<dl_setting MAX="255" MIN="0" STEP="1" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.sw_ver_h" shortname="sw_h"/>
|
||||
<dl_setting MAX="255" MIN="0" STEP="1" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.sw_ver_l" shortname="sw_l"/>
|
||||
<dl_setting MAX="255" MIN="0" STEP="1" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.hw_ver_h" shortname="hw_h"/>
|
||||
<dl_setting MAX="255" MIN="0" STEP="1" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.hw_ver_l" shortname="hw_l"/>
|
||||
<dl_setting MAX="115200" MIN="4800" STEP="100" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.baud_init" shortname="inibaud"/>
|
||||
<dl_setting MAX="115200" MIN="4800" STEP="100" module="gps/gps_ubx_ucenter" VAR="gps_ubx_ucenter.baud_run" shortname="baud"/>
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -205,4 +205,17 @@
|
||||
|
||||
</class>
|
||||
|
||||
<class name="MON" ID="0x0A">
|
||||
|
||||
<message name="GET_VER" ID="0x04" length="0">
|
||||
</message>
|
||||
|
||||
<message name="VER" ID="0x04">
|
||||
<block length="1">
|
||||
<field name="c" format="U1"/>
|
||||
</block>
|
||||
</message>
|
||||
|
||||
</class>
|
||||
|
||||
</ubx>
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
|
||||
#define B1200 UART_BAUD(1200)
|
||||
#define B2400 UART_BAUD(2400)
|
||||
#define B4800 UART_BAUD(4800)
|
||||
#define B9600 UART_BAUD(9600)
|
||||
#define B19200 UART_BAUD(19200)
|
||||
#define B38400 UART_BAUD(38400)
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#define B4800 4800
|
||||
#define B9600 9600
|
||||
#define B38400 38400
|
||||
#define B57600 57600
|
||||
|
||||
@@ -581,12 +581,6 @@ void init_ap( void ) {
|
||||
/** wait 0.5s (historical :-) */
|
||||
sys_time_usleep(500000);
|
||||
|
||||
#ifdef GPS_CONFIGURE
|
||||
#ifndef SITL
|
||||
gps_configure_uart();
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined DATALINK
|
||||
|
||||
#if DATALINK == XBEE
|
||||
|
||||
@@ -92,7 +92,7 @@ extern void uart0_init(void);
|
||||
#define UART0ChAvailable Uart0ChAvailable
|
||||
#define UART0Getch Uart0Getch
|
||||
#define UART0TxRunning Uart0TxRunning
|
||||
#define UART0InitParam Uart0InitParam
|
||||
#define UART0SetBaudrate Uart0SetBaudrate
|
||||
|
||||
#endif // USE_UART0
|
||||
|
||||
@@ -117,7 +117,7 @@ extern void uart1_init(void);
|
||||
#define UART1ChAvailable Uart1ChAvailable
|
||||
#define UART1Getch Uart1Getch
|
||||
#define UART1TxRunning Uart1TxRunning
|
||||
#define UART1InitParam Uart1InitParam
|
||||
#define UART1SetBaudrate Uart1SetBaudrate
|
||||
|
||||
#endif // USE_UART1
|
||||
|
||||
@@ -142,7 +142,7 @@ extern void uart2_init(void);
|
||||
#define UART2ChAvailable Uart2ChAvailable
|
||||
#define UART2Getch Uart2Getch
|
||||
#define UART2TxRunning Uart2TxRunning
|
||||
#define UART2InitParam Uart2InitParam
|
||||
#define UART2SetBaudrate Uart2SetBaudrate
|
||||
|
||||
#endif // USE_UART2
|
||||
|
||||
@@ -167,7 +167,7 @@ extern void uart3_init(void);
|
||||
#define UART3ChAvailable Uart3ChAvailable
|
||||
#define UART3Getch Uart3Getch
|
||||
#define UART3TxRunning Uart3TxRunning
|
||||
#define UART3InitParam Uart3InitParam
|
||||
#define UART3SetBaudrate Uart3SetBaudrate
|
||||
|
||||
#endif // USE_UART3
|
||||
|
||||
@@ -192,7 +192,7 @@ extern void uart5_init(void);
|
||||
#define UART5ChAvailable Uart5ChAvailable
|
||||
#define UART5Getch Uart5Getch
|
||||
#define UART5TxRunning Uart5TxRunning
|
||||
#define UART5InitParam Uart5InitParam
|
||||
#define UART5SetBaudrate Uart5SetBaudrate
|
||||
|
||||
#endif // USE_UART5
|
||||
|
||||
|
||||
@@ -0,0 +1,410 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2011 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* Initial author: C. De Wagter
|
||||
*/
|
||||
|
||||
#include "gps_ubx_ucenter.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// UCENTER: init, periodic and event
|
||||
|
||||
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
|
||||
static bool_t gps_ubx_ucenter_configure(uint8_t nr);
|
||||
|
||||
#define GPS_UBX_UCENTER_STATUS_STOPPED 0
|
||||
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
|
||||
#define GPS_UBX_UCENTER_STATUS_CONFIG 2
|
||||
|
||||
#define GPS_UBX_UCENTER_REPLY_NONE 0
|
||||
#define GPS_UBX_UCENTER_REPLY_ACK 1
|
||||
#define GPS_UBX_UCENTER_REPLY_NACK 2
|
||||
#define GPS_UBX_UCENTER_REPLY_VERSION 3
|
||||
|
||||
// All U-Center data
|
||||
struct gps_ubx_ucenter_struct gps_ubx_ucenter;
|
||||
|
||||
|
||||
/////////////////////////////
|
||||
// Init Function
|
||||
|
||||
void gps_ubx_ucenter_init(void)
|
||||
{
|
||||
// Start UCenter
|
||||
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_AUTOBAUD;
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
gps_ubx_ucenter.cnt = 0;
|
||||
|
||||
gps_ubx_ucenter.baud_init = 0;
|
||||
gps_ubx_ucenter.baud_run = 0;
|
||||
|
||||
gps_ubx_ucenter.sw_ver_h = 0;
|
||||
gps_ubx_ucenter.sw_ver_l = 0;
|
||||
gps_ubx_ucenter.hw_ver_h = 0;
|
||||
gps_ubx_ucenter.hw_ver_l = 0;
|
||||
|
||||
for (int i=0; i<GPS_UBX_UCENTER_CONFIG_STEPS; i++)
|
||||
{
|
||||
gps_ubx_ucenter.replies[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/////////////////////////////
|
||||
// Periodic Function
|
||||
// -time-based configuration
|
||||
|
||||
void gps_ubx_ucenter_periodic(void)
|
||||
{
|
||||
switch (gps_ubx_ucenter.status)
|
||||
{
|
||||
// Save processing time inflight
|
||||
case GPS_UBX_UCENTER_STATUS_STOPPED:
|
||||
return;
|
||||
// Automatically Determine Current Baudrate
|
||||
case GPS_UBX_UCENTER_STATUS_AUTOBAUD:
|
||||
if (gps_ubx_ucenter_autobaud(gps_ubx_ucenter.cnt) == FALSE)
|
||||
{
|
||||
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
|
||||
gps_ubx_ucenter.cnt = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
gps_ubx_ucenter.cnt++;
|
||||
}
|
||||
break;
|
||||
// Send Configuration
|
||||
case GPS_UBX_UCENTER_STATUS_CONFIG:
|
||||
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE)
|
||||
{
|
||||
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
|
||||
gps_ubx_ucenter.cnt = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
gps_ubx_ucenter.cnt++;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// stop this module now...
|
||||
// todo
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/////////////////////////////
|
||||
// Event Function
|
||||
// -fetch replies: ACK and VERSION info
|
||||
|
||||
void gps_ubx_ucenter_event(void)
|
||||
{
|
||||
// Save processing time inflight
|
||||
if (gps_ubx_ucenter.status == GPS_UBX_UCENTER_STATUS_STOPPED)
|
||||
return;
|
||||
|
||||
// Read Configuration Reply's
|
||||
if (gps_ubx.msg_class == UBX_ACK_ID) {
|
||||
if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
|
||||
}
|
||||
else
|
||||
{
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
|
||||
}
|
||||
}
|
||||
// Version info
|
||||
else if (gps_ubx.msg_class == UBX_MON_ID) {
|
||||
if (gps_ubx.msg_id == UBX_MON_VER_ID ) {
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_VERSION;
|
||||
gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,0) - '0';
|
||||
gps_ubx_ucenter.sw_ver_l = 10*(UBX_MON_VER_c(gps_ubx.msg_buf,2) - '0');
|
||||
gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf,3) - '0';
|
||||
gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,33) - '0';
|
||||
gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0');
|
||||
gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0';
|
||||
gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// UCENTER Configuration Functions
|
||||
|
||||
|
||||
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
|
||||
{
|
||||
switch (nr)
|
||||
{
|
||||
case 0:
|
||||
case 1:
|
||||
// Very important for some modules:
|
||||
// Give the GPS some time to boot (up to 0.75 second)
|
||||
break;
|
||||
case 2:
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
GpsUartSetBaudrate(B38400); // Try the most common first?
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 3:
|
||||
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
|
||||
{
|
||||
gps_ubx_ucenter.baud_init = 38400;
|
||||
return FALSE;
|
||||
}
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
GpsUartSetBaudrate(B9600); // Maybe the factory default?
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 4:
|
||||
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
|
||||
{
|
||||
gps_ubx_ucenter.baud_init = 9600;
|
||||
return FALSE;
|
||||
}
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
GpsUartSetBaudrate(B57600); // The high-rate default?
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 5:
|
||||
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
|
||||
{
|
||||
gps_ubx_ucenter.baud_init = 57600;
|
||||
return FALSE;
|
||||
}
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
GpsUartSetBaudrate(B4800); // Default NMEA baudrate finally?
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 6:
|
||||
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
|
||||
{
|
||||
gps_ubx_ucenter.baud_init = 4800;
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// Autoconfig Failed... let's setup the failsafe baudrate
|
||||
// Should we try even a different baudrate?
|
||||
gps_ubx_ucenter.baud_init = 0;
|
||||
GpsUartSetBaudrate(B9600);
|
||||
return FALSE;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/////////////////////////////////////
|
||||
// UBlox internal Navigation Solution
|
||||
|
||||
#define NAV_DYN_STATIONARY 1
|
||||
#define NAV_DYN_PEDESTRIAN 2
|
||||
#define NAV_DYN_AUTOMOTIVE 3
|
||||
#define NAV_DYN_SEA 4
|
||||
#define NAV_DYN_AIRBORNE_1G 5
|
||||
#define NAV_DYN_AIRBORNE_2G 6
|
||||
#define NAV_DYN_AIRBORNE_4G 7
|
||||
|
||||
#define NAV5_DYN_PORTABLE 0
|
||||
#define NAV5_DYN_FIXED 1
|
||||
#define NAV5_DYN_STATIONARY 2
|
||||
#define NAV5_DYN_PEDESTRIAN 3
|
||||
#define NAV5_DYN_AUTOMOTIVE 4
|
||||
#define NAV5_DYN_SEA 5
|
||||
#define NAV5_DYN_AIRBORNE_1G 6
|
||||
#define NAV5_DYN_AIRBORNE_2G 7
|
||||
#define NAV5_DYN_AIRBORNE_4G 8
|
||||
|
||||
#define NAV5_2D_ONLY 1
|
||||
#define NAV5_3D_ONLY 2
|
||||
#define NAV5_AUTO 3
|
||||
|
||||
#define IGNORED 0
|
||||
#define RESERVED 0
|
||||
|
||||
static inline void gps_ubx_ucenter_config_nav(void)
|
||||
{
|
||||
//New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
|
||||
if (gps_ubx_ucenter.sw_ver_h < 5)
|
||||
{
|
||||
UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
|
||||
}
|
||||
else
|
||||
{
|
||||
UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/////////////////////////////////////
|
||||
// UBlox port and protocol GPS configuration
|
||||
|
||||
#define UBX_PROTO_MASK 0x0001
|
||||
#define NMEA_PROTO_MASK 0x0002
|
||||
#define RTCM_PROTO_MASK 0x0004
|
||||
|
||||
#define GPS_PORT_DDC 0x00
|
||||
#define GPS_PORT_UART1 0x01
|
||||
#define GPS_PORT_UART2 0x02
|
||||
#define GPS_PORT_USB 0x03
|
||||
#define GPS_PORT_SPI 0x04
|
||||
|
||||
#ifndef GPS_PORT_ID
|
||||
#define GPS_PORT_ID GPS_PORT_UART1
|
||||
#endif
|
||||
|
||||
#define __UBX_GPS_BAUD(_u) _u##_BAUD
|
||||
#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
|
||||
#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
|
||||
|
||||
static inline void gps_ubx_ucenter_config_port(void)
|
||||
{
|
||||
// I2C Interface
|
||||
#if GPS_PORT_ID == GPS_PORT_DDC
|
||||
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
|
||||
#endif
|
||||
// UART Interface
|
||||
#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
|
||||
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
|
||||
#endif
|
||||
}
|
||||
|
||||
#define GPS_SBAS_ENABLED 0x01
|
||||
|
||||
#define GPS_SBAS_RANGING 0x01
|
||||
#define GPS_SBAS_CORRECTIONS 0x02
|
||||
#define GPS_SBAS_INTEGRITY 0x04
|
||||
|
||||
static inline void gps_ubx_ucenter_config_sbas(void)
|
||||
{
|
||||
// Since March 2nd 2011 EGNOS is released for aviation purposes
|
||||
const uint8_t nrofsat = 1;
|
||||
UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, nrofsat, 0x00, 0x00);
|
||||
//UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
|
||||
}
|
||||
|
||||
static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
|
||||
{
|
||||
#if GPS_PORT_ID == GPS_PORT_UART1
|
||||
UbxSend_CFG_MSG(class, id, 0, rate, 0, 0);
|
||||
#endif
|
||||
#if GPS_PORT_ID == GPS_PORT_UART2
|
||||
UbxSend_CFG_MSG(class, id, 0, 0, rate, 0);
|
||||
#endif
|
||||
#if GPS_PORT_ID == GPS_PORT_DDC
|
||||
UbxSend_CFG_MSG(class, id, rate, 0, 0, 0);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// Text Telemetry for Debugging
|
||||
#ifndef DOWNLINK_DEVICE
|
||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||
#endif
|
||||
#undef GOT_PAYLOAD
|
||||
#include "downlink.h"
|
||||
|
||||
static bool_t gps_ubx_ucenter_configure(uint8_t nr)
|
||||
{
|
||||
// Store the reply of the last configuration step and reset
|
||||
if (nr < GPS_UBX_UCENTER_CONFIG_STEPS)
|
||||
gps_ubx_ucenter.replies[nr] = gps_ubx_ucenter.reply;
|
||||
|
||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
|
||||
|
||||
switch (nr) {
|
||||
case 0:
|
||||
UbxSend_MON_GET_VER();
|
||||
break;
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
case 4:
|
||||
// UBX_G5010 takes 0.7 seconds to answer a firmware request
|
||||
// Version info is important for porper configuration as different firmwares have different settings
|
||||
break;
|
||||
case 5:
|
||||
// Send some debugging info: detected baudrate, software version etc...
|
||||
gps_ubx_ucenter.replies[0] = (gps_ubx_ucenter.baud_init/1000);
|
||||
gps_ubx_ucenter.replies[1] = (gps_ubx_ucenter.baud_init - 1000 * gps_ubx_ucenter.replies[0]) / 100;
|
||||
gps_ubx_ucenter.replies[2] = gps_ubx_ucenter.sw_ver_h;
|
||||
gps_ubx_ucenter.replies[3] = gps_ubx_ucenter.sw_ver_l;
|
||||
gps_ubx_ucenter.replies[4] = gps_ubx_ucenter.hw_ver_h;
|
||||
gps_ubx_ucenter.replies[5] = gps_ubx_ucenter.hw_ver_l;
|
||||
DOWNLINK_SEND_DEBUG(DefaultChannel,6,gps_ubx_ucenter.replies);
|
||||
|
||||
//////////////////////////////////
|
||||
// Actual configuration start
|
||||
|
||||
// Use old baudrate to issue a baudrate change command
|
||||
gps_ubx_ucenter_config_port();
|
||||
break;
|
||||
case 6:
|
||||
// Now the GPS baudrate should have changed
|
||||
GpsUartSetBaudrate(B38400);
|
||||
gps_ubx_ucenter.baud_run = 38400;
|
||||
gps_ubx_ucenter_config_nav();
|
||||
break;
|
||||
case 7:
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID,1);
|
||||
break;
|
||||
case 8:
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1);
|
||||
break;
|
||||
case 9:
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1);
|
||||
break;
|
||||
case 10:
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
|
||||
break;
|
||||
case 11:
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
|
||||
break;
|
||||
case 12:
|
||||
// Disable UTM on old Lea4P
|
||||
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
|
||||
break;
|
||||
case 13:
|
||||
gps_ubx_ucenter_config_sbas();
|
||||
break;
|
||||
case 14:
|
||||
UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
|
||||
break;
|
||||
case 15:
|
||||
// Try to save on non-ROM devices...
|
||||
UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000);
|
||||
break;
|
||||
case 16:
|
||||
// Debug Downlink the result of all configuration steps: see messages
|
||||
DOWNLINK_SEND_DEBUG(DefaultChannel,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies);
|
||||
return FALSE;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return TRUE; // Continue, except for the last case
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2011 The Paparazzi Team
|
||||
*
|
||||
* 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 gps_ucenter_onboard.h
|
||||
* @brief Configure Ublox GPS
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef GPS_UBX_UCENTER_H
|
||||
#define GPS_UBX_UCENTER_H
|
||||
|
||||
/** U-Center Variables */
|
||||
#define GPS_UBX_UCENTER_CONFIG_STEPS 17
|
||||
|
||||
struct gps_ubx_ucenter_struct
|
||||
{
|
||||
uint8_t status;
|
||||
uint8_t reply;
|
||||
uint8_t cnt;
|
||||
|
||||
uint16_t baud_init;
|
||||
uint16_t baud_run;
|
||||
|
||||
uint8_t sw_ver_h;
|
||||
uint8_t sw_ver_l;
|
||||
|
||||
uint16_t hw_ver_h;
|
||||
uint16_t hw_ver_l;
|
||||
|
||||
char replies[GPS_UBX_UCENTER_CONFIG_STEPS];
|
||||
};
|
||||
|
||||
extern struct gps_ubx_ucenter_struct gps_ubx_ucenter;
|
||||
|
||||
extern void gps_ubx_ucenter_init(void);
|
||||
extern void gps_ubx_ucenter_periodic(void);
|
||||
extern void gps_ubx_ucenter_event(void);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -1,27 +0,0 @@
|
||||
#define IGNORED 0
|
||||
#define RESERVED 0
|
||||
|
||||
static bool_t user_gps_configure(bool_t cpt) {
|
||||
switch (cpt) {
|
||||
case 0:
|
||||
/* mask: we set only fixMode and dyn */
|
||||
UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED);
|
||||
break;
|
||||
case 1:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSLLH_ID, 1, 0, 0, 0);
|
||||
break;
|
||||
case 2:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1, 0, 0, 0);
|
||||
break;
|
||||
case 3:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4, 0, 0, 0);
|
||||
break;
|
||||
case 4:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SOL_ID, 8, 0, 0, 0);
|
||||
break;
|
||||
case 5:
|
||||
UbxSend_CFG_RATE(250 /*ms*/, 0x0001, 0x0000);
|
||||
return FALSE;
|
||||
}
|
||||
return TRUE; /* Continue, except for the last case */
|
||||
}
|
||||
@@ -53,7 +53,7 @@
|
||||
|
||||
|
||||
#define GpsUartSend1(c) GpsLink(Transmit(c))
|
||||
#define GpsUartInitParam(_a,_b,_c) GpsLink(InitParam(_a,_b,_c))
|
||||
#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
|
||||
#define GpsUartRunning GpsLink(TxRunning)
|
||||
#define GpsUartSendMessage GpsLink(SendMessage)
|
||||
|
||||
@@ -79,20 +79,11 @@
|
||||
|
||||
struct GpsUbx gps_ubx;
|
||||
|
||||
#ifdef GPS_CONFIGURE
|
||||
bool_t gps_configuring;
|
||||
static uint8_t gps_status_config;
|
||||
#endif
|
||||
|
||||
void gps_impl_init(void) {
|
||||
gps_ubx.status = UNINIT;
|
||||
gps_ubx.msg_available = FALSE;
|
||||
gps_ubx.error_cnt = 0;
|
||||
gps_ubx.error_last = GPS_UBX_ERR_NONE;
|
||||
#ifdef GPS_CONFIGURE
|
||||
gps_status_config = 0;
|
||||
gps_configuring = TRUE;
|
||||
#endif
|
||||
gps_ubx.have_velned = 0;
|
||||
}
|
||||
|
||||
@@ -276,116 +267,10 @@ void gps_ubx_parse( uint8_t c ) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*
|
||||
* GPS dynamic configuration
|
||||
*
|
||||
*
|
||||
*/
|
||||
#ifdef GPS_CONFIGURE
|
||||
|
||||
#define UBX_PROTO_MASK 0x0001
|
||||
#define NMEA_PROTO_MASK 0x0002
|
||||
#define RTCM_PROTO_MASK 0x0004
|
||||
|
||||
#define GPS_PORT_DDC 0x00
|
||||
#define GPS_PORT_UART1 0x01
|
||||
#define GPS_PORT_UART2 0x02
|
||||
#define GPS_PORT_USB 0x03
|
||||
#define GPS_PORT_SPI 0x04
|
||||
|
||||
#ifndef GPS_PORT_ID
|
||||
#define GPS_PORT_ID GPS_PORT_UART1
|
||||
#ifdef GPS_UBX_UCENTER
|
||||
#include GPS_UBX_UCENTER
|
||||
#endif
|
||||
|
||||
#define __UBX_GPS_BAUD(_u) _u##_BAUD
|
||||
#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
|
||||
#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
|
||||
|
||||
/* Configure the GPS baud rate using the current uart baud rate. Busy
|
||||
wait for the end of the transmit. Then, BEFORE waiting for the ACK,
|
||||
change the uart rate. */
|
||||
#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
|
||||
void gps_configure_uart(void) {
|
||||
#ifdef FMS_PERIODIC_FREQ
|
||||
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
|
||||
uint8_t loop=0;
|
||||
while (GpsUartRunning) {
|
||||
//doesn't work unless some printfs are used, so :
|
||||
if (loop<9) {
|
||||
printf("."); loop++;
|
||||
} else {
|
||||
printf("\b"); loop--;
|
||||
}
|
||||
}
|
||||
#else
|
||||
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, UBX_GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
|
||||
while (GpsUartRunning); /* FIXME */
|
||||
#endif
|
||||
|
||||
GpsUartInitParam(UBX_GPS_BAUD, UART_8N1, UART_FIFO_8);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if GPS_PORT_ID == GPS_PORT_DDC
|
||||
void gps_configure_uart(void) {
|
||||
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
|
||||
}
|
||||
#endif
|
||||
|
||||
#define IGNORED 0
|
||||
#define RESERVED 0
|
||||
|
||||
#ifdef USER_GPS_CONFIGURE
|
||||
#include USER_GPS_CONFIGURE
|
||||
#else
|
||||
static bool_t user_gps_configure(bool_t cpt) {
|
||||
switch (cpt) {
|
||||
case 0:
|
||||
//New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
|
||||
//UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
|
||||
UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED);
|
||||
break;
|
||||
case 1:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 2:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 3:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0);
|
||||
break;
|
||||
case 4:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0);
|
||||
break;
|
||||
case 5:
|
||||
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SOL_ID, 0, 8, 0, 0);
|
||||
break;
|
||||
case 6:
|
||||
UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
|
||||
break;
|
||||
case 7:
|
||||
UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
|
||||
return FALSE;
|
||||
}
|
||||
return TRUE; /* Continue, except for the last case */
|
||||
}
|
||||
#endif // ! USER_GPS_CONFIGURE
|
||||
|
||||
/* GPS configuration. Must be called on ack message reception while
|
||||
gps_status_config < GPS_CONFIG_DONE */
|
||||
void gps_configure( void ) {
|
||||
if (gps_ubx.msg_class == UBX_ACK_ID) {
|
||||
if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
|
||||
gps_status_config++;
|
||||
}
|
||||
}
|
||||
gps_configuring = user_gps_configure(gps_status_config);
|
||||
}
|
||||
#endif /* GPS_CONFIGURE */
|
||||
|
||||
void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
|
||||
#ifdef GPS_LINK
|
||||
|
||||
@@ -27,6 +27,10 @@
|
||||
#ifndef GPS_UBX_H
|
||||
#define GPS_UBX_H
|
||||
|
||||
#ifdef GPS_CONFIGURE
|
||||
#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE"
|
||||
#endif
|
||||
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
/** Includes macros generated from ubx.xml */
|
||||
@@ -66,18 +70,11 @@ extern struct GpsUbx gps_ubx;
|
||||
|
||||
#define GpsBuffer() GpsLink(ChAvailable())
|
||||
|
||||
#ifdef GPS_CONFIGURE
|
||||
extern bool_t gps_configuring;
|
||||
#define GpsParseOrConfigure() { \
|
||||
if (gps_configuring) \
|
||||
gps_configure(); \
|
||||
else \
|
||||
gps_ubx_read_message(); \
|
||||
}
|
||||
#else
|
||||
#define GpsParseOrConfigure() gps_ubx_read_message()
|
||||
#ifndef GPS_UBX_UCENTER
|
||||
#define gps_ubx_ucenter_event() {}
|
||||
#endif
|
||||
|
||||
|
||||
/* Gps callback is called when receiving a VELNED or a SOL message
|
||||
* All position/speed messages are sent in one shot and VELNED is the last one on fixedwing
|
||||
* For rotorcraft, only SOL message is needed for pos/speed data
|
||||
@@ -87,7 +84,8 @@ extern bool_t gps_configuring;
|
||||
ReadGpsBuffer(); \
|
||||
} \
|
||||
if (gps_ubx.msg_available) { \
|
||||
GpsParseOrConfigure(); \
|
||||
gps_ubx_read_message(); \
|
||||
gps_ubx_ucenter_event(); \
|
||||
if (gps_ubx.msg_class == UBX_NAV_ID && \
|
||||
(gps_ubx.msg_id == UBX_NAV_VELNED_ID || \
|
||||
(gps_ubx.msg_id == UBX_NAV_SOL_ID && \
|
||||
@@ -133,34 +131,4 @@ extern void ubxsend_cfg_rst(uint16_t, uint8_t);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* dynamic GPS configuration
|
||||
*/
|
||||
#ifdef GPS_CONFIGURE
|
||||
#define NAV_DYN_STATIONARY 1
|
||||
#define NAV_DYN_PEDESTRIAN 2
|
||||
#define NAV_DYN_AUTOMOTIVE 3
|
||||
#define NAV_DYN_SEA 4
|
||||
#define NAV_DYN_AIRBORNE_1G 5
|
||||
#define NAV_DYN_AIRBORNE_2G 6
|
||||
#define NAV_DYN_AIRBORNE_4G 7
|
||||
|
||||
#define NAV5_DYN_PORTABLE 0
|
||||
#define NAV5_DYN_FIXED 1
|
||||
#define NAV5_DYN_STATIONARY 2
|
||||
#define NAV5_DYN_PEDESTRIAN 3
|
||||
#define NAV5_DYN_AUTOMOTIVE 4
|
||||
#define NAV5_DYN_SEA 5
|
||||
#define NAV5_DYN_AIRBORNE_1G 6
|
||||
#define NAV5_DYN_AIRBORNE_2G 7
|
||||
#define NAV5_DYN_AIRBORNE_4G 8
|
||||
|
||||
#define NAV5_2D_ONLY 1
|
||||
#define NAV5_3D_ONLY 2
|
||||
#define NAV5_AUTO 3
|
||||
|
||||
extern void gps_configure(void);
|
||||
extern void gps_configure_uart(void);
|
||||
#endif
|
||||
|
||||
#endif /* GPS_UBX_H */
|
||||
|
||||
Reference in New Issue
Block a user