From f914a905566ed97b414c4475a22ff8a7a6f222a0 Mon Sep 17 00:00:00 2001
From: patacongo Last Updated: January 4, 2013 Last Updated: February 2, 2013CONFIG_DEBUG
: enables built-in debug options.
This includes more extensive parameter checking, debug assertions, and other debug logic.
- This option is also necessary (but not sufficient) to enable debug console output;
- Debug console output must also be enabled on a subsystem-by-subsystem basis as described below.
+ This option is also necessary (but not sufficient) to enable debug syslog output;
+ Debug syslog output must also be enabled on a subsystem-by-subsystem basis as described below.
CONFIG_DEBUG_VERBOSE
: If debug console output is enabled, the option enables more verbose debug output.
+ CONFIG_DEBUG_VERBOSE
: If debug syslog output is enabled, the option enables more verbose debug output.
Ignored if CONFIG_DEBUG
is not defined.
If only CONFIG_DEBUG
then the only output will be errors, warnings, and critical information.
- If CONFIG_DEBUG_VERBOSE
is defined in addition, then general debug comments will also be included in the console output.
+ If CONFIG_DEBUG_VERBOSE
is defined in addition, then general debug comments will also be included in the syslog output.
CONFIG_DEBUG_ENABLE
: Support an interface to enable or disable debug output.
+ CONFIG_SYSLOG_ENABLE
: Support an interface to enable or disable syslog output.
CONFIG_DEBUG_SYMBOLS
: build without optimization and with debug symbols (needed for use with a debugger).
diff --git a/nuttx/Kconfig b/nuttx/Kconfig
index 10d624efb..937bab2be 100644
--- a/nuttx/Kconfig
+++ b/nuttx/Kconfig
@@ -312,11 +312,12 @@ config DEBUG_VERBOSE
often annoying) output will be generated. This means there are two levels
of debug output: errors-only and everything.
-config DEBUG_ENABLE
+config SYSLOG_ENABLE
bool "Enable Debug Controls"
default n
---help---
- Support an interface to dynamically enable or disable debug output.
+ Support an interface called syslog_enable to dynamically enable or
+ disable debug output.
comment "Subsystem Debug Options"
diff --git a/nuttx/TODO b/nuttx/TODO
index 05e0fa99b..6b2c6c883 100644
--- a/nuttx/TODO
+++ b/nuttx/TODO
@@ -1,4 +1,4 @@
-NuttX TODO List (Last updated January 27, 2013)
+NuttX TODO List (Last updated January 28, 2013)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This file summarizes known NuttX bugs, limitations, inconsistencies with
@@ -14,7 +14,7 @@ nuttx/
(6) Binary loaders (binfmt/)
(16) Network (net/, drivers/net)
(4) USB (drivers/usbdev, drivers/usbhost)
- (12) Libraries (libc/, )
+ (11) Libraries (libc/, )
(9) File system/Generic drivers (fs/, drivers/)
(5) Graphics subystem (graphics/)
(1) Pascal add-on (pcode/)
@@ -812,19 +812,6 @@ o Libraries (libc/)
Status: Open
Priority: ??
- Title: SYSLOG INTEGRATION
- Description: There are the beginnings of some system logging capabilities (see
- drivers/syslog, fs/fs_syslog.c, and libc/stdio/lib_librawprintf.c and
- lib_liblowprintf.c. For NuttX, SYSLOG is a concept and includes,
- extends, and replaces the legacy NuttX debug ouput. Some additional
- integration is required to formalized this. For example:
-
- o lib_rawprintf() shjould be renamed syslog().
- o debug.h should be renamed syslog.h
- o And what about lib_lowprintf()? llsyslog?
- Status: Open
- Priority: Low -- more of a roadmap
-
Title: FLOATING POINT FORMATS
Description: Only the %f floating point format is supported. Others are accepted
but treated like %f.
diff --git a/nuttx/arch/arm/src/arm/up_assert.c b/nuttx/arch/arm/src/arm/up_assert.c
index 3d75a4655..3cc79df01 100644
--- a/nuttx/arch/arm/src/arm/up_assert.c
+++ b/nuttx/arch/arm/src/arm/up_assert.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/* The following is just intended to keep some ugliness out of the mainline
@@ -70,7 +70,7 @@
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
- * defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
+ * defined(CONFIG_ARCH_STACKDUMP) <-- Or lowsyslog() is used
*/
#undef CONFIG_PRINT_TASKNAME
diff --git a/nuttx/arch/arm/src/arm/up_dataabort.c b/nuttx/arch/arm/src/arm/up_dataabort.c
index f01941968..60ef8385e 100644
--- a/nuttx/arch/arm/src/arm/up_dataabort.c
+++ b/nuttx/arch/arm/src/arm/up_dataabort.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/arm/src/arm/up_prefetchabort.c b/nuttx/arch/arm/src/arm/up_prefetchabort.c
index 9af644fc0..9d028b0d4 100644
--- a/nuttx/arch/arm/src/arm/up_prefetchabort.c
+++ b/nuttx/arch/arm/src/arm/up_prefetchabort.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/arm/src/arm/up_syscall.c b/nuttx/arch/arm/src/arm/up_syscall.c
index 1bcd66502..eb9bac8ad 100644
--- a/nuttx/arch/arm/src/arm/up_syscall.c
+++ b/nuttx/arch/arm/src/arm/up_syscall.c
@@ -56,7 +56,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/arm/src/arm/up_undefinedinsn.c b/nuttx/arch/arm/src/arm/up_undefinedinsn.c
index d61abe11b..88e3a79d9 100644
--- a/nuttx/arch/arm/src/arm/up_undefinedinsn.c
+++ b/nuttx/arch/arm/src/arm/up_undefinedinsn.c
@@ -54,7 +54,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/arm/src/armv7-m/up_assert.c b/nuttx/arch/arm/src/armv7-m/up_assert.c
index ab30b09f3..8e29bfe80 100644
--- a/nuttx/arch/arm/src/armv7-m/up_assert.c
+++ b/nuttx/arch/arm/src/armv7-m/up_assert.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/* The following is just intended to keep some ugliness out of the mainline
@@ -70,7 +70,7 @@
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
- * defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
+ * defined(CONFIG_ARCH_STACKDUMP) <-- Or lowsyslog() is used
*/
#undef CONFIG_PRINT_TASKNAME
diff --git a/nuttx/arch/arm/src/dm320/dm320_decodeirq.c b/nuttx/arch/arm/src/dm320/dm320_decodeirq.c
index c7032c4b1..98ba76097 100644
--- a/nuttx/arch/arm/src/dm320/dm320_decodeirq.c
+++ b/nuttx/arch/arm/src/dm320/dm320_decodeirq.c
@@ -74,7 +74,7 @@
void up_decodeirq(uint32_t* regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/arm/src/imx/imx_decodeirq.c b/nuttx/arch/arm/src/imx/imx_decodeirq.c
index ba37a60a2..230a4fc20 100644
--- a/nuttx/arch/arm/src/imx/imx_decodeirq.c
+++ b/nuttx/arch/arm/src/imx/imx_decodeirq.c
@@ -74,7 +74,7 @@
void up_decodeirq(uint32_t* regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/arm/src/lpc214x/lpc214x_decodeirq.c b/nuttx/arch/arm/src/lpc214x/lpc214x_decodeirq.c
index 652fe4d61..057e526c2 100644
--- a/nuttx/arch/arm/src/lpc214x/lpc214x_decodeirq.c
+++ b/nuttx/arch/arm/src/lpc214x/lpc214x_decodeirq.c
@@ -112,7 +112,7 @@ static void lpc214x_decodeirq( uint32_t *regs)
#endif
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/arm/src/lpc2378/lpc23xx_decodeirq.c b/nuttx/arch/arm/src/lpc2378/lpc23xx_decodeirq.c
index a55ed339d..3a50beb50 100644
--- a/nuttx/arch/arm/src/lpc2378/lpc23xx_decodeirq.c
+++ b/nuttx/arch/arm/src/lpc2378/lpc23xx_decodeirq.c
@@ -111,7 +111,7 @@ static void lpc23xx_decodeirq(uint32_t *regs)
#endif
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/arm/src/lpc31xx/lpc31_decodeirq.c b/nuttx/arch/arm/src/lpc31xx/lpc31_decodeirq.c
index 41e1d3e13..0e73f131b 100644
--- a/nuttx/arch/arm/src/lpc31xx/lpc31_decodeirq.c
+++ b/nuttx/arch/arm/src/lpc31xx/lpc31_decodeirq.c
@@ -77,7 +77,7 @@
void up_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c
index 18687c6f4..c2d638dd4 100644
--- a/nuttx/arch/arm/src/stm32/stm32_i2c.c
+++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c
@@ -159,7 +159,7 @@
#endif
/* I2C event trace logic. NOTE: trace uses the internal, non-standard, low-level
- * debug interface lib_rawprintf() but does not require that any other debug
+ * debug interface syslog() but does not require that any other debug
* is enabled.
*/
@@ -899,11 +899,11 @@ static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv)
struct stm32_trace_s *trace;
int i;
- lib_rawprintf("Elapsed time: %d\n", clock_systimer() - priv->start_time);
+ syslog("Elapsed time: %d\n", clock_systimer() - priv->start_time);
for (i = 0; i <= priv->tndx; i++)
{
trace = &priv->trace[i];
- lib_rawprintf("%2d. STATUS: %08x COUNT: %3d EVENT: %2d PARM: %08x TIME: %d\n",
+ syslog("%2d. STATUS: %08x COUNT: %3d EVENT: %2d PARM: %08x TIME: %d\n",
i+1, trace->status, trace->count, trace->event, trace->parm,
trace->time - priv->start_time);
}
diff --git a/nuttx/arch/arm/src/str71x/str71x_decodeirq.c b/nuttx/arch/arm/src/str71x/str71x_decodeirq.c
index 326abd763..48c9aa7e4 100644
--- a/nuttx/arch/arm/src/str71x/str71x_decodeirq.c
+++ b/nuttx/arch/arm/src/str71x/str71x_decodeirq.c
@@ -91,7 +91,7 @@ void up_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
up_ledon(LED_INIRQ);
- lib_lowprintf("Unexpected IRQ\n");
+ lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
diff --git a/nuttx/arch/avr/src/avr/up_dumpstate.c b/nuttx/arch/avr/src/avr/up_dumpstate.c
index 902c2ce9f..3ce595bf0 100644
--- a/nuttx/arch/avr/src/avr/up_dumpstate.c
+++ b/nuttx/arch/avr/src/avr/up_dumpstate.c
@@ -63,7 +63,7 @@
*/
#undef lldbg
-#define lldbg lib_lowprintf
+#define lldbg lowsyslog
/****************************************************************************
* Private Data
diff --git a/nuttx/arch/avr/src/avr32/up_dumpstate.c b/nuttx/arch/avr/src/avr32/up_dumpstate.c
index 15a3e7ef8..d6113d40b 100644
--- a/nuttx/arch/avr/src/avr32/up_dumpstate.c
+++ b/nuttx/arch/avr/src/avr32/up_dumpstate.c
@@ -63,7 +63,7 @@
*/
#undef lldbg
-#define lldbg lib_lowprintf
+#define lldbg lowsyslog
/****************************************************************************
* Private Data
diff --git a/nuttx/arch/avr/src/common/up_assert.c b/nuttx/arch/avr/src/common/up_assert.c
index 12bb564d3..3e90094eb 100644
--- a/nuttx/arch/avr/src/common/up_assert.c
+++ b/nuttx/arch/avr/src/common/up_assert.c
@@ -61,7 +61,7 @@
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
- * defined(CONFIG_ARCH_STACKDUMP)) <-- Or lib_lowprintf() is used
+ * defined(CONFIG_ARCH_STACKDUMP)) <-- Or lowsyslog() is used
*/
#undef CONFIG_PRINT_TASKNAME
@@ -75,7 +75,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/hc/src/m9s12/m9s12_assert.c b/nuttx/arch/hc/src/m9s12/m9s12_assert.c
index ff9ce5ca4..65cc75590 100644
--- a/nuttx/arch/hc/src/m9s12/m9s12_assert.c
+++ b/nuttx/arch/hc/src/m9s12/m9s12_assert.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/* The following is just intended to keep some ugliness out of the mainline
@@ -70,7 +70,7 @@
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
- * defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
+ * defined(CONFIG_ARCH_STACKDUMP) <-- Or lowsyslog() is used
*/
#undef CONFIG_PRINT_TASKNAME
diff --git a/nuttx/arch/mips/src/mips32/up_assert.c b/nuttx/arch/mips/src/mips32/up_assert.c
index ed4ee4cf7..7d98e7427 100644
--- a/nuttx/arch/mips/src/mips32/up_assert.c
+++ b/nuttx/arch/mips/src/mips32/up_assert.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/* The following is just intended to keep some ugliness out of the mainline
@@ -70,7 +70,7 @@
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
- * defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
+ * defined(CONFIG_ARCH_STACKDUMP) <-- Or lowsyslog() is used
*/
#undef CONFIG_PRINT_TASKNAME
diff --git a/nuttx/arch/mips/src/mips32/up_dumpstate.c b/nuttx/arch/mips/src/mips32/up_dumpstate.c
index 866c17b4f..1cd426767 100644
--- a/nuttx/arch/mips/src/mips32/up_dumpstate.c
+++ b/nuttx/arch/mips/src/mips32/up_dumpstate.c
@@ -63,7 +63,7 @@
*/
#undef lldbg
-#define lldbg lib_lowprintf
+#define lldbg lowsyslog
/****************************************************************************
* Private Data
diff --git a/nuttx/arch/sh/src/common/up_assert.c b/nuttx/arch/sh/src/common/up_assert.c
index ccc0c98e6..5b56e7731 100644
--- a/nuttx/arch/sh/src/common/up_assert.c
+++ b/nuttx/arch/sh/src/common/up_assert.c
@@ -61,7 +61,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/sh/src/m16c/m16c_dumpstate.c b/nuttx/arch/sh/src/m16c/m16c_dumpstate.c
index 438ff21d2..38571a7e1 100644
--- a/nuttx/arch/sh/src/m16c/m16c_dumpstate.c
+++ b/nuttx/arch/sh/src/m16c/m16c_dumpstate.c
@@ -62,7 +62,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/sh/src/sh1/sh1_dumpstate.c b/nuttx/arch/sh/src/sh1/sh1_dumpstate.c
index 296e20a45..f54ea46ce 100644
--- a/nuttx/arch/sh/src/sh1/sh1_dumpstate.c
+++ b/nuttx/arch/sh/src/sh1/sh1_dumpstate.c
@@ -61,7 +61,7 @@
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
-# define lldbg lib_lowprintf
+# define lldbg lowsyslog
#endif
/****************************************************************************
diff --git a/nuttx/arch/sim/src/up_initialize.c b/nuttx/arch/sim/src/up_initialize.c
index de11c7947..f8ae563e0 100644
--- a/nuttx/arch/sim/src/up_initialize.c
+++ b/nuttx/arch/sim/src/up_initialize.c
@@ -85,13 +85,13 @@
void up_initialize(void)
{
- /* The real purpose of the following is to make sure that lib_rawprintf
+ /* The real purpose of the following is to make sure that syslog
* is drawn into the link. It is needed by up_tapdev which is linked
* separately.
*/
#ifdef CONFIG_NET
- lib_rawprintf("SIM: Initializing");
+ syslog("SIM: Initializing");
#endif
/* Register devices */
diff --git a/nuttx/arch/sim/src/up_tapdev.c b/nuttx/arch/sim/src/up_tapdev.c
index f2bbcc14a..197049a34 100644
--- a/nuttx/arch/sim/src/up_tapdev.c
+++ b/nuttx/arch/sim/src/up_tapdev.c
@@ -61,7 +61,7 @@
#include
@@ -456,6 +456,13 @@
NuttX RTOS
-
+
- The 91st release of NuttX, Version 6.24, was made on December 20, 2012, and is available for download from the
+ The 92nd release of NuttX, Version 6.25, was made on February 1, 2013, and is available for download from the
SourceForge website.
- Note that the release consists of two tarballs: nuttx-6.24.tar.gz
and apps-6.24.tar.gz
.
+ Note that the release consists of two tarballs: nuttx-6.25.tar.gz
and apps-6.25.tar.gz
.
Both may be needed (see the top-level nuttx/README.txt
file for build information)
The change log associated with the release is available here.
Unreleased changes after this release are available in SVN.
These unreleased changes are also listed here.
- This release corresponds with SVN release number: r5447, + This release corresponds with SVN release number: r5594, Note that all SVN information has been stripped from the tarballs. If you need the SVN configuration, you should check out directly from SVN. - Revision r5447 should equivalent to release 6.24 of NuttX: + Revision r5594 should equivalent to release 6.25 of NuttX:
-svn checkout -r5447 svn://svn.code.sf.net/p/nuttx/code/trunk nuttx-code +svn checkout -r5594 svn://svn.code.sf.net/p/nuttx/code/trunk nuttx-code
Or (HTTP):
-svn checkout -r5447 http://svn.code.sf.net/p/nuttx/code/trunk nuttx-code +svn checkout -r5594 http://svn.code.sf.net/p/nuttx/code/trunk nuttx-code
Additional new features and extended functionality
-- RTOS: + OS Initialization +
+CONFIG_BUILTIN_APP_START
.
+ This is not really a useful feature and creates a violation of the OS layered architecture.
+ + Task Creation +
+vfork()
).
+ In NuttX-6.25, this interface is available only for ARM7/9, ARMv7-M (Cortext-M3/4), and MIPS32 (PIC32MX) platforms.
+ exec()
now sets the priority of the new task to the same priority as the parent task (instead of the arbirtrary value of 50).
+ execv()
and execl()
.
+ These are only partially compliant because they do not overlay any existing "process space" but rather create the new task and exit()
.
+ posix_spawn()
.
+ This standard interface is a better match for an MMU-less architecture than are vfork()
) plus execv()
or execl()
.
+ + Task Parentage
pause()
function (still has some compiance issues).
+ Repartitioned tasking data structures.
+ All shared resources are now collected together in a "task group".
+ A "task group" includes the resource for the original task that are shared with all of the pthreads
created by the task.
+ binfmt
), the OS will propogate that environment to child threads and will destroy the address environment when all threads in the "process" exists.
+ Implement support for retaining child task status in the "task group" after the child task exists.
+ This is behavior required by POSIX.
+ But in NuttX is only enabled with CONFIG_SCHED_HAVE_PARENT
and CONFIG_SCHED_CHILD_STATUS
.
PATH
variable is enabled, the OS start up logic will create an initial environment containing the default PATH
setting (CONFIG_PATH_INITIAL
).
- This initial PATH
will then be inherited by all tasks.
+ Add internal logic to "reparent" a task.
+ This is useful, for example, where the child task is created through a trampoline task that redirects I/O.
+ Reparenting allows the caller of posix_spawn()
to be reparented for the eventual child thread.
+ SIGCHLD
.
+ Sent to all members of the parent "task group" when the finall member of the child task group exits.
+ SIGCHLD
and retention of child task exist status are enabled, then a more spec-compliant version of waitpid()
is enabled.
+ waitid()
and wait()
are also enabled when SIGCHLD
is enabled.
- Binfmt
:
+ File System
PATH
environment variable.
- Now, if the PATH
is properly defined, programs can be executed from mass storage using only the file name.
- This feature is added to support more standard behavior (eventually, NSH will support execution of programs in file systems by just entering the file name, perhaps in 6.25?).
+ dup()
and dup2()
can new be used with opened files in a mounted file system.
+ This supports re-direction of output in NSH to files.
+ binfs
file system was moved from apps/builtin
to fs/binfs
.
+ The binfs
file system was extended to support execution of "builtin applications" using exec()
, execv()
, execl()
, or posix_spawn()
.
include/nuttx/arch.h
(currently, this is only supported by the z180).
+ Added logic based on SIGCHLD
to automatically unload and clean-up after running a task that was loaded into memory.
- Drivers: - LCD driver for the Univision UG-2864AMBAG01 OLED + Binary Formats +
+apps/builtin
to nuttx/binfmt/libbuiltin
.
+ Includes some extensions contributed by Mike Smith.
+ exec()
, execv()
, execl()
, or posix_spawn()
.
+ - STM32: - Support for STM32F100 high density chips contributed by Freddie Chopin. + Drivers +
+apps/examples
hidkbd/
and keypadtest/
(the latter contributed by Denis Carlikli).
+ CONFIG_SERIAL_REMOVABLE
.
+ - STM32 Drivers: - Added optional RS-485 direction bit control (from Freddie Chopin). + ARMv7-M +
+BASEPRI
register to disable interrupts (instead of the PRIMASK
register).
+ This eliminates some innocuous hardfaults that interfere with some debug tools.
+ You need to switch to the BASEPRI
method only if you have such tool interference.
+ - STM32 Boards: + STM32 Drivers
stm32f4discovery/nxlines
: STM32F4Discovery support for the UG-2864AMBAG01 OLED.
+ Add support for USART single wire mode (Contributed by the PX4 team).
stm32f4discovery/winbuild
: A version of the NuttX OS test configured to build natively on Windows.
+ Updates to support for SPI DMA on the STM32 F1/F2/F4.
+ From Petteri Aimonen.
+ STM32 Boards +
+stm32f4discovery/elf
: Now uses the PATH
variable to find ELF executables.
+ New configuration to support the UG-2864HSWEG01 OLED on the STM32F4Discovery board.
configs/cloudctrl
: Added for Darcy Gong's CloudController board
+ Added a posix_spawn()
test configuration for the STM32F4Discovery.
- PIC32 Boards: - Update the Mirtoo configuration for Release 2 of the Mirtoo module. + LM3S/LM4F +
+- Calypso: - Add Calypso keypad driver (from Denis Carilki). + LM3S Boards +
+- ZiLOG: + LPC176x/LPC178x
+ LPC176x/LPC178x Boards +
+- Graphics: + LPC2148 Boards
nxtk_subwindowmove()
and nxtk_getwindow()
: Improvements to clipping logic from Petteri Aimonen.
-
- C Library:
- lib/
sub-directory renamed libc/
(there is a new lib/
sub-directory that is used to hold all archives).
+ Simulator
+
- C++: - Exception stubs from Petteri Aimonen. + Networking +
+net_poll()
to create the internal interface psock_poll()
.
+ - Applications: + LCDs
ifconfig
command plus various DHCPC improvements (from Darcy Gong).
+ Added support for LCD1602 alphanumeric LCD (HD4468OU controller).
- apps/examples: + Graphics
ostest
: Replace large tables with algorithmic prime number generation.
- This allows the roundrobin test to run on platforms with minimal SRAM (Freddie Chopin).
+ Added 5x8 monospace font.
+ This tiny font is useful for graph labels and for small bitmapped display.
+ Contributed by Petteri Aimonen.
+ Build System +
+keypadtest
: A new keypad test example contributed by Denis Carikli.
+ Add an options to better manage toolchain prefixes.
elf
and nxflat
: If CONFIG_BINFMT_EXEPATH
is defined, these examples will now use a relative path to the program and expect the binfmt/
logic to find the absolute path to the program using the PATH
variable.
+ Redesigned how the context targer works in the apps/
directory.
+ The old design caused lots of problems when changing configurations because there is no easy way to get the system to rebuild the context.
+ This change should solve most the problems and eliminate questions like "Why don't I see my builtin application in NSH?"
- Build system: + Kconfig Files
- New top-level Makefiles: Makefile.unix
and Makefile.win
(along with numerous changes to other make-related files).
- This adds basic support for building NuttX natively under from Windows console (rather than in a POSIX-like environment).
- This build: (1) Uses all Windows style paths, (2) Uses primarily standard Windows batch commands with (3) a few additional commands from GNUWin32 (such as GNU make).
+ There are several new configurations that use the kconfig-frontends tools and several older configurations that have been converted to use these tools.
+ There is still a long way to go before the conversion is complete:
- This capability should still be considered a work in progress because: (1) It has not been verfied on all targets and tools, and (2) it still lacks some of the creature-comforts of the more mature environments (like a functional configure.sh
script and make menuconfig
support).
+
configs/sim/nxwm
+ configs/sim/nsh
+ configs/stm3220g-eval/nxwm
+ configs/stm32f4discovery/posix_spawn
+ configs/olimex-lpc1766stk/nsh
+ configs/olimex-lpc1766stk/hidkbd
+ configs/olimex-lpc1766stk/nettest
+ configs/open1788/ostest
+ configs/stm32f4discovery/nsh
+ configs/stm32f4discovery/usbnsh
+ configs/lm326965-ek
(all configurations)
+ configs/mcu123-214x/nsh
+ configs/ubw32/ostest
+ + Tools +
+tools/kconfig.bat
: Kludge to run kconfig-frontends from a DOS shell.
kconfig-frontends
mconf
configuration tool: stm32f4discovery/nxlines
, and all eZ80, z16f, z8, Z80, and Z180 configurations.
+ tools/configure.c
: configure.c
can be used to build a work-alike program as a replacement for configure.sh
.
+ This work-alike program would be used in environments that do not support Bash scripting (such as the Windows native environment).
Toolchain.defs
file that can be used to manage toolchains in a more configurable way (most of this contributed by Mike Smith).
+ tools/configure.bat
: configure.bat
is a small Windows batch file that can be used as a replacement for configure.sh
in a Windows native environment.
+ configure.bat
is actually just a thin layer that executes configure.exe
if it is available.
+ If configure.exe
is not available, then configure.bat will attempt to build it first (assumes the MinGW-GCC is available).
- Build tools: + Applications
tools/winlink.sh
to tools/copydir.sh
.
+ New and modified examples:
+ +
apps/examples/wlan
: Remove non-functional example.
+ apps/examples/ostest
: Added a test of vfork()
).
+ Extend signal handler test to catch death-of-child signals (SIGCHLD
).
+ Add a test for waitpid()
, waitid()
, and wait()
.
+ apps/exampes/posix_spawn
: Added a test of posix_spawn()
.
+
tools/mkdeps.bat
, tools/mkdeps.c
, tools/link.bat
, tools/unlink.bat
, and tools/ copydir.bat
.
+ NSH:
+ +
posix_spawn()
.
+ init.d/rcS
script will be executed once when NSH starts;
+ the .nshrc
script will be executed for each session:
+ Once for a serial console, once for each USB connection, and once for each Telnet session.
tools/incdir.sh
and tools/incdir.bat
now support an -s
option to generate system header file paths.
+ Supports a new daemon that can be used to monitor USB trace outpout.
tools/b16.c
: Fixed precision math conversion utility.
+ Removed non-functional wlan
example.
Bugfixes (see the change log for details). - Some of these are very important (marked critical): + Some of these are very important:
- RTOS: - Fix some backward conditional compilation in the work queue logic (Freddie Chopin). + Tasking +
+sched_lock()
is called increments the lockcount on the current TCB (i.e., the one at the head of the ready to run list),
+ (2) sched_mergepending()
is called which may change the task at the head of the ready-to-run list, then
+ (3) sched_unlock()
is called which decrements the lockcount on the wrong TCB.
+ The failure case that I saw was that pre-emption got disabled in the IDLE thread, locking up the whole system.
+ - File System: - Uninitialized variable caused assertions (from Lorenz Meier). + Signals +
+sigtimedwait()
would return a bad signal number if the signal was already pending when the function was called.
+
- Drivers:
- Partial fix for STM32 OTG FS device drivers and fix for short, unaligned writes in the flash translation layer (drivers/mtd/ftl.c
), both from Petteri Aimonen.
+ Drivers
+
- STM32 Drivers: + STM32 Drivers
- LPC17xx Drivers: - Resources not being properly released when I2C driver is un-initialized. + STM32 Boards +
+/dev/ttyS0-5
is there is a serial console, but /dev/ttyS1-6
if there is no serial console.
+ - Graphics: + Binary Formats
+ File Systems +
+nxtk_filltrapwindow()
: Correct an offset problem (also from Peterri Aimonen).
+ Several FAT-related bugs fixed by Petteri Aimonen.
+ Networking +
+nxglib_splitline()
: Correct the "fat flat line" bug.
+ Fix poll/select issure reported by Qiang: poll_interrupt()
must call net_lostconnection()
when a loss of connection is reported.
+ Otherwise, the system will not remember that the connection has been lost and will hang waiting on a unconnected socket later.
+ recvfrom()
and send()
.
+ recv()
ever returned a value <= 0.
- C Library: + Libraries
nrand()
changes to prevent coefficients from becoming zero which would "lock up" the random number generate.
+ fread()
could hang on certain error conditions.
SYSLOG
output to a character device from the IDLE task (because the IDLE task can't block).
- Build system:
- Changes to MIN definitions in all limit.h
header files to avoid integer overflows.
- For example from (-128) to (-127 - 1) (from Petteri Aimonen).
+ Build System
+
- Applications: - Modbus fixes from Freddie Chopin. + Additional Bugfixes +
+sig_timedwait()
and clock_time2ticks.c
: Timing "rounding" logic
+ readline()
return value
+ apps/examples
.
+ - As well as other, less critical bugs. See the ChangeLog for additional, detailed changes.
@@ -3432,311 +3725,421 @@ Other memory:-nuttx-6.24 2012-12-20 Gregory Nutt <gnutt@nuttx.org> - - * arch/arm/src/stm32: Support for STM32F100 high density chips - added by Freddie Chopin. - * configs/stm32f100_generic: Support for generic STM32F100RC board - contributed by Freddie Chopin. - * arch/arm/src/stm32_otgfsdev.c: Partial fix from Petteri Aimonen. - * drivers/lcd/ug-2864ambag01.c and include/nuttx/lcd/ug_2864ambag01.h: - LCD driver for the Univision OLED of the same name (untested on - initial check-in). - * configs/stm32f4discovery/nxlines: Configure to use mconf/Kconfig - tool. - * configs/stm32f4discovery/src/up_ug2864ambag01.c: Board-specific - initialization for UG-2864AMBAG01 OLED connecte to STM32F4Disovery. - * libxx/libxx_stdthrow.cxx: Exception stubs from Petteri Aimonen. - * configs/stm32f4discovery/src/up_ug2864ambag01.c: Driver has been - verified on the STM32F4Discovery platform. Some tuning of the - configuration could improve the presentation. Lower resolution displays - are also more subject to the "fat, flat line bug" that I need to fix - someday. See http://www.nuttx.org/doku.php?id=wiki:graphics:nxgraphics - for a description of the fat, flat line bug. - * libc: Renamed nuttx/lib to nuttx/libc to make space for a true lib/ - directory that will be forthcoming. Also rename libraries: liblib.a -> libc.a, - libulib.a -> libuc.a, libklib.a -> libkc.a, liblibxx.a ->libcxx.a. - (I will probably, eventually rename libxx to libcxx for consistency) - * Makefile, lib/: A new, empty directory that will hold generated libraries. - This simplifies the library patch calculations and lets me get rid of some - bash logic. The change is functional, but only partially complete; - additional logic is needed in the arch/*/src/Makefile's as well. Right - now that logic generate multiple library paths, all pointing to the lib/ - directory. - * arch/*/src/Makefile: Now uses only the libraries in lib/ - Replace bash fragments that test for board/Makefile. - * Makefile.win: The beginnings of a Windows-native build. This is just - the beginning and not yet ready for prime time use. - * configs/stm32f4discovery/winbuild: This is a version of the standard - NuttX OS test, but configured to build natively on Windows. Its only - real purpose is to very the native Windows build logic. - * tools/mkdeps.bat and tools/mkdeps.c: mkdeps.bat is a failed attempt - to leverage mkdeps.sh to CMD.exe. It fails because the are certain - critical CFLAG values that cannot be passed on the CMD.exe command line - (like '='). mkdeps.c is a work in progress that will, hopefully, - replace both mkdeps.sh and mkdeps.bat. - * tools/Config.mk: Centralize the definition of the script that will be - used to generated header file include paths for the compiler. This - needs to be centralized in order to support the Windows native build. - * tools/incdir.bat: A replacement for tools/incdir.sh for use with the - the Windows native build. - * Makefile.unix: The existing top-level Makefile has been renamed - Makefile.unix. - * Makefile: This is a new top-level Makefile that just includes - either Makefile.unix or Makefile.win - * configs/stm3240g-eval/src: Qencoder fixes from Ryan Sundberg. - * arch/arm/src/stm32/stm32_qencoder.c: TIM3 bug fix from Ryan Sundberg. - * tools/mkromfsimg.sh: Correct typo in an error message (Ryan Sundberg) - * arch/*/src/Makefile: Remove tftboot install and creation of System.map - for Windows native build. The first is a necessary change, the second - just needs re-implemented. - * configs/mirtoo: Update Mirtoo pin definitions for Release 2. Provided - by Konstantin Dimitrov. - * Fixed an uninitialized variable in the file system that can cause - assertions if DEBUG on (contributed by Lorenz Meier). - * Config.mk: Defined DELIM to be either / or \, depending upon - CONFIG_WINDOWS_NATIVE. This will allow me to eliminate a lot of - conditional logic elsewhere. - * nuttx/graphics: One a mouse button is pressed, continue to report all - mouse button events to the first window that received the the initial - button down event, even if the mouse attempts to drag outside the - window. From Petteri Aimonen. - * nuttx/graphics/nxmu/nx_block.c: One more fix to the NX block message - logic from Petteri Aimonen. - * include/nuttx/wqueue.h: Some basic definitions to support a user- - space work queue (someday in the future). - * graphics/nxmu: Add semaphores so buffers messages that send buffers - will block until the buffer data has been acted upon. - * graphics/nxmw: Extended the blocked messages to cover mouse movement - and redraw events. These will also cause problems if sent to a window - while it is closing. - * arch/several: Change UARTs are enabled for i.MX, LM3S, ez80, and M16C to - match how they are enabled for other architectures. - * configs/ez80f910200kitg: Convert to use mconf configuration. - * sched/pause.c: Implements the POSIX pause() function. - * ez80: Lots of changes to ez80 configurations and build logic as I - struggle to get a clean Windows build (still not working). - * configs/cloudctrl: Darcy Gong's CloudController board. This is a - small network relay development board. Based on the Shenzhou IV development - board design. It is based on the STM32F107VC MCU. - * arch/arm/src/stm32_serial.c and stm32_lowputc.c: Added optional RS-485 - direction bit control. From Freddie Chopin. - * Lots of build files: ARMv7-M and MIPS32 Make.defs now include a common - Toolchain.defs file that can be used to manage toolchains in a more - configurable way. Contributed by Mike Smith - * configs/stm32f4discovery/winbuild and configs/cloudctrl: Adapted to use - Mike's Toolchain.defs. - * tools/configure.sh: Adapted to handle paths and setenv.bat files correctly - for native Windows builds. - * More of build files: AVR and AVR32 Make.defs now include a common - Toolchain.defs file that can be used to manage toolchains in a more - configurable way. Contributed by Mike Smith - * tools/incdir.sh and incdir.bat: Add -s option to generate system header - file paths. - * nuttx/arch/arm/src/arm/Toolchain.defs: Add support for more ARM toolchains - (from Mike Smith). - * arch/arm/src/stm32/stm32f40xxx_rcc.c: Enabled FLASH prefetch (from Petteri - Aimonen). - * graphics/nxtk/nxtk_filltrapwindow.c: Correct an offset problem (from - Peterri Aimonen). - * graphics/nxglib/nxglib_splitline.c: Fix error in drawing of near horizontal - lines (from Peterri Aimonen). - * sched/task_exithook.c: Missing right bracket with certain conditional - compilation (thanks James Goppert). - * arch/arm/srch/stm32/stm32_otgfshost.c: Replace timeout handling; use - system tick instead of frame counter. The frame counter gets reset to - zero at 0x3fff making it error prone. - * arch/arm/src/stm32/stm32f20xx_rcc.c and stm32f40xx_rcc.c: Added option - CONFIG_STM32_FLASH_PREFETCH. FLASH prefetch will now only be enabled - if this option is selected. - * confgs/ez80f910200zco/ostest: Now uses Kconfig/mconf configuration - tool. Updated to build in native Windows environment. Other ez80f910200zco - build scripts also updated. - * configs/z8f64200100kit/ostest: Update to same level as ez80 configurations. - * nuttx/configs/z8f64200100kit/scripts/setenv.bat: Add support for native - Windows build. - * nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c: Resources not being released when - I2C is uninitialized. - * cloudctrl/src/up_chipid.c and shenzhou/src/up_chipid.c: Add functions to - get chip ID. Contributed by Darcy Gong. These should not be board-dependent, - but should be in arch/arm/src/stm32 where they can be used from any board. - * sched/work_thread.c: Fix backward conditional compilation. This might - has caused a memory leak. From Freddie Chopin. - * configs/<many>/Make.defs: Fix typo -wstrict-prototypes should be - -Wstrict-prototypes (From Denis Carilki). - * arch/arm/src/calapyso/calypso_keypad.c: Add Calypso keypad driver. From - Denis Carilki. - * z8encore000zco/ostest and z8f64200100kit/ostest: Converted to use Kconfig/ - mconf configuration tool. - * arch/arm/src/armv7-m/up_exception.S: missing curly braces for push/pop - From Freddie Chopin. - * z8encore000zco/ostest and z8f64200100kit/ostest: Can now be modified to - support the Windows native builds (see corresponding README.txt files). - * configs/z16f2800100zcog - All configurations updated to use the ZDS-II - 5.0.1 toolchain. - * configs/z16f2800100zcog - All configurations updated to use Kconfig/mconf - configuration tools. - * configs/z16f2800100zcog/ostest - Now supports a native Windows build - (other ZNEO configs may also support the native build, but this has not - been verfiied). - * include/nuttx/input/keypad.h, arch/arm/src/calypso/calypso_keypad.c, and - configs/compal_e99/nsh_highram: First cut at a standard keypad interface - definition. Contributed by Denis Carikli. - * libc/stdlib/lib_rand.c: Always add one to result congruential generators - to avoid the value zero. Suggested by Freddie Chopin. - * tools/b16.c: Fixed precision math conversion utility. - * graphics/nxglib/nxglib_splitline.c: Fix the "fat, flat line bug" - * arch/z80/src/*/Toolchain.defs: Add dummy Toolchain.defs files for the - z80 family. - * configs/z80sim/ostest: Converted to build with the Kconfig/mconf tool. - Current configuration failed to build for me (Ubuntu 12.10, SDCC 3.2.0 - pre-built for Linux) due to a glibc memory corruptionerror in SDCC. - * configs/z80sim/ostest: Default is now the Windows native build. See - configs/z80sim/README.txt for instructions to convert back to a Linux or - or Cygwin build. - * arch/z80/src/Makefile.sdccw: Renamed makefiles with extensions zdiil, - zdiiw, sdccl, and sdccw for the ZDS-II vs SDCC compilers and for the - POSIX vs Windows native builds. - * nuttx/drivers/mtd/ftl.c: Fix for the flash translation layer. Short - unaligned writes were buggy. From Petteri Aimonen. - * nuttx/libc/math/lib_round*.c: Add rounding functions to the math - library. Contributed by Petteri Aimonen. - * include/cxx/cstdlib: Add stroul(). From Petteri Aimonen. - * arch/*/include/limits.h: Change signed minimum values from, for example, - (-128) to (-127 - 1) to avoid overflows under certain conditions. From - Peterri Aimonen. - * graphics/nxtk/nxtk_subwindowmove.c: Previously it was very difficult to - do e.g. "scroll by dx, dy". When given the full window area, nxtk_subwindowmove - would clip the offset always to 0,0. It makes more sense for it to clip the - source area and not modify the offset. From Petteri Aimonen. - * graphics/nxtk/nxtk_getwindow.c: Clipping would change the offset of returned - data, and caller has no way to know what the new offset would be. This messes - up font drawing when the text is partially out of window, e.g. when scrolling. - Also from Petteri Aimonen. - * include/stdbool.h: Can now be disabled for C++ files if CONFIG_C99_BOOL8 is - defined. CONFIG_C99_BOOL8 indicates (1) that the sizeof(_Bool) is one in both - C and C++, and (2) the the C compiler is C99 and supports the _Bool intrinsic - type. Requested by Freddie Chopin. - * include/stdlib/lib_rand.c: Various additional changes so that the integer - value zero can be returned. Requested by Freddie Chopin. - * arch/z80/src/Makefile.sdcc*, z80/up_mem.h: Redesign Z80 build so that it - no longer depends on Bash scripts. - * configs/z80sim/nsh and pashello: Converted to (1) use the kconfig-frontends - configuration tool, and (2) to build natively under Windows. The NSH - configuration is verified; the pashello configuration needs a more TLC. - * tools/copydir.sh: Rename tools/winlink.sh to tools/copydir.sh - * tools/link.bat, unlink.bat, and copydir.bat: Add Windows counterparts - to the link.sh, unlink.sh, and copydir.sh Bash scripts. - * configs/z80sim/pashello: Now builds correctly. - * configs/xtrs/ostest, nsh, and pashello: Converted to (1) use the kconfig- - frontends configuration tool, and (2) to build natively under Windows. - * drivers/serial/Kconfig and sched/Kconfig: Two names for same configuration: - CONFIG_LOWLEVEL_CONSOLE is bogus and CONFIG_DEV_LOWCONSOLE is in the wrong - Kconfig file. Moved to drivers/serial/Kconfig replacing CONFIG_LOWLEVEL_CONSOLE. - * arch/z80/include/z180: Add header files for z180 chips. Initial versions - are just clones of z80 header files. - * arch/z80/src/z180: Add source files for z180 chips. Initial versions - are just clones of z80 source files. - * include/nuttx/arch.h: Add address environment control interfaces (for use - with CPUs the provide MCUs and support process-like address environments). - * arch/z80/src/z180/z180_mmu.*: Add MMU support for z180 tasks. - * configs/p112: Add very basic board support and an examples/ostest - configuration for the venerable P112 board. - * sched/os_bringup.c: If CONFIG_PATH_INITIAL is defined, then the initial - environment of the task started by os_bringup() will have the PATH - environment variable defined to be that string. - * binfmt/binfmt_exepath.c: If CONFIG_BINFMT_EXEPATH is defined, then this - file will be built. It contains logic to search for regular files at - the absolutes paths found in the current PATH environment variable - setting. This is untested and not yet hooked into the binfmt exec() - logic on initial check-in - * binfmt/binfmt_loadmodule.c: load_module() will now traverse the PATH - variable to locate files from their relative path. - * include/nuttx/arch.h and arch/z80/src/z180/z180_mmu.c: Restructure the - address environment interfaces so that they will better integrate with - binfmt/. - * binfmt/libelf/*, binfmt/libnxflat/* and other files: Integrate the - address environment interfaces. If CONFIG_ADDRENV=y, then binfmt/ - will now create an address environment for new tasks (instead of - just malloc'ing the task memory). - * configs/stm32f4discovery/elf: Enable support/test of the PATH - to find executables using a relative path. - -apps-6.24 2012-12-20 Gregory Nutt <gnutt@nuttx.org> - - * apps/examples/ostest/roundrobin.c: Replace large tables with - algorithmic prime number generation. This allows the roundrobin - test to run on platforms with minimal SRAM (Freddie Chopin). - * apps/nshlib/nsh_dbgcmds.c: Add hexdump command to dump the contents - of a file (or character device) to the console Contributed by Petteri +nuttx-6.25 2013-02-01 Gregory Nutt <gnutt@nuttx.org> + + * graphics/: Adds 5x8 monospace font. This tiny font is useful for graph + labels and for small bitmapped display. Contributed by Petteri Aimonen. - * apps/examples/modbus: Fixes from Freddie Chopin - * apps/examples/modbus/Kconfig: Kconfig logic for FreeModBus contributed - by Freddie Chopin. - * Makefile, */Makefile: Various fixes for Windows native build. Now uses - make foreach loops instead of shell loops. - * apps/examples/elf/test/*/Makefile: OSX doesn't support install -D, use - mkdir -p then install without the -D. From Mike Smith. - * apps/examples/relays/Makefile: Reduced stack requirement (Darcy Gong). - * apps/nshlib and apps/netutils/dhcpc: Extend the NSH ifconfig command plus - various DHCPC improvements(Darcy Gong). - * apps/nshlib/nsh_apps.c: Fix compilation errors when CONFIG_NSH_DISABLEBG=y. - From Freddie Chopin. - * Rename CONFIG_PCODE and CONFIG_FICL as CONFIG_INTERPRETERS_PCODE and - CONFIG_INTERPRETERS_FICL for consistency with other configuration naming. - * apps/examples/keypadtest: A keypad test example contributed by Denis - Carikli. - * apps/examples/elf and nxflat: If CONFIG_BINFMT_EXEPATH is defined, these - tests will now use a relative path to the program and expect the binfmt/ - logic to find the absolute path to the program using the PATH variable. - -NxWidgets-1.4 2012-12-20 Gregory Nutt <gnutt@nuttx.org> - - * libnxwidgets/Makefile, NxWidgets/nxwm/Makefile, and - NxWidgets/UnitTests/nxwm/Makefile: Makefile improvements from - submitted by Petteri Aimonen. Other Makefiles in the UnitTests - directory probably also need these changes. - * libnxwidgets/src/ccallback.cxx: Fix misplaced #endif. Provided - by Petteri Aimonen. - * libnxwidgets/src/cnxserver.cxx: Reduce delay to allow NX server - to start. One second was un-necessarily long. Reduced to 50 MS. - Reduction suggested by Petteri Aimonen. - * tools/bitmap_converter.py: This script converts from any image type - supported by Python imaging library to the RLE-encoded format used by - NxWidgets. - * NxWidgets/nxwm/src/capplicationwindow.cxx: If the "desktop" is empty, - users have no need to minimize any windows. If the buttons are small, - it's easy to hit minimize button accidentally when trying to close an - application. Contributed by Petteri Aimonen. - * NxWidgets/nxwm/src/ctaskbar.cxx: Add an option to eliminate the - background image. Contributed by Petteri Aimonen. - * NxWidgets/nxwm/src/chexcalculator.cxx and NxWidgets/nxwm/src/cstartwindow.cxx: - The config settings CONFIG_NXWM_STARTWINDOW_ICON and CONFIG_NXWM_HEXCALCULATOR_ICON - allow changing the icons used for these applications. However, to declare symbols - for these icons user would need to modify NxWidgets header files. - This commit adds a simple forward declaration to the relevant files, based on the - configured icon. If the icon does not exist, linker will give an error about it. - Contributed by Petteri Aimonen. - * NxWidgets::CTaskBar: Highlight the current window in the task bar. - Contributed by Petteri Aimonen. - * NxWidgets/libnxwidgets/src/glyph_cycle.cxx: Width of glyph_cycle was wrong; - Destructor needs to by public. From Petteri Aimonen. - * NxWidgets::CNumericEdit. This is basically a label with plus and minus buttons. - Contributed by Petteri, Aimonen. - * NxWM::CStartWindow: Fix mq_receive error handling with signal is recieved. + * configs/stm3220g-eval/nxwm: Converted to use the kconfig-frontends + configuration tool. + * configs/sim/nxwm: Converted to use the kconfig-frontends configuration + tool. + * include/pthread.h: In sys/prctl.h because it is needed by + pthread_[set|get]name_np() + * tools/kconfig.bat: Kludge to run kconfig-frontends from a DOS shell. + * sched/sig_timedwait.c: Should always move the time up to the next + largest number of system ticks. The logic was rounding. Noted by + Petteri Aimonen. + * arch/arm/src/up_head.S: Fix backward conditional compilation. NOTE + there is a issue of ARM9 systems with low vectors and large memories + that will have to be addressed in the future. + * libc/misc/lib_kbdencode.c and lib_kbddecode.c: Add logic to marshal + and serialized "out-of-band" keyboard commands intermixed with normal + ASCII data (not yet hooked into anything). + * drivers/usbhost/usbhost_hidkbd.c: If CONFIG_HIDKBD_ENCODED is + defined, this driver will now use libc/misc/lib_kbdencode.c to + encode special function keys. + * configs/olimex-lpc1766stk/hidkbd: This configuration has been + converted to use the kconfig-frontends configuration tool. + * drivers/lcd/ug-2864hsweg01.c and include/nuttx/lcd/ug-2864hsweg01.h: + Driver for UG-2864HSWEG01 OLED contributed by Darcy Gong. + * configs/stm32f4discovery/src/up_ug2864hsweg01.c: Support for the + UG-2864HSWEG01 OLED for the STM32F4Discovery board. + * drivers/usbhost/usbhost_hidkbd.c: Correct a logic error in how + tasks waiting for read data are awakened. + * libc/misc/lib_kbdencode.c and lib_kbddecode.c: Now handles keypress + events too. However, the USB HID keyboard driver has not yet been + updated to detect key release events. That is kind of tricky in + the USB HID keyboard report data. + * configs/mcu123-214x/nsh: Converted to use the kconfig-frontends + configuration tool. + * configs/zp214xpa: Add basic support for the The0.net ZP213x/4xPA + board (with the LPC2148 and the UG_2864AMBAG01). + * configs/sim/nxlines: Add an nxlines configuration for the + simulator. + * configs/zp214xpa/nxlines: Add an nxlines configuration for the + ZP213x/4xPA (with the LPC2148 and the UG_2864AMBAG01). Working + as of 2012-12-30. + * configs/olimex-lpc1766stk/wlan: Remove non-functional + configuration. + * configs/stm32f4discovery/src and nuttx/drivers/lcd/ug-2864hsweg01.c: + Updates and correctinos for the UG-2864HSWEG01 from Darcy Gong. + * configs/lm326965-ek: All configurations converted to use the + kconfig-frontends configuration tool. + * configs/Kconfig: NSH_MMCSDSPIPORTNO should depend on MMCSD_SPI, + not just SPI (from Jose Pablo Carballo). + * arch/arm/src/arm/Kconfig and armv7m/Kconfig: Add an option for + buildroot toolchains: They may be EABI or OABI. + * include/nuttx/progmem and arch/arm/src/stm32/stm32_flash.c: + Fix a counting bug plus change interface to use either relative + or absolute FLASH addressing (from Freddie Chopin). + * libc/misc/Make.defs: Fix error in conditional for KBD CODEC. + * libc/Kconfig and configs/*/defconfig (several): The default + setting should be CONFIG_LIB_KBDCODEC=n + * tools/configure.c: configure.c can be used to build a work-alike + program as a replacement for configure.sh. This work-alike + program would be used in environments that do not support Bash + scripting (such as the Windows native environment). + * tools/configure.bat: configure.bat is a small Windows batch + file that can be used as a replacement for configure.sh in a + Windows native environment. configure.bat is actually just a + thin layer that executes configure.exe if it is available. If + configure.exe is not available, then configure.bat will attempt + to build it first. + * arch/arm/src/lpc17xx/lpc17_syscon.h: Correct some typos in bit + definitions (from Rommel Marcelo). + * libc/string/lib_strndup.c: strndup() should use strnlen(), not + strlen(), to determine the size of the string. + * sched/os_bringup.c: Remove support for CONFIG_BUILTIN_APP_START. + This is not really a useful feature and creates a violation of the + OS layered architecture. + * include/unistd.h, arch/arch/src/*: Implement a simple vfork(). + On initial checkin, this API is available only for ARM platforms. + * binfmt/binfmt_exec.c: exec() now sets the priority of the new task + to the same priority as the current task (instead of the arbirtrary + value of 50). + * libc/unisted/lib_execv.c and lib_execl.c: New, somewhat flawed, + implementations of execv() and execl(). + * tools/cfgdefine.c: Strips quotes from CONFIG_EXECFUNCS_SYMTAB + value. + * arch/arm/include/lm3s/chip.h: Move chip definitions into + public include area for compatibility with other architectures. + * arch/arm/src/lm3s/chip: Move register definition header files + into a new chip/ sub-directory. + * arch/arm/src/lm3s/lm3s_internal.h: Broke up into several + smaller header files. + * arch/arm/src/lm: Rename the arch/arm/src/lm3s directory to + arch/arm/src/lm so that is can support other members of the + Stellaris family. + * libc/spawn: Add file action interfaces needed by posix_spawn(). + * sched/clock_time2ticks.c: Another case where time was being + rounded down instead of up (from Mike Smith). + * libc/spawn: Implementation of posix_spawn() is complete but + untested and undocumented. + * drivers/usbdev/pl2303.c: Fix typols in the PL2303 driver + (from Max Holtzberg). + * configs/stm32f4discovery/posix_spawn: Added a configuration + that can be used for testing posix_spawn(). + * arch/arm/src/stm32: Bring F1 support for general DMA and serial + DMA in paricular up to parity with F2/F4 (from Mike Smith). + * libc/stdio/lib_libfread.c: Correct some error handling when + lib_fread() was passed a bad stream. Needed to move the + releasing of a semaphore inside of some conditional logic + (cosmetic). + * include/nuttx/sched.h, sched/task_setup.c, and sched/task_exithook.c: + Add support for remembering the parent task and sending + SIGCHLD to the parent when the task exists. + * sched/task_exithook.c: Fixed a *critical* bug. Here is + the scenario: (1) sched_lock() is called increments the lockcount + on the current TCB (i.e., the one at the head of the ready to run + list), (2) sched_mergepending is called which may change the task + at the head of the ready-to-run list, then (3) sched_unlock() is called + which decrements the lockcount on the wrong TCB. The failure case + that I saw was that pre-emption got disabled in the IDLE thread, + locking up the whole system. + * sched/sched_waitpid.c: Use SIGCHLD instead of a semaphore. This + is a much more spec-compliant implementation. However, there are + some issues with overruning signals because NuttX does not support + queueing of signals (POSIX does not require it). I think it may + need to. + * sched/sched_waitid.c and sched_wait.c: Add support for waitid() + and wait(). See issues with waitpid() above. + * include/nuttx/fs/fs.h and fs/fs_files.c: Add a dup() method to + the struct mountpt_operations. When dup'ing a file that resides + on a mounted volume, let the file system's dup() method do the + work. + * fs/romfs/fs_romfs.c: Implemented the dup() method for the ROMFS + file system. + * fs/fat/fs_fat32.c, fs/nxffs/nxffs_initialize, and + fs/nfs/nfs_vfsops.c: Add hooks for dup() method (not yet + implemented). + * fs/romfs: Remove the rf_open flag. It looks good, but actually + does nothing. + * fs/fat: Remove the ff_open flag. Same story as for the ROMFS + rf_open flag. + * fs/fat/fs_fat32.c, fs/nxffs/nxffs_initialize, and + fs/nfs/nfs_vfsops.c: Completed implementation of the dup() methods. + There is still no good test available. + * sched/sig_timedwait.c: sigtimedwait() would return a bad signal + number if the signal was already pending when the function was + called. + * configs/ubw32/scripts: All common linker scripts moved to this + scripts sub-directory + * configs/ubw32/ostest: Configuration configured to use the + kconfig-frontends tools. + * arch/mips/src/mips32/up_vfork.c, up_vfork.h, and vfork.S: + Implement vfork() for MIPS32 (no floating point support) + * configs/ubw32/ostest: Enable the vfork() test. + * fs/binfs: Move apps/builtin/binfs.c to fs/binfs/fs_binfs.c + CONFIG_APPS_BINDIR rename CONFIG_FS_BINFS + * include/nuttx/binfmt/builtin.h: Some of the content of + apps/include/apps.h moved to include/nuttx/binfmt/builtin.h + * binfmt/libbuiltin/libbuiltin_utils.c: Move builtin + utility functions from apps/builtin/exec_builtins.c to + binfmt/libbuiltin/libbuiltin_utils.c + * binfmt/builtin.c and binfmt/libbuiltin: Add a binary "loader" + that can be used to execute builtin programs from the BINFS + file system. + * configs/sim/nsh: Convert to use kconfig-frontends configuration + tool. + * binfmt/binfmt_schedunload.c: Add logic based on SIGCHLD to + automatically unload and clean-up after running a task that + was loaded into memory. + * binfmt/libbuiltin: Extensions from Mike Smith + * sched/task_reparent.c: Add internal interface to change the + parent task. + * sched/task_posixspawn(): Move libc/spawn/lib_ps.c to + sched/task_posixspawn() now it requires internal, reparenting + interfaces + * include/nuttx/spawn(): Move libc/spawn.h to include/nuttx/spawn.h + * arch/arm/include/lpc17xx/chip.h, irq178x.h: Integrate Marcelo + Rommel's LPC1788 definitions into the base LPC17xx. + * configs/olimex-lpc1766stk/nsh: Convert configuration to use + the kconfig-frontends tools. + * sched/task_reparent.c: Simplify reparenting interface. + * arch/arm/src/[many]: More LPC1788 definitions from Rommel + Marcelo incorporated. + * configs/open1788: Board configuration for the Wave Share + Open1788 board. Still fragmentary (contributed by Rommel + Marcelo, adapted to use kconfig-frontends. + * net/send(): Add logic to work around delayed ACKs by splitting + packets (contributed by Yan T.). + * net/recvfrom(): Fix a bug. When the host closes a connection + (gracefully). recv[from]() returned success and the closure + was never detected. Hmmm.. I don't know why the network monitor + did not catch this event. This is an important bug fix. + * net/recvfrom(): Fix a introduced with the last bugfix. If + the peer does an orderly closure of the socket, report 0 not + -ENOTCONN + * configs/lm3s6965-ek/README.txt and tools/: Add an OpenOCD + configuration for the LM3S (from Jose Pablo Carballo). + * nuttx/lcd/hd4478ou.h and configs/pcblogic-pic32mx/src/up_lcd1602: + Start of support of LCD1602 alphanumeric LCD. I need a few + more parts before I can finish integrating this one. + * arch/arm/src/*/chip.h and arch/arm/include/*/chip.h: Move all + priority ranges from the src to the include chip.h header file. + * arch/arm/include/armv7-m/irq.h: Add inline functions to enable + and disable interrupts via the BASEPRI register. + * arch/arm/Kconfig: Add new option CONFIG_ARM7VM_USEBASEI + * arch/arm/src/*/*_irq.c: Set the priority of the SVCALL exception + to the highest possible value. + * arch/armv7-m/up_hardfault.c: Fail if a hardfault occurs + while CONFIG_ARM7VM_USEBASEPRI=y. + * arch/arm/src/stm32/stm32_serial.c: Add support for USART + single wire mode (Contributed by the PX4 team). + * sched/: Implement support for retaining child task status after + the child task exists. This is behavior required by POSIX. + But in NuttX is only enabled with CONFIG_SCHED_HAVE_PARENT and + CONFIG_SCHED_CHILD_STATUS + * Add support for keyboard encode to the keypad test (from + Denis Carikli). + * configs/olimex-lpc1766stk/nettest: Configuration converted to + use the kconfig-frontends tools. + * net/net_poll.c: Split net_poll() to create psock_poll() too. + * net/net_poll.c: Fix poll/select issure reported by Qiang: + poll_interrupt() must call net_lostconnection() when a + loss of connection is reported. Otherwise, the system will + not know that the connection has been lost. + * sched/group_create.c, group_join.c, and group_leave.c: Add + support for task groups. + * sched/group_signal.c and task_exithook.c: Send signal to all + members for the parent task group. + * include/nuttx/sched.h and sched/env_*.c: Move environment + variables into task group structure. + * sched/: Lots of file changed. Don't keep the parent task's + task ID in the child task's TCB. Instead, keep the parent + task group IN the child task's task group. + * fs/, sched/, include/nuttx/sched.h, and include/nutts/fs/fs.h: + Move file data from the TCB to the task group structure. + * libc/stdio/, sched/, include/nuttx/lib.h, and include/nutts/fs/fs.h: + Move stream data from the TCB to the task group structure. + * net/, sched/, and include/nuttx/net/net.h: Move socket data + from the TCB to the task group structure. + * sched/task_starthook.c, sched/task_start.c, and include/nuttx/sched.h: + Add a task start hook that will be called before the task main + is started. This can be used to schedule C++ constructors to run + automatically in the context of the new task. + * binfmt/binfmt_execmodule: Execute constructors as a start hook. + * sched/os_start.c: Fix ordering of group initialization. + * configs/stm32f4discovery/usbnsh: Add an NSH STM32F4Discovery + configuration that uses USB CDC/ACM for the NSH console. + * configs/stm32f4discovery/nsh: Converted to use the kconfig-frontends + tools. + * configs/*/src/up_userleds.c: Fix a error that was cloned into + all STM32 user LED code. The wrong definitions were being used + to set LEDs on or off. + * arch/*/common/up_internal.h and arch/*/common/up_initialize.c: + Serial was driver was not being built if there is no console + device. Obviously, the serial driver may be needed even in + this case. + * arch/arm/src/stm32/stm32_serial.c: If there is a serial console, + it would be ttyS0 and the others would be ttyS1-5. If there + is not serial console, was labeling them ttyS1-6; now labels them + ttyS0-5. + * fs/fs_syslog.c: Can't handle SYSLOG output to character device from + the IDLE task (because it can't block). syslog_putc now returns EOF + on failure and sets errno. Fixed some errors in error handling. + * libc/stdio/lib_syslogstream.c: Checking of return value from + syslog_putc was bogus. Switching to EOF for all errors solves + this. + * arch/arm/src/lm/chip/lm4f_memorymap.h: More LM4F changes from + Jose Pablo Carballo. + * drivers/serial/serial.c, include/nuttx/serial/serial.h, + drivers/usbdev/cdcacm.c, and drivers/pl2303.c: Add support for + removable serial devices (like USB serial). This support is enabled + by CONFIG_SERIAL_REMOVABLE. + * arch/*/src/*/Toolchain.defs: Change assignment so that we can + override CROSSDEV with a make command line argument. + * include/assert.h: Mark assertion functions as non-returning. + * arch/*/src/*/up_assert.h: Mark _up_assert() as non-returning. + * drivers/mtd/at25.c: When the AT25 device was not available the + initialization did not fail like it should. From Petteri Aimonen. + * fs/fat/fs_configfat.c: Fix some errors in FAT formatting logic + for large devices and for FAT32. From Petteri Aimonen. + * fs/fat/fs_fat32util.c: Fix an initialization error found by + Petteri Aimonen. freecount and next freecount initialization were + reversed. + * drivers/mmcsd/mmcsd_spi.c: Some SD cards will appear busy until + switched to SPI mode for first time. Having a pull-up resistor on + MISO may avoid this problem, but this patch makes it work also + without pull-up. From Petteri Aimonen. + * fs/fat/fs_fat32.c: Fix a compilation error when FAT_DMAMEMORY=y. From Petteri Aimonen. - * NxWidgets::CNxTimer: Replace the original (apparently non-functional) signal- - based solution with a work queue-based solution. This raises some isses about - using the internal work queues from user space. I have decided to implemented - user-space work queues (someday) in order to accomplish that functionaliy. - Submitted by Petteri Aimonen. - * NxWidgets:CText and NxWidgets:CNumericEdite: Fix some memory freeing bugs + * arch/arm/src/stm32/chip/stm32_spi.h: STM32F4 max SPI clock freq is + 37.5 MHz. Patch from Petteri Aimonen. + * arch/arm/src/stm32/stm32_spi.c: Fixes for SPI DMA work on the + STM32F4. Includes untested additions for the F1 implementation as + well. From Petteri Aimonen. + +apps-6.25 2013-02-01 Gregory Nutt <gnutt@nuttx.org> + + * Makefiles: Removed dependency of distclean on clean in most top-level + files. It makes sense for 'leaf' Makefiles to have this dependency, + but it does not make sense for upper-level Makefiles. + * apps/namedapp/: Renamed to builtins in preparation for another change. + * .context: Removed the .context kludge. This caused lots of problems + when changing configurations because there is no easy way to get the + system to rebuild the context. Now, the context will be rebuilt + whenever there is a change in either .config or the Makefile. + * apps/builtin/registry: Updated new built-in registration logic to handle + cases where (1) old apps/.config is used, and (2) applications ared + removed, not just added. + * apps/examples/nettest/Makefile: Fix an error that crept in during + some of the recent, massive build system changes. + * apps/builtin/Makefile: Need to have auto-generated header files + in place early in the dependency generation phase to avoid warnings. + It is not important if they are only stubbed out header files at + this build phase. + * apps/examples/hidbkd: Now supports decoding of encoded special keys + if CONFIG_EXAMPLES_HIDKBD_ENCODED is defined. + * apps/examples/hidbkd: Add support for decoding key release events + as well. However, the USB HID keyboard drier has not yet been + updated to detect key release events. That is kind of tricky in + the USB HID keyboard report data. + * apps/examples/wlan: Remove non-functional example. + * apps/examples/ostest/vfork.c: Added a test of vfork(). + * apps/exampes/posix_spawn: Added a test of posix_spawn(). + * apps/examples/ostest: Extend signal handler test to catch + death-of-child signals (SIGCHLD). + * apps/examples/ostest/waitpid.c: Add a test for waitpid(), waitid(), + and wait(). + * builtin/binfs.c: Add hooks for dup() method (not implemented). + * builtin/exec_builtin.c, nshlib/nsh_parse.c, and nshlib/nsh_builtin.c: + NSH now supports re-direction of I/O to files (but still not from). + * builtin/binfs.c: Greatly simplified (it is going to need to be + very lightweight). Now supports open, close, and a new ioctl to recover + the builtin filename. The latter will be needed to support a binfs + binfmt. + * builtin/binfs.c: Move apps/builtin/binfs.c to fs/binfs/fs_binfs.c + CONFIG_APPS_BINDIR rename CONFIG_FS_BINFS + * apps/include/builtin.h: Some of the content of + apps/include/apps.h moved to include/nuttx/binfmt/builtin.h. + apps/include/apps.h renamed builtin.h + * apps/builtin/exec_builtins.c: Move builtin + utility functions from apps/builtin/exec_builtins.c to + binfmt/libbuiltin/libbuiltin_utils.c + * apps/nshlib/nsh_mountcmds.c: The block driver/source + argument is now optional. Many files systems do not need + a source and it is really stupid to have to enter a bogus + source parameter. + * apps/nshlib/nsh_fileapp.c: Add the ability to execute a file + from a file system using posix_spawn(). + * apps/builtin/: Extensions from Mike Smith. + * apps/examples/ftpd/Makefile: Name ftpd_start is not the name of + the entrypoint. Should be ftpd_main (from Yan T.) + * apps/netutils/telnetd/telnetd_driver: Was stuck in a loop if + recv[from]() ever returned a value <= 0. + * apps/examples/nettest and poll: Complete Kconfig files. + * apps/examples/ostest/waitpid.c: Need to use WEXITSTATUS() + to decode the correct exit status. + * apps/system/usbmonitor: A daemon that can be used to monitor USB + trace outpout. + * apps/nshlib/nsh_usbdev.c, nsh_consolemain.c, nsh_session.c, nsh_script.c: + Add support for a login script. The init.d/rcS script will be executed + once when NSH starts; the .nshrc script will be executed for each session: + Once for serial, once for each USB connection, once for each Telnet + session. + * apps/system/readline: Correct readline() return value. Was not + any returning special values when end-of-file or read errors + occur (it would return an empty string which is not very useful). + +NxWidgets-1.5 2013-02-01 Gregory Nutt <gnutt@nuttx.org> + + * NxWidgets::CGraphicsPort::move(): Fix typo bug in bounding rectangle + calculation (from Petteri Aimonen). + * NxWM::CScrollingPanel::scrollChildren(): Avoid unnecessary redraws in + CScrollingPanel (contributed by Petteri Aimonen). + * NxWM::CCycleButton: Remove the separator from CCycleButton. It draws in + wrong place, and doesnt look very good in the correct place either. (from Petteri Aimonen). - * NxWidgets::CScrollingPanel: Usability improvements. It is borderless for now, - because there was no easy way to redraw only the required part of the border. - Contributed by Petteri Aimonen. - * NxWidgets::CNxWidgets and NxWM::CStartWindow: Small changes to make sub- - classing easier (from Petteri Aimonen). + * NxWidgets::CGraphicsPort: Many times we only want a constant background. + In that case the old code fills the background, reads it back, renders + the text and then writes it back. When used with LCD's (instead of + framebuffers) this causes unnecessary delay and screen flicker. + This commit adds a variant of drawText that takes background color, + so that the background and text can both be rendered at one go. + The old functions still function as before (Petteri Aimonen). + * NxWidgets::CLabel: The label was drawn as a single rectangular region, + then a text was added to the on top of this. The result is that the + text would flicker when the CLabel was updated. With this change, the + two step update is replaced with a five step update: The background + is updated as four rectangulear regions (leaving the previous text in + place), then the new text is updated. This eliminates the flicker + (Petteri Aimonen). + * Kconfig: Many NxWidgets/NxWM settings do not have meaningful, generic + default values. Colors, for example, depend on pixel depth. Some + geometry settings depending on other geometry settings. Font IDs are + not know-able by the configuration system. etc. In these cases, it + is best if the settings are just not undefined so that the system can + calculate a reasonable default. however, if no default is provided + in the .config file, mconf will complain and generate errors. So work + around this, I added several "enabling" settings to override the + default setting. This is awkward and I preferred the configuration as + it was before, but this avoids the mconf errors and warnings. + * UnitTests: Changed occurrences of lib_rawprintf() and lib_lowprintf() + to match recent changes to NuttX (will be in NuttX-6.25) + * CGraphicsPort::_drawText: Renamed from CGraphicsPort::drawText in order + to eliminate some naming collisions when overloaded in some configurations + (i.e., when both bool and nx_pixel_t are uint8_t). From Petteri Aimonen. + * CNxWidgets::drawContents: Change base drawContents from a do-nothing + function to a function that fills the widget with the background color. + This is useful when using CNxWidgets as a "panel" , i.e. a container + for other widgets. Subclasses will override drawContents and decide + themselves how to draw the background. + * CNxWidgets::CTabPanel: A new widget contributed by Petteri Aimonen. + This widget provides a tab panel, which has a button bar at the top + and panels below it. Pressing a button will select the corresponding + panel. uClibc++-1.0 2011-11-05 <gnutt@nuttx.org> diff --git a/nuttx/ReleaseNotes b/nuttx/ReleaseNotes index 02cb8158d..9d146e90a 100644 --- a/nuttx/ReleaseNotes +++ b/nuttx/ReleaseNotes @@ -3440,3 +3440,323 @@ Bugfixes (see the change log for details). Some of these are very important * Applications: Modbus fixes from Freddie Chopin. As well as other, less critical bugs (see the ChangeLog for details) + +NuttX-6.25 +^^^^^^^^^^ + +The 92nd release of NuttX, Version 6.25, was made on February 1, 2013, +and is available for download from the SourceForge website. Note +that release consists of two tarballs: nuttx-6.25.tar.gz and +apps-6.25.tar.gz. Both may be needed (see the top-level nuttx/README.txt +file for build information). + +This release corresponds with SVN release number: r5594 + +Note that all SVN information has been stripped from the tarballs. If you +need the SVN configuration information, you should check out directly from +SVN. Revision r5594 should equivalent to release 6.25 of NuttX: + + svn checkout -r5594 svn://svn.code.sf.net/p/nuttx/code/trunk nuttx-code + +Or (HTTP): + + svn checkout -r5594 http://svn.code.sf.net/p/nuttx/code/trunk nuttx-code + +Additional new features and extended functionality: + + * OS Initialization + + - Removed support for CONFIG_BUILTIN_APP_START. This is not really a + useful feature and creates a violation of the OS layered + architecture. + + * Task Creation: + + - Implement a simple vfork(). In NuttX-6.25, this interface is + available only for ARM7/9, ARMv7-M (Cortext-M3/4), and MIPS32 + (PIC32MX) platforms. + - exec() now sets the priority of the new task to the same priority as + the parent task (instead of the arbirtrary value of 50). + - New, partially complient implementations of execv() and execl(). + These are only partially compliant because they do not overlay any + existing "process space" but rather create the new task and exit(). + - Add a complete implementation of posix_spawn(). This standard + interface is a better match for an MMU-less architecture than are + vfork() plus execv() or execl(). + - Add a task start hook that will be called before the task main + is started. This can be used, for example, to schedule C++ + static constructors to run automatically in the context of the + new task. + + * Task Parentage + + - Repartitioned tasking data structures. All shared resources are now + collected together in a "task group". A task group includes the + original task plus all of the pthreads created by the task. + - Added support for remember the parent "task group" when a new task is + started. + - Added optional support to record the membership of each thread in + the "task group". + - Implement support for retaining child task status in the "task group" + after the child task exists. This is behavior required by POSIX. + But in NuttX is only enabled with CONFIG_SCHED_HAVE_PARENT and + CONFIG_SCHED_CHILD_STATUS + - Add internal logic to "reparent" a task. This is useful, + for example, where the child task is created through a trampoline + task that redirects I/O. Reparenting allows the caller of posix_spawn() + to be reparented for the eventual child thread. + - Added support for SIGCHLD. Sent to all members of the parent task + group when the file member of the child task group exits. + - If SIGCHLD and retention of child task exist status are enabled, then + a more spec-compliant version of waitpid() is enabled. + - New interfaces waitid() and wait() are also enabled when SIGCHLD + is enabled. + + * File System + + - dup() and dup2() can new be used with opened files in a mounted file + system. This supports re-direction of output in NSH to files. + - The binfs file system was moved from apps/builtin to fs/binfs. The + binfs file system was extended to support execution of "builtin + applications" using exec(), execv(), execl(), or posix_spawn(). + - Added logic based on SIGCHLD to automatically unload and clean-up + after running a task that was loaded into memory. + + * Binary Formats + + - Much of the logic for "builtin applications" was moved from + apps/builtin to nuttx/binfmt/libbuiltin. Includes some extensions + contributed by Mike Smith. + - A binary loader was added for builtin applications to support + execution of "builtin applications" using exec(), execv(), + execl(), or posix_spawn(). + + * Drivers: + + - Added logic to marshal and serialized "out-of-band" keyboard + commands (such as cursor controls and key release events) intermixed + with normal ASCII keypress data. The encoding is partially integrated + in the HID keyboard driver and the decoding full integrated into the + apps/examples hidkbd and keypadtest (the latter contributed by Denis + Carlikli). + - Driver for the UG-2864HSWEG01 OLED contributed by Darcy Gong. + - Add support for removable serial devices (like USB serial). This + support is enabled by CONFIG_SERIAL_REMOVABLE. + + * ARMv7-M: + + - Added an option to use the BASEPRI register to disable interrupts + (instead of the PRIMASK). This eliminates some innocuous hardfaults + that interfere with some debug tools. You need to switch to the + BASEPRI method only if you have such tool interference. + + * STM32 Drivers + + - Bring STM32 F1 DMA capabilities up to par with the STM32 F2/F4 + (contributed by Mike Smith). + - Add support for USART single wire mode (Contributed by the PX4 + team). + - Updates to support for SPI DMA on the STM32 F1/F2/F4. From + Petteri Aimonen. + + * STM32 Boards: + + - New configuration to support the UG-2864HSWEG01 OLED on the + STM32F4Discovery board. + - Added a posix_spawn() test configuration for the STM32F4Discovery. + + * LM3S/LM4F + + - Files and directories repartitioned to support both LM3S and LM4F + using the STM32 organization as a model. + - Partial definitions for the LM4F contributed by Jose Pablo Carballo + (this is still a work in progress). + + * LM3S Boards + + - Added scripts and documentation to use OpenOCD with the LM3S (from + Jose Pablo Carballo). + + * LPC176x/LPC178x + + - Files and directories repartitioned to support both LPC175x/LPC176x + and the LPC177x/LPC178x families using the STM32 organization as a + model. The LPC1788 port is a work in progress by Rommel Marcelo. + + * LPC176x/LPC178x Boards: + + - Added a configuration to support the Wave Share Open1788 board. + This is still a work in progress by Rommel Marcelo. + + * LPC2148 Boards: + + - Add basic support for the The0.net ZP213x/4xPA board (with the LPC2148 + and the UG_2864AMBAG01 OLED). + - Add an nxlines configuration for the ZP213x/4xPA (with the LPC2148 + and the UG_2864AMBAG01). + + * Simulator: + + - Add an nxlines configuration for the simulator. + + * Networking: + + - Add logic to work around delayed ACKs by splitting packets + (contributed by Yan T.). + - Split net_poll() to create the internal interface psock_poll(). + + * LCDs: + + - Added support for LCD1602 alphanumeric LCD (HD4468OU controller). + + * Graphics: + + - Added 5x8 monospace font. This tiny font is useful for graph + labels and for small bitmapped display. Contributed by Petteri + Aimonen. + + * Build System: + + - Add an options to better manage toolchain prefixes. + - Redesigned how the context targer works in the apps/ directory. + The old design caused lots of problems when changing configurations + because there is no easy way to get the system to rebuild the + context. This change should solve most the problems and eliminate + questions like "Why don't I see my builtin application in NSH?" + + * Kconfig Files: + + - There are several new configurations that use the kconfig-frontends + tools and several older configurations that have been converted to + use these tools. There is still a long way to go before the conversion + is complete: + + configs/sim/nxwm + configs/sim/nsh + configs/stm3220g-eval/nxwm + configs/stm32f4discovery/posix_spawn + configs/olimex-lpc1766stk/nsh + configs/olimex-lpc1766stk/hidkbd + configs/olimex-lpc1766stk/nettest + configs/open1788/ostest + configs/stm32f4discovery/nsh + configs/stm32f4discovery/usbnsh + configs/lm326965-ek (all configurations) + configs/mcu123-214x/nsh + configs/ubw32/ostest + + * Tools: + + - tools/kconfig.bat: Kludge to run kconfig-frontends from a DOS shell. + - tools/configure.c: configure.c can be used to build a work-alike + program as a replacement for configure.sh. This work-alike + program would be used in environments that do not support Bash + scripting (such as the Windows native environment). + - tools/configure.bat: configure.bat is a small Windows batch + file that can be used as a replacement for configure.sh in a + Windows native environment. configure.bat is actually just a + thin layer that executes configure.exe if it is available. If + configure.exe is not available, then configure.bat will attempt + to build it first. + + * Applications: + + - New and modified examples: + + apps/examples/wlan: Remove non-functional example. + apps/examples/ostest: Added a test of vfork(). Extend signal + handler test to catch death-of-child signals (SIGCHLD). Add a + test for waitpid(), waitid(), and wait(). + apps/exampes/posix_spawn: Added a test of posix_spawn(). + + - NSH: + + NSH now supports re-direction of I/O to files (but still not from). + The block driver source argument to the mount command is now + optional for file systems that do not require a block driver. + NSH can now execute a program from a file system using posix_spawn(). + Added support for a login script. The init.d/rcS script will be + executed once when NSH starts; the .nshrc script will be executed + for each session: Once for serial, once for each USB connection, + once for each Telnet session. + + - Supports a new daemon that can be used to monitor USB trace outpout. + - Removed non-functional wlan example. + +Bugfixes (see the ChangeLog for details). Some of these are very important: + + * Tasking: + + - Fixed a *critical* task exit bug. Here is the failure scenario: + (1) sched_lock() is called increments the lockcount on the current + TCB (i.e., the one at the head of the ready to run list), (2) + sched_mergepending is called which may change the task at the head + of the ready-to-run list, then (3) sched_unlock() is called which + decrements the lockcount on the wrong TCB. The failure case that + I saw was that pre-emption got disabled in the IDLE thread, locking + up the whole system. + + * Signals: + + - sigtimedwait() would return a bad signal number if the signal was + already pending when the function was called. + + * Drivers: + + - Some SD cards will appear busy until switched to SPI mode for + first time. Having a pull-up resistor on MISO may avoid this + problem, but this fix from Petteri Aimonen makes it work also + without pull-up. + + * STM32 Drivers: + + - STM32 FLASH driver counting error (from Freddie Chopin). + - STM32 F4 maximum SPI frequency was wrong (corrected by Petteri + Aimonen). + + * STM32 Boards + + - Due to cloning of untested code, the logic to control on-board + LEDs did not work on any STM32 boards. + - Serial devices number /dev/ttyS0-5 is there is a serial console, + but /dev/ttyS1-6 if there is no serial console. + + * Binary Formats + + - C++ static constructors execute now using a start taskhook + so that they execute in the context of the child task (instead + of in the context of the parent task). + + * File Systems: + + - Several FAT-related bugs fixed by Petteri Aimonen. + + * Networking: + + - Fix poll/select issure reported by Qiang: poll_interrupt() must call + net_lostconnection() when a loss of connection is reported. Otherwise, + the system will not remember that the connection has been lost and will + hang waiting on a unconnected socket later. + - Similar issues corrected for recvfrom() and send(). + - Telnetd would hang in a loop if recv() ever returned a value <= 0. + + * Libraries: + + - fread() could hang on certain error conditions. + - Can't handle SYSLOG output to a character device from the IDLE task + (because the IDLE task can't block). + + * Build System: + + - Serial was driver was not being built if there is no console + device. Obviously, the serial driver may be needed even in + this case. + + * Additional Bugfixes: + + - sig_timedwait() and clock_time2ticks.c: Timing "rounding" logic + - ARM9 Compilation issue with low vectors. + - readline() return value + - Others as detailed in the ChangeLog: HID keyboard, LPC17xx bit + definitions, strndup(), PL2303, SYSLOG error handling, AT25, + apps/examples. -- cgit v1.2.3 From 50b736333f97761e4613354ee853071c652736ca Mon Sep 17 00:00:00 2001 From: Lorenz MeierDate: Mon, 4 Feb 2013 15:57:12 +0100 Subject: Reduced, but functional u-blox series driver --- apps/drivers/drv_gps.h | 80 +++++ apps/drivers/gps/Makefile | 42 +++ apps/drivers/gps/gps.cpp | 627 ++++++++++++++++++++++++++++++++ apps/drivers/gps/gps_helper.h | 47 +++ apps/drivers/gps/ubx.cpp | 818 ++++++++++++++++++++++++++++++++++++++++++ apps/drivers/gps/ubx.h | 369 +++++++++++++++++++ 6 files changed, 1983 insertions(+) create mode 100644 apps/drivers/drv_gps.h create mode 100644 apps/drivers/gps/Makefile create mode 100644 apps/drivers/gps/gps.cpp create mode 100644 apps/drivers/gps/gps_helper.h create mode 100644 apps/drivers/gps/ubx.cpp create mode 100644 apps/drivers/gps/ubx.h (limited to 'apps') diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h new file mode 100644 index 000000000..89fc54b0d --- /dev/null +++ b/apps/drivers/drv_gps.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file GPS driver interface. + */ + +#ifndef _DRV_GPS_H +#define _DRV_GPS_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define GPS_DEVICE_PATH "/dev/gps" + +typedef enum { + GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_MTK19, + GPS_DRIVER_MODE_MTK16, + GPS_DRIVER_MODE_NMEA, +} gps_driver_mode_t; + + + + +/* + * ObjDev tag for GPS data. + */ +ORB_DECLARE(gps); + +/* + * ioctl() definitions + */ +#define _GPSIOCBASE (0x2800) //TODO: arbitrary choice... +#define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n)) + +/** configure ubx mode */ +#define GPS_CONFIGURE_UBX _GPSIOC(0) + +/** configure mtk mode */ +#define GPS_CONFIGURE_MTK19 _GPSIOC(1) +#define GPS_CONFIGURE_MTK16 _GPSIOC(2) + +/** configure mtk mode */ +#define GPS_CONFIGURE_NMEA _GPSIOC(3) + +#endif /* _DRV_GPS_H */ diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile new file mode 100644 index 000000000..3859a88a5 --- /dev/null +++ b/apps/drivers/gps/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# GPS driver +# + +APPNAME = gps +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp new file mode 100644 index 000000000..7d6f7144e --- /dev/null +++ b/apps/drivers/gps/gps.cpp @@ -0,0 +1,627 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps.cpp + * Driver for the GPS on UART6 + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +#include + +#include + +#include "ubx.h" + +#define SEND_BUFFER_LENGTH 100 +#define TIMEOUT 1000000 //1s + +#define NUMBER_OF_BAUDRATES 4 +#define CONFIG_TIMEOUT 2000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + + + +class GPS : public device::CDev +{ +public: + GPS(); + ~GPS(); + + virtual int init(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + int _task_should_exit; + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; + uint8_t _send_buffer[SEND_BUFFER_LENGTH]; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + volatile int _task; ///< worker task + + bool _response_received; + bool _config_needed; + bool _baudrate_changed; + bool _mode_changed; + bool _healthy; + gps_driver_mode_t _mode; + unsigned _messages_received; + + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + + struct vehicle_gps_position_s _report; + orb_advert_t _report_pub; + + orb_advert_t _gps_topic; + + void recv(); + + void config(); + + static void task_main_trampoline(void *arg); + + + /** + * worker task + */ + void task_main(void); + + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_main(int argc, char *argv[]); + +namespace +{ + +GPS *g_dev; + +} + + +GPS::GPS() : + CDev("gps", GPS_DEVICE_PATH), + _task_should_exit(false), + _baudrates_to_try({9600, 38400, 57600, 115200}), + _config_needed(true), + _baudrate_changed(false), + _mode_changed(true), + _healthy(false), + _mode(GPS_DRIVER_MODE_UBX), + _messages_received(0), + _Helper(nullptr) +{ + /* we need this potentially before it could be set in task_main */ + g_dev = this; + + _debug_enabled = true; + debug("[gps driver] instantiated"); +} + +GPS::~GPS() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + task_delete(_task); + g_dev = nullptr; +} + +int +GPS::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + + /* start the IO interface task */ + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + +// if (_gps_topic < 0) +// debug("failed to create GPS object"); + + ret = OK; +out: + return ret; +} + +int +GPS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case GPS_CONFIGURE_UBX: + if (_mode != GPS_DRIVER_MODE_UBX) { + _mode = GPS_DRIVER_MODE_UBX; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK19: + if (_mode != GPS_DRIVER_MODE_MTK19) { + _mode = GPS_DRIVER_MODE_MTK19; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK16: + if (_mode != GPS_DRIVER_MODE_MTK16) { + _mode = GPS_DRIVER_MODE_MTK16; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_NMEA: + if (_mode != GPS_DRIVER_MODE_NMEA) { + _mode = GPS_DRIVER_MODE_NMEA; + _mode_changed = true; + } + break; + case SENSORIOCRESET: + cmd_reset(); + break; + } + + return ret; +} + +void +GPS::config() +{ + int length = 0; + + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + + /* the message needs to be sent at the old baudrate */ + if (length > 0) { + if (length != ::write(_serial_fd, _send_buffer, length)) { + debug("write config failed"); + return; + } + } + + if (_baudrate_changed) { + set_baudrate(_baudrate); + _baudrate_changed = false; + } +} + +void +GPS::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +void +GPS::task_main() +{ + log("starting"); + + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + + memset(&_report, 0, sizeof(_report)); + + /* open the serial port */ + _serial_fd = ::open("/dev/ttyS3", O_RDWR); + + uint8_t buf[256]; + int baud_i = 0; + uint64_t time_before_configuration; + + if (_serial_fd < 0) { + log("failed to open serial port: %d", errno); + goto out; + } + + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + debug("ready"); + + /* lock against the ioctl handler */ + lock(); + + + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + + time_before_configuration = hrt_absolute_time(); + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + if (_mode_changed) { + if (_Helper != nullptr) { + delete(_Helper); + _Helper = nullptr; // XXX is this needed? + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(); + break; + case GPS_DRIVER_MODE_MTK19: + //_Helper = new MTK19(); + break; + case GPS_DRIVER_MODE_MTK16: + //_Helper = new MTK16(); + break; + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); + break; + default: + break; + } + _mode_changed = false; + } + + if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { + baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + time_before_configuration = hrt_absolute_time(); + } + + + int poll_timeout; + if (_config_needed) { + poll_timeout = 50; + } else { + poll_timeout = 250; + } + /* sleep waiting for data, but no more than 1000ms */ + unlock(); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); + lock(); + + + + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + } else if (ret == 0) { + config(); + if (_config_needed == false) { + _config_needed = true; + debug("Lost GPS module"); + } + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + int count; + + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_serial_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ + int j; + for (j = 0; j < count; j++) { + _messages_received += _Helper->parse(buf[j], &_report); + } + if (_messages_received > 0) { + if (_config_needed == true) { + _config_needed = false; + debug("Found GPS module"); + } + + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + _messages_received = 0; + } + } + } + } +out: + debug("exiting"); + + ::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +GPS::set_baudrate(unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_serial_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + return -1; + } + + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } +} + +void +GPS::cmd_reset() +{ + _healthy = false; +} + +void +GPS::print_info() +{ + +} + +/** + * Local functions in support of the shell command. + */ +namespace gps +{ + +GPS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new GPS; + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + printf("Could not open device path: %s\n", GPS_DEVICE_PATH); + goto fail; + } + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +void +stop() +{ + delete g_dev; + g_dev = nullptr; + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + exit(0); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +int +gps_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + gps::start(); + + if (!strcmp(argv[1], "stop")) + gps::stop(); + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps::info(); + + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); +} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h new file mode 100644 index 000000000..baacf7cb4 --- /dev/null +++ b/apps/drivers/gps/gps_helper.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef GPS_HELPER_H + +class GPS_Helper +{ +public: + virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; + virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; +}; + +#endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp new file mode 100644 index 000000000..981b023e5 --- /dev/null +++ b/apps/drivers/gps/ubx.cpp @@ -0,0 +1,818 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol implementation */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" + + +UBX::UBX() : +_waited(0), +_config_state(UBX_CONFIG_STATE_PRT), +_new_nav_posllh(false), +_new_nav_timeutc(false), +_new_nav_dop(false), +_new_nav_sol(false), +_new_nav_velned(false) +{ + decodeInit(); + _waiting_for_ack = false; +} + +UBX::~UBX() +{ +} + +void +UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) +{ + /* make sure the buffer to write the message is long enough */ + assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); + + /* try again after 10 times */ + if (_waited > 10) { + _waiting_for_ack = false; + } + + if (!_waiting_for_ack) { + _waiting_for_ack = true; + _waited = 0; + if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { + config_needed = false; + length = 0; + _config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */ + _waiting_for_ack = false; + return; + } else if (_config_state == UBX_CONFIG_STATE_PRT) { + /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { + +// printf("Send change of baudrate now\n"); + + /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + /* detection when the baudrate has been changed in the first step */ + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + /* set a flag and exit */ + baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; + baudrate_changed = true; + _config_state = UBX_CONFIG_STATE_RATE; + _waiting_for_ack = false; + return; + } + + + + } else if (_config_state == UBX_CONFIG_STATE_RATE) { + + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); + length = sizeof(cfg_rate_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_NAV5) { + /* send a NAV5 message to set the options for the internal estimator */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); + length = sizeof(cfg_nav5_packet)+2; + + } else { + /* catch the remaining config states here */ + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + + switch (_config_state) { + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + cfg_msg_packet.rate[1] = 5; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; +// break; + default: + break; + } + + addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); + length = sizeof(cfg_msg_packet)+2; + } + } else { + _waited++; + } +} + +int +UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +{ + int ret = 0; + //printf("Received char: %c\n", b); + + switch (_decode_state) { + /* First, look for sync1 */ + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + break; + /* Second, look for sync2 */ + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + } else { + /* Second start symbol was wrong, reset state machine */ + decodeInit(); + } + break; + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + addByteToChecksum(b); + /* check for known class */ + switch (b) { + case UBX_CLASS_ACK: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = ACK; + break; + + case UBX_CLASS_NAV: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = NAV; + break; + +// case UBX_CLASS_RXM: +// _decode_state = UBX_DECODE_GOT_CLASS; +// _message_class = RXM; +// break; + + case UBX_CLASS_CFG: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = CFG; + break; + default: //unknown class: reset state machine + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_CLASS: + addByteToChecksum(b); + switch (_message_class) { + case NAV: + switch (b) { + case UBX_MESSAGE_NAV_POSLLH: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_POSLLH; + break; + + case UBX_MESSAGE_NAV_SOL: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SOL; + break; + + case UBX_MESSAGE_NAV_TIMEUTC: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_TIMEUTC; + break; + + case UBX_MESSAGE_NAV_DOP: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_DOP; + break; + + case UBX_MESSAGE_NAV_SVINFO: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SVINFO; + break; + + case UBX_MESSAGE_NAV_VELNED: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_VELNED; + break; + + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; +// case RXM: +// switch (b) { +// case UBX_MESSAGE_RXM_SVSI: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = RXM_SVSI; +// break; +// +// default: //unknown class: reset state machine, should not happen +// decodeInit(); +// break; +// } +// break; + + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; + + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; + default: //should not happen because we set the class + printf("UBX Error, we set a class that we don't know\n"); + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_MESSAGEID: + addByteToChecksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + case UBX_DECODE_GOT_LENGTH1: + addByteToChecksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + case UBX_DECODE_GOT_LENGTH2: + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) + addByteToChecksum(b); + _rx_buffer[_rx_count] = b; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + gps_position->lat = packet->lat; + gps_position->lon = packet->lon; + gps_position->alt = packet->height_msl; + + gps_position->counter_pos_valid++; + gps_position->counter++; + + _new_nav_posllh = true; + + } else { + printf("[gps] NAV_POSLLH: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->fix_type = packet->gpsFix; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + gps_position->s_variance = packet->sAcc; + gps_position->p_variance = packet->pAcc; + + _new_nav_sol = true; + + } else { + printf("[gps] NAV_SOL: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_DOP: { +// printf("GOT NAV_DOP MESSAGE\n"); + gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->eph = packet->hDOP; + gps_position->epv = packet->vDOP; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_dop = true; + + } else { + printf("[gps] NAV_DOP: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + + gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_timeutc = true; + + } else { + printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; + + //Check if checksum is valid and then store the gps information + if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + gps_position->satellite_used[i] = 0; + } + + gps_position->satellite_snr[i] = packet_part2->cno; + gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + gps_position->satellite_used[i] = 0; + gps_position->satellite_snr[i] = 0; + gps_position->satellite_elevation[i] = 0; + gps_position->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + gps_position->satellite_prn[i] = 0; + gps_position->satellite_used[i] = 0; + gps_position->satellite_snr[i] = 0; + gps_position->satellite_elevation[i] = 0; + gps_position->satellite_azimuth[i] = 0; + } + + /* set flag if any sat info is available */ + if (!packet_part1->numCh > 0) { + gps_position->satellite_info_available = 1; + + } else { + gps_position->satellite_info_available = 0; + } + + gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report + + } else { + printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->vel = (uint16_t)packet->speed; + gps_position->vel_n = packet->velN / 100.0f; + gps_position->vel_e = packet->velE / 100.0f; + gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_ned_valid = true; + gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + + _new_nav_velned = true; + + } else { + printf("[gps] NAV_VELNED: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { +// +// gps_position->satellites_visible = packet->numVis; +// gps_position->timestamp = hrt_absolute_time(); +// gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// ret = 1; +// +// } else { +// printf("[gps] RXM_SVSI: checksum invalid\n"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// return ret; +// +// break; +// } + + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + _waiting_for_ack = false; + + switch (_config_state) { + case UBX_CONFIG_STATE_PRT: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; + break; + case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_RATE; + break; + case UBX_CONFIG_STATE_RATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) + _config_state = UBX_CONFIG_STATE_NAV5; + break; + case UBX_CONFIG_STATE_NAV5: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) + _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_CONFIGURED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_CONFIGURED; +// break; + default: + break; + } + } else { + printf("[gps] ACK_ACK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + printf("[gps] the ubx gps returned: not acknowledged\n"); + ret = 1; + + } else { + printf("[gps] ACK_NAK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + default: //we don't know the message + printf("Unknown message received: %d-%d\n",_message_class,_message_id); + decodeInit(); + + break; + } + } // end if _rx_count high enough + if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + printf("Buffer full"); + decodeInit(); + } + break; + default: + break; + } + + /* return 1 when all needed messages have arrived */ + if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + + gps_position->timestamp = hrt_absolute_time(); + ret = 1; + _new_nav_posllh = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + } + + return ret; +} + +void +UBX::decodeInit(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = UBX_DECODE_UNINIT; + _message_class = CLASS_UNKNOWN; + _message_id = ID_UNKNOWN; + _payload_size = 0; +} + +void +UBX::addByteToChecksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} + +void +UBX::addChecksumToMessage(uint8_t* message, const unsigned length) +{ + uint8_t ck_a = 0; + uint8_t ck_b = 0; + unsigned i; + + for (i = 0; i < length-2; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + message[length-2] = ck_a; + message[length-1] = ck_b; +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h new file mode 100644 index 000000000..dff25a518 --- /dev/null +++ b/apps/drivers/gps/ubx.h @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef UBX_H_ +#define UBX_H_ + +#include "gps_helper.h" + +#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ + +#define UBX_SYNC1 0xB5 +#define UBX_SYNC2 0x62 + +//UBX Protocol definitions (this is the subset of the messages that are parsed) +#define UBX_CLASS_NAV 0x01 +//#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_MESSAGE_NAV_POSLLH 0x02 +#define UBX_MESSAGE_NAV_SOL 0x06 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_DOP 0x04 +#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_VELNED 0x12 +//#define UBX_MESSAGE_RXM_SVSI 0x20 +#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_PRT 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_CFG_MSG 0x01 +#define UBX_MESSAGE_CFG_RATE 0x08 + +#define UBX_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ + +#define UBX_CFG_RATE_LENGTH 6 +#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ +#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ +#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ + + +#define UBX_CFG_NAV5_LENGTH 36 +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ + +#define UBX_CFG_MSG_LENGTH 8 +#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ + + +// ************ +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t lon; // Longitude * 1e-7, deg + int32_t lat; // Latitude * 1e-7, deg + int32_t height; // Height above Ellipsoid, mm + int32_t height_msl; // Height above mean sea level, mm + uint32_t hAcc; // Horizontal Accuracy Estimate, mm + uint32_t vAcc; // Vertical Accuracy Estimate, mm + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_posllh_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 + int16_t week; // GPS week (GPS time) + uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; + uint32_t reserved2; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_sol_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint32_t time_accuracy; //Time Accuracy Estimate, ns + int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) + uint16_t year; //Year, range 1999..2099 (UTC) + uint8_t month; //Month, range 1..12 (UTC) + uint8_t day; //Day of Month, range 1..31 (UTC) + uint8_t hour; //Hour of Day, range 0..23 (UTC) + uint8_t min; //Minute of Hour, range 0..59 (UTC) + uint8_t sec; //Seconds of Minute, range 0..59 (UTC) + uint8_t valid_flag; //Validity Flags (see ubx documentation) + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_timeutc_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint16_t gDOP; //Geometric DOP (scaling 0.01) + uint16_t pDOP; //Position DOP (scaling 0.01) + uint16_t tDOP; //Time DOP (scaling 0.01) + uint16_t vDOP; //Vertical DOP (scaling 0.01) + uint16_t hDOP; //Horizontal DOP (scaling 0.01) + uint16_t nDOP; //Northing DOP (scaling 0.01) + uint16_t eDOP; //Easting DOP (scaling 0.01) + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_dop_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint8_t numCh; //Number of channels + uint8_t globalFlags; + uint16_t reserved2; + +} gps_bin_nav_svinfo_part1_packet_t; + +typedef struct { + uint8_t chn; //Channel number, 255 for SVs not assigned to a channel + uint8_t svid; //Satellite ID + uint8_t flags; + uint8_t quality; + uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz + int8_t elev; //Elevation in integer degrees + int16_t azim; //Azimuth in integer degrees + int32_t prRes; //Pseudo range residual in centimetres + +} gps_bin_nav_svinfo_part2_packet_t; + +typedef struct { + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_svinfo_part3_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t velN; //NED north velocity, cm/s + int32_t velE; //NED east velocity, cm/s + int32_t velD; //NED down velocity, cm/s + uint32_t speed; //Speed (3-D), cm/s + uint32_t gSpeed; //Ground Speed (2-D), cm/s + int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 + uint32_t sAcc; //Speed Accuracy Estimate, cm/s + uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_velned_packet_t; + +//typedef struct { +// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week +// int16_t week; //Measurement GPS week number +// uint8_t numVis; //Number of visible satellites +// +// //... rest of package is not used in this implementation +// +//} gps_bin_rxm_svsi_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_ack_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_nak_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t portID; + uint8_t res0; + uint16_t res1; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t pad; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_rate_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint32_t reserved2; + uint32_t reserved3; + uint32_t reserved4; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_nav5_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t msgClass_payload; + uint8_t msgID_payload; + uint8_t rate[6]; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_msg_packet_t; + + +// END the structures of the binary packets +// ************ + +typedef enum { + UBX_CONFIG_STATE_PRT = 0, + UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, + UBX_CONFIG_STATE_RATE, + UBX_CONFIG_STATE_NAV5, + UBX_CONFIG_STATE_MSG_NAV_POSLLH, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, + UBX_CONFIG_STATE_MSG_NAV_DOP, + UBX_CONFIG_STATE_MSG_NAV_SVINFO, + UBX_CONFIG_STATE_MSG_NAV_SOL, + UBX_CONFIG_STATE_MSG_NAV_VELNED, +// UBX_CONFIG_STATE_MSG_RXM_SVSI, + UBX_CONFIG_STATE_CONFIGURED +} ubx_config_state_t; + +typedef enum { + CLASS_UNKNOWN = 0, + NAV = 1, + RXM = 2, + ACK = 3, + CFG = 4 +} ubx_message_class_t; + +typedef enum { + //these numbers do NOT correspond to the message id numbers of the ubx protocol + ID_UNKNOWN = 0, + NAV_POSLLH, + NAV_SOL, + NAV_TIMEUTC, + NAV_DOP, + NAV_SVINFO, + NAV_VELNED, +// RXM_SVSI, + CFG_NAV5, + ACK_ACK, + ACK_NAK, +} ubx_message_id_t; + +typedef enum { + UBX_DECODE_UNINIT = 0, + UBX_DECODE_GOT_SYNC1, + UBX_DECODE_GOT_SYNC2, + UBX_DECODE_GOT_CLASS, + UBX_DECODE_GOT_MESSAGEID, + UBX_DECODE_GOT_LENGTH1, + UBX_DECODE_GOT_LENGTH2 +} ubx_decode_state_t; + +//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; +#pragma pack(pop) + +#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer + +class UBX : public GPS_Helper +{ +public: + UBX(); + ~UBX(); + + void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + int parse(uint8_t, struct vehicle_gps_position_s*); + +private: + void decodeInit(void); + void addByteToChecksum(uint8_t); + void addChecksumToMessage(uint8_t*, const unsigned); + unsigned _waited; + bool _waiting_for_ack; + ubx_config_state_t _config_state; + ubx_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; + ubx_message_id_t _message_id; + unsigned _payload_size; + bool _new_nav_posllh; + bool _new_nav_timeutc; + bool _new_nav_dop; + bool _new_nav_sol; + bool _new_nav_velned; +}; + +#endif /* UBX_H_ */ -- cgit v1.2.3 From 3fd8c73bfbdad1fdb9b5b31337cf6eb3fa46c34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 15:58:53 +0100 Subject: Disabled old-style gps interface, enabled GPS driver --- apps/drivers/gps/gps.cpp | 2 ++ nuttx/configs/px4fmu/nsh/appconfig | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 7d6f7144e..9fd679240 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -471,6 +471,8 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } + + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ } void diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 653f97de1..710e2fb5f 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -80,7 +80,6 @@ CONFIGURED_APPS += uORB CONFIGURED_APPS += mavlink CONFIGURED_APPS += mavlink_onboard -CONFIGURED_APPS += gps CONFIGURED_APPS += commander CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors @@ -114,6 +113,7 @@ CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil +CONFIGURED_APPS += drivers/gps # Testing stuff CONFIGURED_APPS += px4/sensors_bringup -- cgit v1.2.3 From 12f4cb2dc38303b58750a8f3b120738a291ac8d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:13:17 +0100 Subject: Tuned GPS update rates --- apps/mavlink/mavlink.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ceb7c2554..09abcf269 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 2 Hz */ + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { @@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 100 ms */ + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ -- cgit v1.2.3 From 30f028908a69db058c05b9af7e31c8c52c3bc339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:15:48 +0100 Subject: Fixed typo --- apps/mavlink/mavlink.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 09abcf269..b958d5f96 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -626,7 +626,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); @@ -639,7 +639,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -656,7 +656,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ -- cgit v1.2.3 From 13ec0675703ecc9c7bccb22ef3cd049888dd1abb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 17:55:58 +0100 Subject: Minor quick cleanups --- apps/drivers/gps/gps.cpp | 108 ++++++++++++++++++++++++++++++++--------------- apps/drivers/gps/ubx.cpp | 28 ++++++------ 2 files changed, 89 insertions(+), 47 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 9fd679240..6f7310cc5 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -107,30 +107,29 @@ public: private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS + int _task_should_exit; + int _serial_fd; ///< serial interface to GPS unsigned _baudrate; unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + volatile int _task; ///< worker task bool _response_received; bool _config_needed; bool _baudrate_changed; bool _mode_changed; bool _healthy; - gps_driver_mode_t _mode; + gps_driver_mode_t _mode; unsigned _messages_received; + float _rate; ///< position update rate - GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - struct vehicle_gps_position_s _report; - orb_advert_t _report_pub; - - orb_advert_t _gps_topic; + struct vehicle_gps_position_s _report; ///< the current GPS report + orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message void recv(); @@ -140,16 +139,19 @@ private: /** - * worker task + * Worker task. */ - void task_main(void); + void task_main(void); - int set_baudrate(unsigned baud); + /** + * Set the module baud rate + */ + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS */ - void cmd_reset(); + void cmd_reset(); }; @@ -177,13 +179,15 @@ GPS::GPS() : _healthy(false), _mode(GPS_DRIVER_MODE_UBX), _messages_received(0), - _Helper(nullptr) + _Helper(nullptr), + _rate(0.0f), + _report_pub(-1) { /* we need this potentially before it could be set in task_main */ g_dev = this; + memset(&_report, 0, sizeof(_report)); _debug_enabled = true; - debug("[gps driver] instantiated"); } GPS::~GPS() @@ -213,16 +217,13 @@ GPS::init() goto out; /* start the IO interface task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + warnx("task start failed: %d", errno); return -errno; } -// if (_gps_topic < 0) -// debug("failed to create GPS object"); - ret = OK; out: return ret; @@ -231,6 +232,8 @@ out: int GPS::ioctl(struct file *filp, int cmd, unsigned long arg) { + lock(); + int ret = OK; switch (cmd) { @@ -263,6 +266,8 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) break; } + unlock(); + return ret; } @@ -298,10 +303,6 @@ GPS::task_main() { log("starting"); - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - - memset(&_report, 0, sizeof(_report)); - /* open the serial port */ _serial_fd = ::open("/dev/ttyS3", O_RDWR); @@ -311,7 +312,9 @@ GPS::task_main() if (_serial_fd < 0) { log("failed to open serial port: %d", errno); - goto out; + /* tell the dtor that we are exiting, set error code */ + _task = -1; + _exit(1); } /* poll descriptor */ @@ -329,6 +332,9 @@ GPS::task_main() time_before_configuration = hrt_absolute_time(); + uint64_t last_rate_measurement = hrt_absolute_time(); + unsigned last_rate_count = 0; + /* loop handling received serial bytes */ while (!_task_should_exit) { @@ -369,9 +375,9 @@ GPS::task_main() if (_config_needed) { poll_timeout = 50; } else { - poll_timeout = 250; + poll_timeout = 400; } - /* sleep waiting for data, but no more than 1000ms */ + /* sleep waiting for data, but no more than the poll timeout */ unlock(); int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); lock(); @@ -386,7 +392,7 @@ GPS::task_main() config(); if (_config_needed == false) { _config_needed = true; - debug("Lost GPS module"); + warnx("lost GPS module"); } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -411,8 +417,22 @@ GPS::task_main() debug("Found GPS module"); } - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + _messages_received = 0; + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } } } } @@ -484,7 +504,29 @@ GPS::cmd_reset() void GPS::print_info() { - + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + case GPS_DRIVER_MODE_MTK19: + warnx("protocol: MTK 1.9"); + break; + case GPS_DRIVER_MODE_MTK16: + warnx("protocol: MTK 1.6"); + break; + case GPS_DRIVER_MODE_NMEA: + warnx("protocol: NMEA"); + break; + default: + break; + } + warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); + if (_report.timestamp != 0) { + warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type, + int((hrt_absolute_time() - _report.timestamp) / 1000000)); + warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("update rate: %6.2f Hz", (double)_rate); + } } /** diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 981b023e5..7e95514dd 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -371,7 +371,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } break; default: //should not happen because we set the class - printf("UBX Error, we set a class that we don't know\n"); + warnx("UBX Error, we set a class that we don't know\n"); decodeInit(); break; } @@ -410,7 +410,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_posllh = true; } else { - printf("[gps] NAV_POSLLH: checksum invalid\n"); + warnx("NAV_POSLLH: checksum invalid\n"); } // Reset state machine to decode next packet @@ -437,7 +437,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_sol = true; } else { - printf("[gps] NAV_SOL: checksum invalid\n"); + warnx("NAV_SOL: checksum invalid\n"); } // Reset state machine to decode next packet @@ -463,7 +463,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_dop = true; } else { - printf("[gps] NAV_DOP: checksum invalid\n"); + warnx("NAV_DOP: checksum invalid\n"); } // Reset state machine to decode next packet @@ -624,7 +624,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_velned = true; } else { - printf("[gps] NAV_VELNED: checksum invalid\n"); + warnx("NAV_VELNED: checksum invalid\n"); } // Reset state machine to decode next packet @@ -651,7 +651,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // ret = 1; // // } else { -// printf("[gps] RXM_SVSI: checksum invalid\n"); +// warnx("RXM_SVSI: checksum invalid\n"); // } // // // Reset state machine to decode next packet @@ -719,7 +719,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } else { - printf("[gps] ACK_ACK: checksum invalid\n"); + warnx("ACK_ACK: checksum invalid\n"); } // Reset state machine to decode next packet @@ -736,11 +736,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - printf("[gps] the ubx gps returned: not acknowledged\n"); + warnx("the ubx gps returned: not acknowledged\n"); ret = 1; } else { - printf("[gps] ACK_NAK: checksum invalid\n"); + warnx("ACK_NAK: checksum invalid\n"); } // Reset state machine to decode next packet @@ -768,16 +768,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } - /* return 1 when all needed messages have arrived */ + /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { gps_position->timestamp = hrt_absolute_time(); ret = 1; _new_nav_posllh = false; - _new_nav_timeutc = false; - _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; + // _new_nav_timeutc = false; + // _new_nav_dop = false; + // _new_nav_sol = false; + // _new_nav_velned = false; } return ret; -- cgit v1.2.3 From d4bd7225baa8c46e6a93dcd063f66ce53fe88e69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 18:00:10 +0100 Subject: More cleanup --- apps/drivers/gps/gps.cpp | 3 ++- apps/drivers/gps/ubx.cpp | 22 +++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 6f7310cc5..bd77e8969 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -341,7 +341,8 @@ GPS::task_main() if (_mode_changed) { if (_Helper != nullptr) { delete(_Helper); - _Helper = nullptr; // XXX is this needed? + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; } switch (_mode) { diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 7e95514dd..fe4758d8c 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -371,7 +371,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } break; default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know\n"); + warnx("UBX Error, we set a class that we don't know"); decodeInit(); break; } @@ -410,7 +410,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_posllh = true; } else { - warnx("NAV_POSLLH: checksum invalid\n"); + warnx("NAV_POSLLH: checksum invalid"); } // Reset state machine to decode next packet @@ -437,7 +437,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_sol = true; } else { - warnx("NAV_SOL: checksum invalid\n"); + warnx("NAV_SOL: checksum invalid"); } // Reset state machine to decode next packet @@ -463,7 +463,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_dop = true; } else { - warnx("NAV_DOP: checksum invalid\n"); + warnx("NAV_DOP: checksum invalid"); } // Reset state machine to decode next packet @@ -499,7 +499,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_timeutc = true; } else { - printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + warnx("NAV_TIMEUTC: checksum invalid"); } // Reset state machine to decode next packet @@ -593,7 +593,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report } else { - printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + warnx("NAV_SVINFO: checksum invalid"); } // Reset state machine to decode next packet @@ -624,7 +624,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_velned = true; } else { - warnx("NAV_VELNED: checksum invalid\n"); + warnx("NAV_VELNED: checksum invalid"); } // Reset state machine to decode next packet @@ -719,7 +719,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } else { - warnx("ACK_ACK: checksum invalid\n"); + warnx("ACK_ACK: checksum invalid"); } // Reset state machine to decode next packet @@ -736,7 +736,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - warnx("the ubx gps returned: not acknowledged\n"); + warnx("UBX: NO ACK"); ret = 1; } else { @@ -751,7 +751,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } default: //we don't know the message - printf("Unknown message received: %d-%d\n",_message_class,_message_id); + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); decodeInit(); break; @@ -760,7 +760,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - printf("Buffer full"); + warnx("buffer overflow"); decodeInit(); } break; -- cgit v1.2.3 From cb0fd834ae15c45cf6993f8c9eea9c99b2b153dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 18:14:55 +0100 Subject: Minor polishing, fixed rate and last measurement indication --- apps/drivers/gps/gps.cpp | 6 +++--- apps/drivers/gps/ubx.cpp | 6 ------ apps/mavlink/orb_listener.c | 2 +- 3 files changed, 4 insertions(+), 10 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index bd77e8969..49b94fc2e 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -430,7 +430,7 @@ GPS::task_main() /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; } @@ -523,8 +523,8 @@ GPS::print_info() } warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { - warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type, - int((hrt_absolute_time() - _report.timestamp) / 1000000)); + warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, + ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index fe4758d8c..1105e0da4 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -429,7 +429,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->fix_type = packet->gpsFix; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; gps_position->s_variance = packet->sAcc; gps_position->p_variance = packet->pAcc; @@ -457,7 +456,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->eph = packet->hDOP; gps_position->epv = packet->vDOP; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_dop = true; @@ -493,7 +491,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_timeutc = true; @@ -587,7 +584,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -617,7 +613,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->vel_ned_valid = true; gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; @@ -645,7 +640,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { // // gps_position->satellites_visible = packet->numVis; -// gps_position->timestamp = hrt_absolute_time(); // gps_position->counter++; // _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); // ret = 1; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index ec5149745..18da70f61 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -698,7 +698,7 @@ uorb_receive_start(void) /* --- GPS VALUE --- */ mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); -- cgit v1.2.3 From 039d394c207830a2c9c3a22e03302c867bd57af6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Feb 2013 16:27:01 -0800 Subject: Merged with newer, cleaned up code, fixed the checksum error --- apps/drivers/drv_gps.h | 2 - apps/drivers/gps/gps.cpp | 130 +++++++++++++++++++++++------------------- apps/drivers/gps/gps_helper.h | 5 +- apps/drivers/gps/ubx.cpp | 107 +++++++++++++++++----------------- apps/drivers/gps/ubx.h | 114 +++++++++++++++++++----------------- 5 files changed, 191 insertions(+), 167 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 89fc54b0d..1ae27ed2f 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -54,8 +54,6 @@ typedef enum { } gps_driver_mode_t; - - /* * ObjDev tag for GPS data. */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 49b94fc2e..450b3091b 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -33,7 +33,7 @@ /** * @file gps.cpp - * Driver for the GPS on UART6 + * Driver for the GPS on a serial port */ #include @@ -107,46 +107,41 @@ public: private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; - uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; ///< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate - bool _response_received; - bool _config_needed; - bool _baudrate_changed; - bool _mode_changed; - bool _healthy; - gps_driver_mode_t _mode; - unsigned _messages_received; - float _rate; ///< position update rate - - GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - - struct vehicle_gps_position_s _report; ///< the current GPS report - orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message - - void recv(); + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ void config(); + /** + * Trampoline to the worker task + */ static void task_main_trampoline(void *arg); /** - * Worker task. + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running */ void task_main(void); /** - * Set the module baud rate + * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -176,12 +171,10 @@ GPS::GPS() : _config_needed(true), _baudrate_changed(false), _mode_changed(true), - _healthy(false), _mode(GPS_DRIVER_MODE_UBX), - _messages_received(0), _Helper(nullptr), - _rate(0.0f), - _report_pub(-1) + _report_pub(-1), + _rate(0.0f) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -216,7 +209,7 @@ GPS::init() if (CDev::init() != OK) goto out; - /* start the IO interface task */ + /* start the GPS driver worker task */ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { @@ -275,17 +268,22 @@ void GPS::config() { int length = 0; + uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); - /* the message needs to be sent at the old baudrate */ + /* The config message is sent sent at the old baudrate */ if (length > 0) { - if (length != ::write(_serial_fd, _send_buffer, length)) { + + if (length != ::write(_serial_fd, send_buffer, length)) { debug("write config failed"); return; } } + /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed + * from 9600 to 38400 + */ if (_baudrate_changed) { set_baudrate(_baudrate); _baudrate_changed = false; @@ -304,11 +302,10 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); + _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters - uint8_t buf[256]; - int baud_i = 0; - uint64_t time_before_configuration; + /* buffer to read from the serial port */ + uint8_t buf[32]; if (_serial_fd < 0) { log("failed to open serial port: %d", errno); @@ -326,16 +323,16 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - + unsigned baud_i = 0; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); - time_before_configuration = hrt_absolute_time(); + uint64_t time_before_configuration = hrt_absolute_time(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - /* loop handling received serial bytes */ + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_mode_changed) { @@ -364,14 +361,17 @@ GPS::task_main() _mode_changed = false; } + /* If a configuration does not finish in the config timeout, change the baudrate */ if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); + _Helper->reset(); time_before_configuration = hrt_absolute_time(); } - + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, + * but during normal operation, it should never timeout because updates should arrive with 5Hz */ int poll_timeout; if (_config_needed) { poll_timeout = 50; @@ -409,11 +409,21 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; + int ret_parse = 0; for (j = 0; j < count; j++) { - _messages_received += _Helper->parse(buf[j], &_report); + ret_parse += _Helper->parse(buf[j], &_report); } - if (_messages_received > 0) { - if (_config_needed == true) { + + if (ret_parse < 0) { + /* This means something went wrong in the parser, let's reconfigure */ + if (!_config_needed) { + _config_needed = true; + debug("Lost GPS module"); + } + config(); + } else if (ret_parse > 0) { + /* Looks like we got a valid position update, stop configuring and publish it */ + if (_config_needed) { _config_needed = false; debug("Found GPS module"); } @@ -424,8 +434,6 @@ GPS::task_main() } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - - _messages_received = 0; last_rate_count++; /* measure update rate every 5 seconds */ @@ -435,10 +443,10 @@ GPS::task_main() last_rate_count = 0; } } + /* else if ret_parse == 0: just keep parsing */ } } } -out: debug("exiting"); ::close(_serial_fd); @@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } - struct termios uart_config; int termios_state; @@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud) uart_config.c_cflag &= ~(CSTOPB | PARENB); /* set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; } void GPS::cmd_reset() { - _healthy = false; + _config_needed = true; } void @@ -524,10 +533,12 @@ GPS::print_info() warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } + + usleep(100000); } /** @@ -583,6 +594,9 @@ fail: errx(1, "driver start failed"); } +/** + * Stop the driver. + */ void stop() { diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index baacf7cb4..8a6b0148b 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -40,8 +40,9 @@ class GPS_Helper { public: - virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; - virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; + virtual void reset() = 0; + virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0; + virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 1105e0da4..66a891da4 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -48,36 +48,41 @@ UBX::UBX() : -_waited(0), _config_state(UBX_CONFIG_STATE_PRT), +_waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), _new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { - decodeInit(); - _waiting_for_ack = false; + reset(); } UBX::~UBX() { } +void +UBX::reset() +{ + decodeInit(); + _config_state = UBX_CONFIG_STATE_PRT; + _waiting_for_ack = false; +} + void UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) { - /* make sure the buffer to write the message is long enough */ + /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); - /* try again after 10 times */ - if (_waited > 10) { - _waiting_for_ack = false; - } - + /* Only send a new config message when we got the ACK of the last one, + * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command + * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. + */ if (!_waiting_for_ack) { _waiting_for_ack = true; - _waited = 0; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { config_needed = false; length = 0; @@ -85,10 +90,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { - /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; @@ -98,18 +108,20 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + /* Calculate the checksum now */ addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Start with the two sync bytes */ buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; + /* Copy it to the buffer that will be written back in the main gps driver */ memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Set the length of the packet (plus the 2 sync bytes) */ length = sizeof(cfg_prt_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { -// printf("Send change of baudrate now\n"); - - /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + /* Send a CFG-PRT message again, this time change the baudrate */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); @@ -129,18 +141,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); length = sizeof(cfg_prt_packet)+2; - /* detection when the baudrate has been changed in the first step */ + /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - /* set a flag and exit */ baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; baudrate_changed = true; - _config_state = UBX_CONFIG_STATE_RATE; - _waiting_for_ack = false; - return; + /* Don't wait for an ACK, we're switching baudrate and we might never get, + * after that, start fresh */ + reset(); } - - } else if (_config_state == UBX_CONFIG_STATE_RATE) { /* send a CFT-RATE message to define update rate */ @@ -162,9 +171,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_rate_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* send a NAV5 message to set the options for the internal estimator */ + /* send a NAV5 message to set the options for the internal filter */ type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); cfg_nav5_packet.clsID = UBX_CLASS_CFG; cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; @@ -181,15 +190,16 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_nav5_packet)+2; } else { - /* catch the remaining config states here */ + /* Catch the remaining config states here, they all need the same packet type */ type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); cfg_msg_packet.clsID = UBX_CLASS_CFG; cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; switch (_config_state) { case UBX_CONFIG_STATE_MSG_NAV_POSLLH: @@ -207,7 +217,8 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - cfg_msg_packet.rate[1] = 5; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; break; case UBX_CONFIG_STATE_MSG_NAV_SOL: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; @@ -232,16 +243,14 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); length = sizeof(cfg_msg_packet)+2; } - } else { - _waited++; } } int -UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) { + /* if no error happens and no new report is ready yet, ret will stay 0 */ int ret = 0; - //printf("Received char: %c\n", b); switch (_decode_state) { /* First, look for sync1 */ @@ -373,6 +382,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); + ret = -1; break; } break; @@ -415,8 +425,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -441,8 +449,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -466,8 +472,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -501,8 +505,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -594,8 +596,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -615,7 +615,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->counter++; - _new_nav_velned = true; } else { @@ -624,8 +623,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -718,7 +715,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; break; } @@ -730,7 +726,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - warnx("UBX: NO ACK"); + warnx("UBX: Received: Not Acknowledged"); ret = 1; } else { @@ -751,11 +747,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } // end if _rx_count high enough - if (_rx_count < RECV_BUFFER_SIZE) { + else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - warnx("buffer overflow"); + warnx("buffer full, restarting"); decodeInit(); + ret = -1; } break; default: @@ -765,13 +762,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + /* Add timestamp to finish the report */ gps_position->timestamp = hrt_absolute_time(); - ret = 1; + /* Reset the flags */ _new_nav_posllh = false; - // _new_nav_timeutc = false; - // _new_nav_dop = false; - // _new_nav_sol = false; - // _new_nav_velned = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + + ret = 1; } return ret; @@ -807,6 +807,7 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ message[length-2] = ck_a; message[length-1] = ck_b; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index dff25a518..0cac10f0a 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -40,16 +40,17 @@ #include "gps_helper.h" -#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -//UBX Protocol definitions (this is the subset of the messages that are parsed) +/* ClassIDs (the ones that are used) */ #define UBX_CLASS_NAV 0x01 //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 + +/* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 @@ -65,11 +66,11 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ #define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 #define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ @@ -83,30 +84,30 @@ #define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ - +#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ // ************ /** the structures of the binary packets */ #pragma pack(push, 1) typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t lon; // Longitude * 1e-7, deg - int32_t lat; // Latitude * 1e-7, deg - int32_t height; // Height above Ellipsoid, mm - int32_t height_msl; // Height above mean sea level, mm - uint32_t hAcc; // Horizontal Accuracy Estimate, mm - uint32_t vAcc; // Vertical Accuracy Estimate, mm + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 - int16_t week; // GPS week (GPS time) - uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ uint8_t flags; int32_t ecefX; int32_t ecefY; @@ -125,50 +126,50 @@ typedef struct { } gps_bin_nav_sol_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint32_t time_accuracy; //Time Accuracy Estimate, ns - int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) - uint16_t year; //Year, range 1999..2099 (UTC) - uint8_t month; //Month, range 1..12 (UTC) - uint8_t day; //Day of Month, range 1..31 (UTC) - uint8_t hour; //Hour of Day, range 0..23 (UTC) - uint8_t min; //Minute of Hour, range 0..59 (UTC) - uint8_t sec; //Seconds of Minute, range 0..59 (UTC) - uint8_t valid_flag; //Validity Flags (see ubx documentation) + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ + int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint16_t gDOP; //Geometric DOP (scaling 0.01) - uint16_t pDOP; //Position DOP (scaling 0.01) - uint16_t tDOP; //Time DOP (scaling 0.01) - uint16_t vDOP; //Vertical DOP (scaling 0.01) - uint16_t hDOP; //Horizontal DOP (scaling 0.01) - uint16_t nDOP; //Northing DOP (scaling 0.01) - uint16_t eDOP; //Easting DOP (scaling 0.01) + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ + uint16_t pDOP; /**< Position DOP (scaling 0.01) */ + uint16_t tDOP; /**< Time DOP (scaling 0.01) */ + uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ + uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ + uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ + uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_dop_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint8_t numCh; //Number of channels + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { - uint8_t chn; //Channel number, 255 for SVs not assigned to a channel - uint8_t svid; //Satellite ID + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ uint8_t flags; uint8_t quality; - uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz - int8_t elev; //Elevation in integer degrees - int16_t azim; //Azimuth in integer degrees - int32_t prRes; //Pseudo range residual in centimetres + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; @@ -192,9 +193,9 @@ typedef struct { } gps_bin_nav_velned_packet_t; //typedef struct { -// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week -// int16_t week; //Measurement GPS week number -// uint8_t numVis; //Number of visible satellites +// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -210,7 +211,6 @@ typedef struct { typedef struct { uint8_t clsID; uint8_t msgID; - uint8_t ck_a; uint8_t ck_b; } gps_bin_ack_nak_packet_t; @@ -340,17 +340,27 @@ class UBX : public GPS_Helper public: UBX(); ~UBX(); - - void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + void reset(void); + void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length); int parse(uint8_t, struct vehicle_gps_position_s*); private: + /** + * Reset the parse state machine for a fresh start + */ void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ void addByteToChecksum(uint8_t); + + /** + * Add the two checksum bytes to an outgoing message + */ void addChecksumToMessage(uint8_t*, const unsigned); - unsigned _waited; - bool _waiting_for_ack; ubx_config_state_t _config_state; + bool _waiting_for_ack; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; -- cgit v1.2.3 From 53c11f87cb9b231cfb9199ce797d983f4e2c6a40 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Feb 2013 17:57:30 -0800 Subject: Small corrections --- apps/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 450b3091b..f4fd7b88e 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -110,13 +110,13 @@ private: bool _task_should_exit; ///< flag to make the main worker task exit int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to volatile int _task; ///< worker task bool _config_needed; ///< flag to signal that configuration of GPS is needed bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode - GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + GPS_Helper *_Helper; ///< Class for a GPS interface struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate -- cgit v1.2.3 From 368ba0056f4a4597c13781b6b5fd6e65930f9fee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 13:47:31 +0100 Subject: Added option to select port name, minor tweaks to status printing, sacrificied 20 bytes for better status / user debuggability --- apps/drivers/gps/gps.cpp | 76 ++++++++++++++++++++++++++++++------------------ 1 file changed, 47 insertions(+), 29 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index f4fd7b88e..5905db6b8 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -93,7 +94,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(); + GPS(const char* uart_path); ~GPS(); virtual int init(); @@ -107,19 +108,20 @@ public: private: - bool _task_should_exit; ///< flag to make the main worker task exit - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; ///< current baudrate - const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to - volatile int _task; ///< worker task - bool _config_needed; ///< flag to signal that configuration of GPS is needed - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed - bool _mode_changed; ///< flag that the GPS mode has changed - gps_driver_mode_t _mode; ///< current mode - GPS_Helper *_Helper; ///< Class for a GPS interface - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position - float _rate; ///< position update rate + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; //< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< instance of GPS parser + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate /** @@ -141,7 +143,7 @@ private: /** * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -164,7 +166,7 @@ GPS *g_dev; } -GPS::GPS() : +GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _baudrates_to_try({9600, 38400, 57600, 115200}), @@ -176,6 +178,11 @@ GPS::GPS() : _report_pub(-1), _rate(0.0f) { + /* store port name */ + strncpy(_port, uart_path, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report, 0, sizeof(_report)); @@ -198,6 +205,7 @@ GPS::~GPS() if (_task != -1) task_delete(_task); g_dev = nullptr; + } int @@ -302,13 +310,13 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters + _serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters /* buffer to read from the serial port */ uint8_t buf[32]; if (_serial_fd < 0) { - log("failed to open serial port: %d", errno); + log("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); @@ -318,7 +326,6 @@ GPS::task_main() pollfd fds[1]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; - debug("ready"); /* lock against the ioctl handler */ lock(); @@ -418,14 +425,12 @@ GPS::task_main() /* This means something went wrong in the parser, let's reconfigure */ if (!_config_needed) { _config_needed = true; - debug("Lost GPS module"); } config(); } else if (ret_parse > 0) { /* Looks like we got a valid position update, stop configuring and publish it */ if (_config_needed) { _config_needed = false; - debug("Found GPS module"); } /* opportunistic publishing - else invalid data would end up on the bus */ @@ -530,7 +535,7 @@ GPS::print_info() default: break; } - warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); @@ -549,7 +554,7 @@ namespace gps GPS *g_dev; -void start(); +void start(const char *uart_path); void stop(); void test(); void reset(); @@ -559,7 +564,7 @@ void info(); * Start the driver. */ void -start() +start(const char *uart_path) { int fd; @@ -567,7 +572,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new GPS; + g_dev = new GPS(uart_path); if (g_dev == nullptr) goto fail; @@ -644,7 +649,6 @@ info() if (g_dev == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); @@ -656,11 +660,24 @@ info() int gps_main(int argc, char *argv[]) { + + /* set to default */ + char* device_name = "/dev/ttyS3"; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - gps::start(); + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + } else { + goto out; + } + } + gps::start(device_name); + } if (!strcmp(argv[1], "stop")) gps::stop(); @@ -682,5 +699,6 @@ gps_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) gps::info(); - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); +out: + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); } -- cgit v1.2.3 From fbbeef7e29cc6aa82e653916b7d5e8005948b815 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:54:06 +0100 Subject: Update on every position change, do not wait for other measurements --- apps/drivers/gps/ubx.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 66a891da4..a6f181a73 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -765,11 +765,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) /* Add timestamp to finish the report */ gps_position->timestamp = hrt_absolute_time(); /* Reset the flags */ + + /* update on every position change, accept minor delay on other measurements */ _new_nav_posllh = false; - _new_nav_timeutc = false; - _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; + // _new_nav_timeutc = false; + // _new_nav_dop = false; + // _new_nav_sol = false; + // _new_nav_velned = false; ret = 1; } -- cgit v1.2.3 From a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Feb 2013 23:16:32 -0800 Subject: Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant --- apps/drivers/drv_gps.h | 2 ++ apps/drivers/gps/gps.cpp | 73 ++++++++++++++++++++++++------------------- apps/drivers/gps/gps_helper.h | 6 ++-- apps/drivers/gps/ubx.cpp | 65 ++++++++++++++++++++------------------ apps/drivers/gps/ubx.h | 4 +-- 5 files changed, 81 insertions(+), 69 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 1ae27ed2f..dfde115ef 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -44,6 +44,8 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#define GPS_DEFAULT_UART_PORT "/dev/ttyS3" + #define GPS_DEVICE_PATH "/dev/gps" typedef enum { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 5905db6b8..28dc949d4 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -278,7 +278,7 @@ GPS::config() int length = 0; uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate); /* The config message is sent sent at the old baudrate */ if (length > 0) { @@ -339,6 +339,9 @@ GPS::task_main() uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; + bool pos_updated; + bool config_needed_res; + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { @@ -391,17 +394,17 @@ GPS::task_main() lock(); - - /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); } else if (ret == 0) { - config(); + /* no response from the GPS */ if (_config_needed == false) { _config_needed = true; - warnx("lost GPS module"); + warnx("GPS module timeout"); + _Helper->reset(); } + config(); } else if (ret > 0) { /* if we have new data from GPS, go handle it */ if (fds[0].revents & POLLIN) { @@ -416,39 +419,43 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; - int ret_parse = 0; for (j = 0; j < count; j++) { - ret_parse += _Helper->parse(buf[j], &_report); - } + pos_updated = false; + config_needed_res = _config_needed; + _Helper->parse(buf[j], &_report, config_needed_res, pos_updated); + + if (pos_updated) { + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } - if (ret_parse < 0) { - /* This means something went wrong in the parser, let's reconfigure */ - if (!_config_needed) { - _config_needed = true; } - config(); - } else if (ret_parse > 0) { - /* Looks like we got a valid position update, stop configuring and publish it */ - if (_config_needed) { + if (config_needed_res == true && _config_needed == false) { + /* the parser told us that an error happened and reconfiguration is needed */ + _config_needed = true; + warnx("GPS module lost"); + _Helper->reset(); + config(); + } else if (config_needed_res == true && _config_needed == true) { + /* we are still configuring */ + config(); + } else if (config_needed_res == false && _config_needed == true) { + /* the parser is happy, stop configuring */ + warnx("GPS module found"); _config_needed = false; } - - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } } - /* else if ret_parse == 0: just keep parsing */ } } } @@ -662,7 +669,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = "/dev/ttyS3"; + char* device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 8a6b0148b..176b7eba8 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -40,9 +40,9 @@ class GPS_Helper { public: - virtual void reset() = 0; - virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0; - virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0; + virtual void reset(void) = 0; + virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0; + virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a6f181a73..440ec74c5 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -72,7 +72,7 @@ UBX::reset() } void -UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) +UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) { /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); @@ -84,9 +84,10 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, if (!_waiting_for_ack) { _waiting_for_ack = true; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { - config_needed = false; - length = 0; - _config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */ + /* This should never happen, the parser should set the flag, + * if it should be reconfigured, reset() should be called first. + */ + warnx("ubx: already configured"); _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { @@ -246,12 +247,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, } } -int -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) +void +UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) { - /* if no error happens and no new report is ready yet, ret will stay 0 */ - int ret = 0; - switch (_decode_state) { /* First, look for sync1 */ case UBX_DECODE_UNINIT: @@ -382,7 +380,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); - ret = -1; + config_needed = true; break; } break; @@ -417,7 +415,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->counter_pos_valid++; gps_position->counter++; + /* Add timestamp to finish the report */ + gps_position->timestamp = hrt_absolute_time(); + _new_nav_posllh = true; + /* set flag to trigger publishing of new position */ + pos_updated = true; } else { warnx("NAV_POSLLH: checksum invalid"); @@ -436,11 +439,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; - - gps_position->counter++; gps_position->s_variance = packet->sAcc; gps_position->p_variance = packet->pAcc; + gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); + + _new_nav_sol = true; } else { @@ -463,6 +468,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->epv = packet->vDOP; gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_dop = true; @@ -496,6 +502,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_timeutc = true; @@ -587,6 +594,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -614,6 +622,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_velned = true; @@ -647,8 +656,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) // // // Reset state machine to decode next packet // decodeInit(); -// return ret; -// // break; // } @@ -701,6 +708,8 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) case UBX_CONFIG_STATE_MSG_NAV_VELNED: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_CONFIGURED; + /* set the flag to tell the driver that configuration was successful */ + config_needed = false; break; // case UBX_CONFIG_STATE_MSG_RXM_SVSI: // if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) @@ -715,7 +724,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) // Reset state machine to decode next packet decodeInit(); - break; } @@ -727,16 +735,14 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { warnx("UBX: Received: Not Acknowledged"); - ret = 1; - + /* configuration obviously not successful */ + config_needed = true; } else { warnx("ACK_NAK: checksum invalid\n"); } // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -752,7 +758,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) } else { warnx("buffer full, restarting"); decodeInit(); - ret = -1; + config_needed = true; } break; default: @@ -762,21 +768,18 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { - /* Add timestamp to finish the report */ - gps_position->timestamp = hrt_absolute_time(); - /* Reset the flags */ + /* we have received everything, this most probably means that the configuration is fine */ + config_needed = false; - /* update on every position change, accept minor delay on other measurements */ + /* Reset the flags */ _new_nav_posllh = false; - // _new_nav_timeutc = false; - // _new_nav_dop = false; - // _new_nav_sol = false; - // _new_nav_velned = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; - ret = 1; } - - return ret; + return; } void diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 0cac10f0a..d3c6c6d4c 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -341,8 +341,8 @@ public: UBX(); ~UBX(); void reset(void); - void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length); - int parse(uint8_t, struct vehicle_gps_position_s*); + void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate); + void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); private: /** -- cgit v1.2.3 From fc4be3e7280db480b67b7c6cec11e35481969bbb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 12:41:05 -0800 Subject: Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps --- apps/commander/commander.c | 14 ++-- apps/drivers/gps/gps.cpp | 4 +- apps/drivers/gps/ubx.cpp | 110 +++++++++++++++----------------- apps/drivers/gps/ubx.h | 30 ++++----- apps/examples/kalman_demo/KalmanNav.cpp | 23 +++---- apps/mavlink/mavlink_receiver.c | 24 +++---- apps/mavlink/orb_listener.c | 10 +-- apps/uORB/topics/home_position.h | 8 +-- apps/uORB/topics/vehicle_gps_position.h | 55 ++++++++-------- 9 files changed, 139 insertions(+), 139 deletions(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3a6fecf74..f19f1d0e6 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1678,8 +1678,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph / 100.0f; - float vdop_m = gps_position.epv / 100.0f; + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; /* check if gps fix is ok */ // XXX magic number @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) && (vdop_m < dop_threshold_m) && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp < 2000000) + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { warnx("setting home position"); @@ -1706,11 +1706,11 @@ int commander_thread_main(int argc, char *argv[]) home.lon = gps_position.lon; home.alt = gps_position.alt; - home.eph = gps_position.eph; - home.epv = gps_position.epv; + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; - home.s_variance = gps_position.s_variance; - home.p_variance = gps_position.p_variance; + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; /* announce new home position */ if (home_pub > 0) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 28dc949d4..8c2775adb 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -543,9 +543,9 @@ GPS::print_info() break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); - if (_report.timestamp != 0) { + if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 440ec74c5..eec1df980 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -52,7 +53,7 @@ _config_state(UBX_CONFIG_STATE_PRT), _waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), -_new_nav_dop(false), +//_new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { @@ -211,10 +212,10 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; @@ -316,10 +317,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = NAV_TIMEUTC; break; - case UBX_MESSAGE_NAV_DOP: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_DOP; - break; +// case UBX_MESSAGE_NAV_DOP: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = NAV_DOP; +// break; case UBX_MESSAGE_NAV_SVINFO: _decode_state = UBX_DECODE_GOT_MESSAGEID; @@ -412,11 +413,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->counter_pos_valid++; - gps_position->counter++; + gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m /* Add timestamp to finish the report */ - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_position = hrt_absolute_time(); _new_nav_posllh = true; /* set flag to trigger publishing of new position */ @@ -439,11 +440,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; - gps_position->s_variance = packet->sAcc; - gps_position->p_variance = packet->pAcc; + gps_position->s_variance_m_s = packet->sAcc; + gps_position->p_variance_m = packet->pAcc; - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_variance = hrt_absolute_time(); _new_nav_sol = true; @@ -457,29 +457,28 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; } - case NAV_DOP: { -// printf("GOT NAV_DOP MESSAGE\n"); - gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->eph = packet->hDOP; - gps_position->epv = packet->vDOP; - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); - - _new_nav_dop = true; - - } else { - warnx("NAV_DOP: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { +// +// gps_position->eph_m = packet->hDOP; +// gps_position->epv = packet->vDOP; +// +// gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// } else { +// warnx("NAV_DOP: checksum invalid"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// break; +// } case NAV_TIMEUTC: { // printf("GOT NAV_TIMEUTC MESSAGE\n"); @@ -501,8 +500,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_time = hrt_absolute_time(); _new_nav_timeutc = true; @@ -593,8 +591,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_satellites = hrt_absolute_time(); // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -614,18 +611,17 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->vel = (uint16_t)packet->speed; - gps_position->vel_n = packet->velN / 100.0f; - gps_position->vel_e = packet->velE / 100.0f; - gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_m_s = (float)packet->speed * 1e-2f; + gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; + gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; + gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; gps_position->vel_ned_valid = true; - gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_velocity = hrt_absolute_time(); _new_nav_velned = true; + } else { warnx("NAV_VELNED: checksum invalid"); } @@ -690,13 +686,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; break; case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; @@ -766,7 +762,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { /* we have received everything, this most probably means that the configuration is fine */ config_needed = false; @@ -774,7 +770,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Reset the flags */ _new_nav_posllh = false; _new_nav_timeutc = false; - _new_nav_dop = false; +// _new_nav_dop = false; _new_nav_sol = false; _new_nav_velned = false; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index d3c6c6d4c..43cded02f 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -54,7 +54,7 @@ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_DOP 0x04 +//#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 @@ -140,18 +140,18 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ - uint16_t pDOP; /**< Position DOP (scaling 0.01) */ - uint16_t tDOP; /**< Time DOP (scaling 0.01) */ - uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ - uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ - uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ - uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_dop_packet_t; +//typedef struct { +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint8_t ck_a; +// uint8_t ck_b; +//} gps_bin_nav_dop_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ @@ -311,7 +311,7 @@ typedef enum { NAV_POSLLH, NAV_SOL, NAV_TIMEUTC, - NAV_DOP, +// NAV_DOP, NAV_SVINFO, NAV_VELNED, // RXM_SVSI, @@ -371,7 +371,7 @@ private: unsigned _payload_size; bool _new_nav_posllh; bool _new_nav_timeutc; - bool _new_nav_dop; +// bool _new_nav_dop; bool _new_nav_sol; bool _new_nav_velned; }; diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 7e89dffb2..b7f651d8a 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -190,11 +190,12 @@ void KalmanNav::update() if (!_positionInitialized && _attitudeInitialized && // wait for attitude first gpsUpdate && - _gps.fix_type > 2 && - _gps.counter_pos_valid > 10) { - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); @@ -259,7 +260,7 @@ void KalmanNav::updatePublications() // position publication _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp; + _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); @@ -631,8 +632,8 @@ int KalmanNav::correctPos() // residual Vector y(5); - y(0) = _gps.vel_n - vN; - y(1) = _gps.vel_e - vE; + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; y(4) = double(_gps.alt) / 1.0e3 - alt; @@ -651,9 +652,9 @@ int KalmanNav::correctPos() // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); // fallback to GPS - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 86732d07c..b3b4b1e0b 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* gps */ - hil_gps.timestamp = gps.time_usec; - hil_gps.counter = hil_counter++; + hil_gps.timestamp_position = gps.time_usec; +// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.counter_pos_valid = hil_counter++; - hil_gps.eph = gps.eph; - hil_gps.epv = gps.epv; - hil_gps.s_variance = 100; - hil_gps.p_variance = 100; - hil_gps.vel = gps.vel; - hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_d = 0.0f; - hil_gps.cog = gps.cog; +// hil_gps.counter_pos_valid = hil_counter++; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance? + hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 18da70f61..9f85d5801 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l) /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp, + gps.timestamp_position, gps.fix_type, gps.lat, gps.lon, gps.alt, - gps.eph, - gps.epv, - gps.vel, - gps.cog, + (uint16_t)(gps.eph_m * 1e2f), // from m to cm + (uint16_t)(gps.epv_m * 1e2f), // from m to cm + (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s + (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 gps.satellites_visible); if (gps.satellite_info_available && (gps_counter % 4 == 0)) { diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h index dec34b6ab..7e1b82a0f 100644 --- a/apps/uORB/topics/home_position.h +++ b/apps/uORB/topics/home_position.h @@ -61,10 +61,10 @@ struct home_position_s int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ }; /** diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index db529da06..aa75c22ac 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,35 +55,38 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint32_t counter; /**< Count of GPS messages */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - int32_t lat; /**< Latitude in 1E7 degrees //LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees //LOGME */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */ - uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ - uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ - float vel_n; /**< GPS ground speed in m/s */ - float vel_e; /**< GPS ground speed in m/s */ - float vel_d; /**< GPS ground speed in m/s */ - uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint64_t timestamp_variance; + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - /* flags */ - float vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + + uint64_t timestamp_time; /**< Timestamp for time information */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint8_t satellite_prn[20]; /**< Global satellite ID */ + uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** -- cgit v1.2.3 From d962e6c403678e14a64a6b01be8773e98660bb24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 13:50:32 -0800 Subject: Removed some unnecessairy flags, home position back working --- apps/commander/commander.c | 9 ++++-- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/gps/ubx.cpp | 49 ++++++--------------------------- apps/drivers/gps/ubx.h | 5 ---- apps/uORB/topics/vehicle_gps_position.h | 2 +- 5 files changed, 16 insertions(+), 51 deletions(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f19f1d0e6..6b1bc0f9b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1683,7 +1683,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if gps fix is ok */ // XXX magic number - float dop_threshold_m = 2.0f; + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; /* * If horizontal dilution of precision (hdop / eph) @@ -1694,8 +1695,10 @@ int commander_thread_main(int argc, char *argv[]) * the system is currently not armed, set home * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) - && (vdop_m < dop_threshold_m) + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 8c2775adb..45f18158f 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -591,7 +591,7 @@ start(const char *uart_path) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - printf("Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } exit(0); diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index eec1df980..a82327175 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -50,12 +50,7 @@ UBX::UBX() : _config_state(UBX_CONFIG_STATE_PRT), -_waiting_for_ack(false), -_new_nav_posllh(false), -_new_nav_timeutc(false), -//_new_nav_dop(false), -_new_nav_sol(false), -_new_nav_velned(false) +_waiting_for_ack(false) { reset(); } @@ -413,13 +408,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m + gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m /* Add timestamp to finish the report */ gps_position->timestamp_position = hrt_absolute_time(); - _new_nav_posllh = true; /* set flag to trigger publishing of new position */ pos_updated = true; @@ -445,9 +439,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_variance = hrt_absolute_time(); - - _new_nav_sol = true; - } else { warnx("NAV_SOL: checksum invalid"); } @@ -502,8 +493,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_time = hrt_absolute_time(); - _new_nav_timeutc = true; - } else { warnx("NAV_TIMEUTC: checksum invalid"); } @@ -581,20 +570,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->satellite_elevation[i] = 0; gps_position->satellite_azimuth[i] = 0; } + gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set flag if any sat info is available */ - if (!packet_part1->numCh > 0) { - gps_position->satellite_info_available = 1; - + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + gps_position->satellite_info_available = true; } else { - gps_position->satellite_info_available = 0; + gps_position->satellite_info_available = false; } - - gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones gps_position->timestamp_satellites = hrt_absolute_time(); - // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report - } else { warnx("NAV_SVINFO: checksum invalid"); } @@ -619,9 +604,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->vel_ned_valid = true; gps_position->timestamp_velocity = hrt_absolute_time(); - _new_nav_velned = true; - - } else { warnx("NAV_VELNED: checksum invalid"); } @@ -760,21 +742,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ default: break; } - - /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { - - /* we have received everything, this most probably means that the configuration is fine */ - config_needed = false; - - /* Reset the flags */ - _new_nav_posllh = false; - _new_nav_timeutc = false; -// _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; - - } return; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 43cded02f..a23bb5502 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -369,11 +369,6 @@ private: ubx_message_class_t _message_class; ubx_message_id_t _message_id; unsigned _payload_size; - bool _new_nav_posllh; - bool _new_nav_timeutc; -// bool _new_nav_dop; - bool _new_nav_sol; - bool _new_nav_velned; }; #endif /* UBX_H_ */ diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index aa75c22ac..5463a460d 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -86,7 +86,7 @@ struct vehicle_gps_position_s uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** -- cgit v1.2.3 From 6ed5d97aea29a284015708a6089b7910afea8369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 18:47:32 -0800 Subject: Merged mtk16 and mtk19 helper classes, configure() now writes directly instead of buffering --- apps/drivers/drv_gps.h | 13 +-------- apps/drivers/gps/gps.cpp | 53 +++++------------------------------- apps/drivers/gps/gps_helper.h | 3 ++- apps/drivers/gps/ubx.cpp | 62 +++++++++++++++++-------------------------- apps/drivers/gps/ubx.h | 11 +++++--- 5 files changed, 41 insertions(+), 101 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index dfde115ef..67248efcd 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -50,8 +50,7 @@ typedef enum { GPS_DRIVER_MODE_UBX = 0, - GPS_DRIVER_MODE_MTK19, - GPS_DRIVER_MODE_MTK16, + GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NMEA, } gps_driver_mode_t; @@ -67,14 +66,4 @@ ORB_DECLARE(gps); #define _GPSIOCBASE (0x2800) //TODO: arbitrary choice... #define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n)) -/** configure ubx mode */ -#define GPS_CONFIGURE_UBX _GPSIOC(0) - -/** configure mtk mode */ -#define GPS_CONFIGURE_MTK19 _GPSIOC(1) -#define GPS_CONFIGURE_MTK16 _GPSIOC(2) - -/** configure mtk mode */ -#define GPS_CONFIGURE_NMEA _GPSIOC(3) - #endif /* _DRV_GPS_H */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 45f18158f..3e1aca810 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -72,6 +72,7 @@ #include #include "ubx.h" +#include "mtk.h" #define SEND_BUFFER_LENGTH 100 #define TIMEOUT 1000000 //1s @@ -238,30 +239,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case GPS_CONFIGURE_UBX: - if (_mode != GPS_DRIVER_MODE_UBX) { - _mode = GPS_DRIVER_MODE_UBX; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_MTK19: - if (_mode != GPS_DRIVER_MODE_MTK19) { - _mode = GPS_DRIVER_MODE_MTK19; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_MTK16: - if (_mode != GPS_DRIVER_MODE_MTK16) { - _mode = GPS_DRIVER_MODE_MTK16; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_NMEA: - if (_mode != GPS_DRIVER_MODE_NMEA) { - _mode = GPS_DRIVER_MODE_NMEA; - _mode_changed = true; - } - break; case SENSORIOCRESET: cmd_reset(); break; @@ -275,19 +252,7 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) void GPS::config() { - int length = 0; - uint8_t send_buffer[SEND_BUFFER_LENGTH]; - - _Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate); - - /* The config message is sent sent at the old baudrate */ - if (length > 0) { - - if (length != ::write(_serial_fd, send_buffer, length)) { - debug("write config failed"); - return; - } - } + _Helper->configure(_serial_fd, _baudrate_changed, _baudrate); /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed * from 9600 to 38400 @@ -356,11 +321,8 @@ GPS::task_main() case GPS_DRIVER_MODE_UBX: _Helper = new UBX(); break; - case GPS_DRIVER_MODE_MTK19: - //_Helper = new MTK19(); - break; - case GPS_DRIVER_MODE_MTK16: - //_Helper = new MTK16(); + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); @@ -530,11 +492,8 @@ GPS::print_info() case GPS_DRIVER_MODE_UBX: warnx("protocol: UBX"); break; - case GPS_DRIVER_MODE_MTK19: - warnx("protocol: MTK 1.9"); - break; - case GPS_DRIVER_MODE_MTK16: - warnx("protocol: MTK 1.6"); + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); break; case GPS_DRIVER_MODE_NMEA: warnx("protocol: NMEA"); diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 176b7eba8..576692c2a 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -36,12 +36,13 @@ /* @file U-Blox protocol definitions */ #ifndef GPS_HELPER_H +#define GPS_HELPER_H class GPS_Helper { public: virtual void reset(void) = 0; - virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0; + virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0; virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; }; diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a82327175..8e2396564 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -68,11 +68,8 @@ UBX::reset() } void -UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) +UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) { - /* make sure the buffer, where the message is written to, is long enough */ - assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); - /* Only send a new config message when we got the ACK of the last one, * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. @@ -105,16 +102,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - /* Calculate the checksum now */ - addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* Start with the two sync bytes */ - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - /* Copy it to the buffer that will be written back in the main gps driver */ - memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); - /* Set the length of the packet (plus the 2 sync bytes) */ - length = sizeof(cfg_prt_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { @@ -131,12 +119,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); - length = sizeof(cfg_prt_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { @@ -160,12 +143,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); - length = sizeof(cfg_rate_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); } else if (_config_state == UBX_CONFIG_STATE_NAV5) { /* send a NAV5 message to set the options for the internal filter */ @@ -179,12 +157,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); - length = sizeof(cfg_nav5_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); } else { /* Catch the remaining config states here, they all need the same packet type */ @@ -233,12 +206,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba break; } - addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); - length = sizeof(cfg_msg_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); } } } @@ -779,3 +747,21 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) message[length-2] = ck_a; message[length-1] = ck_b; } + +void +UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +{ + ssize_t ret = 0; + + /* Calculate the checksum now */ + addChecksumToMessage(packet, length); + + const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + + /* Start with the two sync bytes */ + ret += write(fd, sync_bytes, sizeof(sync_bytes)); + ret += write(fd, packet, length); + + if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? + warnx("ubx: config write fail"); +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index a23bb5502..c420e83b9 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -40,7 +40,6 @@ #include "gps_helper.h" - #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -341,7 +340,7 @@ public: UBX(); ~UBX(); void reset(void); - void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate); + void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); private: @@ -358,7 +357,13 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void addChecksumToMessage(uint8_t*, const unsigned); + void addChecksumToMessage(uint8_t* message, const unsigned length); + + /** + * Helper to send a config packet + */ + void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); + ubx_config_state_t _config_state; bool _waiting_for_ack; ubx_decode_state_t _decode_state; -- cgit v1.2.3 From 0d54661ce90dfe2440daea2639a9853520d8366c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 20:04:49 -0800 Subject: Added MTK 1.6, works after some seconds, work in progress --- apps/drivers/gps/gps.cpp | 43 +++++++---- apps/drivers/gps/mtk.cpp | 184 +++++++++++++++++++++++++++++++++++++++++++++++ apps/drivers/gps/mtk.h | 108 ++++++++++++++++++++++++++++ 3 files changed, 320 insertions(+), 15 deletions(-) create mode 100644 apps/drivers/gps/mtk.cpp create mode 100644 apps/drivers/gps/mtk.h (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 3e1aca810..c749e8b7f 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -77,7 +77,7 @@ #define SEND_BUFFER_LENGTH 100 #define TIMEOUT 1000000 //1s -#define NUMBER_OF_BAUDRATES 4 +#define NUMBER_OF_TRIES 5 #define CONFIG_TIMEOUT 2000000 /* oddly, ERROR is not defined for c++ */ @@ -113,7 +113,8 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + const unsigned _baudrates_to_try[NUMBER_OF_TRIES]; ///< try different baudrates that GPS could be set to + const gps_driver_mode_t _modes_to_try[NUMBER_OF_TRIES]; ///< try different modes volatile int _task; //< worker task bool _config_needed; ///< flag to signal that configuration of GPS is needed bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed @@ -170,11 +171,11 @@ GPS *g_dev; GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), - _baudrates_to_try({9600, 38400, 57600, 115200}), + _baudrates_to_try({9600, 38400, 57600, 115200, 38400}), + _modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}), _config_needed(true), _baudrate_changed(false), - _mode_changed(true), - _mode(GPS_DRIVER_MODE_UBX), + _mode_changed(false), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -295,8 +296,10 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - unsigned baud_i = 0; - _baudrate = _baudrates_to_try[baud_i]; + unsigned try_i = 0; + _baudrate = _baudrates_to_try[try_i]; + _mode = _modes_to_try[try_i]; + _mode_changed = true; set_baudrate(_baudrate); uint64_t time_before_configuration = hrt_absolute_time(); @@ -310,6 +313,23 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { + /* If a configuration does not finish in the config timeout, change the baudrate */ + if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { + /* loop through possible modes/baudrates */ + try_i = (try_i + 1) % NUMBER_OF_TRIES; + _baudrate = _baudrates_to_try[try_i]; + set_baudrate(_baudrate); + if (_mode != _modes_to_try[try_i]) { + _mode_changed = true; + } + _mode = _modes_to_try[try_i]; + + if (_Helper != nullptr) { + _Helper->reset(); + } + time_before_configuration = hrt_absolute_time(); + } + if (_mode_changed) { if (_Helper != nullptr) { delete(_Helper); @@ -333,14 +353,7 @@ GPS::task_main() _mode_changed = false; } - /* If a configuration does not finish in the config timeout, change the baudrate */ - if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { - baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; - _baudrate = _baudrates_to_try[baud_i]; - set_baudrate(_baudrate); - _Helper->reset(); - time_before_configuration = hrt_absolute_time(); - } + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, * but during normal operation, it should never timeout because updates should arrive with 5Hz */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp new file mode 100644 index 000000000..555fb7a51 --- /dev/null +++ b/apps/drivers/gps/mtk.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mkt.cpp */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mtk.h" + + +MTK::MTK() +{ + decodeInit(); +} + +MTK::~MTK() +{ +} + +void +MTK::reset() +{ + +} + +void +MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) +{ + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) + warnx("mtk: config write failed"); + + return; +} + +void +MTK::decodeInit(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = MTK_DECODE_UNINIT; +} + +void +MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +{ + if (_decode_state == MTK_DECODE_UNINIT) { + + if (b == 0xd0) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_A) { + if (b == 0xdd) { + _decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + decodeInit(); + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (_rx_count < 33) + addByteToChecksum(b); + + // Fill packet buffer + _rx_buffer[_rx_count] = b; + _rx_count++; + + /* Packet size minus checksum */ + if (_rx_count >= 35) { + type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;; + + /* Check if checksum is valid */ + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm + gps_position->fix_type = packet->fix_type; + gps_position->eph_m = packet->hdop; + gps_position->epv_m = 0.0; //unknown in mtk custom mode + gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s + gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + gps_position->satellites_visible = packet->satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet->date * 1e-4; + timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet->utc_time * 1e-7; + timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + gps_position->timestamp_position = gps_position->timestamp_time = hrt_absolute_time(); + + pos_updated = true; + + + } else { + warnx("mtk Checksum invalid, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b); + } + + // Reset state machine to decode next packet + decodeInit(); + } + } + return; +} + +void +MTK::addByteToChecksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h new file mode 100644 index 000000000..8929eb041 --- /dev/null +++ b/apps/drivers/gps/mtk.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mtk.h */ + +#ifndef MTK_H_ +#define MTK_H_ + +#include "gps_helper.h" + +#define MTK_SYNC1 0xd0 +#define MTK_SYNC2 0xdd + +#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define WAAS_ON "$PMTK301,2*2E\r\n" +#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" + +typedef enum { + MTK_DECODE_UNINIT = 0, + MTK_DECODE_GOT_CK_A = 1, + MTK_DECODE_GOT_CK_B = 2 +} mtk_decode_state_t; + +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint8_t payload; ///< Number of payload bytes + int32_t latitude; ///< Latitude in degrees * 10^7 + int32_t longitude; ///< Longitude in degrees * 10^7 + uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 + uint32_t ground_speed; ///< FIXME SPEC UNCLEAR + int32_t heading; + uint8_t satellites; + uint8_t fix_type; + uint32_t date; + uint32_t utc_time; + uint16_t hdop; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_mtk_packet; + +#pragma pack(pop) + +#define MTK_RECV_BUFFER_SIZE 40 + +class MTK : public GPS_Helper +{ +public: + MTK(); + ~MTK(); + void reset(void); + void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); + void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + +private: + /** + * Reset the parse state machine for a fresh start + */ + void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void addByteToChecksum(uint8_t); + + mtk_decode_state_t _decode_state; + uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; +}; + +#endif /* MTK_H_ */ -- cgit v1.2.3 From b620136af4f8de913fd12872a91a80f62861dc4c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 22:58:52 -0800 Subject: Added support for MTK revision 19, working condition but configuration of MTK is very slow and needs improvement --- apps/drivers/gps/mtk.cpp | 25 +++++++++++++++++++------ apps/drivers/gps/mtk.h | 4 +++- 2 files changed, 22 insertions(+), 7 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 555fb7a51..bed388c49 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -48,7 +48,8 @@ #include "mtk.h" -MTK::MTK() +MTK::MTK() : +_mtk_revision(0) { decodeInit(); } @@ -102,13 +103,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ { if (_decode_state == MTK_DECODE_UNINIT) { - if (b == 0xd0) { + if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; config_needed = false; + _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + _mtk_revision = 19; } } else if (_decode_state == MTK_DECODE_GOT_CK_A) { - if (b == 0xdd) { + if (b == MTK_SYNC2) { _decode_state = MTK_DECODE_GOT_CK_B; } else { @@ -131,11 +137,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Check if checksum is valid */ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + if (_mtk_revision == 16) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + gps_position->lat = packet->latitude; // both degrees*1e7 + gps_position->lon = packet->longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + } gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm gps_position->fix_type = packet->fix_type; - gps_position->eph_m = packet->hdop; + gps_position->eph_m = packet->hdop; // XXX: Check this because eph_m is in m and hdop is without unit gps_position->epv_m = 0.0; //unknown in mtk custom mode gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 8929eb041..7a4b4f197 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -40,7 +40,8 @@ #include "gps_helper.h" -#define MTK_SYNC1 0xd0 +#define MTK_SYNC1_V16 0xd0 +#define MTK_SYNC1_V19 0xd1 #define MTK_SYNC2 0xdd #define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" @@ -99,6 +100,7 @@ private: void addByteToChecksum(uint8_t); mtk_decode_state_t _decode_state; + uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; -- cgit v1.2.3 From d36eb8a3fcce358409a7205bbd75576a447ac7b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 23:25:09 -0800 Subject: Sped up MTK configuration but the detection time can still be improved: timeouts/usleeps --- apps/drivers/gps/mtk.cpp | 34 ++++++++++++++++++++-------------- apps/drivers/gps/mtk.h | 3 ++- 2 files changed, 22 insertions(+), 15 deletions(-) (limited to 'apps') diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index bed388c49..026b0660b 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -49,6 +49,7 @@ MTK::MTK() : +_config_sent(false), _mtk_revision(0) { decodeInit(); @@ -67,24 +68,29 @@ MTK::reset() void MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) { - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) - warnx("mtk: config write failed"); - usleep(10000); + if (_config_sent == false) { - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) - warnx("mtk: config write failed"); + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) + warnx("mtk: config write failed"); + + _config_sent = true; + } return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 7a4b4f197..e43637195 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -99,7 +99,8 @@ private: */ void addByteToChecksum(uint8_t); - mtk_decode_state_t _decode_state; + mtk_decode_state_t _decode_state; + bool _config_sent; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; -- cgit v1.2.3 From 5f52d3dc7fe8dee773eb67cb2541ef1d061478e6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 3 Feb 2013 19:19:57 -0800 Subject: lib_lowprintf -> lowsyslog --- apps/px4io/mixer.cpp | 4 ++-- apps/systemlib/mixer/mixer_group.cpp | 2 +- apps/systemlib/mixer/mixer_multirotor.cpp | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index c24cb8e52..8e00781a0 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -144,7 +144,7 @@ mixer_tick(void) rc_channel_data[THROTTLE] = 1000; } - // lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", + // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); @@ -156,7 +156,7 @@ mixer_tick(void) // XXX builtin failsafe would activate here control_count = 0; } - //lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); + //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); /* this is for multicopters, etc. where manual override does not make sense */ } else { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 25d19f9ad..60eeff225 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -56,7 +56,7 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) //#include -//#define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : Mixer(control_cb, cb_handle), diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index f6e91b1bf..4b9cfc023 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -57,7 +57,7 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) //#include -//#define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) /* * Clockwise: 1 @@ -217,11 +217,11 @@ unsigned MultirotorMixer::mix(float *outputs, unsigned space) { float roll = get_control(0, 0) * _roll_scale; - //lib_lowprintf("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); + //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); float pitch = get_control(0, 1) * _pitch_scale; float yaw = get_control(0, 2) * _yaw_scale; float thrust = get_control(0, 3); - //lib_lowprintf("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); + //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float max = 0.0f; float fixup_scale; -- cgit v1.2.3 From 8cb583cf8031f4e7db77e1b88ea164e7ff5994c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 6 Feb 2013 23:50:55 -0800 Subject: Assorted compile fixes. --- apps/drivers/boards/px4fmu/px4fmu_init.c | 4 ++-- apps/px4/tests/tests.h | 4 ++-- apps/px4io/px4io.c | 4 ++-- apps/px4io/px4io.h | 2 +- apps/systemlib/err.c | 8 ++++---- nuttx/include/nuttx/serial/serial.h | 3 --- 6 files changed, 11 insertions(+), 14 deletions(-) (limited to 'apps') diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index e88d2861e..9960c6bbd 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -79,13 +79,13 @@ #ifdef CONFIG_CPP_HAVE_VARARGS # ifdef CONFIG_DEBUG -# define message(...) lib_lowprintf(__VA_ARGS__) +# define message(...) lowsyslog(__VA_ARGS__) # else # define message(...) printf(__VA_ARGS__) # endif #else # ifdef CONFIG_DEBUG -# define message lib_lowprintf +# define message lowsyslog # else # define message printf # endif diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h index cc3f5493a..c02ea6808 100644 --- a/apps/px4/tests/tests.h +++ b/apps/px4/tests/tests.h @@ -48,7 +48,7 @@ #ifdef CONFIG_CPP_HAVE_VARARGS # ifdef CONFIG_DEBUG -# define message(...) lib_rawprintf(__VA_ARGS__) +# define message(...) lowsyslog(__VA_ARGS__) # define msgflush() # else # define message(...) printf(__VA_ARGS__) @@ -56,7 +56,7 @@ # endif #else # ifdef CONFIG_DEBUG -# define message lib_rawprintf +# define message lowsyslog # define msgflush() # else # define message printf diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index bea9d59bc..88342816e 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -73,7 +73,7 @@ int user_start(int argc, char *argv[]) hrt_init(); /* print some startup info */ - lib_lowprintf("\nPX4IO: starting\n"); + lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); @@ -98,7 +98,7 @@ int user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); /* we're done here, go run the communications loop */ comms_main(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index e388f65e3..3ce6afc31 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -58,7 +58,7 @@ #ifdef DEBUG # include -# define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) +# define debug(fmt, args...) lowsyslog(fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) #endif diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c index 3011743a1..daf17ef8b 100644 --- a/apps/systemlib/err.c +++ b/apps/systemlib/err.c @@ -85,17 +85,17 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC - lib_lowprintf("%s: ", getprogname()); - lib_lowvprintf(fmt, args); + lowsyslog("%s: ", getprogname()); + lowvyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; if (errcode < NOCODE) - lib_lowprintf(": %s", strerror(errcode)); + lowsyslog(": %s", strerror(errcode)); - lib_lowprintf("\n"); + lowsyslog("\n"); #endif } diff --git a/nuttx/include/nuttx/serial/serial.h b/nuttx/include/nuttx/serial/serial.h index d72038e17..4ee2005ef 100644 --- a/nuttx/include/nuttx/serial/serial.h +++ b/nuttx/include/nuttx/serial/serial.h @@ -220,7 +220,6 @@ struct uart_dev_s #ifndef CONFIG_DISABLE_POLL struct pollfd *fds[CONFIG_SERIAL_NPOLLWAITERS]; #endif -<<<<<<< HEAD /* Terminal control flags */ @@ -230,8 +229,6 @@ struct uart_dev_s tcflag_t tc_lflag; /* Local modes */ #endif -======= ->>>>>>> 049c93446561c6ad3e59183c139f3916230ddee5 }; typedef struct uart_dev_s uart_dev_t; -- cgit v1.2.3 From a88b9f4eefe8315cb692779dd300400d8052eb44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 7 Feb 2013 14:48:00 -0800 Subject: Restructered the parsing/configuring, MTK working --- apps/drivers/gps/gps.cpp | 243 ++++++++-------------------------------- apps/drivers/gps/gps_helper.cpp | 90 +++++++++++++++ apps/drivers/gps/gps_helper.h | 11 +- apps/drivers/gps/mtk.cpp | 238 ++++++++++++++++++++++++--------------- apps/drivers/gps/mtk.h | 35 ++++-- 5 files changed, 314 insertions(+), 303 deletions(-) create mode 100644 apps/drivers/gps/gps_helper.cpp (limited to 'apps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index c749e8b7f..76b5f4533 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -53,7 +53,6 @@ #include #include #include -#include #include #include @@ -71,7 +70,7 @@ #include -#include "ubx.h" +//#include "ubx.h" #include "mtk.h" #define SEND_BUFFER_LENGTH 100 @@ -113,10 +112,8 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - const unsigned _baudrates_to_try[NUMBER_OF_TRIES]; ///< try different baudrates that GPS could be set to - const gps_driver_mode_t _modes_to_try[NUMBER_OF_TRIES]; ///< try different modes volatile int _task; //< worker task - bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _healthy; ///< flag to signal if the GPS is ok bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode @@ -171,11 +168,9 @@ GPS *g_dev; GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), - _baudrates_to_try({9600, 38400, 57600, 115200, 38400}), - _modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}), - _config_needed(true), - _baudrate_changed(false), + _healthy(false), _mode_changed(false), + _mode(GPS_DRIVER_MODE_MTK), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -250,20 +245,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } -void -GPS::config() -{ - _Helper->configure(_serial_fd, _baudrate_changed, _baudrate); - - /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed - * from 9600 to 38400 - */ - if (_baudrate_changed) { - set_baudrate(_baudrate); - _baudrate_changed = false; - } -} - void GPS::task_main_trampoline(void *arg) { @@ -276,10 +257,7 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters - - /* buffer to read from the serial port */ - uint8_t buf[32]; + _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); @@ -288,151 +266,65 @@ GPS::task_main() _exit(1); } - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - /* lock against the ioctl handler */ - lock(); - - unsigned try_i = 0; - _baudrate = _baudrates_to_try[try_i]; - _mode = _modes_to_try[try_i]; - _mode_changed = true; - set_baudrate(_baudrate); - - uint64_t time_before_configuration = hrt_absolute_time(); - uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - bool pos_updated; - bool config_needed_res; - /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - /* If a configuration does not finish in the config timeout, change the baudrate */ - if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { - /* loop through possible modes/baudrates */ - try_i = (try_i + 1) % NUMBER_OF_TRIES; - _baudrate = _baudrates_to_try[try_i]; - set_baudrate(_baudrate); - if (_mode != _modes_to_try[try_i]) { - _mode_changed = true; - } - _mode = _modes_to_try[try_i]; - - if (_Helper != nullptr) { - _Helper->reset(); - } - time_before_configuration = hrt_absolute_time(); + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; } - if (_mode_changed) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(); - break; + switch (_mode) { +// case GPS_DRIVER_MODE_UBX: +// _Helper = new UBX(); +// break; case GPS_DRIVER_MODE_MTK: + printf("try new mtk\n"); _Helper = new MTK(); break; case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); + //_Helper = new NMEA(); //TODO: add NMEA break; default: break; - } - _mode_changed = false; } - - - - /* during configuration, the timeout should be small, so that we can send config messages in between parsing, - * but during normal operation, it should never timeout because updates should arrive with 5Hz */ - int poll_timeout; - if (_config_needed) { - poll_timeout = 50; - } else { - poll_timeout = 400; - } - /* sleep waiting for data, but no more than the poll timeout */ + // XXX unlock/lock makes sense? unlock(); - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); - lock(); + if (_Helper->configure(_serial_fd, _baudrate) == 0) { + + while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + last_rate_count++; - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - } else if (ret == 0) { - /* no response from the GPS */ - if (_config_needed == false) { - _config_needed = true; - warnx("GPS module timeout"); - _Helper->reset(); - } - config(); - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - int count; - - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_serial_fd, buf, sizeof(buf)); - - /* pass received bytes to the packet decoder */ - int j; - for (j = 0; j < count; j++) { - pos_updated = false; - config_needed_res = _config_needed; - _Helper->parse(buf[j], &_report, config_needed_res, pos_updated); - - if (pos_updated) { - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } - - } - if (config_needed_res == true && _config_needed == false) { - /* the parser told us that an error happened and reconfiguration is needed */ - _config_needed = true; - warnx("GPS module lost"); - _Helper->reset(); - config(); - } else if (config_needed_res == true && _config_needed == true) { - /* we are still configuring */ - config(); - } else if (config_needed_res == false && _config_needed == true) { - /* the parser is happy, stop configuring */ - warnx("GPS module found"); - _config_needed = false; - } + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } + + if (!_healthy) { + warnx("module found"); + _healthy = true; } } + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } } + lock(); } debug("exiting"); @@ -443,59 +335,12 @@ GPS::task_main() _exit(0); } -int -GPS::set_baudrate(unsigned baud) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); - return -EINVAL; - } - struct termios uart_config; - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - /* no parity, one stop bit */ - uart_config.c_cflag &= ~(CSTOPB | PARENB); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); - return -1; - } - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); - return -1; - } - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); - return -1; - } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ - return 0; -} void GPS::cmd_reset() { - _config_needed = true; + //XXX add reset? } void @@ -514,7 +359,7 @@ GPS::print_info() default: break; } - warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp new file mode 100644 index 000000000..2caa82af1 --- /dev/null +++ b/apps/drivers/gps/gps_helper.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include "gps_helper.h" + +/* @file gps_helper.cpp */ + +int +GPS_Helper::set_baudrate(const int &fd, unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + return -1; + } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; +} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 576692c2a..537ca819c 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -33,17 +33,20 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/* @file gps_helper.h */ #ifndef GPS_HELPER_H #define GPS_HELPER_H +#include +#include + class GPS_Helper { public: - virtual void reset(void) = 0; - virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0; - virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; + virtual int configure(const int &fd, unsigned &baud) = 0; + virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + int set_baudrate(const int &fd, unsigned baud); }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 026b0660b..4ccbbfbe4 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -37,85 +37,143 @@ #include #include +#include #include #include #include #include -#include -#include #include #include "mtk.h" MTK::MTK() : -_config_sent(false), _mtk_revision(0) { - decodeInit(); + decode_init(); } MTK::~MTK() { } -void -MTK::reset() +int +MTK::configure(const int &fd, unsigned &baudrate) { + /* set baudrate first */ + if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) + return -1; -} + baudrate = MTK_BAUDRATE; -void -MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - if (_config_sent == false) { + /* Write config messages, don't wait for an answer */ + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + warnx("mtk: config write failed"); + return -1; + } - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) - warnx("mtk: config write failed"); + return 0; +} - _config_sent = true; - } +int +MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + gps_mtk_packet_t packet; + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* first read whatever is left */ + if (j < count) { + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j], packet) > 0) { + handle_message(packet, gps_position); + return 1; + } + j++; + } + /* everything is read */ + j = count = 0; + } - return; + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(fd, buf, sizeof(buf)); + } + } + } } void -MTK::decodeInit(void) +MTK::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; _rx_count = 0; _decode_state = MTK_DECODE_UNINIT; } - -void -MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) { + int ret = 0; + if (_decode_state == MTK_DECODE_UNINIT) { if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; - config_needed = false; _mtk_revision = 16; } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; - config_needed = false; _mtk_revision = 19; } @@ -125,78 +183,82 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } else { // Second start symbol was wrong, reset state machine - decodeInit(); + decode_init(); } } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum if (_rx_count < 33) - addByteToChecksum(b); + add_byte_to_checksum(b); // Fill packet buffer - _rx_buffer[_rx_count] = b; + ((uint8_t*)(&packet))[_rx_count] = b; _rx_count++; - /* Packet size minus checksum */ - if (_rx_count >= 35) { - type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;; - - /* Check if checksum is valid */ - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - if (_mtk_revision == 16) { - gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 - } else if (_mtk_revision == 19) { - gps_position->lat = packet->latitude; // both degrees*1e7 - gps_position->lon = packet->longitude; // both degrees*1e7 - } else { - warnx("mtk: unknown revision"); - } - gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm - gps_position->fix_type = packet->fix_type; - gps_position->eph_m = packet->hdop; // XXX: Check this because eph_m is in m and hdop is without unit - gps_position->epv_m = 0.0; //unknown in mtk custom mode - gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s - gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - gps_position->satellites_visible = packet->satellites; - - /* convert time and date information to unix timestamp */ - struct tm timeinfo; //TODO: test this conversion - uint32_t timeinfo_conversion_temp; - - timeinfo.tm_mday = packet->date * 1e-4; - timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; - timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; - timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; - - timeinfo.tm_hour = packet->utc_time * 1e-7; - timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; - timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; - timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; - timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; - timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; - time_t epoch = mktime(&timeinfo); - - gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; - gps_position->timestamp_position = gps_position->timestamp_time = hrt_absolute_time(); - - pos_updated = true; - - + /* Packet size minus checksum, XXX ? */ + if (_rx_count >= sizeof(packet)) { + /* Compare checksum */ + if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { + ret = 1; } else { - warnx("mtk Checksum invalid, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b); + warnx("MTK Checksum invalid"); + ret = -1; } - // Reset state machine to decode next packet - decodeInit(); + decode_init(); } } + return ret; +} + +void +MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +{ + if (_mtk_revision == 16) { + gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + gps_position.lat = packet.latitude; // both degrees*1e7 + gps_position.lon = packet.longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + gps_position.lat = 0; + gps_position.lon = 0; + } + gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + gps_position.fix_type = packet.fix_type; + gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + gps_position.epv_m = 0.0; //unknown in mtk custom mode + gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + gps_position.satellites_visible = packet.satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet.date * 1e-4; + timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet.utc_time * 1e-7; + timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + gps_position.time_gps_usec = epoch * 1e6; //TODO: test this + gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; + gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); + return; } void -MTK::addByteToChecksum(uint8_t b) +MTK::add_byte_to_checksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index e43637195..39063beab 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -50,6 +50,9 @@ #define WAAS_ON "$PMTK301,2*2E\r\n" #define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" +#define MTK_TIMEOUT_5HZ 400 +#define MTK_BAUDRATE 38400 + typedef enum { MTK_DECODE_UNINIT = 0, MTK_DECODE_GOT_CK_A = 1, @@ -64,16 +67,16 @@ typedef struct { int32_t latitude; ///< Latitude in degrees * 10^7 int32_t longitude; ///< Longitude in degrees * 10^7 uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 - uint32_t ground_speed; ///< FIXME SPEC UNCLEAR - int32_t heading; - uint8_t satellites; - uint8_t fix_type; + uint32_t ground_speed; ///< velocity in m/s + int32_t heading; ///< heading in degrees * 10^2 + uint8_t satellites; ///< number of sattelites used + uint8_t fix_type; ///< fix type: XXX correct for that uint32_t date; uint32_t utc_time; - uint16_t hdop; + uint16_t hdop; ///< horizontal dilution of position (without unit) uint8_t ck_a; uint8_t ck_b; -} type_gps_mtk_packet; +} gps_mtk_packet_t; #pragma pack(pop) @@ -84,23 +87,31 @@ class MTK : public GPS_Helper public: MTK(); ~MTK(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(const int &fd, struct vehicle_gps_position_s &gps_position); + int configure(const int &fd, unsigned &baudrate); private: + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b, gps_mtk_packet_t &packet); + + /** + * Handle the package once it has arrived + */ + void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); mtk_decode_state_t _decode_state; - bool _config_sent; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; -- cgit v1.2.3 From df6cf142e7d67fa8c868245ff144f4e1d4ebdfb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Feb 2013 11:05:57 -0800 Subject: Another rewrite: most of the polling, reading and writing is now inside the GPS classes --- apps/drivers/drv_gps.h | 3 +- apps/drivers/gps/gps.cpp | 62 +-- apps/drivers/gps/gps_helper.cpp | 2 + apps/drivers/gps/gps_helper.h | 4 +- apps/drivers/gps/mtk.cpp | 69 +-- apps/drivers/gps/mtk.h | 10 +- apps/drivers/gps/ubx.cpp | 930 ++++++++++++++++++++-------------------- apps/drivers/gps/ubx.h | 32 +- 8 files changed, 564 insertions(+), 548 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 67248efcd..1dda8ef0b 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -49,7 +49,8 @@ #define GPS_DEVICE_PATH "/dev/gps" typedef enum { - GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_NONE = 0, + GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NMEA, } gps_driver_mode_t; diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 76b5f4533..6d7cfcc68 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -36,10 +36,7 @@ * Driver for the GPS on a serial port */ -#include - -#include - +#include #include #include #include @@ -54,30 +51,24 @@ #include #include #include - +#include #include -#include - #include - #include - +#include #include #include #include - #include - +#include #include -//#include "ubx.h" +#include "ubx.h" #include "mtk.h" -#define SEND_BUFFER_LENGTH 100 -#define TIMEOUT 1000000 //1s -#define NUMBER_OF_TRIES 5 -#define CONFIG_TIMEOUT 2000000 +#define TIMEOUT_5HZ 400 +#define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) : _task_should_exit(false), _healthy(false), _mode_changed(false), - _mode(GPS_DRIVER_MODE_MTK), + _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -279,12 +270,11 @@ GPS::task_main() } switch (_mode) { -// case GPS_DRIVER_MODE_UBX: -// _Helper = new UBX(); -// break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; case GPS_DRIVER_MODE_MTK: - printf("try new mtk\n"); - _Helper = new MTK(); + _Helper = new MTK(_serial_fd, &_report); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); //TODO: add NMEA @@ -292,11 +282,11 @@ GPS::task_main() default: break; } - // XXX unlock/lock makes sense? unlock(); - if (_Helper->configure(_serial_fd, _baudrate) == 0) { - - while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + if (_Helper->configure(_baudrate) == 0) { + unlock(); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { +// lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -307,7 +297,7 @@ GPS::task_main() last_rate_count++; /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; @@ -323,8 +313,26 @@ GPS::task_main() _healthy = false; _rate = 0.0f; } + + lock(); } lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + // case GPS_DRIVER_MODE_NMEA: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + default: + break; + } + } debug("exiting"); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp index 2caa82af1..9c1fad569 100644 --- a/apps/drivers/gps/gps_helper.cpp +++ b/apps/drivers/gps/gps_helper.cpp @@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; + warnx("try baudrate: %d\n", speed); + default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 537ca819c..f3d3bc40b 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -44,8 +44,8 @@ class GPS_Helper { public: - virtual int configure(const int &fd, unsigned &baud) = 0; - virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + virtual int configure(unsigned &baud) = 0; + virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); }; diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 4ccbbfbe4..4762bd503 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -47,7 +47,9 @@ #include "mtk.h" -MTK::MTK() : +MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _mtk_revision(0) { decode_init(); @@ -58,40 +60,40 @@ MTK::~MTK() } int -MTK::configure(const int &fd, unsigned &baudrate) +MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) return -1; baudrate = MTK_BAUDRATE; /* Write config messages, don't wait for an answer */ - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { + if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { warnx("mtk: config write failed"); return -1; } @@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate) } int -MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +MTK::receive(unsigned timeout) { /* poll descriptor */ pollfd fds[1]; - fds[0].fd = fd; + fds[0].fd = _fd; fds[0].events = POLLIN; uint8_t buf[32]; gps_mtk_packet_t packet; + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + int j = 0; ssize_t count = 0; @@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) /* pass received bytes to the packet decoder */ while (j < count) { if (parse_char(buf[j], packet) > 0) { - handle_message(packet, gps_position); + handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } j++; } /* everything is read */ @@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) } /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { /* something went wrong when polling */ @@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(fd, buf, sizeof(buf)); + count = ::read(_fd, buf, sizeof(buf)); } } } @@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } void -MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +MTK::handle_message(gps_mtk_packet_t &packet) { if (_mtk_revision == 16) { - gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 } else if (_mtk_revision == 19) { - gps_position.lat = packet.latitude; // both degrees*1e7 - gps_position.lon = packet.longitude; // both degrees*1e7 + _gps_position->lat = packet.latitude; // both degrees*1e7 + _gps_position->lon = packet.longitude; // both degrees*1e7 } else { warnx("mtk: unknown revision"); - gps_position.lat = 0; - gps_position.lon = 0; + _gps_position->lat = 0; + _gps_position->lon = 0; } - gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm - gps_position.fix_type = packet.fix_type; - gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - gps_position.epv_m = 0.0; //unknown in mtk custom mode - gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s - gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - gps_position.satellites_visible = packet.satellites; + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + _gps_position->fix_type = packet.fix_type; + _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + _gps_position->satellites_visible = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion @@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - gps_position.time_gps_usec = epoch * 1e6; //TODO: test this - gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; - gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 39063beab..d4e390b01 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -85,10 +85,10 @@ typedef struct { class MTK : public GPS_Helper { public: - MTK(); + MTK(const int &fd, struct vehicle_gps_position_s *gps_position); ~MTK(); - int receive(const int &fd, struct vehicle_gps_position_s &gps_position); - int configure(const int &fd, unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** @@ -99,7 +99,7 @@ private: /** * Handle the package once it has arrived */ - void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); + void handle_message(gps_mtk_packet_t &packet); /** * Reset the parse state machine for a fresh start @@ -111,6 +111,8 @@ private: */ void add_byte_to_checksum(uint8_t); + int _fd; + struct vehicle_gps_position_s *_gps_position; mtk_decode_state_t _decode_state; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 8e2396564..74cbc5aaf 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -47,172 +48,251 @@ #include "ubx.h" +#define UBX_CONFIG_TIMEOUT 100 -UBX::UBX() : -_config_state(UBX_CONFIG_STATE_PRT), +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _waiting_for_ack(false) { - reset(); + decode_init(); } UBX::~UBX() { } -void -UBX::reset() +int +UBX::configure(unsigned &baudrate) { - decodeInit(); - _config_state = UBX_CONFIG_STATE_PRT; - _waiting_for_ack = false; -} + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* Define the package contents, don't change the baudrate */ + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } -void -UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - /* Only send a new config message when we got the ACK of the last one, - * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command - * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. - */ - if (!_waiting_for_ack) { - _waiting_for_ack = true; - if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { - /* This should never happen, the parser should set the flag, - * if it should be reconfigured, reset() should be called first. - */ - warnx("ubx: already configured"); - _waiting_for_ack = false; - return; - } else if (_config_state == UBX_CONFIG_STATE_PRT) { - - /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { - - /* Send a CFG-PRT message again, this time change the baudrate */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* If the new baudrate will be different from the current one, we should report that back to the driver */ - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; - baudrate_changed = true; - /* Don't wait for an ACK, we're switching baudrate and we might never get, - * after that, start fresh */ - reset(); - } + /* Send a CFG-PRT message again, this time change the baudrate */ + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } - } else if (_config_state == UBX_CONFIG_STATE_RATE) { + /* no ack is ecpected here, keep going configuring */ - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; - sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; - } else { - /* Catch the remaining config states here, they all need the same packet type */ + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - switch (_config_state) { - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - break; -// case UBX_CONFIG_STATE_MSG_NAV_DOP: -// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; -// break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* For satelites info 1Hz is enough */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; -// break; - default: - break; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; +} + +int +UBX::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; } + j++; + } - sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } } } } -void -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ @@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _decode_state = UBX_DECODE_GOT_SYNC2; } else { /* Second start symbol was wrong, reset state machine */ - decodeInit(); + decode_init(); } break; /* Now look for class */ case UBX_DECODE_GOT_SYNC2: /* everything except sync1 and sync2 needs to be added to the checksum */ - addByteToChecksum(b); + add_byte_to_checksum(b); /* check for known class */ switch (b) { case UBX_CLASS_ACK: @@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_class = CFG; break; default: //unknown class: reset state machine - decodeInit(); + decode_init(); break; } break; case UBX_DECODE_GOT_CLASS: - addByteToChecksum(b); + add_byte_to_checksum(b); switch (_message_class) { case NAV: switch (b) { @@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ // break; // // default: //unknown class: reset state machine, should not happen -// decodeInit(); +// decode_init(); // break; // } // break; @@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -337,384 +417,282 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = ACK_NAK; break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); - decodeInit(); - config_needed = true; + decode_init(); +// config_needed = true; break; } break; case UBX_DECODE_GOT_MESSAGEID: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; break; case UBX_DECODE_GOT_LENGTH1: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; break; case UBX_DECODE_GOT_LENGTH2: /* Add to checksum if not yet at checksum byte */ if (_rx_count < _payload_size) - addByteToChecksum(b); + add_byte_to_checksum(b); _rx_buffer[_rx_count] = b; /* once the payload has arrived, we can process the information */ if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->lat = packet->lat; - gps_position->lon = packet->lon; - gps_position->alt = packet->height_msl; - - gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); + } - /* Add timestamp to finish the report */ - gps_position->timestamp_position = hrt_absolute_time(); + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + break; + default: + break; + } + return 0; //XXX ? +} - /* set flag to trigger publishing of new position */ - pos_updated = true; - } else { - warnx("NAV_POSLLH: checksum invalid"); - } +int +UBX::handle_message() +{ + int ret = 0; - // Reset state machine to decode next packet - decodeInit(); - break; - } + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - gps_position->fix_type = packet->gpsFix; - gps_position->s_variance_m_s = packet->sAcc; - gps_position->p_variance_m = packet->pAcc; + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } - gps_position->timestamp_variance = hrt_absolute_time(); + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - } else { - warnx("NAV_SOL: checksum invalid"); - } + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; - // Reset state machine to decode next packet - decodeInit(); - break; - } + _gps_position->timestamp_variance = hrt_absolute_time(); + } + break; + } -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// //Check if checksum is valid and the store the gps information -// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { -// -// gps_position->eph_m = packet->hDOP; -// gps_position->epv = packet->vDOP; -// -// gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; // -// } else { -// warnx("NAV_DOP: checksum invalid"); -// } +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; // -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - - gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - - gps_position->timestamp_time = hrt_absolute_time(); - - } else { - warnx("NAV_TIMEUTC: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); - - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; - - //Check if checksum is valid and then store the gps information - if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - - /* Write satellite information in the global storage */ - gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - gps_position->satellite_used[i] = 0; - } - - gps_position->satellite_snr[i] = packet_part2->cno; - gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - gps_position->satellite_prn[i] = 0; - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - gps_position->satellite_info_available = true; - } else { - gps_position->satellite_info_available = false; - } - gps_position->timestamp_satellites = hrt_absolute_time(); - - } else { - warnx("NAV_SVINFO: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->vel_m_s = (float)packet->speed * 1e-2f; - gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; - gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; - gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; - gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - gps_position->vel_ned_valid = true; - gps_position->timestamp_velocity = hrt_absolute_time(); - - } else { - warnx("NAV_VELNED: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// _gps_position->timestamp_posdilution = hrt_absolute_time(); // -// //Check if checksum is valid and the store the gps information -// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { +// _new_nav_dop = true; // -// gps_position->satellites_visible = packet->numVis; -// gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// ret = 1; -// -// } else { -// warnx("RXM_SVSI: checksum invalid\n"); -// } -// -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - _waiting_for_ack = false; - - switch (_config_state) { - case UBX_CONFIG_STATE_PRT: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; - break; - case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_RATE; - break; - case UBX_CONFIG_STATE_RATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) - _config_state = UBX_CONFIG_STATE_NAV5; - break; - case UBX_CONFIG_STATE_NAV5: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) - _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; - break; -// case UBX_CONFIG_STATE_MSG_NAV_DOP: -// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) -// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; -// break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_CONFIGURED; - /* set the flag to tell the driver that configuration was successful */ - config_needed = false; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) -// _config_state = UBX_CONFIG_STATE_CONFIGURED; -// break; - default: - break; - } - } else { - warnx("ACK_ACK: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } +// break; +// } - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - config_needed = true; - } else { - warnx("ACK_NAK: checksum invalid\n"); - } + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; - // Reset state machine to decode next packet - decodeInit(); - break; - } + time_t epoch = mktime(&timeinfo); - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - decodeInit(); + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + _gps_position->timestamp_time = hrt_absolute_time(); + } + break; + } - break; + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + _gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + _gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + _gps_position->satellite_used[i] = 0; } - } // end if _rx_count high enough - else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; + + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + } else { - warnx("buffer full, restarting"); - decodeInit(); - config_needed = true; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; } - break; - default: + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + } else { + _gps_position->satellite_info_available = false; + } + _gps_position->timestamp_satellites = hrt_absolute_time(); + } + break; - } - return; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + } + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? } void -UBX::decodeInit(void) +UBX::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; @@ -726,14 +704,14 @@ UBX::decodeInit(void) } void -UBX::addByteToChecksum(uint8_t b) +UBX::add_byte_to_checksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void -UBX::addChecksumToMessage(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) } void -UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; /* Calculate the checksum now */ - addChecksumToMessage(packet, length); + add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index c420e83b9..e3dd1c7ea 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -86,6 +86,8 @@ #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_MAX_PAYLOAD_LENGTH 500 + // ************ /** the structures of the binary packets */ #pragma pack(push, 1) @@ -337,35 +339,49 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position); ~UBX(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: + + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b); + + /** + * Handle the package once it has arrived + */ + int handle_message(void); + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ - void addChecksumToMessage(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t* message, const unsigned length); /** * Helper to send a config packet */ - void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + int _fd; + struct vehicle_gps_position_s *_gps_position; ubx_config_state_t _config_state; bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; -- cgit v1.2.3 From a0780a20b5e10eafa1ddf7e16bce788b94514bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 Feb 2013 18:00:58 +0100 Subject: Fixed default airspeed for fixed wing control app --- apps/examples/control_demo/params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 6525b70f5..8cefc298f 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -52,9 +52,9 @@ PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain // speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // trim PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) -- cgit v1.2.3 From 9fb024b1f236ff63c70c9942a2775cec2fb0ddec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 13:07:40 +0100 Subject: Removed old GPS app --- apps/gps/.context | 0 apps/gps/.gitignore | 3 - apps/gps/Makefile | 59 --- apps/gps/gps.c | 589 --------------------- apps/gps/gps.h | 18 - apps/gps/mtk.c | 432 ---------------- apps/gps/mtk.h | 98 ---- apps/gps/nmea_helper.c | 345 ------------- apps/gps/nmea_helper.h | 47 -- apps/gps/nmealib/LICENSE.TXT | 506 ------------------ apps/gps/nmealib/README.TXT | 26 - apps/gps/nmealib/context.c | 67 --- apps/gps/nmealib/generate.c | 229 --------- apps/gps/nmealib/generator.c | 399 --------------- apps/gps/nmealib/gmath.c | 376 -------------- apps/gps/nmealib/info.c | 21 - apps/gps/nmealib/nmea/config.h | 51 -- apps/gps/nmealib/nmea/context.h | 44 -- apps/gps/nmealib/nmea/generate.h | 44 -- apps/gps/nmealib/nmea/generator.h | 79 --- apps/gps/nmealib/nmea/gmath.h | 92 ---- apps/gps/nmealib/nmea/info.h | 112 ---- apps/gps/nmealib/nmea/nmea.h | 25 - apps/gps/nmealib/nmea/parse.h | 39 -- apps/gps/nmealib/nmea/parser.h | 59 --- apps/gps/nmealib/nmea/sentence.h | 128 ----- apps/gps/nmealib/nmea/time.h | 47 -- apps/gps/nmealib/nmea/tok.h | 31 -- apps/gps/nmealib/nmea/units.h | 30 -- apps/gps/nmealib/parse.c | 501 ------------------ apps/gps/nmealib/parser.c | 400 --------------- apps/gps/nmealib/sentence.c | 54 -- apps/gps/nmealib/time.c | 63 --- apps/gps/nmealib/tok.c | 267 ---------- apps/gps/ubx.c | 1022 ------------------------------------- apps/gps/ubx.h | 415 --------------- 36 files changed, 6718 deletions(-) delete mode 100644 apps/gps/.context delete mode 100644 apps/gps/.gitignore delete mode 100644 apps/gps/Makefile delete mode 100644 apps/gps/gps.c delete mode 100644 apps/gps/gps.h delete mode 100644 apps/gps/mtk.c delete mode 100644 apps/gps/mtk.h delete mode 100644 apps/gps/nmea_helper.c delete mode 100644 apps/gps/nmea_helper.h delete mode 100644 apps/gps/nmealib/LICENSE.TXT delete mode 100644 apps/gps/nmealib/README.TXT delete mode 100644 apps/gps/nmealib/context.c delete mode 100644 apps/gps/nmealib/generate.c delete mode 100644 apps/gps/nmealib/generator.c delete mode 100644 apps/gps/nmealib/gmath.c delete mode 100644 apps/gps/nmealib/info.c delete mode 100644 apps/gps/nmealib/nmea/config.h delete mode 100644 apps/gps/nmealib/nmea/context.h delete mode 100644 apps/gps/nmealib/nmea/generate.h delete mode 100644 apps/gps/nmealib/nmea/generator.h delete mode 100644 apps/gps/nmealib/nmea/gmath.h delete mode 100644 apps/gps/nmealib/nmea/info.h delete mode 100644 apps/gps/nmealib/nmea/nmea.h delete mode 100644 apps/gps/nmealib/nmea/parse.h delete mode 100644 apps/gps/nmealib/nmea/parser.h delete mode 100644 apps/gps/nmealib/nmea/sentence.h delete mode 100644 apps/gps/nmealib/nmea/time.h delete mode 100644 apps/gps/nmealib/nmea/tok.h delete mode 100644 apps/gps/nmealib/nmea/units.h delete mode 100644 apps/gps/nmealib/parse.c delete mode 100644 apps/gps/nmealib/parser.c delete mode 100644 apps/gps/nmealib/sentence.c delete mode 100644 apps/gps/nmealib/time.c delete mode 100644 apps/gps/nmealib/tok.c delete mode 100644 apps/gps/ubx.c delete mode 100644 apps/gps/ubx.h (limited to 'apps') diff --git a/apps/gps/.context b/apps/gps/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/gps/.gitignore b/apps/gps/.gitignore deleted file mode 100644 index a02827195..000000000 --- a/apps/gps/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -include -mavlink-* -pymavlink-* diff --git a/apps/gps/Makefile b/apps/gps/Makefile deleted file mode 100644 index 7aaaf50cb..000000000 --- a/apps/gps/Makefile +++ /dev/null @@ -1,59 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the GPS receiver application -# - -APPNAME = gps -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -CSRCS = gps.c \ - ubx.c \ - mtk.c \ - nmea_helper.c \ - nmealib/context.c \ - nmealib/generate.c \ - nmealib/generator.c \ - nmealib/gmath.c \ - nmealib/info.c \ - nmealib/parse.c \ - nmealib/parser.c \ - nmealib/sentence.c \ - nmealib/time.c \ - nmealib/tok.c - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/apps/gps/gps.c b/apps/gps/gps.c deleted file mode 100644 index 8a9512054..000000000 --- a/apps/gps/gps.c +++ /dev/null @@ -1,589 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file gps.c - * GPS app main loop. - */ - -#include "gps.h" -#include -#include -#include -#include -#include -#include -#include "nmealib/nmea/nmea.h" // the nmea library -#include "nmea_helper.h" //header files for interacting with the nmea library -#include "mtk.h" //header files for the custom protocol for the mediatek diydrones chip -#include "ubx.h" //header files for the ubx protocol -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -static bool thread_should_exit; /**< Deamon status flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/** - * GPS module readout and publishing. - * - * This function reads the onboard gps and publishes the vehicle_gps_positon topic. - * - * @see vehicle_gps_position_s - * @ingroup apps - */ -__EXPORT int gps_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int gps_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/**************************************************************************** - * Definitions - ****************************************************************************/ -#define IMPORTANT_GPS_BAUD_RATES_N 2 -#define RETRY_INTERVAL_SECONDS 10 - -//gps_bin_ubx_state_t * ubx_state; -bool gps_mode_try_all; -bool gps_baud_try_all; -bool gps_mode_success; -bool terminate_gps_thread; -bool gps_verbose; -int current_gps_speed; - -enum GPS_MODES { - GPS_MODE_START = 0, - GPS_MODE_UBX = 1, - GPS_MODE_MTK = 2, - GPS_MODE_NMEA = 3, - GPS_MODE_END = 4 -}; - - -#define AUTO_DETECTION_COUNT 8 -const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600}; -const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea - -/**************************************************************************** - * Private functions - ****************************************************************************/ -int open_port(char *port); - -void close_port(int *fd); - -void setup_port(char *device, int speed, int *fd); - - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int gps_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("gps already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("gps", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - gps_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tgps is running\n"); - } else { - printf("\tgps not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/* - * Main function of gps app. - */ -int gps_thread_main(int argc, char *argv[]) { - - /* welcome message */ - printf("[gps] Initialized. Searching for GPS receiver..\n"); - - /* default values */ - const char *commandline_usage = "\tusage: %s {start|stop|status} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n"; - char *device = "/dev/ttyS3"; - char mode[10]; - strcpy(mode, "all"); - int baudrate = -1; - gps_mode_try_all = false; - gps_baud_try_all = false; - gps_mode_success = true; - terminate_gps_thread = false; - bool retry = false; - gps_verbose = false; - - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - /* read arguments */ - int i; - - for (i = 0; i < argc; i++) { - if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set - if (argc > i + 1) { - device = argv[i + 1]; - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--retry") == 0) { - if (argc > i + 1) { - retry = atoi(argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { - if (argc > i + 1) { - baudrate = atoi(argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { - if (argc > i + 1) { - strcpy(mode, argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - gps_verbose = true; - } - } - - /* - * In case a baud rate is set only this baud rate will be tried, - * otherwise a array of usual baud rates for gps receivers is used - */ - - -// printf("baudrate = %d\n",baudrate); - switch (baudrate) { - case -1: gps_baud_try_all = true; break; - - case 0: current_gps_speed = B0; break; - case 50: current_gps_speed = B50; break; - case 75: current_gps_speed = B75; break; - case 110: current_gps_speed = B110; break; - case 134: current_gps_speed = B134; break; - case 150: current_gps_speed = B150; break; - case 200: current_gps_speed = B200; break; - case 300: current_gps_speed = B300; break; - case 600: current_gps_speed = B600; break; - case 1200: current_gps_speed = B1200; break; - case 1800: current_gps_speed = B1800; break; - case 2400: current_gps_speed = B2400; break; - case 4800: current_gps_speed = B4800; break; - case 9600: current_gps_speed = B9600; break; - case 19200: current_gps_speed = B19200; break; - case 38400: current_gps_speed = B38400; break; - case 57600: current_gps_speed = B57600; break; - case 115200: current_gps_speed = B115200; break; - case 230400: current_gps_speed = B230400; break; - case 460800: current_gps_speed = B460800; break; - case 921600: current_gps_speed = B921600; break; - - default: - fprintf(stderr, "[gps] ERROR: Unsupported baudrate: %d\n", baudrate); - fflush(stdout); - return -EINVAL; - } - - - enum GPS_MODES current_gps_mode = GPS_MODE_UBX; - - if (strcmp(mode, "ubx") == 0) { - current_gps_mode = GPS_MODE_UBX; - - } else if (strcmp(mode, "mtkcustom") == 0) { - current_gps_mode = GPS_MODE_MTK; - - } else if (strcmp(mode, "nmea") == 0) { - current_gps_mode = GPS_MODE_NMEA; - - } else if (strcmp(mode, "all") == 0) { - gps_mode_try_all = true; - - } else { - fprintf(stderr, "\t[gps] Invalid mode argument\n"); - printf(commandline_usage); - thread_running = false; - return ERROR; - } - - /* Declare file descriptor for gps here, open port later in setup_port */ - int fd; - - while (!thread_should_exit) { - /* Infinite retries or break if retry == false */ - - /* Loop over all configurations of baud rate and protocol */ - for (i = 0; i < AUTO_DETECTION_COUNT; i++) { - if (gps_mode_try_all) { - current_gps_mode = autodetection_gpsmodes[i]; - - if (false == gps_baud_try_all && autodetection_baudrates[i] != current_gps_speed) //there is no need to try modes which are not configured to run with the selcted baud rate - continue; - } - - if (gps_baud_try_all) { - current_gps_speed = autodetection_baudrates[i]; - - if (false == gps_mode_try_all && autodetection_gpsmodes[i] != current_gps_mode) //there is no need to try baud rates which are not usual for the selected mode - continue; - } - - - /* - * The watchdog_thread will return and set gps_mode_success to false if no data can be parsed. - * if the gps was once running the wtachdog thread will not return but instead try to reconfigure the gps (depending on the mode/protocol) - */ - - if (current_gps_mode == GPS_MODE_UBX) { - - if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed); - - mavlink_log_info(mavlink_fd, "[gps] trying to connect to a ubx module"); - - setup_port(device, current_gps_speed, &fd); - - /* start ubx thread and watchdog */ - pthread_t ubx_thread; - pthread_t ubx_watchdog_thread; - - pthread_mutex_t ubx_mutex_d; - ubx_mutex = &ubx_mutex_d; - pthread_mutex_init(ubx_mutex, NULL); - gps_bin_ubx_state_t ubx_state_d; - ubx_state = &ubx_state_d; - ubx_decode_init(); - - pthread_attr_t ubx_loop_attr; - pthread_attr_init(&ubx_loop_attr); - pthread_attr_setstacksize(&ubx_loop_attr, 3000); - - struct arg_struct args; - args.fd_ptr = &fd; - args.thread_should_exit_ptr = &thread_should_exit; - - pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args); - - pthread_attr_t ubx_wd_attr; - pthread_attr_init(&ubx_wd_attr); - pthread_attr_setstacksize(&ubx_wd_attr, 1400); - int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&args); - - if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res); - - /* wait for threads to complete */ - pthread_join(ubx_watchdog_thread, NULL); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] no success with UBX mode and %d baud\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(ubx_thread, NULL); - - gps_mode_success = true; - terminate_gps_thread = false; - - /* maybe the watchdog was stopped through the thread_should_exit flag */ - } else if (thread_should_exit) { - pthread_join(ubx_thread, NULL); - if (gps_verbose) printf("[gps] ubx watchdog and ubx loop threads have been terminated, exiting."); - close(mavlink_fd); - close_port(&fd); - thread_running = false; - return 0; - } - - close_port(&fd); - } else if (current_gps_mode == GPS_MODE_MTK) { - if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed); - - mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module"); - - setup_port(device, current_gps_speed, &fd); - - /* start mtk thread and watchdog */ - pthread_t mtk_thread; - pthread_t mtk_watchdog_thread; - - pthread_mutex_t mtk_mutex_d; - mtk_mutex = &mtk_mutex_d; - pthread_mutex_init(mtk_mutex, NULL); - - - gps_bin_mtk_state_t mtk_state_d; - mtk_state = &mtk_state_d; - mtk_decode_init(); - - - pthread_attr_t mtk_loop_attr; - pthread_attr_init(&mtk_loop_attr); - pthread_attr_setstacksize(&mtk_loop_attr, 2048); - - struct arg_struct args; - args.fd_ptr = &fd; - args.thread_should_exit_ptr = &thread_should_exit; - - pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&args); - sleep(2); - pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&args); - - /* wait for threads to complete */ - pthread_join(mtk_watchdog_thread, (void *)&fd); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] No success with MTK binary mode and %d baud\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(mtk_thread, NULL); - - //if(true == gps_mode_try_all) - //strcpy(mode, "nmea"); - - gps_mode_success = true; - terminate_gps_thread = false; - } - - close_port(&fd); - - } else if (current_gps_mode == GPS_MODE_NMEA) { - if (gps_verbose) printf("[gps] Trying NMEA mode at %d baud\n", current_gps_speed); - - mavlink_log_info(mavlink_fd, "[gps] trying to connect to a NMEA module"); - - - setup_port(device, current_gps_speed, &fd); - - /* start nmea thread and watchdog */ - pthread_t nmea_thread; - pthread_t nmea_watchdog_thread; - - pthread_mutex_t nmea_mutex_d; - nmea_mutex = &nmea_mutex_d; - pthread_mutex_init(nmea_mutex, NULL); - - gps_bin_nmea_state_t nmea_state_d; - nmea_state = &nmea_state_d; - - pthread_attr_t nmea_loop_attr; - pthread_attr_init(&nmea_loop_attr); - pthread_attr_setstacksize(&nmea_loop_attr, 4096); - - struct arg_struct args; - args.fd_ptr = &fd; - args.thread_should_exit_ptr = &thread_should_exit; - - pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&args); - sleep(2); - pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&args); - - /* wait for threads to complete */ - pthread_join(nmea_watchdog_thread, (void *)&fd); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] No success with NMEA mode and %d baud\r\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(nmea_thread, NULL); - - gps_mode_success = true; - terminate_gps_thread = false; - } - - close_port(&fd); - } - - /* exit quickly if stop command has been received */ - if (thread_should_exit) { - printf("[gps] stopped, exiting.\n"); - close(mavlink_fd); - thread_running = false; - return 0; - } - - /* if both, mode and baud is set by argument, we only need one loop*/ - if (gps_mode_try_all == false && gps_baud_try_all == false) - break; - } - - - if (retry) { - printf("[gps] No configuration was successful, retrying in %d seconds \n", RETRY_INTERVAL_SECONDS); - mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, retrying..."); - fflush(stdout); - - } else { - fprintf(stderr, "[gps] No configuration was successful, exiting... \n"); - fflush(stdout); - mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, exiting..."); - break; - } - - sleep(RETRY_INTERVAL_SECONDS); - } - - printf("[gps] exiting.\n"); - close(mavlink_fd); - thread_running = false; - return 0; -} - - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "\tusage: gps {start|status|stop} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n"); - exit(1); -} - -int open_port(char *port) -{ - int fd; /**< File descriptor for the gps port */ - - /* Open serial port */ - fd = open(port, O_CREAT | O_RDWR | O_NOCTTY); /* O_RDWR - Read and write O_NOCTTY - Ignore special chars like CTRL-C */ - return (fd); -} - - -void close_port(int *fd) -{ - /* Close serial port */ - close(*fd); -} - -void setup_port(char *device, int speed, int *fd) -{ - /* open port (baud rate is set in defconfig file) */ - *fd = open_port(device); - - if (*fd != -1) { - if (gps_verbose) printf("[gps] Port opened: %s at %d baud\n", device, speed); - - } else { - fprintf(stderr, "[gps] Could not open port, exiting gps app!\n"); - fflush(stdout); - } - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - - if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) { - fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\n", device, termios_state); - close(*fd); - } - if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed); - /* Set baud rate */ - cfsetispeed(&uart_config, speed); - cfsetospeed(&uart_config, speed); - if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\n", device); - close(*fd); - } -} diff --git a/apps/gps/gps.h b/apps/gps/gps.h deleted file mode 100644 index 499a6164f..000000000 --- a/apps/gps/gps.h +++ /dev/null @@ -1,18 +0,0 @@ -/* - * gps.h - * - * Created on: Mar 8, 2012 - * Author: thomasgubler - */ - -#ifndef GPS_H_ -#define GPS_H - -#include - -struct arg_struct { - int *fd_ptr; - bool *thread_should_exit_ptr; -}; - -#endif /* GPS_H_ */ diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c deleted file mode 100644 index 3b0ee4565..000000000 --- a/apps/gps/mtk.c +++ /dev/null @@ -1,432 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Julian Oes - * Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file MTK custom binary (3DR) protocol implementation */ - -#include "gps.h" -#include "mtk.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define MTK_HEALTH_SUCCESS_COUNTER_LIMIT 2 -#define MTK_HEALTH_FAIL_COUNTER_LIMIT 2 - -// XXX decrease this substantially, it should be only a few dozen bytes max. -#warning XXX trying 128 for now -#define MTK_BUFFER_SIZE 128 - -pthread_mutex_t *mtk_mutex; -gps_bin_mtk_state_t *mtk_state; -static struct vehicle_gps_position_s *mtk_gps; - -extern bool gps_mode_try_all; -extern bool gps_mode_success; -extern bool terminate_gps_thread; -extern bool gps_baud_try_all; -extern bool gps_verbose; -extern int current_gps_speed; - - -void mtk_decode_init(void) -{ - mtk_state->ck_a = 0; - mtk_state->ck_b = 0; - mtk_state->rx_count = 0; - mtk_state->decode_state = MTK_DECODE_UNINIT; - mtk_state->print_errors = false; -} - -void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) -{ - *(ck_a) = *(ck_a) + b; - *(ck_b) = *(ck_b) + *(ck_a); -// printf("Checksum now: %d\n",*(ck_b)); -} - - - -int mtk_parse(uint8_t b, char *gps_rx_buffer) -{ -// printf("b=%x\n",b); - // Debug output to telemetry port - // PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, &b, 1); - if (mtk_state->decode_state == MTK_DECODE_UNINIT) { - - if (b == 0xd0) { - mtk_state->decode_state = MTK_DECODE_GOT_CK_A; - } - - } else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_A) { - if (b == 0xdd) { - mtk_state->decode_state = MTK_DECODE_GOT_CK_B; - - } else { - // Second start symbol was wrong, reset state machine - mtk_decode_init(); - } - - } else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_B) { - // Add to checksum - if (mtk_state->rx_count < 33) mtk_checksum(b, &(mtk_state->ck_a), &(mtk_state->ck_b)); - - // Fill packet buffer - gps_rx_buffer[mtk_state->rx_count] = b; - (mtk_state->rx_count)++; -// printf("Rx count: %d\n",mtk_state->rx_count); - uint8_t ret = 0; - - /* Packet size minus checksum */ - if (mtk_state->rx_count >= 35) { - gps_bin_mtk_packet_t *packet = (gps_bin_mtk_packet_t *) gps_rx_buffer; - - /* Check if checksum is valid */ - if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) { - mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7 - mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7 - mtk_gps->alt = (int32_t)(packet->msl_altitude * 10); // conversion from centimeters to millimeters, and from uint32_t to int16_t - mtk_gps->fix_type = packet->fix_type; - mtk_gps->eph = packet->hdop; - mtk_gps->epv = 65535; //unknown in mtk custom mode - mtk_gps->vel = packet->ground_speed; - mtk_gps->cog = (uint16_t)packet->heading; //mtk: degrees *1e2, mavlink/ubx: degrees *1e2 - mtk_gps->satellites_visible = packet->satellites; - - /* convert time and date information to unix timestamp */ - struct tm timeinfo; //TODO: test this conversion - uint32_t timeinfo_conversion_temp; - - timeinfo.tm_mday = packet->date * 1e-4; - timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; - timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; - timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; - - timeinfo.tm_hour = packet->utc_time * 1e-7; - timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; - timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; - timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; - timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; - timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; - time_t epoch = mktime(&timeinfo); - mtk_gps->timestamp = hrt_absolute_time(); - mtk_gps->time_gps_usec = epoch * 1e6; //TODO: test this - mtk_gps->time_gps_usec += timeinfo_conversion_temp * 1e3; - - mtk_gps->counter_pos_valid++; - - mtk_gps->timestamp = hrt_absolute_time(); - -// printf("%lu; %lu; %d.%d.%d %d:%d:%d:%d\n", packet->date, packet->utc_time,timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, timeinfo_conversion_temp); - - pthread_mutex_lock(mtk_mutex); -// printf("Write timestamp /n"); - mtk_state->last_message_timestamp = hrt_absolute_time(); - pthread_mutex_unlock(mtk_mutex); - - ret = 1; -// printf("found package\n"); - - } else { - if (gps_verbose) printf("[gps] Checksum invalid\r\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - mtk_decode_init(); -// printf("prepared for next state\n"); - return ret; - } - } - - return 0; // no valid packet found - -} - -int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate -{ -// printf("in read_gps_mtk\n"); - uint8_t ret = 0; - - uint8_t c; - - int rx_count = 0; - int gpsRxOverflow = 0; - - struct pollfd fds; - fds.fd = *fd; - fds.events = POLLIN; - - // This blocks the task until there is something on the buffer - while (1) { - //check if the thread should terminate - if (terminate_gps_thread == true) { -// printf("terminate_gps_thread=%u ", terminate_gps_thread); -// printf("exiting mtk thread\n"); -// fflush(stdout); - ret = 1; - break; - } - - if (poll(&fds, 1, 1000) > 0) { - if (read(*fd, &c, 1) > 0) { -// printf("Read %x\n",c); - if (rx_count >= buffer_size) { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - rx_count = 0; - mtk_decode_init(); - - if (gps_verbose) printf("[gps] Buffer full\r\n"); - - } else { - //gps_rx_buffer[rx_count] = c; - rx_count++; - - } - - int msg_read = mtk_parse(c, gps_rx_buffer); - - if (msg_read > 0) { - // printf("Found sequence\n"); - break; - } - - } else { - break; - } - - } else { - break; - } - - } - - return ret; -} - -int configure_gps_mtk(int *fd) -{ - int success = 0; - size_t result_write; - result_write = write(*fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ)); - - if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) { - printf("[gps] Set update speed to 10 Hz failed\r\n"); - success = 1; - - } else { - if (gps_verbose) printf("[gps] Attempted to set update speed to 10 Hz..\r\n"); - } - - //set custom mode - result_write = write(*fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE)); - - if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) { - //global_data_send_subsystem_info(&mtk_present); - printf("[gps] Set MTK custom mode failed\r\n"); - success = 1; - - } else { - //global_data_send_subsystem_info(&mtk_present_enabled); - if (gps_verbose) printf("[gps] Attempted to set MTK custom mode..\r\n"); - } - - return success; -} - -void *mtk_loop(void *args) -{ -// int oldstate; -// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate); -// -// printf("in mtk loop\n"); - /* Set thread name */ - prctl(PR_SET_NAME, "gps mtk read", getpid()); - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - /* Initialize gps stuff */ -// int buffer_size = 1000; -// char * gps_rx_buffer = malloc(buffer_size*sizeof(char)); - char gps_rx_buffer[MTK_BUFFER_SIZE]; - - /* set parameters for mtk custom */ - - if (configure_gps_mtk(fd) != 0) { - printf("[gps] Could not write serial port..\r\n"); - - /* Write shared variable sys_status */ - - //global_data_send_subsystem_info(&mtk_present); - - } else { - if (gps_verbose) printf("[gps] Configuration finished, awaiting GPS data..\r\n"); - - - /* Write shared variable sys_status */ - - //global_data_send_subsystem_info(&mtk_present_enabled); - } - - /* advertise GPS topic */ - struct vehicle_gps_position_s mtk_gps_d; - mtk_gps = &mtk_gps_d; - orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps); - - while (!(*thread_should_exit)) { - /* Parse a message from the gps receiver */ - if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) { - - /* publish new GPS position */ - orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps); - - } else { - /* de-advertise */ - close(gps_handle); - break; - } - - } - - close(gps_handle); - if(gps_verbose) printf("[gps] mtk loop is going to terminate\n"); - return NULL; -} - -void *mtk_watchdog_loop(void *args) -{ -// printf("in mtk watchdog loop\n"); - fflush(stdout); - - /* Set thread name */ - prctl(PR_SET_NAME, "gps mtk watchdog", getpid()); - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - bool mtk_healthy = false; - - uint8_t mtk_fail_count = 0; - uint8_t mtk_success_count = 0; - bool once_ok = false; - - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - - while (!(*thread_should_exit)) { - fflush(stdout); - - /* if we have no update for a long time reconfigure gps */ - pthread_mutex_lock(mtk_mutex); - uint64_t timestamp_now = hrt_absolute_time(); - bool all_okay = true; - - if (timestamp_now - mtk_state->last_message_timestamp > MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { - all_okay = false; - } - - pthread_mutex_unlock(mtk_mutex); - - if (!all_okay) { -// printf("mtk unhealthy\n"); - mtk_fail_count++; - /* gps error */ -// if (err_skip_counter == 0) -// { -// printf("[gps] GPS module not connected or not responding..\n"); -// err_skip_counter = 20; -// } -// err_skip_counter--; - -// printf("gps_mode_try_all =%u, mtk_fail_count=%u, mtk_healthy=%u, once_ok=%u\n", gps_mode_try_all, mtk_fail_count, mtk_healthy, once_ok); - - /* If we have too many failures and another mode or baud should be tried, exit... */ - if ((gps_mode_try_all == true || gps_baud_try_all == true) && (mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) && (mtk_healthy == false) && once_ok == false) { - if (gps_verbose) printf("[gps] Connection attempt failed, no MTK module found\r\n"); - - gps_mode_success = false; - break; - } - - if (mtk_healthy && mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) { - printf("[gps] ERROR: MTK GPS module stopped responding\r\n"); - // global_data_send_subsystem_info(&mtk_present_enabled); - mavlink_log_critical(mavlink_fd, "[gps] MTK module stopped responding\n"); - mtk_healthy = false; - mtk_success_count = 0; - - } - - /* trying to reconfigure the gps configuration */ - configure_gps_mtk(fd); - fflush(stdout); - - } else { - /* gps healthy */ - mtk_success_count++; - mtk_fail_count = 0; - once_ok = true; // XXX Should this be true on a single success, or on same criteria as mtk_healthy? - - if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) { - printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed); - /* MTK never has sat info */ - // XXX Check if lock makes sense here - mtk_gps->satellite_info_available = 0; - // global_data_send_subsystem_info(&mtk_present_enabled_healthy); - mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n"); - mtk_healthy = true; - } - } - - usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS); - } - if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n"); - close(mavlink_fd); - return NULL; -} diff --git a/apps/gps/mtk.h b/apps/gps/mtk.h deleted file mode 100644 index 9fc1caec8..000000000 --- a/apps/gps/mtk.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * mtk.h - * - * Created on: Mar 6, 2012 - * Author: thomasgubler - */ - -#ifndef MTK_H_ -#define MTK_H_ - -#include -#include -#include -#include -#include -#include -#include - -//Definition for mtk custom mode -#define MEDIATEK_REFRESH_RATE_4HZ "$PMTK220,250*29\r\n" //refresh rate - 4Hz - 250 milliseconds -#define MEDIATEK_REFRESH_RATE_5HZ "$PMTK220,200*2C\r\n" -#define MEDIATEK_REFRESH_RATE_10HZ "$PMTK220,100*2F\r\n" //refresh rate - 10Hz - 100 milliseconds -#define MEDIATEK_FACTORY_RESET "$PMTK104*37\r\n" //clear current settings -#define MEDIATEK_CUSTOM_BINARY_MODE "$PGCMD,16,0,0,0,0,0*6A\r\n" -#define MEDIATEK_FULL_COLD_RESTART "$PMTK104*37\r\n" -//#define NMEA_GGA_ENABLE "$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*27\r\n" //Set GGA messages - -//definitions for watchdog -#define MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000 -#define MTK_WATCHDOG_WAIT_TIME_MICROSECONDS 800000 - - - - -// ************ -// the structure of the binary packet - -typedef struct { - uint8_t payload; ///< Number of payload bytes - int32_t latitude; ///< Latitude in degrees * 10^7 - int32_t longitude; ///< Longitude in degrees * 10^7 - uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 - uint32_t ground_speed; ///< FIXME SPEC UNCLEAR - int32_t heading; - uint8_t satellites; - uint8_t fix_type; - uint32_t date; - uint32_t utc_time; - uint16_t hdop; - uint8_t ck_a; - uint8_t ck_b; -} __attribute__((__packed__)) type_gps_bin_mtk_packet; - -typedef type_gps_bin_mtk_packet gps_bin_mtk_packet_t; - -enum MTK_DECODE_STATES { - MTK_DECODE_UNINIT = 0, - MTK_DECODE_GOT_CK_A = 1, - MTK_DECODE_GOT_CK_B = 2 -}; - -typedef struct { - union { - uint16_t ck; - struct { - uint8_t ck_a; - uint8_t ck_b; - }; - }; - uint8_t decode_state; -// bool new_data; -// uint8_t fix; - bool print_errors; - int16_t rx_count; - - uint64_t last_message_timestamp; -} __attribute__((__packed__)) type_gps_bin_mtk_state; - -typedef type_gps_bin_mtk_state gps_bin_mtk_state_t; - -extern pthread_mutex_t *mtk_mutex; -extern gps_bin_mtk_state_t *mtk_state; - -void mtk_decode_init(void); - -void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b); - -int mtk_parse(uint8_t b, char *gps_rx_buffer); - -int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size); - -int configure_gps_mtk(int *fd); - -void *mtk_loop(void *args); - -void *mtk_watchdog_loop(void *args); - -#endif /* MTK_H_ */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c deleted file mode 100644 index 1a50371c1..000000000 --- a/apps/gps/nmea_helper.c +++ /dev/null @@ -1,345 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Julian Oes - * Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file NMEA protocol implementation */ -#include "gps.h" -#include "nmea_helper.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2 -#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2 - -#define NMEA_BUFFER_SIZE 1000 - -pthread_mutex_t *nmea_mutex; -gps_bin_nmea_state_t *nmea_state; -static struct vehicle_gps_position_s *nmea_gps; - -extern bool gps_mode_try_all; -extern bool gps_mode_success; -extern bool terminate_gps_thread; -extern bool gps_baud_try_all; -extern bool gps_verbose; -extern int current_gps_speed; - - -int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser) -{ - int ret = 1; - char c; - int start_flag = 0; - int found_cr = 0; - int rx_count = 0; - int gpsRxOverflow = 0; - - struct pollfd fds; - fds.fd = *fd; - fds.events = POLLIN; - - // NMEA or SINGLE-SENTENCE GPS mode - - - while (1) { - //check if the thread should terminate - if (terminate_gps_thread == true) { -// printf("terminate_gps_thread=%u ", terminate_gps_thread); -// printf("exiting mtk thread\n"); -// fflush(stdout); - ret = 2; - break; - } - - if (poll(&fds, 1, 1000) > 0) { - if (read(*fd, &c, 1) > 0) { - // detect start while acquiring stream - // printf("Char = %c\n", c); - if (!start_flag && (c == '$')) { - start_flag = 1; - found_cr = 0; - rx_count = 0; - - } else if (!start_flag) { // keep looking for start sign - continue; - } - - if (rx_count >= buffer_size) { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - start_flag = 0; - found_cr = 0; - rx_count = 0; - - if (gps_verbose) printf("\t[gps] Buffer full\n"); - - } else { - // store chars in buffer - gps_rx_buffer[rx_count] = c; - rx_count++; - } - - // look for carriage return CR - if (start_flag && c == 0x0d) { - found_cr = 1; - } - - // and then look for line feed LF - if (start_flag && found_cr && c == 0x0a) { - // parse one NMEA line, use buffer up to rx_count - if (nmea_parse(parser, gps_rx_buffer, rx_count, info) > 0) { - ret = 0; - } - - break; - } - - } else { - break; - } - - } else { - break; - } - } - - - - // As soon as one NMEA message has been parsed, we break out of the loop and end here - return(ret); -} - - -/** - * \brief Convert NDEG (NMEA degree) to fractional degree - */ -float ndeg2degree(float val) -{ - float deg = ((int)(val / 100)); - val = deg + (val - deg * 100) / 60; - return val; -} - -void *nmea_loop(void *args) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "gps nmea read", getpid()); - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - /* Initialize gps stuff */ - nmeaINFO info_d; - nmeaINFO *info = &info_d; - char gps_rx_buffer[NMEA_BUFFER_SIZE]; - - /* gps parser (nmea) */ - nmeaPARSER parser; - nmea_parser_init(&parser); - nmea_zero_INFO(info); - - /* advertise GPS topic */ - struct vehicle_gps_position_s nmea_gps_d = {.counter=0}; - nmea_gps = &nmea_gps_d; - orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); - - while (!(*thread_should_exit)) { - /* Parse a message from the gps receiver */ - uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser); - - if (0 == read_res) { - - /* convert data, ready it for publishing */ - - /* convert nmea utc time to usec */ - struct tm timeinfo; - timeinfo.tm_year = info->utc.year; - timeinfo.tm_mon = info->utc.mon; - timeinfo.tm_mday = info->utc.day; - timeinfo.tm_hour = info->utc.hour; - timeinfo.tm_min = info->utc.min; - timeinfo.tm_sec = info->utc.sec; - - time_t epoch = mktime(&timeinfo); - - // printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, info->utc.hsec); - - nmea_gps->timestamp = hrt_absolute_time(); - nmea_gps->time_gps_usec = (uint64_t)((epoch)*1000000 + (info->utc.hsec)*10000); - nmea_gps->fix_type = (uint8_t)info->fix; - nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7f); - nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7f); - nmea_gps->alt = (int32_t)(info->elv * 1000.0f); - nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling - nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling - nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100 - nmea_gps->cog = (uint16_t)info->direction*100; //nmea: degrees float, ubx/mavlink: degrees*1e2 - nmea_gps->satellites_visible = (uint8_t)info->satinfo.inview; - - int i = 0; - - /* Write info about individual satellites */ - for (i = 0; i < 12; i++) { - nmea_gps->satellite_prn[i] = (uint8_t)info->satinfo.sat[i].id; - nmea_gps->satellite_used[i] = (uint8_t)info->satinfo.sat[i].in_use; - nmea_gps->satellite_elevation[i] = (uint8_t)info->satinfo.sat[i].elv; - nmea_gps->satellite_azimuth[i] = (uint8_t)info->satinfo.sat[i].azimuth; - nmea_gps->satellite_snr[i] = (uint8_t)info->satinfo.sat[i].sig; - } - - if (nmea_gps->satellites_visible > 0) { - nmea_gps->satellite_info_available = 1; - - } else { - nmea_gps->satellite_info_available = 0; - } - - nmea_gps->counter_pos_valid++; - - nmea_gps->timestamp = hrt_absolute_time(); - nmea_gps->counter++; - - pthread_mutex_lock(nmea_mutex); - nmea_state->last_message_timestamp = hrt_absolute_time(); - pthread_mutex_unlock(nmea_mutex); - - /* publish new GPS position */ - orb_publish(ORB_ID(vehicle_gps_position), gps_handle, nmea_gps); - - } else if (read_res == 2) { //termination - /* de-advertise */ - close(gps_handle); - break; - } - - } - - //destroy gps parser - nmea_parser_destroy(&parser); - if(gps_verbose) printf("[gps] nmea loop is going to terminate\n"); - return NULL; - -} - -void *nmea_watchdog_loop(void *args) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "gps nmea watchdog", getpid()); - - bool nmea_healthy = false; - - uint8_t nmea_fail_count = 0; - uint8_t nmea_success_count = 0; - bool once_ok = false; - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - //int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - while (!(*thread_should_exit)) { -// printf("nmea_watchdog_loop : while "); - /* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */ - pthread_mutex_lock(nmea_mutex); - uint64_t timestamp_now = hrt_absolute_time(); - bool all_okay = true; - - if (timestamp_now - nmea_state->last_message_timestamp > NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { - all_okay = false; - } - - pthread_mutex_unlock(nmea_mutex); - - if (!all_okay) { - /* gps error */ - nmea_fail_count++; -// printf("nmea error, nmea_fail_count=%u\n", nmea_fail_count); -// fflush(stdout); - - /* If we have too many failures and another mode or baud should be tried, exit... */ - if ((gps_mode_try_all == true || gps_baud_try_all == true) && (nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) && (nmea_healthy == false) && once_ok == false) { - if (gps_verbose) printf("\t[gps] no NMEA module found\n"); - - gps_mode_success = false; - break; - } - - if (nmea_healthy && nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) { - printf("\t[gps] ERROR: NMEA GPS module stopped responding\n"); - // global_data_send_subsystem_info(&nmea_present_enabled); - mavlink_log_critical(mavlink_fd, "[gps] NMEA module stopped responding\n"); - nmea_healthy = false; - nmea_success_count = 0; - - } - - - - fflush(stdout); - sleep(1); - - } else { - /* gps healthy */ -// printf("\t[gps] nmea success\n"); - nmea_success_count++; - - if (!nmea_healthy && nmea_success_count >= NMEA_HEALTH_SUCCESS_COUNTER_LIMIT) { - printf("[gps] NMEA module found, status ok (baud=%d)\r\n", current_gps_speed); - // global_data_send_subsystem_info(&nmea_present_enabled_healthy); - mavlink_log_info(mavlink_fd, "[gps] NMEA module found, status ok\n"); - nmea_healthy = true; - nmea_fail_count = 0; - once_ok = true; - } - - } - - usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS); - } - if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n"); - close(mavlink_fd); - return NULL; -} diff --git a/apps/gps/nmea_helper.h b/apps/gps/nmea_helper.h deleted file mode 100644 index 3a853dd13..000000000 --- a/apps/gps/nmea_helper.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * nmea_helper.h - * - * Created on: Mar 15, 2012 - * Author: thomasgubler - */ - -#ifndef NMEA_H_ -#define NMEA_H_ - -#include -#include -#include -#include -#include "nmealib/nmea/nmea.h" - - -//definitions for watchdog -#define NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000 -#define NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS 800000 - -typedef struct { - uint64_t last_message_timestamp; -} __attribute__((__packed__)) type_gps_bin_nmea_state; - -typedef type_gps_bin_nmea_state gps_bin_nmea_state_t; - -extern gps_bin_nmea_state_t *nmea_state; -extern pthread_mutex_t *nmea_mutex; - - - -int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser); - -void *nmea_loop(void *arg); - -void *nmea_watchdog_loop(void *arg); - -/** - * \brief Convert NDEG (NMEA degree) to fractional degree - */ -float ndeg2degree(float val); - -void nmea_init(void); - - -#endif /* NMEA_H_ */ diff --git a/apps/gps/nmealib/LICENSE.TXT b/apps/gps/nmealib/LICENSE.TXT deleted file mode 100644 index 807db7916..000000000 --- a/apps/gps/nmealib/LICENSE.TXT +++ /dev/null @@ -1,506 +0,0 @@ - GNU LESSER GENERAL PUBLIC LICENSE - Version 2.1, February 1999 - - Copyright (C) 1991, 1999 Free Software Foundation, Inc. - 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - -[This is the first released version of the Lesser GPL. It also counts - as the successor of the GNU Library Public License, version 2, hence - the version number 2.1.] - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -Licenses are intended to guarantee your freedom to share and change -free software--to make sure the software is free for all its users. - - This license, the Lesser General Public License, applies to some -specially designated software packages--typically libraries--of the -Free Software Foundation and other authors who decide to use it. You -can use it too, but we suggest you first think carefully about whether -this license or the ordinary General Public License is the better -strategy to use in any particular case, based on the explanations below. - - When we speak of free software, we are referring to freedom of use, -not price. Our General Public Licenses are designed to make sure that -you have the freedom to distribute copies of free software (and charge -for this service if you wish); that you receive source code or can get -it if you want it; that you can change the software and use pieces of -it in new free programs; and that you are informed that you can do -these things. - - To protect your rights, we need to make restrictions that forbid -distributors to deny you these rights or to ask you to surrender these -rights. These restrictions translate to certain responsibilities for -you if you distribute copies of the library or if you modify it. - - For example, if you distribute copies of the library, whether gratis -or for a fee, you must give the recipients all the rights that we gave -you. You must make sure that they, too, receive or can get the source -code. If you link other code with the library, you must provide -complete object files to the recipients, so that they can relink them -with the library after making changes to the library and recompiling -it. And you must show them these terms so they know their rights. - - We protect your rights with a two-step method: (1) we copyright the -library, and (2) we offer you this license, which gives you legal -permission to copy, distribute and/or modify the library. - - To protect each distributor, we want to make it very clear that -there is no warranty for the free library. Also, if the library is -modified by someone else and passed on, the recipients should know -that what they have is not the original version, so that the original -author's reputation will not be affected by problems that might be -introduced by others. - - Finally, software patents pose a constant threat to the existence of -any free program. We wish to make sure that a company cannot -effectively restrict the users of a free program by obtaining a -restrictive license from a patent holder. Therefore, we insist that -any patent license obtained for a version of the library must be -consistent with the full freedom of use specified in this license. - - Most GNU software, including some libraries, is covered by the -ordinary GNU General Public License. This license, the GNU Lesser -General Public License, applies to certain designated libraries, and -is quite different from the ordinary General Public License. We use -this license for certain libraries in order to permit linking those -libraries into non-free programs. - - When a program is linked with a library, whether statically or using -a shared library, the combination of the two is legally speaking a -combined work, a derivative of the original library. The ordinary -General Public License therefore permits such linking only if the -entire combination fits its criteria of freedom. The Lesser General -Public License permits more lax criteria for linking other code with -the library. - - We call this license the "Lesser" General Public License because it -does Less to protect the user's freedom than the ordinary General -Public License. It also provides other free software developers Less -of an advantage over competing non-free programs. These disadvantages -are the reason we use the ordinary General Public License for many -libraries. However, the Lesser license provides advantages in certain -special circumstances. - - For example, on rare occasions, there may be a special need to -encourage the widest possible use of a certain library, so that it becomes -a de-facto standard. To achieve this, non-free programs must be -allowed to use the library. A more frequent case is that a free -library does the same job as widely used non-free libraries. In this -case, there is little to gain by limiting the free library to free -software only, so we use the Lesser General Public License. - - In other cases, permission to use a particular library in non-free -programs enables a greater number of people to use a large body of -free software. For example, permission to use the GNU C Library in -non-free programs enables many more people to use the whole GNU -operating system, as well as its variant, the GNU/Linux operating -system. - - Although the Lesser General Public License is Less protective of the -users' freedom, it does ensure that the user of a program that is -linked with the Library has the freedom and the wherewithal to run -that program using a modified version of the Library. - - The precise terms and conditions for copying, distribution and -modification follow. Pay close attention to the difference between a -"work based on the library" and a "work that uses the library". The -former contains code derived from the library, whereas the latter must -be combined with the library in order to run. - - GNU LESSER GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License Agreement applies to any software library or other -program which contains a notice placed by the copyright holder or -other authorized party saying it may be distributed under the terms of -this Lesser General Public License (also called "this License"). -Each licensee is addressed as "you". - - A "library" means a collection of software functions and/or data -prepared so as to be conveniently linked with application programs -(which use some of those functions and data) to form executables. - - The "Library", below, refers to any such software library or work -which has been distributed under these terms. A "work based on the -Library" means either the Library or any derivative work under -copyright law: that is to say, a work containing the Library or a -portion of it, either verbatim or with modifications and/or translated -straightforwardly into another language. (Hereinafter, translation is -included without limitation in the term "modification".) - - "Source code" for a work means the preferred form of the work for -making modifications to it. For a library, complete source code means -all the source code for all modules it contains, plus any associated -interface definition files, plus the scripts used to control compilation -and installation of the library. - - Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running a program using the Library is not restricted, and output from -such a program is covered only if its contents constitute a work based -on the Library (independent of the use of the Library in a tool for -writing it). Whether that is true depends on what the Library does -and what the program that uses the Library does. - - 1. You may copy and distribute verbatim copies of the Library's -complete source code as you receive it, in any medium, provided that -you conspicuously and appropriately publish on each copy an -appropriate copyright notice and disclaimer of warranty; keep intact -all the notices that refer to this License and to the absence of any -warranty; and distribute a copy of this License along with the -Library. - - You may charge a fee for the physical act of transferring a copy, -and you may at your option offer warranty protection in exchange for a -fee. - - 2. You may modify your copy or copies of the Library or any portion -of it, thus forming a work based on the Library, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) The modified work must itself be a software library. - - b) You must cause the files modified to carry prominent notices - stating that you changed the files and the date of any change. - - c) You must cause the whole of the work to be licensed at no - charge to all third parties under the terms of this License. - - d) If a facility in the modified Library refers to a function or a - table of data to be supplied by an application program that uses - the facility, other than as an argument passed when the facility - is invoked, then you must make a good faith effort to ensure that, - in the event an application does not supply such function or - table, the facility still operates, and performs whatever part of - its purpose remains meaningful. - - (For example, a function in a library to compute square roots has - a purpose that is entirely well-defined independent of the - application. Therefore, Subsection 2d requires that any - application-supplied function or table used by this function must - be optional: if the application does not supply it, the square - root function must still compute square roots.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Library, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Library, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote -it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Library. - -In addition, mere aggregation of another work not based on the Library -with the Library (or with a work based on the Library) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may opt to apply the terms of the ordinary GNU General Public -License instead of this License to a given copy of the Library. To do -this, you must alter all the notices that refer to this License, so -that they refer to the ordinary GNU General Public License, version 2, -instead of to this License. (If a newer version than version 2 of the -ordinary GNU General Public License has appeared, then you can specify -that version instead if you wish.) Do not make any other change in -these notices. - - Once this change is made in a given copy, it is irreversible for -that copy, so the ordinary GNU General Public License applies to all -subsequent copies and derivative works made from that copy. - - This option is useful when you wish to copy part of the code of -the Library into a program that is not a library. - - 4. You may copy and distribute the Library (or a portion or -derivative of it, under Section 2) in object code or executable form -under the terms of Sections 1 and 2 above provided that you accompany -it with the complete corresponding machine-readable source code, which -must be distributed under the terms of Sections 1 and 2 above on a -medium customarily used for software interchange. - - If distribution of object code is made by offering access to copy -from a designated place, then offering equivalent access to copy the -source code from the same place satisfies the requirement to -distribute the source code, even though third parties are not -compelled to copy the source along with the object code. - - 5. A program that contains no derivative of any portion of the -Library, but is designed to work with the Library by being compiled or -linked with it, is called a "work that uses the Library". Such a -work, in isolation, is not a derivative work of the Library, and -therefore falls outside the scope of this License. - - However, linking a "work that uses the Library" with the Library -creates an executable that is a derivative of the Library (because it -contains portions of the Library), rather than a "work that uses the -library". The executable is therefore covered by this License. -Section 6 states terms for distribution of such executables. - - When a "work that uses the Library" uses material from a header file -that is part of the Library, the object code for the work may be a -derivative work of the Library even though the source code is not. -Whether this is true is especially significant if the work can be -linked without the Library, or if the work is itself a library. The -threshold for this to be true is not precisely defined by law. - - If such an object file uses only numerical parameters, data -structure layouts and accessors, and small macros and small inline -functions (ten lines or less in length), then the use of the object -file is unrestricted, regardless of whether it is legally a derivative -work. (Executables containing this object code plus portions of the -Library will still fall under Section 6.) - - Otherwise, if the work is a derivative of the Library, you may -distribute the object code for the work under the terms of Section 6. -Any executables containing that work also fall under Section 6, -whether or not they are linked directly with the Library itself. - - 6. As an exception to the Sections above, you may also combine or -link a "work that uses the Library" with the Library to produce a -work containing portions of the Library, and distribute that work -under terms of your choice, provided that the terms permit -modification of the work for the customer's own use and reverse -engineering for debugging such modifications. - - You must give prominent notice with each copy of the work that the -Library is used in it and that the Library and its use are covered by -this License. You must supply a copy of this License. If the work -during execution displays copyright notices, you must include the -copyright notice for the Library among them, as well as a reference -directing the user to the copy of this License. Also, you must do one -of these things: - - a) Accompany the work with the complete corresponding - machine-readable source code for the Library including whatever - changes were used in the work (which must be distributed under - Sections 1 and 2 above); and, if the work is an executable linked - with the Library, with the complete machine-readable "work that - uses the Library", as object code and/or source code, so that the - user can modify the Library and then relink to produce a modified - executable containing the modified Library. (It is understood - that the user who changes the contents of definitions files in the - Library will not necessarily be able to recompile the application - to use the modified definitions.) - - b) Use a suitable shared library mechanism for linking with the - Library. A suitable mechanism is one that (1) uses at run time a - copy of the library already present on the user's computer system, - rather than copying library functions into the executable, and (2) - will operate properly with a modified version of the library, if - the user installs one, as long as the modified version is - interface-compatible with the version that the work was made with. - - c) Accompany the work with a written offer, valid for at - least three years, to give the same user the materials - specified in Subsection 6a, above, for a charge no more - than the cost of performing this distribution. - - d) If distribution of the work is made by offering access to copy - from a designated place, offer equivalent access to copy the above - specified materials from the same place. - - e) Verify that the user has already received a copy of these - materials or that you have already sent this user a copy. - - For an executable, the required form of the "work that uses the -Library" must include any data and utility programs needed for -reproducing the executable from it. However, as a special exception, -the materials to be distributed need not include anything that is -normally distributed (in either source or binary form) with the major -components (compiler, kernel, and so on) of the operating system on -which the executable runs, unless that component itself accompanies -the executable. - - It may happen that this requirement contradicts the license -restrictions of other proprietary libraries that do not normally -accompany the operating system. Such a contradiction means you cannot -use both them and the Library together in an executable that you -distribute. - - 7. You may place library facilities that are a work based on the -Library side-by-side in a single library together with other library -facilities not covered by this License, and distribute such a combined -library, provided that the separate distribution of the work based on -the Library and of the other library facilities is otherwise -permitted, and provided that you do these two things: - - a) Accompany the combined library with a copy of the same work - based on the Library, uncombined with any other library - facilities. This must be distributed under the terms of the - Sections above. - - b) Give prominent notice with the combined library of the fact - that part of it is a work based on the Library, and explaining - where to find the accompanying uncombined form of the same work. - - 8. You may not copy, modify, sublicense, link with, or distribute -the Library except as expressly provided under this License. Any -attempt otherwise to copy, modify, sublicense, link with, or -distribute the Library is void, and will automatically terminate your -rights under this License. However, parties who have received copies, -or rights, from you under this License will not have their licenses -terminated so long as such parties remain in full compliance. - - 9. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Library or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Library (or any work based on the -Library), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Library or works based on it. - - 10. Each time you redistribute the Library (or any work based on the -Library), the recipient automatically receives a license from the -original licensor to copy, distribute, link with or modify the Library -subject to these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties with -this License. - - 11. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Library at all. For example, if a patent -license would not permit royalty-free redistribution of the Library by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Library. - -If any portion of this section is held invalid or unenforceable under any -particular circumstance, the balance of the section is intended to apply, -and the section as a whole is intended to apply in other circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 12. If the distribution and/or use of the Library is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Library under this License may add -an explicit geographical distribution limitation excluding those countries, -so that distribution is permitted only in or among countries not thus -excluded. In such case, this License incorporates the limitation as if -written in the body of this License. - - 13. The Free Software Foundation may publish revised and/or new -versions of the Lesser General Public License from time to time. -Such new versions will be similar in spirit to the present version, -but may differ in detail to address new problems or concerns. - -Each version is given a distinguishing version number. If the Library -specifies a version number of this License which applies to it and -"any later version", you have the option of following the terms and -conditions either of that version or of any later version published by -the Free Software Foundation. If the Library does not specify a -license version number, you may choose any version ever published by -the Free Software Foundation. - - 14. If you wish to incorporate parts of the Library into other free -programs whose distribution conditions are incompatible with these, -write to the author to ask for permission. For software which is -copyrighted by the Free Software Foundation, write to the Free -Software Foundation; we sometimes make exceptions for this. Our -decision will be guided by the two goals of preserving the free status -of all derivatives of our free software and of promoting the sharing -and reuse of software generally. - - NO WARRANTY - - 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO -WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. -EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR -OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY -KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE -LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME -THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN -WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY -AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU -FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR -CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE -LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING -RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A -FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF -SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH -DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Libraries - - If you develop a new library, and you want it to be of the greatest -possible use to the public, we recommend making it free software that -everyone can redistribute and change. You can do so by permitting -redistribution under these terms (or, alternatively, under the terms of the -ordinary General Public License). - - To apply these terms, attach the following notices to the library. It is -safest to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least the -"copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library 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 - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - -Also add information on how to contact you by electronic and paper mail. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the library, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the - library `Frob' (a library for tweaking knobs) written by James Random Hacker. - - , 1 April 1990 - Ty Coon, President of Vice - -That's all there is to it! - - - - diff --git a/apps/gps/nmealib/README.TXT b/apps/gps/nmealib/README.TXT deleted file mode 100644 index 8ede6a036..000000000 --- a/apps/gps/nmealib/README.TXT +++ /dev/null @@ -1,26 +0,0 @@ -NMEA library see: http://nmea.sourceforge.net/ - -Disclaimer - -The National Marine Electronics Association (NMEA) has developed a specification that defines the interface between various pieces of marine electronic equipment. The standard permits marine electronics to send information to computers and to other marine equipment. -Most computer programs that provide real time position information understand and expect data to be in NMEA format. This data includes the complete PVT (position, velocity, time) solution computed by the GPS receiver. The idea of NMEA is to send a line of data called a sentence that is totally self contained and independent from other sentences. All NMEA sentences is sequences of ACSII symbols begins with a '$' and ends with a carriage return/line feed sequence and can be no longer than 80 characters of visible text (plus the line terminators). - -Introduction - -We present library in 'C' programming language for work with NMEA protocol. Small and easy to use. The library build on different compilers under different platforms (see below). The code was tested in real projects. Just download and try... - -Features - -- Analysis NMEA sentences and granting GPS data in C structures -- Generate NMEA sentences -- Supported sentences: GPGGA, GPGSA, GPGSV, GPRMC, GPVTG -- Multilevel architecture of algorithms -- Additional functions of geographical mathematics and work with navigation data - -Supported (tested) platforms - -- Microsoft Windows (MS Visual Studio 8.0, GCC) -- Windows Mobile, Windows CE (MS Visual Studio 8.0) -- UNIX (GCC) - -Licence: LGPL diff --git a/apps/gps/nmealib/context.c b/apps/gps/nmealib/context.c deleted file mode 100644 index 6ee2f5ad3..000000000 --- a/apps/gps/nmealib/context.c +++ /dev/null @@ -1,67 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include "nmea/context.h" - -#include -#include -#include - -nmeaPROPERTY * nmea_property(void) -{ - static nmeaPROPERTY prop = { - 0, 0, NMEA_DEF_PARSEBUFF - }; - - return ∝ -} - -void nmea_trace(const char *str, ...) -{ - int size; - va_list arg_list; - char buff[NMEA_DEF_PARSEBUFF]; - nmeaTraceFunc func = nmea_property()->trace_func; - - if(func) - { - va_start(arg_list, str); - size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); - va_end(arg_list); - - if(size > 0) - (*func)(&buff[0], size); - } -} - -void nmea_trace_buff(const char *buff, int buff_size) -{ - nmeaTraceFunc func = nmea_property()->trace_func; - if(func && buff_size) - (*func)(buff, buff_size); -} - -void nmea_error(const char *str, ...) -{ - int size; - va_list arg_list; - char buff[NMEA_DEF_PARSEBUFF]; - nmeaErrorFunc func = nmea_property()->error_func; - - if(func) - { - va_start(arg_list, str); - size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); - va_end(arg_list); - - if(size > 0) - (*func)(&buff[0], size); - } -} diff --git a/apps/gps/nmealib/generate.c b/apps/gps/nmealib/generate.c deleted file mode 100644 index 4c9389300..000000000 --- a/apps/gps/nmealib/generate.c +++ /dev/null @@ -1,229 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: generate.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include "nmea/tok.h" -#include "nmea/sentence.h" -#include "nmea/generate.h" -#include "nmea/units.h" - -#include -#include -#include - -int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack) -{ - return nmea_printf(buff, buff_sz, - "$GPGGA,%02d%02d%02d.%02d,%07.4f,%C,%07.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d", - pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, - pack->lat, pack->ns, pack->lon, pack->ew, - pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units, - pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid); -} - -int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack) -{ - return nmea_printf(buff, buff_sz, - "$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f", - pack->fix_mode, pack->fix_type, - pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5], - pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11], - pack->PDOP, pack->HDOP, pack->VDOP); -} - -int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack) -{ - return nmea_printf(buff, buff_sz, - "$GPGSV,%1d,%1d,%02d," - "%02d,%02d,%03d,%02d," - "%02d,%02d,%03d,%02d," - "%02d,%02d,%03d,%02d," - "%02d,%02d,%03d,%02d", - pack->pack_count, pack->pack_index + 1, pack->sat_count, - pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig, - pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig, - pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig, - pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig); -} - -int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack) -{ - return nmea_printf(buff, buff_sz, - "$GPRMC,%02d%02d%02d.%02d,%C,%07.4f,%C,%07.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C", - pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, - pack->status, pack->lat, pack->ns, pack->lon, pack->ew, - pack->speed, pack->direction, - pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100, - pack->declination, pack->declin_ew, pack->mode); -} - -int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack) -{ - return nmea_printf(buff, buff_sz, - "$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C", - pack->dir, pack->dir_t, - pack->dec, pack->dec_m, - pack->spn, pack->spn_n, - pack->spk, pack->spk_k); -} - -void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack) -{ - nmea_zero_GPGGA(pack); - - pack->utc = info->utc; - pack->lat = fabs(info->lat); - pack->ns = ((info->lat > 0)?'N':'S'); - pack->lon = fabs(info->lon); - pack->ew = ((info->lon > 0)?'E':'W'); - pack->sig = info->sig; - pack->satinuse = info->satinfo.inuse; - pack->HDOP = info->HDOP; - pack->elv = info->elv; -} - -void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack) -{ - int it; - - nmea_zero_GPGSA(pack); - - pack->fix_type = info->fix; - pack->PDOP = info->PDOP; - pack->HDOP = info->HDOP; - pack->VDOP = info->VDOP; - - for(it = 0; it < NMEA_MAXSAT; ++it) - { - pack->sat_prn[it] = - ((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0); - } -} - -int nmea_gsv_npack(int sat_count) -{ - int pack_count = (int)ceil(((float)sat_count) / NMEA_SATINPACK); - - if(0 == pack_count) - pack_count = 1; - - return pack_count; -} - -void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx) -{ - int sit, pit; - - nmea_zero_GPGSV(pack); - - pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT; - pack->pack_count = nmea_gsv_npack(pack->sat_count); - - if(pack->pack_count == 0) - pack->pack_count = 1; - - if(pack_idx >= pack->pack_count) - pack->pack_index = pack_idx % pack->pack_count; - else - pack->pack_index = pack_idx; - - for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit) - pack->sat_data[pit] = info->satinfo.sat[sit]; -} - -void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) -{ - nmea_zero_GPRMC(pack); - - pack->utc = info->utc; - pack->status = ((info->sig > 0)?'A':'V'); - pack->lat = fabs(info->lat); - pack->ns = ((info->lat > 0)?'N':'S'); - pack->lon = fabs(info->lon); - pack->ew = ((info->lon > 0)?'E':'W'); - pack->speed = info->speed / NMEA_TUD_KNOTS; - pack->direction = info->direction; - pack->declination = info->declination; - pack->declin_ew = 'E'; - pack->mode = ((info->sig > 0)?'A':'N'); -} - -void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) -{ - nmea_zero_GPVTG(pack); - - pack->dir = info->direction; - pack->dec = info->declination; - pack->spn = info->speed / NMEA_TUD_KNOTS; - pack->spk = info->speed; -} - -int nmea_generate( - char *buff, int buff_sz, - const nmeaINFO *info, - int generate_mask - ) -{ - int gen_count = 0, gsv_it, gsv_count; - int pack_mask = generate_mask; - - nmeaGPGGA gga; - nmeaGPGSA gsa; - nmeaGPGSV gsv; - nmeaGPRMC rmc; - nmeaGPVTG vtg; - - if(!buff) - return 0; - - while(pack_mask) - { - if(pack_mask & GPGGA) - { - nmea_info2GPGGA(info, &gga); - gen_count += nmea_gen_GPGGA(buff + gen_count, buff_sz - gen_count, &gga); - pack_mask &= ~GPGGA; - } - else if(pack_mask & GPGSA) - { - nmea_info2GPGSA(info, &gsa); - gen_count += nmea_gen_GPGSA(buff + gen_count, buff_sz - gen_count, &gsa); - pack_mask &= ~GPGSA; - } - else if(pack_mask & GPGSV) - { - gsv_count = nmea_gsv_npack(info->satinfo.inview); - for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it) - { - nmea_info2GPGSV(info, &gsv, gsv_it); - gen_count += nmea_gen_GPGSV(buff + gen_count, buff_sz - gen_count, &gsv); - } - pack_mask &= ~GPGSV; - } - else if(pack_mask & GPRMC) - { - nmea_info2GPRMC(info, &rmc); - gen_count += nmea_gen_GPRMC(buff + gen_count, buff_sz - gen_count, &rmc); - pack_mask &= ~GPRMC; - } - else if(pack_mask & GPVTG) - { - nmea_info2GPVTG(info, &vtg); - gen_count += nmea_gen_GPVTG(buff + gen_count, buff_sz - gen_count, &vtg); - pack_mask &= ~GPVTG; - } - else - break; - - if(buff_sz - gen_count <= 0) - break; - } - - return gen_count; -} diff --git a/apps/gps/nmealib/generator.c b/apps/gps/nmealib/generator.c deleted file mode 100644 index ce40b0f1a..000000000 --- a/apps/gps/nmealib/generator.c +++ /dev/null @@ -1,399 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include "nmea/gmath.h" -#include "nmea/generate.h" -#include "nmea/generator.h" -#include "nmea/context.h" - -#include -#include - -#if defined(NMEA_WIN) && defined(_MSC_VER) -# pragma warning(disable: 4100) /* unreferenced formal parameter */ -#endif - -float nmea_random(float min, float max) -{ - static float rand_max = MAX_RAND;//RAND_MAX; //nuttx defines MAX_RAND instead of RAND_MAX - float rand_val = rand(); - float bounds = max - min; - return min + (rand_val * bounds) / rand_max; -} - -/* - * low level - */ - -int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int RetVal = 1; int smask = info->smask; - nmeaGENERATOR *igen = gen; - - nmea_zero_INFO(info); - info->smask = smask; - - info->lat = NMEA_DEF_LAT; - info->lon = NMEA_DEF_LON; - - while(RetVal && igen) - { - if(igen->init_call) - RetVal = (*igen->init_call)(igen, info); - igen = igen->next; - } - - return RetVal; -} - -int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int RetVal = 1; - - if(gen->loop_call) - RetVal = (*gen->loop_call)(gen, info); - - if(RetVal && gen->next) - RetVal = nmea_gen_loop(gen->next, info); - - return RetVal; -} - -int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int RetVal = 1; - - if(gen->reset_call) - RetVal = (*gen->reset_call)(gen, info); - - return RetVal; -} - -void nmea_gen_destroy(nmeaGENERATOR *gen) -{ - if(gen->next) - { - nmea_gen_destroy(gen->next); - gen->next = 0; - } - - if(gen->destroy_call) - (*gen->destroy_call)(gen); - - free(gen); -} - -void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen) -{ - if(to->next) - nmea_gen_add(to->next, gen); - else - to->next = gen; -} - -int nmea_generate_from( - char *buff, int buff_sz, - nmeaINFO *info, - nmeaGENERATOR *gen, - int generate_mask - ) -{ - int retval; - - if(0 != (retval = nmea_gen_loop(gen, info))) - retval = nmea_generate(buff, buff_sz, info, generate_mask); - - return retval; -} - -/* - * NOISE generator - */ - -int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info) -{ - return 1; -} - -int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int it; - int in_use; - - info->sig = (int)nmea_random(1, 3); - info->PDOP = nmea_random(0, 9); - info->HDOP = nmea_random(0, 9); - info->VDOP = nmea_random(0, 9); - info->fix = (int)nmea_random(2, 3); - info->lat = nmea_random(0, 100); - info->lon = nmea_random(0, 100); - info->speed = nmea_random(0, 100); - info->direction = nmea_random(0, 360); - info->declination = nmea_random(0, 360); - info->elv = (int)nmea_random(-100, 100); - - info->satinfo.inuse = 0; - info->satinfo.inview = 0; - - for(it = 0; it < 12; ++it) - { - info->satinfo.sat[it].id = it; - info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); - info->satinfo.sat[it].elv = (int)nmea_random(0, 90); - info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359); - info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); - - if(in_use) - info->satinfo.inuse++; - if(info->satinfo.sat[it].sig > 0) - info->satinfo.inview++; - } - - return 1; -} - -int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info) -{ - return 1; -} - -/* - * STATIC generator - */ - -int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info) -{ - nmea_time_now(&info->utc); - return 1; -}; - -int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info) -{ - info->satinfo.inuse = 4; - info->satinfo.inview = 4; - - info->satinfo.sat[0].id = 1; - info->satinfo.sat[0].in_use = 1; - info->satinfo.sat[0].elv = 50; - info->satinfo.sat[0].azimuth = 0; - info->satinfo.sat[0].sig = 99; - - info->satinfo.sat[1].id = 2; - info->satinfo.sat[1].in_use = 1; - info->satinfo.sat[1].elv = 50; - info->satinfo.sat[1].azimuth = 90; - info->satinfo.sat[1].sig = 99; - - info->satinfo.sat[2].id = 3; - info->satinfo.sat[2].in_use = 1; - info->satinfo.sat[2].elv = 50; - info->satinfo.sat[2].azimuth = 180; - info->satinfo.sat[2].sig = 99; - - info->satinfo.sat[3].id = 4; - info->satinfo.sat[3].in_use = 1; - info->satinfo.sat[3].elv = 50; - info->satinfo.sat[3].azimuth = 270; - info->satinfo.sat[3].sig = 99; - - return 1; -} - -int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info) -{ - info->sig = 3; - info->fix = 3; - - nmea_igen_static_reset(gen, info); - - return 1; -} - -/* - * SAT_ROTATE generator - */ - -int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int it; - int count = info->satinfo.inview; - float deg = 360 / (count?count:1); - float srt = (count?(info->satinfo.sat[0].azimuth):0) + 5; - - nmea_time_now(&info->utc); - - for(it = 0; it < count; ++it) - { - info->satinfo.sat[it].azimuth = - (int)((srt >= 360)?srt - 360:srt); - srt += deg; - } - - return 1; -}; - -int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info) -{ - int it; - float deg = 360 / 8; - float srt = 0; - - info->satinfo.inuse = 8; - info->satinfo.inview = 8; - - for(it = 0; it < info->satinfo.inview; ++it) - { - info->satinfo.sat[it].id = it + 1; - info->satinfo.sat[it].in_use = 1; - info->satinfo.sat[it].elv = 5; - info->satinfo.sat[it].azimuth = (int)srt; - info->satinfo.sat[it].sig = 80; - srt += deg; - } - - return 1; -} - -int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info) -{ - info->sig = 3; - info->fix = 3; - - nmea_igen_rotate_reset(gen, info); - - return 1; -} - -/* - * POS_RANDMOVE generator - */ - -int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info) -{ - info->sig = 3; - info->fix = 3; - info->direction = info->declination = 0; - info->speed = 20; - return 1; -} - -int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info) -{ - nmeaPOS crd; - - info->direction += nmea_random(-10, 10); - info->speed += nmea_random(-2, 3); - - if(info->direction < 0) - info->direction = 359 + info->direction; - if(info->direction > 359) - info->direction -= 359; - - if(info->speed > 40) - info->speed = 40; - if(info->speed < 1) - info->speed = 1; - - nmea_info2pos(info, &crd); - nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600); - nmea_pos2info(&crd, info); - - info->declination = info->direction; - - return 1; -}; - -int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen) -{ - return 1; -}; - -/* - * generator create - */ - -nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info) -{ - nmeaGENERATOR *gen = 0; - - switch(type) - { - case NMEA_GEN_NOISE: - if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) - nmea_error("Insufficient memory!"); - else - { - memset(gen, 0, sizeof(nmeaGENERATOR)); - gen->init_call = &nmea_igen_noise_init; - gen->loop_call = &nmea_igen_noise_loop; - gen->reset_call = &nmea_igen_noise_reset; - } - break; - case NMEA_GEN_STATIC: - case NMEA_GEN_SAT_STATIC: - if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) - nmea_error("Insufficient memory!"); - else - { - memset(gen, 0, sizeof(nmeaGENERATOR)); - gen->init_call = &nmea_igen_static_init; - gen->loop_call = &nmea_igen_static_loop; - gen->reset_call = &nmea_igen_static_reset; - } - break; - case NMEA_GEN_SAT_ROTATE: - if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) - nmea_error("Insufficient memory!"); - else - { - memset(gen, 0, sizeof(nmeaGENERATOR)); - gen->init_call = &nmea_igen_rotate_init; - gen->loop_call = &nmea_igen_rotate_loop; - gen->reset_call = &nmea_igen_rotate_reset; - } - break; - case NMEA_GEN_POS_RANDMOVE: - if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) - nmea_error("Insufficient memory!"); - else - { - memset(gen, 0, sizeof(nmeaGENERATOR)); - gen->init_call = &nmea_igen_pos_rmove_init; - gen->loop_call = &nmea_igen_pos_rmove_loop; - gen->destroy_call = &nmea_igen_pos_rmove_destroy; - } - break; - case NMEA_GEN_ROTATE: - gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); - nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); - break; - }; - - return gen; -} - -nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info) -{ - nmeaGENERATOR *gen = __nmea_create_generator(type, info); - - if(gen) - nmea_gen_init(gen, info); - - return gen; -} - -void nmea_destroy_generator(nmeaGENERATOR *gen) -{ - nmea_gen_destroy(gen); -} - -#if defined(NMEA_WIN) && defined(_MSC_VER) -# pragma warning(default: 4100) -#endif diff --git a/apps/gps/nmealib/gmath.c b/apps/gps/nmealib/gmath.c deleted file mode 100644 index 327b982ef..000000000 --- a/apps/gps/nmealib/gmath.c +++ /dev/null @@ -1,376 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -/*! \file gmath.h */ -#include "nmea/gmath.h" - -#include -#include - -/** - * \fn nmea_degree2radian - * \brief Convert degree to radian - */ -float nmea_degree2radian(float val) -{ return (val * NMEA_PI180); } - -/** - * \fn nmea_radian2degree - * \brief Convert radian to degree - */ -float nmea_radian2degree(float val) -{ return (val / NMEA_PI180); } - -/** - * \brief Convert NDEG (NMEA degree) to fractional degree - */ -float nmea_ndeg2degree(float val) -{ - float deg = ((int)(val / 100)); - val = deg + (val - deg * 100) / 60; - return val; -} - -/** - * \brief Convert fractional degree to NDEG (NMEA degree) - */ -float nmea_degree2ndeg(float val) -{ - float int_part; - float fra_part; - fra_part = modf(val, &int_part); - val = int_part * 100 + fra_part * 60; - return val; -} - -/** - * \fn nmea_ndeg2radian - * \brief Convert NDEG (NMEA degree) to radian - */ -float nmea_ndeg2radian(float val) -{ return nmea_degree2radian(nmea_ndeg2degree(val)); } - -/** - * \fn nmea_radian2ndeg - * \brief Convert radian to NDEG (NMEA degree) - */ -float nmea_radian2ndeg(float val) -{ return nmea_degree2ndeg(nmea_radian2degree(val)); } - -/** - * \brief Calculate PDOP (Position Dilution Of Precision) factor - */ -float nmea_calc_pdop(float hdop, float vdop) -{ - return sqrt(pow(hdop, 2) + pow(vdop, 2)); -} - -float nmea_dop2meters(float dop) -{ return (dop * NMEA_DOP_FACTOR); } - -float nmea_meters2dop(float meters) -{ return (meters / NMEA_DOP_FACTOR); } - -/** - * \brief Calculate distance between two points - * \return Distance in meters - */ -float nmea_distance( - const nmeaPOS *from_pos, /**< From position in radians */ - const nmeaPOS *to_pos /**< To position in radians */ - ) -{ - float dist = ((float)NMEA_EARTHRADIUS_M) * acos( - sin(to_pos->lat) * sin(from_pos->lat) + - cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) - ); - return dist; -} - -/** - * \brief Calculate distance between two points - * This function uses an algorithm for an oblate spheroid earth model. - * The algorithm is described here: - * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf - * \return Distance in meters - */ -float nmea_distance_ellipsoid( - const nmeaPOS *from_pos, /**< From position in radians */ - const nmeaPOS *to_pos, /**< To position in radians */ - float *from_azimuth, /**< (O) azimuth at "from" position in radians */ - float *to_azimuth /**< (O) azimuth at "to" position in radians */ - ) -{ - /* All variables */ - float f, a, b, sqr_a, sqr_b; - float L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; - float sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; - int remaining_steps; - float sqr_u, A, B, delta_sigma; - - /* Check input */ - //NMEA_ASSERT(from_pos != 0); - //NMEA_ASSERT(to_pos != 0); - - if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) - { /* Identical points */ - if ( from_azimuth != 0 ) - *from_azimuth = 0; - if ( to_azimuth != 0 ) - *to_azimuth = 0; - return 0; - } /* Identical points */ - - /* Earth geometry */ - f = NMEA_EARTH_FLATTENING; - a = NMEA_EARTH_SEMIMAJORAXIS_M; - b = (1 - f) * a; - sqr_a = a * a; - sqr_b = b * b; - - /* Calculation */ - L = to_pos->lon - from_pos->lon; - phi1 = from_pos->lat; - phi2 = to_pos->lat; - U1 = atan((1 - f) * tan(phi1)); - U2 = atan((1 - f) * tan(phi2)); - sin_U1 = sin(U1); - sin_U2 = sin(U2); - cos_U1 = cos(U1); - cos_U2 = cos(U2); - - /* Initialize iteration */ - sigma = 0; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - cos_2_sigmam = 0; - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - sqr_cos_alpha = 0; - lambda = L; - sin_lambda = sin(lambda); - cos_lambda = cos(lambda); - delta_lambda = lambda; - remaining_steps = 20; - - while ((delta_lambda > 1e-12) && (remaining_steps > 0)) - { /* Iterate */ - /* Variables */ - float tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; - - /* Calculation */ - tmp1 = cos_U2 * sin_lambda; - tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; - sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); - cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; - tan_sigma = sin_sigma / cos_sigma; - sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; - cos_alpha = cos(asin(sin_alpha)); - sqr_cos_alpha = cos_alpha * cos_alpha; - cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); - lambda_prev = lambda; - sigma = asin(sin_sigma); - lambda = L + - (1 - C) * f * sin_alpha - * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); - delta_lambda = lambda_prev - lambda; - if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; - sin_lambda = sin(lambda); - cos_lambda = cos(lambda); - remaining_steps--; - } /* Iterate */ - - /* More calculation */ - sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; - A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); - B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); - delta_sigma = B * sin_sigma * ( - cos_2_sigmam + B / 4 * ( - cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - - B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) - )); - - /* Calculate result */ - if ( from_azimuth != 0 ) - { - float tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); - *from_azimuth = atan(tan_alpha_1); - } - if ( to_azimuth != 0 ) - { - float tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); - *to_azimuth = atan(tan_alpha_2); - } - - return b * A * (sigma - delta_sigma); -} - -/** - * \brief Horizontal move of point position - */ -int nmea_move_horz( - const nmeaPOS *start_pos, /**< Start position in radians */ - nmeaPOS *end_pos, /**< Result position in radians */ - float azimuth, /**< Azimuth (degree) [0, 359] */ - float distance /**< Distance (km) */ - ) -{ - nmeaPOS p1 = *start_pos; - int RetVal = 1; - - distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ - azimuth = nmea_degree2radian(azimuth); - - end_pos->lat = asin( - sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); - end_pos->lon = p1.lon + atan2( - sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); - - if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) - { - end_pos->lat = 0; end_pos->lon = 0; - RetVal = 0; - } - - return RetVal; -} - -/** - * \brief Horizontal move of point position - * This function uses an algorithm for an oblate spheroid earth model. - * The algorithm is described here: - * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf - */ -int nmea_move_horz_ellipsoid( - const nmeaPOS *start_pos, /**< Start position in radians */ - nmeaPOS *end_pos, /**< (O) Result position in radians */ - float azimuth, /**< Azimuth in radians */ - float distance, /**< Distance (km) */ - float *end_azimuth /**< (O) Azimuth at end position in radians */ - ) -{ - /* Variables */ - float f, a, b, sqr_a, sqr_b; - float phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; - float tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; - float sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; - int remaining_steps; - float tmp1, phi2, lambda, C, L; - - /* Check input */ - //NMEA_ASSERT(start_pos != 0); - //NMEA_ASSERT(end_pos != 0); - - if (fabs(distance) < 1e-12) - { /* No move */ - *end_pos = *start_pos; - if ( end_azimuth != 0 ) *end_azimuth = azimuth; - return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); - } /* No move */ - - /* Earth geometry */ - f = NMEA_EARTH_FLATTENING; - a = NMEA_EARTH_SEMIMAJORAXIS_M; - b = (1 - f) * a; - sqr_a = a * a; - sqr_b = b * b; - - /* Calculation */ - phi1 = start_pos->lat; - tan_U1 = (1 - f) * tan(phi1); - cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); - sin_U1 = tan_U1 * cos_U1; - s = distance; - alpha1 = azimuth; - sin_alpha1 = sin(alpha1); - cos_alpha1 = cos(alpha1); - tan_sigma1 = tan_U1 / cos_alpha1; - sigma1 = atan2(tan_U1, cos_alpha1); - sin_alpha = cos_U1 * sin_alpha1; - sqr_cos_alpha = 1 - sin_alpha * sin_alpha; - cos_alpha = sqrt(sqr_cos_alpha); - sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; - A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); - B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); - - /* Initialize iteration */ - sigma_initial = s / (b * A); - sigma = sigma_initial; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - cos_2_sigmam = cos(2 * sigma1 + sigma); - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - delta_sigma = 0; - sigma_prev = 2 * NMEA_PI; - remaining_steps = 20; - - while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) - { /* Iterate */ - cos_2_sigmam = cos(2 * sigma1 + sigma); - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - delta_sigma = B * sin_sigma * ( - cos_2_sigmam + B / 4 * ( - cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - - B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) - )); - sigma_prev = sigma; - sigma = sigma_initial + delta_sigma; - remaining_steps --; - } /* Iterate */ - - /* Calculate result */ - tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); - phi2 = atan2( - sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, - (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) - ); - lambda = atan2( - sin_sigma * sin_alpha1, - cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 - ); - C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); - L = lambda - - (1 - C) * f * sin_alpha * ( - sigma + C * sin_sigma * - (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) - ); - - /* Result */ - end_pos->lon = start_pos->lon + L; - end_pos->lat = phi2; - if ( end_azimuth != 0 ) - { - *end_azimuth = atan2( - sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 - ); - } - return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); -} - -/** - * \brief Convert position from INFO to radians position - */ -void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos) -{ - pos->lat = nmea_ndeg2radian(info->lat); - pos->lon = nmea_ndeg2radian(info->lon); -} - -/** - * \brief Convert radians position to INFOs position - */ -void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info) -{ - info->lat = nmea_radian2ndeg(pos->lat); - info->lon = nmea_radian2ndeg(pos->lon); -} diff --git a/apps/gps/nmealib/info.c b/apps/gps/nmealib/info.c deleted file mode 100644 index 1d531ffc4..000000000 --- a/apps/gps/nmealib/info.c +++ /dev/null @@ -1,21 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: info.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include - -#include "nmea/info.h" - -void nmea_zero_INFO(nmeaINFO *info) -{ - memset(info, 0, sizeof(nmeaINFO)); - nmea_time_now(&info->utc); - info->sig = NMEA_SIG_BAD; - info->fix = NMEA_FIX_BAD; -} diff --git a/apps/gps/nmealib/nmea/config.h b/apps/gps/nmealib/nmea/config.h deleted file mode 100644 index 501466218..000000000 --- a/apps/gps/nmealib/nmea/config.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#ifndef __NMEA_CONFIG_H__ -#define __NMEA_CONFIG_H__ - -#define NMEA_VERSION ("0.5.3") -#define NMEA_VERSION_MAJOR (0) -#define NMEA_VERSION_MINOR (5) -#define NMEA_VERSION_PATCH (3) - -#define NMEA_CONVSTR_BUF (256) -#define NMEA_TIMEPARSE_BUF (256) - -#if defined(WINCE) || defined(UNDER_CE) -# define NMEA_CE -#endif - -#if defined(WIN32) || defined(NMEA_CE) -# define NMEA_WIN -#else -# define NMEA_UNI -#endif - -#if defined(NMEA_WIN) && (_MSC_VER >= 1400) -# pragma warning(disable: 4996) /* declared deprecated */ -#endif - -#if defined(_MSC_VER) -# define NMEA_POSIX(x) _##x -# define NMEA_INLINE __inline -#else -# define NMEA_POSIX(x) x -# define NMEA_INLINE inline -#endif - -#if !defined(NDEBUG) && !defined(NMEA_CE) -# include -# define NMEA_ASSERT(x) assert(x) -#else -# define NMEA_ASSERT(x) -#endif - -#endif /* __NMEA_CONFIG_H__ */ diff --git a/apps/gps/nmealib/nmea/context.h b/apps/gps/nmealib/nmea/context.h deleted file mode 100644 index 06e558327..000000000 --- a/apps/gps/nmealib/nmea/context.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_CONTEXT_H__ -#define __NMEA_CONTEXT_H__ - -#include "config.h" - -#define NMEA_DEF_PARSEBUFF (1024) -#define NMEA_MIN_PARSEBUFF (256) - -#ifdef __cplusplus -extern "C" { -#endif - -typedef void (*nmeaTraceFunc)(const char *str, int str_size); -typedef void (*nmeaErrorFunc)(const char *str, int str_size); - -typedef struct _nmeaPROPERTY -{ - nmeaTraceFunc trace_func; - nmeaErrorFunc error_func; - int parse_buff_size; - -} nmeaPROPERTY; - -nmeaPROPERTY * nmea_property(void); - -void nmea_trace(const char *str, ...); -void nmea_trace_buff(const char *buff, int buff_size); -void nmea_error(const char *str, ...); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_CONTEXT_H__ */ diff --git a/apps/gps/nmealib/nmea/generate.h b/apps/gps/nmealib/nmea/generate.h deleted file mode 100644 index 9d7fdee51..000000000 --- a/apps/gps/nmealib/nmea/generate.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: generate.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_GENERATE_H__ -#define __NMEA_GENERATE_H__ - -#include "sentence.h" - -#ifdef __cplusplus -extern "C" { -#endif - -int nmea_generate( - char *buff, int buff_sz, /* buffer */ - const nmeaINFO *info, /* source info */ - int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ - ); - -int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack); -int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack); -int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack); -int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack); -int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack); - -void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack); -void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack); -void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack); -void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack); - -int nmea_gsv_npack(int sat_count); -void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_GENERATE_H__ */ diff --git a/apps/gps/nmealib/nmea/generator.h b/apps/gps/nmealib/nmea/generator.h deleted file mode 100644 index a97b91b13..000000000 --- a/apps/gps/nmealib/nmea/generator.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: generator.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_GENERATOR_H__ -#define __NMEA_GENERATOR_H__ - -#include "info.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - * high level - */ - -struct _nmeaGENERATOR; - -enum nmeaGENTYPE -{ - NMEA_GEN_NOISE = 0, - NMEA_GEN_STATIC, - NMEA_GEN_ROTATE, - - NMEA_GEN_SAT_STATIC, - NMEA_GEN_SAT_ROTATE, - NMEA_GEN_POS_RANDMOVE, - - NMEA_GEN_LAST -}; - -struct _nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info); -void nmea_destroy_generator(struct _nmeaGENERATOR *gen); - -int nmea_generate_from( - char *buff, int buff_sz, /* buffer */ - nmeaINFO *info, /* source info */ - struct _nmeaGENERATOR *gen, /* generator */ - int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ - ); - -/* - * low level - */ - -typedef int (*nmeaNMEA_GEN_INIT)(struct _nmeaGENERATOR *gen, nmeaINFO *info); -typedef int (*nmeaNMEA_GEN_LOOP)(struct _nmeaGENERATOR *gen, nmeaINFO *info); -typedef int (*nmeaNMEA_GEN_RESET)(struct _nmeaGENERATOR *gen, nmeaINFO *info); -typedef int (*nmeaNMEA_GEN_DESTROY)(struct _nmeaGENERATOR *gen); - -typedef struct _nmeaGENERATOR -{ - void *gen_data; - nmeaNMEA_GEN_INIT init_call; - nmeaNMEA_GEN_LOOP loop_call; - nmeaNMEA_GEN_RESET reset_call; - nmeaNMEA_GEN_DESTROY destroy_call; - struct _nmeaGENERATOR *next; - -} nmeaGENERATOR; - -int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info); -int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info); -int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info); -void nmea_gen_destroy(nmeaGENERATOR *gen); -void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_GENERATOR_H__ */ diff --git a/apps/gps/nmealib/nmea/gmath.h b/apps/gps/nmealib/nmea/gmath.h deleted file mode 100644 index 3133b7228..000000000 --- a/apps/gps/nmealib/nmea/gmath.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: gmath.h 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#ifndef __NMEA_GMATH_H__ -#define __NMEA_GMATH_H__ - -#include "info.h" - -#define NMEA_PI (3.141592653589793) /**< PI value */ -#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ -#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ -#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ -#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ -#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ -#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ -#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ - -#ifdef __cplusplus -extern "C" { -#endif - -/* - * degree VS radian - */ - -float nmea_degree2radian(float val); -float nmea_radian2degree(float val); - -/* - * NDEG (NMEA degree) - */ - -float nmea_ndeg2degree(float val); -float nmea_degree2ndeg(float val); - -float nmea_ndeg2radian(float val); -float nmea_radian2ndeg(float val); - -/* - * DOP - */ - -float nmea_calc_pdop(float hdop, float vdop); -float nmea_dop2meters(float dop); -float nmea_meters2dop(float meters); - -/* - * positions work - */ - -void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos); -void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info); - -float nmea_distance( - const nmeaPOS *from_pos, - const nmeaPOS *to_pos - ); - -float nmea_distance_ellipsoid( - const nmeaPOS *from_pos, - const nmeaPOS *to_pos, - float *from_azimuth, - float *to_azimuth - ); - -int nmea_move_horz( - const nmeaPOS *start_pos, - nmeaPOS *end_pos, - float azimuth, - float distance - ); - -int nmea_move_horz_ellipsoid( - const nmeaPOS *start_pos, - nmeaPOS *end_pos, - float azimuth, - float distance, - float *end_azimuth - ); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_GMATH_H__ */ diff --git a/apps/gps/nmealib/nmea/info.h b/apps/gps/nmealib/nmea/info.h deleted file mode 100644 index 09ccd4c09..000000000 --- a/apps/gps/nmealib/nmea/info.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: info.h 10 2007-11-15 14:50:15Z xtimor $ - * - */ - -/*! \file */ - -#ifndef __NMEA_INFO_H__ -#define __NMEA_INFO_H__ - -#include "time.h" - -#define NMEA_SIG_BAD (0) -#define NMEA_SIG_LOW (1) -#define NMEA_SIG_MID (2) -#define NMEA_SIG_HIGH (3) - -#define NMEA_FIX_BAD (1) -#define NMEA_FIX_2D (2) -#define NMEA_FIX_3D (3) - -#define NMEA_MAXSAT (12) -#define NMEA_SATINPACK (4) -#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) - -#define NMEA_DEF_LAT (5001.2621) -#define NMEA_DEF_LON (3613.0595) - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Position data in fractional degrees or radians - */ -typedef struct _nmeaPOS -{ - float lat; /**< Latitude */ - float lon; /**< Longitude */ - -} nmeaPOS; - -/** - * Information about satellite - * @see nmeaSATINFO - * @see nmeaGPGSV - */ -typedef struct _nmeaSATELLITE -{ - int id; /**< Satellite PRN number */ - int in_use; /**< Used in position fix */ - int elv; /**< Elevation in degrees, 90 maximum */ - int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ - int sig; /**< Signal, 00-99 dB */ - -} nmeaSATELLITE; - -/** - * Information about all satellites in view - * @see nmeaINFO - * @see nmeaGPGSV - */ -typedef struct _nmeaSATINFO -{ - int inuse; /**< Number of satellites in use (not those in view) */ - int inview; /**< Total number of satellites in view */ - nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ - -} nmeaSATINFO; - -/** - * Summary GPS information from all parsed packets, - * used also for generating NMEA stream - * @see nmea_parse - * @see nmea_GPGGA2info, nmea_...2info - */ -typedef struct _nmeaINFO -{ - int smask; /**< Mask specifying types of packages from which data have been obtained */ - - nmeaTIME utc; /**< UTC of position */ - - int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ - int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ - - float PDOP; /**< Position Dilution Of Precision */ - float HDOP; /**< Horizontal Dilution Of Precision */ - float VDOP; /**< Vertical Dilution Of Precision */ - - float lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ - float lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ - float elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ - float speed; /**< Speed over the ground in kilometers/hour */ - float direction; /**< Track angle in degrees True */ - float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ - - nmeaSATINFO satinfo; /**< Satellites information */ - -} nmeaINFO; - -void nmea_zero_INFO(nmeaINFO *info); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_INFO_H__ */ diff --git a/apps/gps/nmealib/nmea/nmea.h b/apps/gps/nmealib/nmea/nmea.h deleted file mode 100644 index 62692230f..000000000 --- a/apps/gps/nmealib/nmea/nmea.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: nmea.h 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#ifndef __NMEA_H__ -#define __NMEA_H__ - -#include "./config.h" -#include "./units.h" -#include "./gmath.h" -#include "./info.h" -#include "./sentence.h" -#include "./generate.h" -#include "./generator.h" -#include "./parse.h" -#include "./parser.h" -#include "./context.h" - -#endif /* __NMEA_H__ */ diff --git a/apps/gps/nmealib/nmea/parse.h b/apps/gps/nmealib/nmea/parse.h deleted file mode 100644 index 3e6b425db..000000000 --- a/apps/gps/nmealib/nmea/parse.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: parse.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_PARSE_H__ -#define __NMEA_PARSE_H__ - -#include "sentence.h" - -#ifdef __cplusplus -extern "C" { -#endif - -int nmea_pack_type(const char *buff, int buff_sz); -int nmea_find_tail(const char *buff, int buff_sz, int *res_crc); - -int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack); -int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack); -int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack); -int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack); -int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack); - -void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info); -void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info); -void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info); -void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info); -void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_PARSE_H__ */ diff --git a/apps/gps/nmealib/nmea/parser.h b/apps/gps/nmealib/nmea/parser.h deleted file mode 100644 index 51a3fab7f..000000000 --- a/apps/gps/nmealib/nmea/parser.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: parser.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_PARSER_H__ -#define __NMEA_PARSER_H__ - -#include "info.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - * high level - */ - -typedef struct _nmeaPARSER -{ - void *top_node; - void *end_node; - unsigned char *buffer; - int buff_size; - int buff_use; - -} nmeaPARSER; - -int nmea_parser_init(nmeaPARSER *parser); -void nmea_parser_destroy(nmeaPARSER *parser); - -int nmea_parse( - nmeaPARSER *parser, - const char *buff, int buff_sz, - nmeaINFO *info - ); - -/* - * low level - */ - -int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz); -int nmea_parser_top(nmeaPARSER *parser); -int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr); -int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr); -int nmea_parser_drop(nmeaPARSER *parser); -int nmea_parser_buff_clear(nmeaPARSER *parser); -int nmea_parser_queue_clear(nmeaPARSER *parser); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_PARSER_H__ */ diff --git a/apps/gps/nmealib/nmea/sentence.h b/apps/gps/nmealib/nmea/sentence.h deleted file mode 100644 index 2aa975c71..000000000 --- a/apps/gps/nmealib/nmea/sentence.h +++ /dev/null @@ -1,128 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: sentence.h 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -/*! \file */ - -#ifndef __NMEA_SENTENCE_H__ -#define __NMEA_SENTENCE_H__ - -#include "info.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * NMEA packets type which parsed and generated by library - */ -enum nmeaPACKTYPE -{ - GPNON = 0x0000, /**< Unknown packet type. */ - GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ - GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ - GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ - GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ - GPVTG = 0x0010 /**< VTG - Actual track made good and speed over ground. */ -}; - -/** - * GGA packet information structure (Global Positioning System Fix Data) - */ -typedef struct _nmeaGPGGA -{ - nmeaTIME utc; /**< UTC of position (just time) */ - float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ - char ns; /**< [N]orth or [S]outh */ - float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ - char ew; /**< [E]ast or [W]est */ - int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ - int satinuse; /**< Number of satellites in use (not those in view) */ - float HDOP; /**< Horizontal dilution of precision */ - float elv; /**< Antenna altitude above/below mean sea level (geoid) */ - char elv_units; /**< [M]eters (Antenna height unit) */ - float diff; /**< Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. '-' = geoid is below WGS-84 ellipsoid) */ - char diff_units; /**< [M]eters (Units of geoidal separation) */ - float dgps_age; /**< Time in seconds since last DGPS update */ - int dgps_sid; /**< DGPS station ID number */ - -} nmeaGPGGA; - -/** - * GSA packet information structure (Satellite status) - */ -typedef struct _nmeaGPGSA -{ - char fix_mode; /**< Mode (M = Manual, forced to operate in 2D or 3D; A = Automatic, 3D/2D) */ - int fix_type; /**< Type, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ - int sat_prn[NMEA_MAXSAT]; /**< PRNs of satellites used in position fix (null for unused fields) */ - float PDOP; /**< Dilution of precision */ - float HDOP; /**< Horizontal dilution of precision */ - float VDOP; /**< Vertical dilution of precision */ - -} nmeaGPGSA; - -/** - * GSV packet information structure (Satellites in view) - */ -typedef struct _nmeaGPGSV -{ - int pack_count; /**< Total number of messages of this type in this cycle */ - int pack_index; /**< Message number */ - int sat_count; /**< Total number of satellites in view */ - nmeaSATELLITE sat_data[NMEA_SATINPACK]; - -} nmeaGPGSV; - -/** - * RMC packet information structure (Recommended Minimum sentence C) - */ -typedef struct _nmeaGPRMC -{ - nmeaTIME utc; /**< UTC of position */ - char status; /**< Status (A = active or V = void) */ - float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ - char ns; /**< [N]orth or [S]outh */ - float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ - char ew; /**< [E]ast or [W]est */ - float speed; /**< Speed over the ground in knots */ - float direction; /**< Track angle in degrees True */ - float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ - char declin_ew; /**< [E]ast or [W]est */ - char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ - -} nmeaGPRMC; - -/** - * VTG packet information structure (Track made good and ground speed) - */ -typedef struct _nmeaGPVTG -{ - float dir; /**< True track made good (degrees) */ - char dir_t; /**< Fixed text 'T' indicates that track made good is relative to true north */ - float dec; /**< Magnetic track made good */ - char dec_m; /**< Fixed text 'M' */ - float spn; /**< Ground speed, knots */ - char spn_n; /**< Fixed text 'N' indicates that speed over ground is in knots */ - float spk; /**< Ground speed, kilometers per hour */ - char spk_k; /**< Fixed text 'K' indicates that speed over ground is in kilometers/hour */ - -} nmeaGPVTG; - -void nmea_zero_GPGGA(nmeaGPGGA *pack); -void nmea_zero_GPGSA(nmeaGPGSA *pack); -void nmea_zero_GPGSV(nmeaGPGSV *pack); -void nmea_zero_GPRMC(nmeaGPRMC *pack); -void nmea_zero_GPVTG(nmeaGPVTG *pack); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_SENTENCE_H__ */ diff --git a/apps/gps/nmealib/nmea/time.h b/apps/gps/nmealib/nmea/time.h deleted file mode 100644 index bbe59f65c..000000000 --- a/apps/gps/nmealib/nmea/time.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -/*! \file */ - -#ifndef __NMEA_TIME_H__ -#define __NMEA_TIME_H__ - -#include "config.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Date and time data - * @see nmea_time_now - */ -typedef struct _nmeaTIME -{ - int year; /**< Years since 1900 */ - int mon; /**< Months since January - [0,11] */ - int day; /**< Day of the month - [1,31] */ - int hour; /**< Hours since midnight - [0,23] */ - int min; /**< Minutes after the hour - [0,59] */ - int sec; /**< Seconds after the minute - [0,59] */ - int hsec; /**< Hundredth part of second - [0,99] */ - -} nmeaTIME; - -/** - * \brief Get time now to nmeaTIME structure - */ -void nmea_time_now(nmeaTIME *t); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_TIME_H__ */ diff --git a/apps/gps/nmealib/nmea/tok.h b/apps/gps/nmealib/nmea/tok.h deleted file mode 100644 index 8f948dd2d..000000000 --- a/apps/gps/nmealib/nmea/tok.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: tok.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_TOK_H__ -#define __NMEA_TOK_H__ - -#include "config.h" -#include - -#ifdef __cplusplus -extern "C" { -#endif - -int nmea_calc_crc(const char *buff, int buff_sz); -int nmea_atoi(const char *str, int str_sz, int radix); -float nmea_atof(const char *str, int str_sz); -int nmea_printf(char *buff, int buff_sz, const char *format, ...); -int nmea_scanf(const char *buff, int buff_sz, const char *format, ...); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_TOK_H__ */ diff --git a/apps/gps/nmealib/nmea/units.h b/apps/gps/nmealib/nmea/units.h deleted file mode 100644 index 767f980e2..000000000 --- a/apps/gps/nmealib/nmea/units.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: units.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_UNITS_H__ -#define __NMEA_UNITS_H__ - -#include "config.h" - -/* - * Distance units - */ - -#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ -#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ -#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ - -/* - * Speed units - */ - -#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ - -#endif /* __NMEA_UNITS_H__ */ diff --git a/apps/gps/nmealib/parse.c b/apps/gps/nmealib/parse.c deleted file mode 100644 index 99bdf075b..000000000 --- a/apps/gps/nmealib/parse.c +++ /dev/null @@ -1,501 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -/** - * \file parse.h - * \brief Functions of a low level for analysis of - * packages of NMEA stream. - * - * \code - * ... - * ptype = nmea_pack_type( - * (const char *)parser->buffer + nparsed + 1, - * parser->buff_use - nparsed - 1); - * - * if(0 == (node = malloc(sizeof(nmeaParserNODE)))) - * goto mem_fail; - * - * node->pack = 0; - * - * switch(ptype) - * { - * case GPGGA: - * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) - * goto mem_fail; - * node->packType = GPGGA; - * if(!nmea_parse_GPGGA( - * (const char *)parser->buffer + nparsed, - * sen_sz, (nmeaGPGGA *)node->pack)) - * { - * free(node); - * node = 0; - * } - * break; - * case GPGSA: - * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) - * goto mem_fail; - * node->packType = GPGSA; - * if(!nmea_parse_GPGSA( - * (const char *)parser->buffer + nparsed, - * sen_sz, (nmeaGPGSA *)node->pack)) - * { - * free(node); - * node = 0; - * } - * break; - * ... - * \endcode - */ -#include "nmea/tok.h" -#include "nmea/parse.h" -#include "nmea/context.h" -#include "nmea/gmath.h" -#include "nmea/units.h" - -#include -#include - -int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res) -{ - int success = 0; - - switch(buff_sz) - { - case sizeof("hhmmss") - 1: - success = (3 == nmea_scanf(buff, buff_sz, - "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec) - )); - break; - case sizeof("hhmmss.s") - 1: - case sizeof("hhmmss.ss") - 1: - case sizeof("hhmmss.sss") - 1: - success = (4 == nmea_scanf(buff, buff_sz, - "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec) - )); - break; - default: - nmea_error("Parse of time error (format error)!"); - success = 0; - break; - } - - return (success?0:-1); -} - -/** - * \brief Define packet type by header (nmeaPACKTYPE). - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @return The defined packet type - * @see nmeaPACKTYPE - */ -int nmea_pack_type(const char *buff, int buff_sz) -{ - static const char *pheads[] = { - "GPGGA", - "GPGSA", - "GPGSV", - "GPRMC", - "GPVTG", - }; - - //NMEA_ASSERT(buff); - - if(buff_sz < 5) - return GPNON; - else if(0 == memcmp(buff, pheads[0], 5)) - return GPGGA; - else if(0 == memcmp(buff, pheads[1], 5)) - return GPGSA; - else if(0 == memcmp(buff, pheads[2], 5)) - return GPGSV; - else if(0 == memcmp(buff, pheads[3], 5)) - return GPRMC; - else if(0 == memcmp(buff, pheads[4], 5)) - return GPVTG; - - return GPNON; -} - -/** - * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC). - * @param buff a constant character pointer of packets buffer. - * @param buff_sz buffer size. - * @param res_crc a integer pointer for return CRC of packet (must be defined). - * @return Number of bytes to packet tail. - */ -int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) -{ - static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; - - const char *end_buff = buff + buff_sz; - int nread = 0; - int crc = 0; - - //NMEA_ASSERT(buff && res_crc); - - *res_crc = -1; - - for(;buff < end_buff; ++buff, ++nread) - { - if(('$' == *buff) && nread) - { - buff = 0; - break; - } - else if('*' == *buff) - { - if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) - { - *res_crc = nmea_atoi(buff + 1, 2, 16); - nread = buff_sz - (int)(end_buff - (buff + tail_sz)); - if(*res_crc != crc) - { - *res_crc = -1; - buff = 0; - } - } - - break; - } - else if(nread) - crc ^= (int)*buff; - } - - if(*res_crc < 0 && buff) - nread = 0; - - return nread; -} - -/** - * \brief Parse GGA packet from buffer. - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @param pack a pointer of packet which will filled by function. - * @return 1 (true) - if parsed successfully or 0 (false) - if fail. - */ -int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack) -{ - char time_buff[NMEA_TIMEPARSE_BUF]; - - //NMEA_ASSERT(buff && pack); - - memset(pack, 0, sizeof(nmeaGPGGA)); - - nmea_trace_buff(buff, buff_sz); - - if(14 != nmea_scanf(buff, buff_sz, - "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*", - &(time_buff[0]), - &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), - &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units), - &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid))) - { - nmea_error("GPGGA parse error!"); - return 0; - } - - if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) - { - nmea_error("GPGGA time parse error!"); - return 0; - } - - return 1; -} - -/** - * \brief Parse GSA packet from buffer. - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @param pack a pointer of packet which will filled by function. - * @return 1 (true) - if parsed successfully or 0 (false) - if fail. - */ -int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack) -{ - //NMEA_ASSERT(buff && pack); - - memset(pack, 0, sizeof(nmeaGPGSA)); - - nmea_trace_buff(buff, buff_sz); - - if(17 != nmea_scanf(buff, buff_sz, - "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*", - &(pack->fix_mode), &(pack->fix_type), - &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]), - &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]), - &(pack->PDOP), &(pack->HDOP), &(pack->VDOP))) - { - nmea_error("GPGSA parse error!"); - return 0; - } - - return 1; -} - -/** - * \brief Parse GSV packet from buffer. - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @param pack a pointer of packet which will filled by function. - * @return 1 (true) - if parsed successfully or 0 (false) - if fail. - */ -int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack) -{ - int nsen, nsat; - - //NMEA_ASSERT(buff && pack); - - memset(pack, 0, sizeof(nmeaGPGSV)); - - nmea_trace_buff(buff, buff_sz); - - nsen = nmea_scanf(buff, buff_sz, - "$GPGSV,%d,%d,%d," - "%d,%d,%d,%d," - "%d,%d,%d,%d," - "%d,%d,%d,%d," - "%d,%d,%d,%d*", - &(pack->pack_count), &(pack->pack_index), &(pack->sat_count), - &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig), - &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig), - &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig), - &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig)); - - nsat = (pack->pack_index - 1) * NMEA_SATINPACK; - nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; - nsat = nsat * 4 + 3 /* first three sentence`s */; - - if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3)) - { - nmea_error("GPGSV parse error!"); - return 0; - } - - return 1; -} - -/** - * \brief Parse RMC packet from buffer. - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @param pack a pointer of packet which will filled by function. - * @return 1 (true) - if parsed successfully or 0 (false) - if fail. - */ -int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack) -{ - int nsen; - char time_buff[NMEA_TIMEPARSE_BUF]; - - //NMEA_ASSERT(buff && pack); - - memset(pack, 0, sizeof(nmeaGPRMC)); - - nmea_trace_buff(buff, buff_sz); - - nsen = nmea_scanf(buff, buff_sz, - "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*", - &(time_buff[0]), - &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), - &(pack->speed), &(pack->direction), - &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), - &(pack->declination), &(pack->declin_ew), &(pack->mode)); - - if(nsen != 13 && nsen != 14) - { - nmea_error("GPRMC parse error!"); - return 0; - } - - if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) - { - nmea_error("GPRMC time parse error!"); - return 0; - } - - if(pack->utc.year < 90) - pack->utc.year += 100; - pack->utc.mon -= 1; - - return 1; -} - -/** - * \brief Parse VTG packet from buffer. - * @param buff a constant character pointer of packet buffer. - * @param buff_sz buffer size. - * @param pack a pointer of packet which will filled by function. - * @return 1 (true) - if parsed successfully or 0 (false) - if fail. - */ -int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack) -{ - //NMEA_ASSERT(buff && pack); - - memset(pack, 0, sizeof(nmeaGPVTG)); - - nmea_trace_buff(buff, buff_sz); - - if(8 != nmea_scanf(buff, buff_sz, - "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*", - &(pack->dir), &(pack->dir_t), - &(pack->dec), &(pack->dec_m), - &(pack->spn), &(pack->spn_n), - &(pack->spk), &(pack->spk_k))) - { - nmea_error("GPVTG parse error!"); - return 0; - } - - if( pack->dir_t != 'T' || - pack->dec_m != 'M' || - pack->spn_n != 'N' || - pack->spk_k != 'K') - { - nmea_error("GPVTG parse error (format error)!"); - return 0; - } - - return 1; -} - -/** - * \brief Fill nmeaINFO structure by GGA packet data. - * @param pack a pointer of packet structure. - * @param info a pointer of summary information structure. - */ -void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info) -{ - //NMEA_ASSERT(pack && info); - - info->utc.hour = pack->utc.hour; - info->utc.min = pack->utc.min; - info->utc.sec = pack->utc.sec; - info->utc.hsec = pack->utc.hsec; - info->sig = pack->sig; - info->HDOP = pack->HDOP; - info->elv = pack->elv; - info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); - info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); - info->smask |= GPGGA; -} - -/** - * \brief Fill nmeaINFO structure by GSA packet data. - * @param pack a pointer of packet structure. - * @param info a pointer of summary information structure. - */ -void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info) -{ - int i, j, nuse = 0; - - //NMEA_ASSERT(pack && info); - - info->fix = pack->fix_type; - info->PDOP = pack->PDOP; - info->HDOP = pack->HDOP; - info->VDOP = pack->VDOP; - - for(i = 0; i < NMEA_MAXSAT; ++i) - { - for(j = 0; j < info->satinfo.inview; ++j) - { - if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id) - { - info->satinfo.sat[j].in_use = 1; - nuse++; - } - } - } - - info->satinfo.inuse = nuse; - info->smask |= GPGSA; -} - -/** - * \brief Fill nmeaINFO structure by GSV packet data. - * @param pack a pointer of packet structure. - * @param info a pointer of summary information structure. - */ -void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info) -{ - int isat, isi, nsat; - - //NMEA_ASSERT(pack && info); - - if(pack->pack_index > pack->pack_count || - pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT) - return; - - if(pack->pack_index < 1) - pack->pack_index = 1; - - info->satinfo.inview = pack->sat_count; - - nsat = (pack->pack_index - 1) * NMEA_SATINPACK; - nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; - - for(isat = 0; isat < nsat; ++isat) - { - isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat; - info->satinfo.sat[isi].id = pack->sat_data[isat].id; - info->satinfo.sat[isi].elv = pack->sat_data[isat].elv; - info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth; - info->satinfo.sat[isi].sig = pack->sat_data[isat].sig; - } - - info->smask |= GPGSV; -} - -/** - * \brief Fill nmeaINFO structure by RMC packet data. - * @param pack a pointer of packet structure. - * @param info a pointer of summary information structure. - */ -void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info) -{ - //NMEA_ASSERT(pack && info); - - if('A' == pack->status) - { - if(NMEA_SIG_BAD == info->sig) - info->sig = NMEA_SIG_MID; - if(NMEA_FIX_BAD == info->fix) - info->fix = NMEA_FIX_2D; - } - else if('V' == pack->status) - { - info->sig = NMEA_SIG_BAD; - info->fix = NMEA_FIX_BAD; - } - - - info->utc = pack->utc; - info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); - info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); - info->speed = pack->speed * NMEA_TUD_KNOTS; - info->direction = pack->direction; - info->smask |= GPRMC; -} - -/** - * \brief Fill nmeaINFO structure by VTG packet data. - * @param pack a pointer of packet structure. - * @param info a pointer of summary information structure. - */ -void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info) -{ - //NMEA_ASSERT(pack && info); - - info->direction = pack->dir; - info->declination = pack->dec; - info->speed = pack->spk; - info->smask |= GPVTG; -} diff --git a/apps/gps/nmealib/parser.c b/apps/gps/nmealib/parser.c deleted file mode 100644 index f8b415866..000000000 --- a/apps/gps/nmealib/parser.c +++ /dev/null @@ -1,400 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: parser.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -/** - * \file parser.h - */ -#include "nmea/tok.h" -#include "nmea/parse.h" -#include "nmea/parser.h" -#include "nmea/context.h" - -#include -#include - -typedef struct _nmeaParserNODE -{ - int packType; - void *pack; - struct _nmeaParserNODE *next_node; - -} nmeaParserNODE; - -/* - * high level - */ - -/** - * \brief Initialization of parser object - * @return true (1) - success or false (0) - fail - */ -int nmea_parser_init(nmeaPARSER *parser) -{ - int resv = 0; - int buff_size = nmea_property()->parse_buff_size; - - //NMEA_ASSERT(parser); - - if(buff_size < NMEA_MIN_PARSEBUFF) - buff_size = NMEA_MIN_PARSEBUFF; - - memset(parser, 0, sizeof(nmeaPARSER)); - - if(0 == (parser->buffer = malloc(buff_size))) - nmea_error("Insufficient memory!"); - else - { - parser->buff_size = buff_size; - resv = 1; - } - - return resv; -} - -/** - * \brief Destroy parser object - */ -void nmea_parser_destroy(nmeaPARSER *parser) -{ - //NMEA_ASSERT(parser && parser->buffer); - free(parser->buffer); - nmea_parser_queue_clear(parser); - memset(parser, 0, sizeof(nmeaPARSER)); -} - -/** - * \brief Analysis of buffer and put results to information structure - * @return Number of packets wos parsed - */ -int nmea_parse( - nmeaPARSER *parser, - const char *buff, int buff_sz, - nmeaINFO *info - ) -{ - int ptype, nread = 0; - void *pack = 0; - - //NMEA_ASSERT(parser && parser->buffer); - - nmea_parser_push(parser, buff, buff_sz); - - while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) - { - nread++; - - switch(ptype) - { - case GPGGA: - nmea_GPGGA2info((nmeaGPGGA *)pack, info); - break; - case GPGSA: - nmea_GPGSA2info((nmeaGPGSA *)pack, info); - break; - case GPGSV: - nmea_GPGSV2info((nmeaGPGSV *)pack, info); - break; - case GPRMC: - nmea_GPRMC2info((nmeaGPRMC *)pack, info); - break; - case GPVTG: - nmea_GPVTG2info((nmeaGPVTG *)pack, info); - break; - }; - - free(pack); - } - - return nread; -} - -/* - * low level - */ - -int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) -{ - int nparsed = 0, crc, sen_sz, ptype; - nmeaParserNODE *node = 0; - - //NMEA_ASSERT(parser && parser->buffer); - - /* clear unuse buffer (for debug) */ - /* - memset( - parser->buffer + parser->buff_use, 0, - parser->buff_size - parser->buff_use - ); - */ - - /* add */ - if(parser->buff_use + buff_sz >= parser->buff_size) - nmea_parser_buff_clear(parser); - - memcpy(parser->buffer + parser->buff_use, buff, buff_sz); - parser->buff_use += buff_sz; - - /* parse */ - for(;;node = 0) - { - sen_sz = nmea_find_tail( - (const char *)parser->buffer + nparsed, - (int)parser->buff_use - nparsed, &crc); - - if(!sen_sz) - { - if(nparsed) - memcpy( - parser->buffer, - parser->buffer + nparsed, - parser->buff_use -= nparsed); - break; - } - else if(crc >= 0) - { - ptype = nmea_pack_type( - (const char *)parser->buffer + nparsed + 1, - parser->buff_use - nparsed - 1); - - if(0 == (node = malloc(sizeof(nmeaParserNODE)))) - goto mem_fail; - - node->pack = 0; - - switch(ptype) - { - case GPGGA: - if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) - goto mem_fail; - node->packType = GPGGA; - if(!nmea_parse_GPGGA( - (const char *)parser->buffer + nparsed, - sen_sz, (nmeaGPGGA *)node->pack)) - { - free(node); - node = 0; - } - break; - case GPGSA: - if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) - goto mem_fail; - node->packType = GPGSA; - if(!nmea_parse_GPGSA( - (const char *)parser->buffer + nparsed, - sen_sz, (nmeaGPGSA *)node->pack)) - { - free(node); - node = 0; - } - break; - case GPGSV: - if(0 == (node->pack = malloc(sizeof(nmeaGPGSV)))) - goto mem_fail; - node->packType = GPGSV; - if(!nmea_parse_GPGSV( - (const char *)parser->buffer + nparsed, - sen_sz, (nmeaGPGSV *)node->pack)) - { - free(node); - node = 0; - } - break; - case GPRMC: - if(0 == (node->pack = malloc(sizeof(nmeaGPRMC)))) - goto mem_fail; - node->packType = GPRMC; - if(!nmea_parse_GPRMC( - (const char *)parser->buffer + nparsed, - sen_sz, (nmeaGPRMC *)node->pack)) - { - free(node); - node = 0; - } - break; - case GPVTG: - if(0 == (node->pack = malloc(sizeof(nmeaGPVTG)))) - goto mem_fail; - node->packType = GPVTG; - if(!nmea_parse_GPVTG( - (const char *)parser->buffer + nparsed, - sen_sz, (nmeaGPVTG *)node->pack)) - { - free(node); - node = 0; - } - break; - default: - free(node); - node = 0; - break; - }; - - if(node) - { - if(parser->end_node) - ((nmeaParserNODE *)parser->end_node)->next_node = node; - parser->end_node = node; - if(!parser->top_node) - parser->top_node = node; - node->next_node = 0; - } - } - - nparsed += sen_sz; - } - - return nparsed; - -mem_fail: - if(node) - free(node); - - nmea_error("Insufficient memory!"); - - return -1; -} - -/** - * \brief Analysis of buffer and keep results into parser - * @return Number of bytes wos parsed from buffer - */ -int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz) -{ - int nparse, nparsed = 0; - - do - { - if(buff_sz > parser->buff_size) - nparse = parser->buff_size; - else - nparse = buff_sz; - - nparsed += nmea_parser_real_push( - parser, buff, nparse); - - buff_sz -= nparse; - - } while(buff_sz); - - return nparsed; -} - -/** - * \brief Get type of top packet keeped into parser - * @return Type of packet - * @see nmeaPACKTYPE - */ -int nmea_parser_top(nmeaPARSER *parser) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; - - //NMEA_ASSERT(parser && parser->buffer); - - if(node) - retval = node->packType; - - return retval; -} - -/** - * \brief Withdraw top packet from parser - * @return Received packet type - * @see nmeaPACKTYPE - */ -int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; - - //NMEA_ASSERT(parser && parser->buffer); - - if(node) - { - *pack_ptr = node->pack; - retval = node->packType; - parser->top_node = node->next_node; - if(!parser->top_node) - parser->end_node = 0; - free(node); - } - - return retval; -} - -/** - * \brief Get top packet from parser without withdraw - * @return Received packet type - * @see nmeaPACKTYPE - */ -int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; - - //NMEA_ASSERT(parser && parser->buffer); - - if(node) - { - *pack_ptr = node->pack; - retval = node->packType; - } - - return retval; -} - -/** - * \brief Delete top packet from parser - * @return Deleted packet type - * @see nmeaPACKTYPE - */ -int nmea_parser_drop(nmeaPARSER *parser) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; - - //NMEA_ASSERT(parser && parser->buffer); - - if(node) - { - if(node->pack) - free(node->pack); - retval = node->packType; - parser->top_node = node->next_node; - if(!parser->top_node) - parser->end_node = 0; - free(node); - } - - return retval; -} - -/** - * \brief Clear cache of parser - * @return true (1) - success - */ -int nmea_parser_buff_clear(nmeaPARSER *parser) -{ - //NMEA_ASSERT(parser && parser->buffer); - parser->buff_use = 0; - return 1; -} - -/** - * \brief Clear packets queue into parser - * @return true (1) - success - */ -int nmea_parser_queue_clear(nmeaPARSER *parser) -{ - //NMEA_ASSERT(parser); - while(parser->top_node) - nmea_parser_drop(parser); - return 1; -} diff --git a/apps/gps/nmealib/sentence.c b/apps/gps/nmealib/sentence.c deleted file mode 100644 index a66393a4d..000000000 --- a/apps/gps/nmealib/sentence.c +++ /dev/null @@ -1,54 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: sentence.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include "nmea/sentence.h" - -#include - -void nmea_zero_GPGGA(nmeaGPGGA *pack) -{ - memset(pack, 0, sizeof(nmeaGPGGA)); - nmea_time_now(&pack->utc); - pack->ns = 'N'; - pack->ew = 'E'; - pack->elv_units = 'M'; - pack->diff_units = 'M'; -} - -void nmea_zero_GPGSA(nmeaGPGSA *pack) -{ - memset(pack, 0, sizeof(nmeaGPGSA)); - pack->fix_mode = 'A'; - pack->fix_type = NMEA_FIX_BAD; -} - -void nmea_zero_GPGSV(nmeaGPGSV *pack) -{ - memset(pack, 0, sizeof(nmeaGPGSV)); -} - -void nmea_zero_GPRMC(nmeaGPRMC *pack) -{ - memset(pack, 0, sizeof(nmeaGPRMC)); - nmea_time_now(&pack->utc); - pack->status = 'V'; - pack->ns = 'N'; - pack->ew = 'E'; - pack->declin_ew = 'E'; -} - -void nmea_zero_GPVTG(nmeaGPVTG *pack) -{ - memset(pack, 0, sizeof(nmeaGPVTG)); - pack->dir_t = 'T'; - pack->dec_m = 'M'; - pack->spn_n = 'N'; - pack->spk_k = 'K'; -} diff --git a/apps/gps/nmealib/time.c b/apps/gps/nmealib/time.c deleted file mode 100644 index a24319630..000000000 --- a/apps/gps/nmealib/time.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: time.c 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -/*! \file time.h */ - -#include "nmea/time.h" - -#ifdef NMEA_WIN -# pragma warning(disable: 4201) -# pragma warning(disable: 4214) -# pragma warning(disable: 4115) -# include -# pragma warning(default: 4201) -# pragma warning(default: 4214) -# pragma warning(default: 4115) -#else -# include -#endif - -#ifdef NMEA_WIN - -void nmea_time_now(nmeaTIME *stm) -{ - SYSTEMTIME st; - - GetSystemTime(&st); - - stm->year = st.wYear - 1900; - stm->mon = st.wMonth - 1; - stm->day = st.wDay; - stm->hour = st.wHour; - stm->min = st.wMinute; - stm->sec = st.wSecond; - stm->hsec = st.wMilliseconds / 10; -} - -#else /* NMEA_WIN */ - -void nmea_time_now(nmeaTIME *stm) -{ - time_t lt; - struct tm *tt; - - time(<); - tt = gmtime(<); - - stm->year = tt->tm_year; - stm->mon = tt->tm_mon; - stm->day = tt->tm_mday; - stm->hour = tt->tm_hour; - stm->min = tt->tm_min; - stm->sec = tt->tm_sec; - stm->hsec = 0; -} - -#endif diff --git a/apps/gps/nmealib/tok.c b/apps/gps/nmealib/tok.c deleted file mode 100644 index c7cf2f4dc..000000000 --- a/apps/gps/nmealib/tok.c +++ /dev/null @@ -1,267 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -/*! \file tok.h */ - -#include "nmea/tok.h" - -#include -#include -#include -#include -#include -#include - -#define NMEA_TOKS_COMPARE (1) -#define NMEA_TOKS_PERCENT (2) -#define NMEA_TOKS_WIDTH (3) -#define NMEA_TOKS_TYPE (4) - -//memchr replacement -void * -memchr(s, c, n) - const void *s; - unsigned char c; - size_t n; -{ - if (n != 0) { - const unsigned char *p = s; - - do { - if (*p++ == c) - return ((void *)(p - 1)); - } while (--n != 0); - } - return (NULL); -} - - -/** - * \brief Calculate control sum of binary buffer - */ -int nmea_calc_crc(const char *buff, int buff_sz) -{ - int chsum = 0, - it; - - for(it = 0; it < buff_sz; ++it) - chsum ^= (int)buff[it]; - - return chsum; -} - -/** - * \brief Convert string to number - */ -int nmea_atoi(const char *str, int str_sz, int radix) -{ - char *tmp_ptr; - char buff[NMEA_CONVSTR_BUF]; - int res = 0; - - if(str_sz < NMEA_CONVSTR_BUF) - { - memcpy(&buff[0], str, str_sz); - buff[str_sz] = '\0'; - res = strtol(&buff[0], &tmp_ptr, radix); - } - return res; -} - -/** - * \brief Convert string to fraction number - */ -float nmea_atof(const char *str, int str_sz) -{ - char *tmp_ptr; - char buff[NMEA_CONVSTR_BUF]; - float res = 0; - - if(str_sz < NMEA_CONVSTR_BUF) - { - memcpy(&buff[0], str, str_sz); - buff[str_sz] = '\0'; - res = (float)strtod(&buff[0], &tmp_ptr); - } - return res; -} - -/** - * \brief Formating string (like standart printf) with CRC tail (*CRC) - */ -int nmea_printf(char *buff, int buff_sz, const char *format, ...) -{ - int retval, add = 0; - va_list arg_ptr; - - if(buff_sz <= 0) - return 0; - - va_start(arg_ptr, format); - - retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); - - if(retval > 0) - { - add = NMEA_POSIX(snprintf)( - buff + retval, buff_sz - retval, "*%02x\r\n", - nmea_calc_crc(buff + 1, retval - 1)); - } - - retval += add; - - if(retval < 0 || retval > buff_sz) - { - memset(buff, ' ', buff_sz); - retval = buff_sz; - } - - va_end(arg_ptr); - - return retval; -} - -/** - * \brief Analyse string (specificate for NMEA sentences) - */ -int nmea_scanf(const char *buff, int buff_sz, const char *format, ...) -{ - const char *beg_tok; - const char *end_buf = buff + buff_sz; - - va_list arg_ptr; - int tok_type = NMEA_TOKS_COMPARE; - int width = 0; - const char *beg_fmt = 0; - int snum = 0, unum = 0; - - int tok_count = 0; - void *parg_target; - - va_start(arg_ptr, format); - - for(; *format && buff < end_buf; ++format) - { - switch(tok_type) - { - case NMEA_TOKS_COMPARE: - if('%' == *format) - tok_type = NMEA_TOKS_PERCENT; - else if(*buff++ != *format) - goto fail; - break; - case NMEA_TOKS_PERCENT: - width = 0; - beg_fmt = format; - tok_type = NMEA_TOKS_WIDTH; - case NMEA_TOKS_WIDTH: - if(isdigit(*format)) - break; - { - tok_type = NMEA_TOKS_TYPE; - if(format > beg_fmt) - width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10); - } - case NMEA_TOKS_TYPE: - beg_tok = buff; - - if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) - width = 1; - - if(width) - { - if(buff + width <= end_buf) - buff += width; - else - goto fail; - } - else - { - if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) - buff = end_buf; - } - - if(buff > end_buf) - goto fail; - - tok_type = NMEA_TOKS_COMPARE; - tok_count++; - - parg_target = 0; width = (int)(buff - beg_tok); - - switch(*format) - { - case 'c': - case 'C': - parg_target = (void *)va_arg(arg_ptr, char *); - if(width && 0 != (parg_target)) - *((char *)parg_target) = *beg_tok; - break; - case 's': - case 'S': - parg_target = (void *)va_arg(arg_ptr, char *); - if(width && 0 != (parg_target)) - { - memcpy(parg_target, beg_tok, width); - ((char *)parg_target)[width] = '\0'; - } - break; - case 'f': - case 'g': - case 'G': - case 'e': - case 'E': - parg_target = (void *)va_arg(arg_ptr, float *); - if(width && 0 != (parg_target)) - *((float *)parg_target) = nmea_atof(beg_tok, width); - break; - }; - - if(parg_target) - break; - if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) - break; - if(!width) - break; - - switch(*format) - { - case 'd': - case 'i': - snum = nmea_atoi(beg_tok, width, 10); - memcpy(parg_target, &snum, sizeof(int)); - break; - case 'u': - unum = nmea_atoi(beg_tok, width, 10); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - case 'x': - case 'X': - unum = nmea_atoi(beg_tok, width, 16); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - case 'o': - unum = nmea_atoi(beg_tok, width, 8); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - default: - goto fail; - }; - - break; - }; - } - -fail: - - va_end(arg_ptr); - - return tok_count; -} diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c deleted file mode 100644 index b9f8a28a0..000000000 --- a/apps/gps/ubx.c +++ /dev/null @@ -1,1022 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file U-Blox protocol implementation */ - - -#include "ubx.h" -#include "gps.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2 -#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3 -#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4 - -#define UBX_BUFFER_SIZE 500 - -extern bool gps_mode_try_all; -extern bool gps_mode_success; -extern bool terminate_gps_thread; -extern bool gps_baud_try_all; -extern bool gps_verbose; -extern int current_gps_speed; - -pthread_mutex_t *ubx_mutex; -gps_bin_ubx_state_t *ubx_state; -enum UBX_CONFIG_STATE ubx_config_state; -static struct vehicle_gps_position_s *ubx_gps; - - - -void ubx_decode_init(void) -{ - ubx_state->ck_a = 0; - ubx_state->ck_b = 0; - ubx_state->rx_count = 0; - ubx_state->decode_state = UBX_DECODE_UNINIT; - ubx_state->message_class = CLASS_UNKNOWN; - ubx_state->message_id = ID_UNKNOWN; - ubx_state->payload_size = 0; - ubx_state->print_errors = false; -} - -void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) -{ - *(ck_a) = *(ck_a) + b; - *(ck_b) = *(ck_b) + *(ck_a); -} - - - - - -int ubx_parse(uint8_t b, char *gps_rx_buffer) -{ - //printf("b=%x\n",b); - if (ubx_state->decode_state == UBX_DECODE_UNINIT) { - - if (b == UBX_SYNC_1) { - ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; - } - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { - if (b == UBX_SYNC_2) { - ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; - - } else { - // Second start symbol was wrong, reset state machine - ubx_decode_init(); - } - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC2) { - // Add to checksum - ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); - - //check for known class - switch (b) { - case UBX_CLASS_ACK: - ubx_state->decode_state = UBX_DECODE_GOT_CLASS; - ubx_state->message_class = ACK; - break; - - case UBX_CLASS_NAV: - ubx_state->decode_state = UBX_DECODE_GOT_CLASS; - ubx_state->message_class = NAV; - break; - - case UBX_CLASS_RXM: - ubx_state->decode_state = UBX_DECODE_GOT_CLASS; - ubx_state->message_class = RXM; - break; - - case UBX_CLASS_CFG: - ubx_state->decode_state = UBX_DECODE_GOT_CLASS; - ubx_state->message_class = CFG; - break; - default: //unknown class: reset state machine - ubx_decode_init(); - break; - } - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_CLASS) { - // Add to checksum - ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); - - //depending on class look for message id - switch (ubx_state->message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: //NAV-POSLLH: Geodetic Position Solution - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_POSLLH; - break; - - case UBX_MESSAGE_NAV_SOL: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_SOL; - break; - - case UBX_MESSAGE_NAV_TIMEUTC: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_TIMEUTC; - break; - - case UBX_MESSAGE_NAV_DOP: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_DOP; - break; - - case UBX_MESSAGE_NAV_SVINFO: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - ubx_decode_init(); - break; - } - - break; - - case RXM: - switch (b) { - case UBX_MESSAGE_RXM_SVSI: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = RXM_SVSI; - break; - - default: //unknown class: reset state machine, should not happen - ubx_decode_init(); - break; - } - break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - ubx_decode_init(); - break; - } - break; - - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; - ubx_state->message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - ubx_decode_init(); - break; - } - break; - default: //should not happen - ubx_decode_init(); - break; - } - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_MESSAGEID) { - // Add to checksum - ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); - - ubx_state->payload_size = b; - ubx_state->decode_state = UBX_DECODE_GOT_LENGTH1; - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH1) { - // Add to checksum - ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); - - ubx_state->payload_size += b << 8; - ubx_state->decode_state = UBX_DECODE_GOT_LENGTH2; - - } else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH2) { - uint8_t ret = 0; - - // Add to checksum if not yet at checksum byte - if (ubx_state->rx_count < ubx_state->payload_size) ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); - - // Fill packet buffer - gps_rx_buffer[ubx_state->rx_count] = b; - - //if whole payload + checksum is in buffer: - if (ubx_state->rx_count >= ubx_state->payload_size + 1) { - //convert to correct struct - switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - ubx_gps->lat = packet->lat; - ubx_gps->lon = packet->lon; - ubx_gps->alt = packet->height_msl; - - ubx_gps->counter_pos_valid++; - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_POSLLH - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] NAV_POSLLH: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - - ubx_gps->fix_type = packet->gpsFix; - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - ubx_gps->s_variance = packet->sAcc; - ubx_gps->p_variance = packet->pAcc; - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] NAV_SOL: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case NAV_DOP: { -// printf("GOT NAV_DOP MESSAGE\n"); - gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - - ubx_gps->eph = packet->hDOP; - ubx_gps->epv = packet->vDOP; - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_DOP - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] NAV_DOP: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - -// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, packet->time_nanoseconds); - - - - ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_TIMEUTC - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); - - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char gps_rx_buffer_part1[length_part1]; - memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) gps_rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char gps_rx_buffer_part3[length_part3]; - memcpy(gps_rx_buffer_part3, &(gps_rx_buffer[ubx_state->rx_count - 1]), length_part3); - gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) gps_rx_buffer_part3; - - //Check if checksum is valid and then store the gps information - if (ubx_state->ck_a == packet_part3->ck_a && ubx_state->ck_b == packet_part3->ck_b) { - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char gps_rx_buffer_part2[length_part2]; //for temporal storage - - - int i; - - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - - memcpy(gps_rx_buffer_part2, &(gps_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) gps_rx_buffer_part2; - - - /* Write satellite information in the global storage */ - - ubx_gps->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - - if ((packet_part2->flags) & 1) { //flags is a bitfield - ubx_gps->satellite_used[i] = 1; - - } else { - ubx_gps->satellite_used[i] = 0; - } - - ubx_gps->satellite_snr[i] = packet_part2->cno; - ubx_gps->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - ubx_gps->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - ubx_gps->satellite_used[i] = 0; - ubx_gps->satellite_snr[i] = 0; - ubx_gps->satellite_elevation[i] = 0; - ubx_gps->satellite_azimuth[i] = 0; - } - - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - ubx_gps->satellite_prn[i] = 0; - ubx_gps->satellite_used[i] = 0; - ubx_gps->satellite_snr[i] = 0; - ubx_gps->satellite_elevation[i] = 0; - ubx_gps->satellite_azimuth[i] = 0; - } - - /* set flag if any sat info is available */ - if (!packet_part1->numCh > 0) { - ubx_gps->satellite_info_available = 1; - - } else { - ubx_gps->satellite_info_available = 0; - } - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_SVINFO - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("\t[gps] NAV_SVINFO: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - - ubx_gps->vel = (uint16_t)packet->speed; - ubx_gps->vel_n = packet->velN / 100.0f; - ubx_gps->vel_e = packet->velE / 100.0f; - ubx_gps->vel_d = packet->velD / 100.0f; - ubx_gps->vel_ned_valid = true; - ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[NAV_VELNED - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] NAV_VELNED: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); - const int length_part1 = 7; - char gps_rx_buffer_part1[length_part1]; - memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); - gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) gps_rx_buffer_part1; - - //Check if checksum is valid and the store the gps information - if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { - - ubx_gps->satellites_visible = packet->numVis; - - ubx_gps->timestamp = hrt_absolute_time(); - ubx_gps->counter++; - - - //pthread_mutex_lock(ubx_mutex); - ubx_state->last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); - //pthread_mutex_unlock(ubx_mutex); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] RXM_SVSI: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer; - - //Check if checksum is valid - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - - switch (ubx_config_state) { - case UBX_CONFIG_STATE_PRT: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - ubx_config_state++; - break; - case UBX_CONFIG_STATE_NAV5: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) - ubx_config_state++; - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - case UBX_CONFIG_STATE_MSG_NAV_DOP: - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - case UBX_CONFIG_STATE_MSG_NAV_SOL: - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - case UBX_CONFIG_STATE_MSG_RXM_SVSI: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - ubx_config_state++; - break; - default: - break; - } - - - ret = 1; - - } else { - if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer; - - //Check if checksum is valid - if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - - if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n"); - ret = 1; - - } else { - if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n"); - - ret = 0; - } - - // Reset state machine to decode next packet - ubx_decode_init(); - return ret; - - break; - } - - default: //something went wrong - ubx_decode_init(); - - break; - } - } - - (ubx_state->rx_count)++; - - - - } - - - return 0; // no valid packet found - -} - -void calculate_ubx_checksum(uint8_t *message, uint8_t length) -{ - uint8_t ck_a = 0; - uint8_t ck_b = 0; - - int i; - - for (i = 2; i < length - 2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } - - message[length - 2] = ck_a; - message[length - 1] = ck_b; -} - -int configure_gps_ubx(int *fd) -{ - // only needed once like this - const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = { - .clsID = UBX_CLASS_CFG, - .msgID = UBX_MESSAGE_CFG_PRT, - .length = UBX_CFG_PRT_LENGTH, - .portID = UBX_CFG_PRT_PAYLOAD_PORTID, - .mode = UBX_CFG_PRT_PAYLOAD_MODE, - .baudRate = current_gps_speed, - .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK, - .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK, - .ck_a = 0, - .ck_b = 0 - }; - - // only needed once like this - const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = { - .clsID = UBX_CLASS_CFG, - .msgID = UBX_MESSAGE_CFG_NAV5, - .length = UBX_CFG_NAV5_LENGTH, - .mask = UBX_CFG_NAV5_PAYLOAD_MASK, - .dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL, - .fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE, - .ck_a = 0, - .ck_b = 0 - }; - - // this message is reusable for different configuration commands, so not const - type_gps_bin_cfg_msg_packet cfg_msg_packet = { - .clsID = UBX_CLASS_CFG, - .msgID = UBX_MESSAGE_CFG_MSG, - .length = UBX_CFG_MSG_LENGTH, - .rate = UBX_CFG_MSG_PAYLOAD_RATE - }; - - uint64_t time_before_config = hrt_absolute_time(); - - while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) { - -// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state); - - switch (ubx_config_state) { - case UBX_CONFIG_STATE_PRT: -// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate); - - write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); - break; - case UBX_CONFIG_STATE_NAV5: - write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_RXM_SVSI: - cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_CONFIGURED: - if (gps_verbose) printf("[gps] ubx configuration finished\n"); - return OK; - break; - default: - break; - } - usleep(10000); - } - if (gps_verbose) printf("[gps] ubx configuration timeout\n"); - return ERROR; -} - - - -int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) -{ - uint8_t ret = 0; - uint8_t c; - int rx_count = 0; - int gpsRxOverflow = 0; - - struct pollfd fds; - fds.fd = *fd; - fds.events = POLLIN; - - // UBX GPS mode - // This blocks the task until there is something on the buffer - while (1) { - //check if the thread should terminate - if (terminate_gps_thread == true) { - ret = 1; - break; - } - if (poll(&fds, 1, 1000) > 0) { - if (read(*fd, &c, 1) > 0) { - -// printf("Read %x\n",c); - if (rx_count >= buffer_size) { - // The buffer is already full and we haven't found a valid ubx sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - rx_count = 0; - ubx_decode_init(); - - if (gps_verbose) printf("[gps] Buffer full\n"); - - } else { - //gps_rx_buffer[rx_count] = c; - rx_count++; - - } - - int msg_read = ubx_parse(c, gps_rx_buffer); - - if (msg_read > 0) { - //printf("Found sequence\n"); - break; - } - - } else { - break; - } - - } else { - break; - } - - } - - return ret; -} - -int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd) -{ - uint8_t ck_a = 0; - uint8_t ck_b = 0; - - unsigned int i; - - uint8_t buffer[2]; - ssize_t result_write = 0; - - //calculate and write checksum to the end - for (i = 0; i < length-2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } - - // write sync bytes first - buffer[0] = UBX_SYNC_1; - buffer[1] = UBX_SYNC_2; - - // write config message without the checksum - result_write = write(*fd, buffer, sizeof(buffer)); - result_write += write(*fd, message, length-2); - - buffer[0] = ck_a; - buffer[1] = ck_b; - - // write the checksum - result_write += write(*fd, buffer, sizeof(buffer)); - - fsync(*fd); - if ((unsigned int)result_write != length + 2) - return ERROR; - - return OK; -} - -void *ubx_watchdog_loop(void *args) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "gps ubx watchdog", getpid()); - - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - ubx_config_state = UBX_CONFIG_STATE_PRT; - /* first try to configure the GPS anyway */ - configure_gps_ubx(fd); - - /* GPS watchdog error message skip counter */ - - bool ubx_healthy = false; - - uint8_t ubx_fail_count = 0; - uint8_t ubx_success_count = 0; - bool once_ok = false; - - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - //int err_skip_counter = 0; - - while (!(*thread_should_exit)) { - /* if some values are to old reconfigure gps */ - int i; - pthread_mutex_lock(ubx_mutex); - bool all_okay = true; - uint64_t timestamp_now = hrt_absolute_time(); - - for (i = 0; i < UBX_NO_OF_MESSAGES; i++) { -// printf("timestamp_now=%llu\n", timestamp_now); -// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]); - if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { - //printf("Warning: GPS ubx message %d not received for a long time\n", i); - all_okay = false; - } - } - - pthread_mutex_unlock(ubx_mutex); - - if (!all_okay) { - /* gps error */ - ubx_fail_count++; -// if (err_skip_counter == 0) -// { -// printf("GPS Watchdog detected gps not running or having problems\n"); -// err_skip_counter = 20; -// } -// err_skip_counter--; - //printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok); - - - /* If we have too many failures and another mode or baud should be tried, exit... */ - if ((gps_mode_try_all == true || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_PROBE_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) { - if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\n"); - - gps_mode_success = false; - break; - } - - if (ubx_healthy && ubx_fail_count == UBX_HEALTH_FAIL_COUNTER_LIMIT) { - printf("[gps] ERROR: UBX GPS module stopped responding\n"); - // global_data_send_subsystem_info(&ubx_present_enabled); - mavlink_log_critical(mavlink_fd, "[gps] UBX module stopped responding\n"); - ubx_healthy = false; - ubx_success_count = 0; - } - - /* trying to reconfigure the gps configuration */ - ubx_config_state = UBX_CONFIG_STATE_PRT; - configure_gps_ubx(fd); - fflush(stdout); - sleep(1); - - } else { - /* gps healthy */ - ubx_success_count++; - ubx_fail_count = 0; - once_ok = true; // XXX Should this be true on a single success, or on same criteria as ubx_healthy? - - if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) { - //printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed); - // global_data_send_subsystem_info(&ubx_present_enabled_healthy); - mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n"); - ubx_healthy = true; - } - } - usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS); - } - - if(gps_verbose) printf("[gps] ubx loop is going to terminate\n"); - close(mavlink_fd); - return NULL; -} - -void *ubx_loop(void *args) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "gps ubx read", getpid()); - - /* Retrieve file descriptor and thread flag */ - struct arg_struct *arguments = (struct arg_struct *)args; - int *fd = arguments->fd_ptr; - bool *thread_should_exit = arguments->thread_should_exit_ptr; - - /* Initialize gps stuff */ - char gps_rx_buffer[UBX_BUFFER_SIZE]; - - - if (gps_verbose) printf("[gps] UBX protocol driver starting..\n"); - - //set parameters for ubx_state - - //ubx state - ubx_state = malloc(sizeof(gps_bin_ubx_state_t)); - //printf("gps: ubx_state created\n"); - ubx_decode_init(); - ubx_state->print_errors = false; - - - /* set parameters for ubx */ - - struct vehicle_gps_position_s ubx_gps_d = {.counter = 0}; - - ubx_gps = &ubx_gps_d; - - orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); - - while (!(*thread_should_exit)) { - /* Parse a message from the gps receiver */ - if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) { - /* publish new GPS position */ - orb_publish(ORB_ID(vehicle_gps_position), gps_pub, ubx_gps); - - } else { - /* de-advertise */ - close(gps_pub); - break; - } - } - - if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); - close(gps_pub); - return NULL; - -} diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h deleted file mode 100644 index f3313a3c6..000000000 --- a/apps/gps/ubx.h +++ /dev/null @@ -1,415 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file U-Blox protocol definitions */ - -#ifndef UBX_H_ -#define UBX_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - - -//internal definitions (not depending on the ubx protocol -#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ -#define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */ -#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 2000000 /**< Check for current state every two seconds */ - -#define UBX_CONFIG_TIMEOUT 1000000 - -#define APPNAME "gps: ubx" - -#define UBX_SYNC_1 0xB5 -#define UBX_SYNC_2 0x62 - -//UBX Protocoll definitions (this is the subset of the messages that are parsed) -#define UBX_CLASS_NAV 0x01 -#define UBX_CLASS_RXM 0x02 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 -#define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 -#define UBX_MESSAGE_NAV_VELNED 0x12 -#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 -#define UBX_MESSAGE_ACK_NAK 0x00 -#define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 -#define UBX_MESSAGE_CFG_MSG 0x01 - -#define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ - -#define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ -#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ - -#define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */ - - -// ************ -/** the structures of the binary packets */ -#pragma pack(push, 1) -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t lon; // Longitude * 1e-7, deg - int32_t lat; // Latitude * 1e-7, deg - int32_t height; // Height above Ellipsoid, mm - int32_t height_msl; // Height above mean sea level, mm - uint32_t hAcc; // Horizontal Accuracy Estimate, mm - uint32_t vAcc; // Vertical Accuracy Estimate, mm - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_nav_posllh_packet; - -typedef type_gps_bin_nav_posllh_packet gps_bin_nav_posllh_packet_t; - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 - int16_t week; // GPS week (GPS time) - uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix - uint8_t flags; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; - uint16_t pDOP; - uint8_t reserved1; - uint8_t numSV; - uint32_t reserved2; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_nav_sol_packet; - -typedef type_gps_bin_nav_sol_packet gps_bin_nav_sol_packet_t; - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint32_t time_accuracy; //Time Accuracy Estimate, ns - int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) - uint16_t year; //Year, range 1999..2099 (UTC) - uint8_t month; //Month, range 1..12 (UTC) - uint8_t day; //Day of Month, range 1..31 (UTC) - uint8_t hour; //Hour of Day, range 0..23 (UTC) - uint8_t min; //Minute of Hour, range 0..59 (UTC) - uint8_t sec; //Seconds of Minute, range 0..59 (UTC) - uint8_t valid_flag; //Validity Flags (see ubx documentation) - - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_nav_timeutc_packet; - -typedef type_gps_bin_nav_timeutc_packet gps_bin_nav_timeutc_packet_t; - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint16_t gDOP; //Geometric DOP (scaling 0.01) - uint16_t pDOP; //Position DOP (scaling 0.01) - uint16_t tDOP; //Time DOP (scaling 0.01) - uint16_t vDOP; //Vertical DOP (scaling 0.01) - uint16_t hDOP; //Horizontal DOP (scaling 0.01) - uint16_t nDOP; //Northing DOP (scaling 0.01) - uint16_t eDOP; //Easting DOP (scaling 0.01) - - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_nav_dop_packet; - -typedef type_gps_bin_nav_dop_packet gps_bin_nav_dop_packet_t; - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint8_t numCh; //Number of channels - uint8_t globalFlags; - uint16_t reserved2; - -} type_gps_bin_nav_svinfo_part1_packet; - -typedef type_gps_bin_nav_svinfo_part1_packet gps_bin_nav_svinfo_part1_packet_t; - -typedef struct { - uint8_t chn; //Channel number, 255 for SVs not assigned to a channel - uint8_t svid; //Satellite ID - uint8_t flags; - uint8_t quality; - uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz - int8_t elev; //Elevation in integer degrees - int16_t azim; //Azimuth in integer degrees - int32_t prRes; //Pseudo range residual in centimetres - -} type_gps_bin_nav_svinfo_part2_packet; - -typedef type_gps_bin_nav_svinfo_part2_packet gps_bin_nav_svinfo_part2_packet_t; - -typedef struct { - uint8_t ck_a; - uint8_t ck_b; - -} type_gps_bin_nav_svinfo_part3_packet; - -typedef type_gps_bin_nav_svinfo_part3_packet gps_bin_nav_svinfo_part3_packet_t; - - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t velN; //NED north velocity, cm/s - int32_t velE; //NED east velocity, cm/s - int32_t velD; //NED down velocity, cm/s - uint32_t speed; //Speed (3-D), cm/s - uint32_t gSpeed; //Ground Speed (2-D), cm/s - int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 - uint32_t sAcc; //Speed Accuracy Estimate, cm/s - uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_nav_velned_packet; - -typedef type_gps_bin_nav_velned_packet gps_bin_nav_velned_packet_t; - -typedef struct { - int32_t time_milliseconds; // Measurement integer millisecond GPS time of week - int16_t week; //Measurement GPS week number - uint8_t numVis; //Number of visible satellites - - //... rest of package is not used in this implementation - -} type_gps_bin_rxm_svsi_packet; - -typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_ack_ack_packet; - -typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_ack_nak_packet; - -typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t portID; - uint8_t res0; - uint16_t res1; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t pad; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_prt_packet; - -typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint32_t reserved2; - uint32_t reserved3; - uint32_t reserved4; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_nav5_packet; - -typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t msgClass_payload; - uint8_t msgID_payload; - uint8_t rate[6]; - - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_packet; - -typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t; - - -// END the structures of the binary packets -// ************ - -enum UBX_CONFIG_STATE { - UBX_CONFIG_STATE_NONE = 0, - UBX_CONFIG_STATE_PRT = 1, - UBX_CONFIG_STATE_NAV5 = 2, - UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4, - UBX_CONFIG_STATE_MSG_NAV_DOP = 5, - UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6, - UBX_CONFIG_STATE_MSG_NAV_SOL = 7, - UBX_CONFIG_STATE_MSG_NAV_VELNED = 8, - UBX_CONFIG_STATE_MSG_RXM_SVSI = 9, - UBX_CONFIG_STATE_CONFIGURED = 10 -}; - -enum UBX_MESSAGE_CLASSES { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -}; - -enum UBX_MESSAGE_IDS { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH = 1, - NAV_SOL = 2, - NAV_TIMEUTC = 3, - NAV_DOP = 4, - NAV_SVINFO = 5, - NAV_VELNED = 6, - RXM_SVSI = 7, - CFG_NAV5 = 8, - ACK_ACK = 9, - ACK_NAK = 10 -}; - -enum UBX_DECODE_STATES { - UBX_DECODE_UNINIT = 0, - UBX_DECODE_GOT_SYNC1 = 1, - UBX_DECODE_GOT_SYNC2 = 2, - UBX_DECODE_GOT_CLASS = 3, - UBX_DECODE_GOT_MESSAGEID = 4, - UBX_DECODE_GOT_LENGTH1 = 5, - UBX_DECODE_GOT_LENGTH2 = 6 -}; - -typedef struct { - union { - uint16_t ck; - struct { - uint8_t ck_a; - uint8_t ck_b; - }; - }; - enum UBX_DECODE_STATES decode_state; - bool print_errors; - int16_t rx_count; - uint16_t payload_size; - - enum UBX_MESSAGE_CLASSES message_class; - enum UBX_MESSAGE_IDS message_id; - uint64_t last_message_timestamps[UBX_NO_OF_MESSAGES]; - -} type_gps_bin_ubx_state; - -typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; -#pragma pack(pop) - -extern pthread_mutex_t *ubx_mutex; -extern gps_bin_ubx_state_t *ubx_state; - -void ubx_decode_init(void); - -void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b); - - - -int ubx_parse(uint8_t b, char *gps_rx_buffer); - -int configure_gps_ubx(int *fd); - -int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size); - -int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd); - -void calculate_ubx_checksum(uint8_t *message, uint8_t length); - -void *ubx_watchdog_loop(void *args); - -void *ubx_loop(void *args); - - -#endif /* UBX_H_ */ -- cgit v1.2.3 From 3d3a68a7fb8315eb1d28b3f2906bb835f0e72994 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 18:11:33 +0100 Subject: Removed confusing non-error message, updated start scripts to match wiki and most recent SW revs --- ROMFS/scripts/rc.FMU_quad_x | 83 +++++++++++++++++++++++++++--------------- ROMFS/scripts/rc.PX4IO | 87 ++++++++++++++++++++++++--------------------- ROMFS/scripts/rc.PX4IOAR | 36 ++++++++++++++----- apps/commander/commander.c | 3 -- 4 files changed, 129 insertions(+), 80 deletions(-) (limited to 'apps') diff --git a/ROMFS/scripts/rc.FMU_quad_x b/ROMFS/scripts/rc.FMU_quad_x index d9c9a8457..8787443ea 100644 --- a/ROMFS/scripts/rc.FMU_quad_x +++ b/ROMFS/scripts/rc.FMU_quad_x @@ -1,40 +1,67 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB # -# Startup for X-quad on FMU1.5/1.6 -# - -echo "[init] uORB" uorb start - -echo "[init] eeprom" -eeprom start -if [ -f /eeprom/parameters ] + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] then - param load + param load /fs/microsd/parameters fi - -echo "[init] sensors" -#bma180 start -#l3gd20 start -mpu6000 start -hmc5883 start -ms5611 start - -sensors start - -echo "[init] mavlink" + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -echo "[init] commander" + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# commander start - -echo "[init] attitude control" + +# +# Start the attitude estimator +# attitude_estimator_ekf start -multirotor_att_control start - + echo "[init] starting PWM output" fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - + +# +# Start attitude control +# +multirotor_att_control start + echo "[init] startup done, exiting" -exit +exit \ No newline at end of file diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 84e181a5a..1e3963b9a 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -1,73 +1,80 @@ #!nsh - + +# Disable USB and autostart set USB no - +set MODE camflyer + # -# Start the object request broker +# Start the ORB # uorb start - + # -# Init the EEPROM +# Load microSD params # -echo "[init] eeprom" -eeprom start -if [ -f /eeprom/parameters ] +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] then - param load + param load /fs/microsd/parameters fi - -# -# Enable / connect to PX4IO -# -px4io start - + # -# Load an appropriate mixer. FMU_pass.mix is a passthru mixer -# which is good for testing. See ROMFS/mixers for a full list of mixers. +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ # -mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix - +param set MAV_TYPE 1 + # # Start the sensors. # sh /etc/init.d/rc.sensors - + # -# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable) +# Start MAVLink # -mavlink start -d /dev/ttyS0 -b 57600 +mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - + # # Start the commander. # commander start - + +# +# Start GPS interface +# +gps start + # # Start the attitude estimator # -attitude_estimator_ekf start - +kalman_demo start + # -# Start the attitude and position controller +# Start PX4IO interface # -fixedwing_att_control start -fixedwing_pos_control start - +px4io start + # -# Start GPS capture. Comment this out if you do not have a GPS. +# Load mixer and start controllers # -gps start - +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging # -# Start logging to microSD if we can -# -sh /etc/init.d/rc.logging +sdlog start -s 10 # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start system state # -echo "[init] startup done" -exit +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 640cdf541..72df68e35 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -17,7 +17,7 @@ echo "[init] doing PX4IOAR startup..." uorb start # -# Load microSD params +# Init the parameter storage # echo "[init] loading microSD params" param select /fs/microsd/parameters @@ -27,16 +27,23 @@ then fi # -# Start MAVLink +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ # -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 +param set MAV_TYPE 2 # -# Start the sensors and test them. +# Start the sensors. # sh /etc/init.d/rc.sensors +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + # # Start the commander. # @@ -62,15 +69,26 @@ multirotor_att_control start # ardrone_interface start -d /dev/ttyS1 +# +# Start GPS capture +# +gps start + # # Start logging # -#sdlog start - +sdlog start -s 10 + # -# Start GPS capture +# Start system state # -gps start +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi # # startup is done; we don't want the shell because we diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b..5dfb7a945 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1459,9 +1459,6 @@ int commander_thread_main(int argc, char *argv[]) } else { current_status.flag_external_manual_override_ok = true; } - - } else { - warnx("ARMED, rejecting sys type change\n"); } } -- cgit v1.2.3