优化卡尔曼矩阵计算部分
parent
ecd6217d63
commit
474931c228
|
|
@ -0,0 +1 @@
|
|||
.DS_store
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
Statement of Purpose
|
||||
|
||||
The laws of most jurisdictions throughout the world automatically confer
|
||||
exclusive Copyright and Related Rights (defined below) upon the creator and
|
||||
subsequent owner(s) (each and all, an "owner") of an original work of
|
||||
authorship and/or a database (each, a "Work").
|
||||
|
||||
Certain owners wish to permanently relinquish those rights to a Work for the
|
||||
purpose of contributing to a commons of creative, cultural and scientific works
|
||||
("Commons") that the public can reliably and without fear of later claims of
|
||||
infringement build upon, modify, incorporate in other works, reuse and
|
||||
redistribute as freely as possible in any form whatsoever and for any purposes,
|
||||
including without limitation commercial purposes. These owners may contribute to
|
||||
the Commons to promote the ideal of a free culture and the further production of
|
||||
creative, cultural and scientific works, or to gain reputation or greater
|
||||
distribution for their Work in part through the use and efforts of others.
|
||||
|
||||
For these and/or other purposes and motivations, and without any expectation of
|
||||
additional consideration or compensation, the person associating CC0 with a Work
|
||||
(the "Affirmer"), to the extent that he or she is an owner of Copyright and
|
||||
Related Rights in the Work, voluntarily elects to apply CC0 to the Work and
|
||||
publicly distribute the Work under its terms, with knowledge of his or her
|
||||
Copyright and Related Rights in the Work and the meaning and intended legal
|
||||
effect of CC0 on those rights.
|
||||
|
||||
1. Copyright and Related Rights. A Work made available under CC0 may be
|
||||
protected by copyright and related or neighboring rights ("Copyright and Related
|
||||
Rights"). Copyright and Related Rights include, but are not limited to, the
|
||||
following:
|
||||
|
||||
the right to reproduce, adapt, distribute, perform, display, communicate, and
|
||||
translate a Work; moral rights retained by the original author(s) and/or
|
||||
performer(s); publicity and privacy rights pertaining to a person's image or
|
||||
likeness depicted in a Work; rights protecting against unfair competition in
|
||||
regards to a Work, subject to the limitations in paragraph 4(a), below; rights
|
||||
protecting the extraction, dissemination, use and reuse of data in a Work;
|
||||
database rights (such as those arising under Directive 96/9/EC of the European
|
||||
Parliament and of the Council of 11 March 1996 on the legal protection of
|
||||
databases, and under any national implementation thereof, including any amended
|
||||
or successor version of such directive); and other similar, equivalent or
|
||||
corresponding rights throughout the world based on applicable law or treaty, and
|
||||
any national implementations thereof. 2. Waiver. To the greatest extent
|
||||
permitted by, but not in contravention of, applicable law, Affirmer hereby
|
||||
overtly, fully, permanently, irrevocably and unconditionally waives, abandons,
|
||||
and surrenders all of Affirmer's Copyright and Related Rights and associated
|
||||
claims and causes of action, whether now known or unknown (including existing as
|
||||
well as future claims and causes of action), in the Work (i) in all territories
|
||||
worldwide, (ii) for the maximum duration provided by applicable law or treaty
|
||||
(including future time extensions), (iii) in any current or future medium and
|
||||
for any number of copies, and (iv) for any purpose whatsoever, including without
|
||||
limitation commercial, advertising or promotional purposes (the "Waiver").
|
||||
Affirmer makes the Waiver for the benefit of each member of the public at large
|
||||
and to the detriment of Affirmer's heirs and successors, fully intending that
|
||||
such Waiver shall not be subject to revocation, rescission, cancellation,
|
||||
termination, or any other legal or equitable action to disrupt the quiet
|
||||
enjoyment of the Work by the public as contemplated by Affirmer's express
|
||||
Statement of Purpose.
|
||||
|
||||
3. Public License Fallback. Should any part of the Waiver for any reason be
|
||||
judged legally invalid or ineffective under applicable law, then the Waiver
|
||||
shall be preserved to the maximum extent permitted taking into account
|
||||
Affirmer's express Statement of Purpose. In addition, to the extent the Waiver
|
||||
is so judged Affirmer hereby grants to each affected person a royalty-free, non
|
||||
transferable, non sublicensable, non exclusive, irrevocable and unconditional
|
||||
license to exercise Affirmer's Copyright and Related Rights in the Work (i) in
|
||||
all territories worldwide, (ii) for the maximum duration provided by applicable
|
||||
law or treaty (including future time extensions), (iii) in any current or future
|
||||
medium and for any number of copies, and (iv) for any purpose whatsoever,
|
||||
including without limitation commercial, advertising or promotional purposes
|
||||
(the "License"). The License shall be deemed effective as of the date CC0 was
|
||||
applied by Affirmer to the Work. Should any part of the License for any reason
|
||||
be judged legally invalid or ineffective under applicable law, such partial
|
||||
invalidity or ineffectiveness shall not invalidate the remainder of the License,
|
||||
and in such case Affirmer hereby affirms that he or she will not (i) exercise
|
||||
any of his or her remaining Copyright and Related Rights in the Work or (ii)
|
||||
assert any associated claims and causes of action with respect to the Work, in
|
||||
either case contrary to Affirmer's express Statement of Purpose.
|
||||
|
||||
4. Limitations and Disclaimers.
|
||||
|
||||
No trademark or patent rights held by Affirmer are waived, abandoned,
|
||||
surrendered, licensed or otherwise affected by this document. Affirmer offers
|
||||
the Work as-is and makes no representations or warranties of any kind concerning
|
||||
the Work, express, implied, statutory or otherwise, including without limitation
|
||||
warranties of title, merchantability, fitness for a particular purpose, non
|
||||
infringement, or the absence of latent or other defects, accuracy, or the
|
||||
present or absence of errors, whether or not discoverable, all to the greatest
|
||||
extent permissible under applicable law. Affirmer disclaims responsibility for
|
||||
clearing rights of other persons that may apply to the Work or any use thereof,
|
||||
including without limitation any person's Copyright and Related Rights in the
|
||||
Work. Further, Affirmer disclaims responsibility for obtaining any necessary
|
||||
consents, permissions or other rights required for any use of the Work. Affirmer
|
||||
understands and acknowledges that Creative Commons is not a party to this
|
||||
document and has no duty or obligation with respect to this CC0 or use of the
|
||||
Work.
|
||||
|
|
@ -0,0 +1,244 @@
|
|||
# NMEAParser
|
||||
|
||||
An Arduino library to parse NMEA sentences.
|
||||
|
||||
NMEA is a communication standard in the marine equipment industry: GPS, anemometers,... The NMEAParser library allows you to analyze NMEA sentences and associate handlers to each of those that need to be recognized. The library provides the means to associate a handler to each identifier and also provides functions to retrieve the arguments of the sentence in different data formats: character, integer, float, character string.
|
||||
|
||||
## Changelog
|
||||
|
||||
* ```1.1``` : Added joker in type. Remove binaries from ```extra```
|
||||
* ```1.0``` : Initial release
|
||||
|
||||
## Memory footprint
|
||||
|
||||
On an Arduino Uno, an instance of a NMEAParser requires 97 bytes with only one handler. 8 bytes per additional handler are required.
|
||||
|
||||
## Using NMEAParser
|
||||
|
||||
As usual, the library should be included at the beginning of the sketch.
|
||||
|
||||
```C++
|
||||
#include <NMEAParser.h>
|
||||
```
|
||||
|
||||
Then, you must instantiate a parser as follows:
|
||||
|
||||
```C++
|
||||
NMEAParser<4> parser;
|
||||
```
|
||||
|
||||
The ```4``` is the maximum number of handlers desired, ```parser``` is the name of the object that will allow the analysis of NMEA sentences. you can of course use the name you want.
|
||||
|
||||
In ```setup``` you configure your parser as you wish using the following functions.
|
||||
|
||||
### ```void addHandler(<type>, <handler>)```
|
||||
|
||||
where ```<type>``` is a character string and the type of sentence to recongnize, and ```<handler>``` the function to call when a sentence is recognize. ```<type>``` can be a string stored un RAM or a string stored in flash : ```F("ASTRI")```. If ```<type>``` has more than 5 characters, it is trucated.
|
||||
|
||||
For example, suppose you want to recognize the following sounder sentences which give the water depth below keel (DBK) and below surface (DBS):
|
||||
|
||||
```
|
||||
$SDDBK,...
|
||||
```
|
||||
|
||||
and
|
||||
|
||||
```
|
||||
$SDDBS,...
|
||||
```
|
||||
|
||||
You will install two handlers as follows (assuming your parsing object is named ```parser```):
|
||||
|
||||
```C++
|
||||
parser.addHandler("SDDBK", handleDepthBelowKeel);
|
||||
parser.addHanlder("SDDBS", handleDepthBelowSurface);
|
||||
```
|
||||
|
||||
```handleDepthBelowKeel``` and ```handleDepthBelowSurface``` are functions that will be called when sentences are recognized.
|
||||
|
||||
With version 1.1, ```<type>``` may include hyphens. An hyphens matches any character. For instance if you want the handler to match all sentences coming from the sounder, you would write:
|
||||
|
||||
```C++
|
||||
parser.addHandler("SD---", handleSounder);
|
||||
```
|
||||
|
||||
```handleSounder``` function would be called for any sentence with the type beginning with SD.
|
||||
|
||||
### ```void setDefaultHandler(<handler>)```
|
||||
|
||||
When a sentence is succefully parsed but none of the handler correspond to it, ```<handler>``` is called. It is a function corresponding to a ```void f(void)``` prototype. By default, no function is called.
|
||||
|
||||
### ```void setErrorHandler(<handler>)```
|
||||
|
||||
When a sentence is malformed : too long, wrong characters, etc, ```<handler>``` is called. It is a function corresponding to a ```void f(void)``` prototype. By default, no function is called.
|
||||
|
||||
### ```void setHandleCRC(<doCRC>)```
|
||||
|
||||
Specifies whether the CRC is checked or not. By default, the CRC is checked. If you do not want CRC verification, pass ```false``` to ```setHandleCRC```.
|
||||
|
||||
---
|
||||
In the handlers, you will get the arguments of the sentence, the sentence type or the error if any by using the following functions:
|
||||
|
||||
### ```bool getArg(<argnum>, <argval>)```
|
||||
|
||||
is used to get the arguments of the sentence. ```<argnum>``` is the number of the argument, starting at 0 for the argument that follows the identifier. ```<argval``` is a variable where the argument value will be stored if successful. ```getArg``` returns a boolean which is true if successfull, false if not.
|
||||
|
||||
Continuing with the example, [both sentences have the same 6 arguments](https://gpsd.gitlab.io/gpsd/NMEA.html#_dbk_depth_below_keel)
|
||||
|
||||
* Argument 0 is a float number giving the depth in feet.
|
||||
* Argument 1 is a ```f``` for feet.
|
||||
* Argument 2 is a float number giving the depth in meters.
|
||||
* Argument 3 is a ```M``` for Meters.
|
||||
* Argument 4 is a float number giving the depth in fathoms.
|
||||
* At last Argument 5 is a ```F``` for Fathoms.
|
||||
|
||||
Suppose you are interested by the depths in meters and you have two float variables to store theses data:
|
||||
|
||||
```C++
|
||||
float depthBelowKeel;
|
||||
float depthBelowSurface;
|
||||
```
|
||||
|
||||
You would implement ```handleDepthBelowKeel``` and ```handleDepthBelowSurface``` as follow:
|
||||
|
||||
```C++
|
||||
void handleDepthBelowKeel(void)
|
||||
{
|
||||
float depth;
|
||||
if (parser.getArg(2, depth))
|
||||
{
|
||||
depthBelowKeel = depth;
|
||||
}
|
||||
}
|
||||
|
||||
void handleDepthBelowSurface(void)
|
||||
{
|
||||
float depth;
|
||||
if (parser.getArg(2, depth))
|
||||
{
|
||||
depthBelowSurface = depth;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### ```bool getType(<type>) / bool getType(<num>, <charType>)```
|
||||
|
||||
3 versions of ```getType``` exist. The first one puts the type of the sentence in ```<type>``` which is a ```char *```. The second one does the same but ```<type>``` is a ```String```. Return ```true``` if a type has been parsed, ```false``` otherwise. The third one puts a character of the type at position ```<num>```
|
||||
|
||||
### ```uint8_t argCount()```
|
||||
|
||||
Return the number of arguments.
|
||||
|
||||
### ```NMEA::ErrorCode error()```
|
||||
|
||||
Return the error. The returned code can be:
|
||||
|
||||
* ```NMEA::NO_ERROR```: no error;
|
||||
* ```NMEA::UNEXPECTED_CHAR```: a char which is not expected in the sentence has been encountered;
|
||||
* ```NMEA::BUFFER_FULL```: the sentence is too long to fit in the buffer;
|
||||
* ```NMEA::TYPE_TOO_LONG```: the sentence type has more than 5 characters;
|
||||
* ```NMEA::CRC_ERROR```: the CRC is wrong;
|
||||
* ```NMEA::INTERNAL_ERROR```: the internal state of the parser is wrong, should not happen by the way.
|
||||
|
||||
### Feeding characters to the parser
|
||||
|
||||
Characters are fed to the parser in ```loop```, assuming we get the characters from ```Serial```, the following way:
|
||||
|
||||
```C++
|
||||
while (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
```
|
||||
|
||||
This can also be done in ```serialEvent```. ```while``` could be replaced by ```if```.
|
||||
|
||||
## A complete example
|
||||
|
||||
Let's say you want to turn the Arduino's LED on and off. We define a NMEA sentence taking a single argument: 1 to turn on the LED and 0 to turn it off. The sentence can therefore be:
|
||||
|
||||
```
|
||||
$ARLED,1*43
|
||||
```
|
||||
|
||||
to turn the LED on and
|
||||
|
||||
```
|
||||
$ARLED,0*42
|
||||
```
|
||||
|
||||
to turn the LED off. We define a single handler to retrieve the argument and control the LED accordingly:
|
||||
|
||||
```C++
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLED with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int ledState;
|
||||
if (parser.getArg(0,ledState)) {
|
||||
digitalWrite(LED_BUILTIN, ledState == 0 ? LOW : HIGH);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
We define 2 other handlers for anything else than ```ARLED``` and for errors
|
||||
|
||||
```C++
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
```
|
||||
|
||||
In ```setup```, the handlers are installed:
|
||||
|
||||
```C++
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLED", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
```
|
||||
|
||||
At last in loop, we feed the parser with the chars coming from ```Serial```.
|
||||
|
||||
```C++
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
# Extra software
|
||||
|
||||
Additional software can be found in the ```extra``` directory.
|
||||
|
||||
## NMEA sentences generator
|
||||
|
||||
The ```gen``` subdirectory contains ```nmeagen```, a NMEA sentence generator program. This program generates well formed sentences with good or bad CRC. It can be used to test the parser. To build ```nmeagen```, run the ```build.sh``` script. ```nmeagen``` takes 1 or 2 arguments. The first argument is the number of sentences to generate. The second optional one is the number of sentences with bad CRC.
|
||||
|
||||
## Test program
|
||||
|
||||
The ```test``` subdirectory contains a test program that compile on Linux or Mac OS X. It takes sentences from the standard input, parse them and print out the type, the arguments and if an error occured.
|
||||
|
||||
---
|
||||
# Additional links
|
||||
|
||||
* [NMEA CRC calculator](https://nmeachecksum.eqth.net)
|
||||
* [NMEA 0183 on Wikipedia](https://fr.wikipedia.org/wiki/NMEA_0183)
|
||||
* [NMEA revealed](https://gpsd.gitlab.io/gpsd/NMEA.html)
|
||||
|
||||
That's all folks !
|
||||
60
1.Software/PDR 1.04/3rdparty/NMEAParser-master/examples/NMEALedLight/NMEALedLight.ino
vendored
Normal file
60
1.Software/PDR 1.04/3rdparty/NMEAParser-master/examples/NMEALedLight/NMEALedLight.ino
vendored
Normal file
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* A NMEA example to switch ON or OFF the built in LED
|
||||
*
|
||||
* Sentences to send are :
|
||||
*
|
||||
* $ARLED,1*43
|
||||
*
|
||||
* to switch the LED on and
|
||||
*
|
||||
* $ARLED,0*42
|
||||
*
|
||||
* to switch the LED off
|
||||
*
|
||||
* Set the serial monitor end of line to <cr><lf>
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser with only one handler */
|
||||
NMEAParser<1> parser;
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLED with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int ledState;
|
||||
if (parser.getArg(0,ledState)) {
|
||||
digitalWrite(LED_BUILTIN, ledState == 0 ? LOW : HIGH);
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLED", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* A NMEA example to switch ON or OFF the built in LED but with the use
|
||||
* of wild char in the type
|
||||
*
|
||||
* Sentences to send are :
|
||||
*
|
||||
* $ARLE1*2B
|
||||
*
|
||||
* to switch the LED on and
|
||||
*
|
||||
* $ARLE0*2A
|
||||
*
|
||||
* to switch the LED off
|
||||
*
|
||||
* Set the serial monitor end of line to <cr><lf>
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser with only one handler */
|
||||
NMEAParser<1> parser;
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLEx with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
|
||||
char wantedLedState;
|
||||
if (parser.getType(4, wantedLedState)) { // get the 4th character ot the type
|
||||
if (wantedLedState == '0' || wantedLedState == '1') {
|
||||
digitalWrite(LED_BUILTIN, wantedLedState == '0' ? LOW : HIGH);
|
||||
}
|
||||
else {
|
||||
Serial.println("x should be 0 or 1");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLE-", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
77
1.Software/PDR 1.04/3rdparty/NMEAParser-master/examples/parsingExample/parsingExample.ino
vendored
Normal file
77
1.Software/PDR 1.04/3rdparty/NMEAParser-master/examples/parsingExample/parsingExample.ino
vendored
Normal file
|
|
@ -0,0 +1,77 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* Parsing example.
|
||||
*
|
||||
* 2 handlers are added and 2 sentences are parsed
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser is declared with 2 handlers at most */
|
||||
NMEAParser<2> parser;
|
||||
|
||||
const char firstSentence[] = "$DBAXK*54\r\n";
|
||||
const char secondSentence[] = "$EJCHQ,03,O,UUEIE,S,953.11,S,4.172,ASBUX,J*54\r\n";
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void firstHandler()
|
||||
{
|
||||
Serial.print("Got DBAXK with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
}
|
||||
|
||||
void secondHandler()
|
||||
{
|
||||
Serial.print("Got $EJCHQ with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int arg0;
|
||||
char arg1;
|
||||
char arg2[10];
|
||||
char arg3;
|
||||
float arg4;
|
||||
char arg5;
|
||||
float arg6;
|
||||
String arg7;
|
||||
char arg8;
|
||||
if (parser.getArg(0,arg0)) Serial.println(arg0);
|
||||
if (parser.getArg(1,arg1)) Serial.println(arg1);
|
||||
if (parser.getArg(2,arg2)) Serial.println(arg2);
|
||||
if (parser.getArg(3,arg3)) Serial.println(arg3);
|
||||
if (parser.getArg(4,arg4)) Serial.println(arg4);
|
||||
if (parser.getArg(5,arg5)) Serial.println(arg5);
|
||||
if (parser.getArg(6,arg6)) Serial.println(arg6);
|
||||
if (parser.getArg(7,arg7)) Serial.println(arg7);
|
||||
if (parser.getArg(8,arg8)) Serial.println(arg8);
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("DBAXK", firstHandler);
|
||||
parser.addHandler("EJCHQ", secondHandler);
|
||||
|
||||
Serial.print("Parsing : ");
|
||||
Serial.print(firstSentence);
|
||||
for (uint8_t i = 0; i < strlen(firstSentence); i++) {
|
||||
parser << firstSentence[i];
|
||||
}
|
||||
|
||||
Serial.print("Parsing : ");
|
||||
Serial.print(secondSentence);
|
||||
for (uint8_t i = 0; i < strlen(secondSentence); i++) {
|
||||
parser << secondSentence[i];
|
||||
}
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
}
|
||||
101
1.Software/PDR 1.04/3rdparty/NMEAParser-master/extra/gen/NMEASentencesGenerator.c
vendored
Normal file
101
1.Software/PDR 1.04/3rdparty/NMEAParser-master/extra/gen/NMEASentencesGenerator.c
vendored
Normal file
|
|
@ -0,0 +1,101 @@
|
|||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
|
||||
char intToHex(uint8_t v)
|
||||
{
|
||||
if (v < 10) return '0' + v;
|
||||
else if (v < 16) return 'A' + v - 10;
|
||||
else return '-';
|
||||
}
|
||||
|
||||
void generateSentence(bool goodCRC)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
putchar('$');
|
||||
/* generate the id */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
int numberOfArgs = random() % 10;
|
||||
for (int i = 0; i < numberOfArgs; i++) {
|
||||
putchar(',');
|
||||
crc ^= ',';
|
||||
int kind = random() % 5;
|
||||
switch (kind) {
|
||||
case 0: /* integer */
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
break;
|
||||
case 1: /* char */
|
||||
if (1) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
break;
|
||||
case 2: /* string */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
break;
|
||||
case 3: /* float */
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
putchar('.');
|
||||
crc ^= '.';
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
case 4: /* nothing */
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (! goodCRC) crc = ~crc;
|
||||
putchar('*');
|
||||
putchar(intToHex(crc >> 4));
|
||||
putchar(intToHex(crc & 0x0F));
|
||||
putchar('\r');
|
||||
putchar('\n');
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2 || argc > 3) {
|
||||
printf("Usage: nmeagen <n> [<c>] where <n> is the number of sentences\n"
|
||||
" and <c> the number of sentences with a wrong CRC\n");
|
||||
exit(1);
|
||||
}
|
||||
else {
|
||||
int numberOfSentences = strtol(argv[1], NULL, 0);
|
||||
int numberOfWrongCRCSentences = 0;
|
||||
if (argc == 3) {
|
||||
numberOfWrongCRCSentences = strtol(argv[2], NULL, 0);
|
||||
}
|
||||
|
||||
/* Init a the random seed with the epoch */
|
||||
srandom(time(NULL));
|
||||
|
||||
for (int i = 0; i < numberOfSentences; i++) {
|
||||
generateSentence(true);
|
||||
}
|
||||
for (int i = 0; i < numberOfWrongCRCSentences; i++) {
|
||||
generateSentence(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/bin/bash
|
||||
|
||||
gcc -o nmeagen NMEASentencesGenerator.c
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
$FRMIO,G,33,081,I,Q,Q,J,48*2E
|
||||
$APYOG,LDCXT,JSCIE,KUJAU,64,EJMVQ,F,P,AJQLD*12
|
||||
$SUHEG,GAGCE,G,Q*31
|
||||
$KIEHA,E,W,R,SXGMG,64,40,WCIEC,2253,THWNT*6E
|
||||
$WPHGT,FRYBV,5689,C*68
|
||||
$YDXHC,A,J*45
|
||||
$GUIAZ,CWIMW*2B
|
||||
$SOEOE,ZBKFY,HKTHJ,536,504,946,0,705,BIDPI,ACEOK*4B
|
||||
$TEHJW,NNHPF,4,34,DFSFJ,8638,F,B*59
|
||||
$LKGPI,XTUUO*36
|
||||
$ZCHFN,769,0202,12*4E
|
||||
$DQTMV,Y,712,0*2B
|
||||
$MQPEM,H,95,E,M,U,92,N,340,D*47
|
||||
$APUXX,RKPFP,MUQOA,VOCGB,39,6,D,V,U*78
|
||||
$MBLXP,22573,3,AVNFF*3C
|
||||
$LCBXF,V,21,AKCLF,YIASR,PHIUS,VPRJM*11
|
||||
$OFXPI,2,B,14,A*7C
|
||||
$IUKFO,D,MSETP,99,GYBDX*05
|
||||
$UDSFE,QHPNK,D,N,R*55
|
||||
$EDULQ,ISGJX,FDXLJ,O,H,M,B,ROZIU,C*4A
|
||||
$FUONE,P,Y*5E
|
||||
$VLVIU*50
|
||||
$WTBCG,14,819,ATHAO,V,MSUHV,NSCLD*76
|
||||
$WJZLK,3,Q,31*0C
|
||||
$MRFGM,NCVPP,NZCBL,F,U,C,672,LPFAS*56
|
||||
$UFOJW,V,AIUHA,BFBKN*2C
|
||||
$OHOSS,HOIBI,8695,031,B,CVSMD,154,DEUCB*79
|
||||
$MYHGK,ZKTDK,YFJBI,OLBOI,74,KPASB,XBCSE,Q,U*00
|
||||
$ZIENW*4F
|
||||
$ONAYP,N,27,A*6F
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/bin/bash
|
||||
|
||||
c++ -o testNMEA testNMEA.cpp
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
#include "../../src/NMEAParser.h"
|
||||
#include <string.h>
|
||||
|
||||
NMEAParser<4> commandNMEA;
|
||||
int errorCount = 0;
|
||||
|
||||
void error()
|
||||
{
|
||||
printf("=================================================\n");
|
||||
int err = commandNMEA.error();
|
||||
printf("*** ERROR %d ",err);
|
||||
switch (err) {
|
||||
case NMEA::UNEXPECTED_CHAR:
|
||||
printf("(UNEXPECTED CHAR)\n");
|
||||
break;
|
||||
case NMEA::BUFFER_FULL:
|
||||
printf("(BUFFER FULL)\n");
|
||||
break;
|
||||
case NMEA::CRC_ERROR:
|
||||
printf("(CRC ERROR)\n");
|
||||
break;
|
||||
case NMEA::INTERNAL_ERROR:
|
||||
printf("(INTERNAL ERROR)\n");
|
||||
break;
|
||||
default:
|
||||
printf("(?)\n");
|
||||
break;
|
||||
}
|
||||
commandNMEA.printBuffer();
|
||||
printf("=================================================\n");
|
||||
errorCount++;
|
||||
}
|
||||
|
||||
void defaultHandler()
|
||||
{
|
||||
printf("------------\n");
|
||||
char buf[82];
|
||||
if (commandNMEA.getType(buf)) {
|
||||
printf("%s\n", buf);
|
||||
for (int i = 0; i < commandNMEA.argCount(); i++) {
|
||||
if (commandNMEA.getArg(i, buf)) {
|
||||
printf(" %s\n", buf);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
printf("Debut du test\n");
|
||||
|
||||
commandNMEA.setErrorHandler(error);
|
||||
commandNMEA.setDefaultHandler(defaultHandler);
|
||||
|
||||
int count = 0;
|
||||
int v;
|
||||
while ((v = getchar()) != EOF) {
|
||||
commandNMEA << v;
|
||||
if (v == '\n') count++;
|
||||
}
|
||||
printf("*** Processed %d NMEA sentences\n", count);
|
||||
printf("*** Got %d error(s)\n", errorCount);
|
||||
}
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
#######################################
|
||||
# Syntax Coloring Map For NMEAParser
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
NMEAParser KEYWORD1
|
||||
NMEA KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
addHandler KEYWORD2
|
||||
setErrorHandler KEYWORD2
|
||||
setDefaultHandler KEYWORD2
|
||||
argCount KEYWORD2
|
||||
getArg KEYWORD2
|
||||
getType KEYWORD2
|
||||
error KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
NO_ERROR LITERAL1
|
||||
UNEXPECTED_CHAR LITERAL1
|
||||
BUFFER_FULL LITERAL1
|
||||
CRC_ERROR LITERAL1
|
||||
INTERNAL_ERROR LITERAL1
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
name=NMEAParser
|
||||
version=1.1
|
||||
author=Glinnes Hulden
|
||||
maintainer=Glinnes Hulden
|
||||
sentence=A simple Arduino library to parse NMEA sentences.
|
||||
paragraph=A simple Arduino library to parse NMEA sentences.
|
||||
category=Communication
|
||||
url=https://github.com/Glinnes/NMEAParser
|
||||
architectures=*
|
||||
|
|
@ -0,0 +1,657 @@
|
|||
/*
|
||||
* NMEA parser library for Arduino
|
||||
*
|
||||
* Simple and compact NMEA parser designed for Arduino
|
||||
*
|
||||
* Author : Glinnes Hulden
|
||||
*
|
||||
* This work is distributed under license CC0.
|
||||
* Check https://creativecommons.org/publicdomain/zero/1.0/deed.en
|
||||
*
|
||||
* No Copyright
|
||||
*
|
||||
* The person who associated a work with this deed has dedicated the work
|
||||
* to the public domain by waiving all of his or her rights to the work
|
||||
* worldwide under copyright law, including all related and neighboring rights,
|
||||
* to the extent allowed by law.
|
||||
*
|
||||
* You can copy, modify, distribute and perform the work, even for commercial
|
||||
* purposes, all without asking permission. See Other Information below.
|
||||
*
|
||||
* Other Information
|
||||
*
|
||||
* In no way are the patent or trademark rights of any person affected by CC0,
|
||||
* nor are the rights that other persons may have in the work or in how the
|
||||
* work is used, such as publicity or privacy rights.
|
||||
* Unless expressly stated otherwise, the person who associated a work with
|
||||
* this deed makes no warranties about the work, and disclaims liability for
|
||||
* all uses of the work, to the fullest extent permitted by applicable law.
|
||||
* When using or citing the work, you should not imply endorsement by the
|
||||
* author or the affirmer.
|
||||
*/
|
||||
|
||||
#ifndef __NMEAParser_h__
|
||||
#define __NMEAParser_h__
|
||||
|
||||
#ifdef __amd64__
|
||||
/* To use on my development platform */
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#else
|
||||
#include <Arduino.h>
|
||||
#endif
|
||||
|
||||
namespace NMEA {
|
||||
/*
|
||||
* Error codes
|
||||
*/
|
||||
typedef enum {
|
||||
NO_ERROR,
|
||||
UNEXPECTED_CHAR,
|
||||
BUFFER_FULL,
|
||||
TYPE_TOO_LONG,
|
||||
CRC_ERROR,
|
||||
INTERNAL_ERROR
|
||||
} ErrorCode;
|
||||
}
|
||||
|
||||
/*
|
||||
* The library consists of a single template: NMEAParser.
|
||||
*/
|
||||
template <size_t S> class NMEAParser {
|
||||
|
||||
private:
|
||||
typedef void (*NMEAErrorHandler)(void);
|
||||
typedef void (*NMEAHandler)(void);
|
||||
typedef struct { char mToken[6]; NMEAHandler mHandler; } NMEAHandlerEntry;
|
||||
typedef enum { INIT, SENT, ARG, CRCH, CRCL, CRLFCR, CRLFLF } State;
|
||||
|
||||
public:
|
||||
/*
|
||||
* maximum sentence size is 82 including the starting '$' and the <cr><lf>
|
||||
* at the end. Since '$', the '*', the 2 characters CRC and the <cr><lf>
|
||||
* are not bufferized, 82 - 6 + 1 = 77 chars are enough.
|
||||
* is enough.
|
||||
*/
|
||||
static const uint8_t kSentenceMaxSize = 77;
|
||||
|
||||
private:
|
||||
/*
|
||||
* buffer to store the NMEA sentence excluding starting '$', the ','
|
||||
* separator, the '*', the CRC and the <cr><lf>. The tail of the buffer
|
||||
* is used to store the index of the arguments.
|
||||
*/
|
||||
char mBuffer[kSentenceMaxSize];
|
||||
|
||||
/*
|
||||
* Current index to store a char of the sentence
|
||||
*/
|
||||
uint8_t mIndex;
|
||||
|
||||
/*
|
||||
* Current index to store the index of an argument
|
||||
*/
|
||||
uint8_t mArgIndex;
|
||||
|
||||
/*
|
||||
* A handler to notify a malformed sentence
|
||||
*/
|
||||
NMEAErrorHandler mErrorHandler;
|
||||
|
||||
/*
|
||||
* A handler for well formed but unrecognized sentences
|
||||
*/
|
||||
NMEAHandler mDefaultHandler;
|
||||
|
||||
/*
|
||||
* An array of NMEA handler : pointers to functions to call when a sentence
|
||||
* is recognized
|
||||
*/
|
||||
NMEAHandlerEntry mHandlers[S];
|
||||
|
||||
/*
|
||||
* The current number of mHandlers
|
||||
*/
|
||||
uint8_t mHandlerCount;
|
||||
|
||||
/*
|
||||
* Parsing automaton variable
|
||||
*/
|
||||
State mState;
|
||||
|
||||
/*
|
||||
* mError
|
||||
*/
|
||||
NMEA::ErrorCode mError;
|
||||
|
||||
/*
|
||||
* True if CRC is handled, false otherwise. Defaults to true
|
||||
*/
|
||||
bool mHandleCRC;
|
||||
|
||||
/*
|
||||
* Variables used to computed and parse the CRC
|
||||
*/
|
||||
uint8_t mComputedCRC;
|
||||
uint8_t mGotCRC;
|
||||
|
||||
/*
|
||||
* NMEAParserStringify is used internally to temporarely replace a char
|
||||
* in the buffer by a '\0' so that libc string functions may be used.
|
||||
* Instantiating a NMEAParserStringify object in a pair of {} defines
|
||||
* a section in which the 'stringification' is done : the constructor
|
||||
* does that according to the arguments and se destructor restore the buffer.
|
||||
*/
|
||||
class NMEAParserStringify {
|
||||
uint8_t mPos;
|
||||
char mTmp;
|
||||
NMEAParser<S> *mParent;
|
||||
public:
|
||||
NMEAParserStringify(NMEAParser<S> *inParent, uint8_t inPos) :
|
||||
mPos(inPos),
|
||||
mParent(inParent)
|
||||
{
|
||||
mTmp = mParent->mBuffer[mPos];
|
||||
mParent->mBuffer[mPos] = '\0';
|
||||
}
|
||||
~NMEAParserStringify()
|
||||
{
|
||||
mParent->mBuffer[mPos] = mTmp;
|
||||
}
|
||||
};
|
||||
|
||||
/*
|
||||
* Call the error handler if defined
|
||||
*/
|
||||
void callErrorHandler(void)
|
||||
{
|
||||
if (mErrorHandler != NULL) {
|
||||
mErrorHandler();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the parser
|
||||
*/
|
||||
void reset() {
|
||||
mState = INIT;
|
||||
mIndex = 0;
|
||||
mArgIndex = kSentenceMaxSize;
|
||||
mError = NMEA::NO_ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the parser encounter a char that should not be there
|
||||
*/
|
||||
void unexpectedChar()
|
||||
{
|
||||
mError = NMEA::UNEXPECTED_CHAR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the buffer is full because of a malformed sentence
|
||||
*/
|
||||
void bufferFull()
|
||||
{
|
||||
mError = NMEA::BUFFER_FULL;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the type of the sentence is longer than 5 characters
|
||||
*/
|
||||
void typeTooLong()
|
||||
{
|
||||
mError = NMEA::TYPE_TOO_LONG;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
/*
|
||||
* Called when the CRC is wrong
|
||||
*/
|
||||
void crcError()
|
||||
{
|
||||
mError = NMEA::CRC_ERROR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the state of the parser is not ok
|
||||
*/
|
||||
void internalError()
|
||||
{
|
||||
mError = NMEA::INTERNAL_ERROR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* retuns true is there is at least one byte left in the buffer
|
||||
*/
|
||||
bool spaceAvail()
|
||||
{
|
||||
return (mIndex < mArgIndex);
|
||||
}
|
||||
|
||||
/*
|
||||
* convert a one hex digit char into an int. Used for the CRC
|
||||
*/
|
||||
static int8_t hexToNum(const char inChar)
|
||||
{
|
||||
if (isdigit(inChar)) return inChar - '0';
|
||||
else if (isupper(inChar) && inChar <= 'F') return inChar - 'A' + 10;
|
||||
else if (islower(inChar) && inChar <= 'f') return inChar - 'a' + 10;
|
||||
else return -1;
|
||||
}
|
||||
|
||||
static bool strnwcmp(const char *s1, const char *s2, uint8_t len)
|
||||
{
|
||||
while (len-- > 0) {
|
||||
if (*s1 != *s2 && *s1 != '-' && *s2 != '-') return false;
|
||||
s1++;
|
||||
s2++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* return the slot number for a handler. -1 if not found
|
||||
*/
|
||||
int8_t getHandler(const char *inToken)
|
||||
{
|
||||
/* Look for the token */
|
||||
for (uint8_t i = 0; i < mHandlerCount; i++) {
|
||||
if (strnwcmp(mHandlers[i].mToken, inToken, 5)) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* When all the sentence has been parsed, process it by calling the handler
|
||||
*/
|
||||
void processSentence()
|
||||
{
|
||||
/* Look for the token */
|
||||
uint8_t endPos = startArgPos(0);
|
||||
int8_t slot;
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
slot = getHandler(mBuffer);
|
||||
}
|
||||
if (slot != -1) {
|
||||
mHandlers[slot].mHandler();
|
||||
}
|
||||
else {
|
||||
if (mDefaultHandler != NULL) {
|
||||
mDefaultHandler();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if inArgNum corresponds to an actual argument
|
||||
*/
|
||||
bool validArgNum(uint8_t inArgNum)
|
||||
{
|
||||
return inArgNum < (kSentenceMaxSize - mArgIndex);
|
||||
}
|
||||
|
||||
/*
|
||||
* Return the start index of the inArgNum th argument
|
||||
*/
|
||||
uint8_t startArgPos(uint8_t inArgNum)
|
||||
{
|
||||
return mBuffer[kSentenceMaxSize - 1 - inArgNum];
|
||||
}
|
||||
|
||||
/*
|
||||
* Return the end index of the inArgNum th argument
|
||||
*/
|
||||
uint8_t endArgPos(uint8_t inArgNum)
|
||||
{
|
||||
return mBuffer[kSentenceMaxSize - 2 - inArgNum];
|
||||
}
|
||||
|
||||
public:
|
||||
/*
|
||||
* Constructor initialize the parser.
|
||||
*/
|
||||
NMEAParser() :
|
||||
mErrorHandler(NULL),
|
||||
mDefaultHandler(NULL),
|
||||
mHandlerCount(0),
|
||||
mError(NMEA::NO_ERROR),
|
||||
mHandleCRC(true),
|
||||
mComputedCRC(0),
|
||||
mGotCRC(0)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Add a sentence handler
|
||||
*/
|
||||
void addHandler(const char *inToken, NMEAHandler inHandler)
|
||||
{
|
||||
if (mHandlerCount < S) {
|
||||
if (getHandler(inToken) == -1) {
|
||||
strncpy(mHandlers[mHandlerCount].mToken, inToken, 5);
|
||||
mHandlers[mHandlerCount].mToken[5] = '\0';
|
||||
mHandlers[mHandlerCount].mHandler = inHandler;
|
||||
mHandlerCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef __AVR__
|
||||
/*
|
||||
* Add a sentence handler. Version with a token stored in flash.
|
||||
*/
|
||||
void addHandler(const __FlashStringHelper *ifsh, NMEAHandler inHandler)
|
||||
{
|
||||
char buf[6];
|
||||
PGM_P p = reinterpret_cast<PGM_P>(ifsh);
|
||||
for (uint8_t i = 0; i < 6; i++) {
|
||||
char c = pgm_read_byte(p++);
|
||||
buf[i] = c;
|
||||
if (c == '\0') break;
|
||||
}
|
||||
addHandler(buf, inHandler);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Set the error handler which is called when a sentence is malformed
|
||||
*/
|
||||
void setErrorHandler(NMEAErrorHandler inHandler)
|
||||
{
|
||||
mErrorHandler = inHandler;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set the default handler which is called when a sentence is well formed
|
||||
* but has no handler associated to
|
||||
*/
|
||||
void setDefaultHandler(NMEAHandler inHandler)
|
||||
{
|
||||
mDefaultHandler = inHandler;
|
||||
}
|
||||
|
||||
/*
|
||||
* Give a character to the parser
|
||||
*/
|
||||
void operator<<(char inChar)
|
||||
{
|
||||
int8_t tmp;
|
||||
|
||||
switch (mState) {
|
||||
|
||||
/* Waiting for the starting $ character */
|
||||
case INIT:
|
||||
mError = NMEA::NO_ERROR;
|
||||
if (inChar == '$') {
|
||||
mComputedCRC = 0;
|
||||
mState = SENT;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case SENT:
|
||||
if (isalnum(inChar)) {
|
||||
if (spaceAvail()) {
|
||||
if (mIndex < 5) {
|
||||
mBuffer[mIndex++] = inChar;
|
||||
mComputedCRC ^= inChar;
|
||||
}
|
||||
else {
|
||||
typeTooLong();
|
||||
}
|
||||
}
|
||||
else bufferFull();
|
||||
}
|
||||
else {
|
||||
switch(inChar) {
|
||||
case ',' :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = ARG;
|
||||
break;
|
||||
case '*' :
|
||||
mGotCRC = 0;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = CRCH;
|
||||
break;
|
||||
default :
|
||||
unexpectedChar();
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ARG:
|
||||
if (spaceAvail()) {
|
||||
switch(inChar) {
|
||||
case ',' :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
break;
|
||||
case '*' :
|
||||
mGotCRC = 0;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = CRCH;
|
||||
break;
|
||||
default :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[mIndex++] = inChar;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else bufferFull();
|
||||
break;
|
||||
|
||||
case CRCH:
|
||||
tmp = hexToNum(inChar);
|
||||
if (tmp != -1) {
|
||||
mGotCRC |= (uint8_t)tmp << 4;
|
||||
mState = CRCL;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRCL:
|
||||
tmp = hexToNum(inChar);
|
||||
if (tmp != -1) {
|
||||
mGotCRC |= (uint8_t)tmp;
|
||||
mState = CRLFCR;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRLFCR:
|
||||
if (inChar == '\r') {
|
||||
mState = CRLFLF;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRLFLF:
|
||||
if (inChar == '\n') {
|
||||
if (mHandleCRC && (mGotCRC != mComputedCRC)) {
|
||||
crcError();
|
||||
}
|
||||
else {
|
||||
processSentence();
|
||||
}
|
||||
reset();
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
default:
|
||||
internalError();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the number of arguments discovered in a well formed sentence.
|
||||
*/
|
||||
uint8_t argCount()
|
||||
{
|
||||
return kSentenceMaxSize - mArgIndex - 1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns one of the arguments. Different versions according to data type.
|
||||
*/
|
||||
bool getArg(uint8_t num, char &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
arg = mBuffer[startPos];
|
||||
return (endPos - startPos) == 1;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
bool getArg(uint8_t num, char *arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
strcpy(arg, &mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
#ifdef ARDUINO
|
||||
bool getArg(uint8_t num, String &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = &mBuffer[startPos];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool getArg(uint8_t num, int &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = atoi(&mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
bool getArg(uint8_t num, float &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = atof(&mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
/*
|
||||
* Returns the type of sentence.
|
||||
*/
|
||||
bool getType(char *arg)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
strncpy(arg, mBuffer, 5);
|
||||
arg[5] = '\0';
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
#ifdef ARDUINO
|
||||
bool getType(String &arg)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = mBuffer;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool getType(uint8_t inIndex, char &outTypeChar)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
if (inIndex < endPos) {
|
||||
outTypeChar = mBuffer[inIndex];
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
NMEA::ErrorCode error() {
|
||||
return mError;
|
||||
}
|
||||
|
||||
void setHandleCRC(bool inHandleCRC)
|
||||
{
|
||||
mHandleCRC = inHandleCRC;
|
||||
}
|
||||
#ifdef __amd64__
|
||||
void printBuffer()
|
||||
{
|
||||
{
|
||||
NMEAParserStringify stfy(this, startArgPos(0));
|
||||
printf("%s\n", mBuffer);
|
||||
}
|
||||
for (uint8_t i = 0; i < argCount(); i++) {
|
||||
uint8_t startPos = startArgPos(i);
|
||||
uint8_t endPos = endArgPos(i);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
printf("%s\n", &mBuffer[startPos]);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
# Auto detect text files and perform LF normalization
|
||||
* text=auto
|
||||
|
||||
# Custom for Visual Studio
|
||||
*.cs diff=csharp
|
||||
|
||||
# Standard to msysgit
|
||||
*.doc diff=astextplain
|
||||
*.DOC diff=astextplain
|
||||
*.docx diff=astextplain
|
||||
*.DOCX diff=astextplain
|
||||
*.dot diff=astextplain
|
||||
*.DOT diff=astextplain
|
||||
*.pdf diff=astextplain
|
||||
*.PDF diff=astextplain
|
||||
*.rtf diff=astextplain
|
||||
*.RTF diff=astextplain
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
# These are supported funding model platforms
|
||||
|
||||
custom: ['paypal.me/tilz0R']
|
||||
|
|
@ -0,0 +1,385 @@
|
|||
#Build Keil files
|
||||
*.rar
|
||||
*.o
|
||||
*.d
|
||||
*.crf
|
||||
*.htm
|
||||
*.dep
|
||||
*.map
|
||||
*.bak
|
||||
*.axf
|
||||
*.lnp
|
||||
*.lst
|
||||
*.ini
|
||||
*.scvd
|
||||
*.iex
|
||||
*.sct
|
||||
*.MajerleT
|
||||
*.tjuln
|
||||
*.tilen
|
||||
*.dbgconf
|
||||
*.uvguix
|
||||
*.uvoptx
|
||||
*.__i
|
||||
*.i
|
||||
!docs/*.txt
|
||||
RTE/
|
||||
|
||||
# IAR Settings
|
||||
**/settings/*.crun
|
||||
**/settings/*.dbgdt
|
||||
**/settings/*.cspy
|
||||
**/settings/*.cspy.*
|
||||
**/settings/*.xcl
|
||||
**/settings/*.dni
|
||||
**/settings/*.wsdt
|
||||
**/settings/*.wspos
|
||||
|
||||
# IAR Debug Exe
|
||||
**/Exe/*.sim
|
||||
|
||||
# IAR Debug Obj
|
||||
**/Obj/*.pbd
|
||||
**/Obj/*.pbd.*
|
||||
**/Obj/*.pbi
|
||||
**/Obj/*.pbi.*
|
||||
|
||||
*.TMP
|
||||
/docs_src/x_Doxyfile.doxy
|
||||
|
||||
.DS_Store
|
||||
|
||||
## Ignore Visual Studio temporary files, build results, and
|
||||
## files generated by popular Visual Studio add-ons.
|
||||
##
|
||||
## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore
|
||||
|
||||
# User-specific files
|
||||
*.suo
|
||||
*.user
|
||||
*.userosscache
|
||||
*.sln.docstates
|
||||
|
||||
# User-specific files (MonoDevelop/Xamarin Studio)
|
||||
*.userprefs
|
||||
|
||||
# Build results
|
||||
[Dd]ebug/
|
||||
[Dd]ebugPublic/
|
||||
[Rr]elease/
|
||||
[Rr]eleases/
|
||||
x64/
|
||||
x86/
|
||||
bld/
|
||||
[Bb]in/
|
||||
[Oo]bj/
|
||||
[Ll]og/
|
||||
_build/
|
||||
|
||||
# Visual Studio 2015/2017 cache/options directory
|
||||
.vs/
|
||||
# Uncomment if you have tasks that create the project's static files in wwwroot
|
||||
#wwwroot/
|
||||
|
||||
# Visual Studio 2017 auto generated files
|
||||
Generated\ Files/
|
||||
|
||||
# MSTest test Results
|
||||
[Tt]est[Rr]esult*/
|
||||
[Bb]uild[Ll]og.*
|
||||
|
||||
# NUNIT
|
||||
*.VisualState.xml
|
||||
TestResult.xml
|
||||
|
||||
# Build Results of an ATL Project
|
||||
[Dd]ebugPS/
|
||||
[Rr]eleasePS/
|
||||
dlldata.c
|
||||
|
||||
# Benchmark Results
|
||||
BenchmarkDotNet.Artifacts/
|
||||
|
||||
# .NET Core
|
||||
project.lock.json
|
||||
project.fragment.lock.json
|
||||
artifacts/
|
||||
**/Properties/launchSettings.json
|
||||
|
||||
# StyleCop
|
||||
StyleCopReport.xml
|
||||
|
||||
# Files built by Visual Studio
|
||||
*_i.c
|
||||
*_p.c
|
||||
*_i.h
|
||||
*.ilk
|
||||
*.meta
|
||||
*.obj
|
||||
*.pch
|
||||
*.pdb
|
||||
*.pgc
|
||||
*.pgd
|
||||
*.rsp
|
||||
*.sbr
|
||||
*.tlb
|
||||
*.tli
|
||||
*.tlh
|
||||
*.tmp
|
||||
*.tmp_proj
|
||||
*.log
|
||||
*.vspscc
|
||||
*.vssscc
|
||||
.builds
|
||||
*.pidb
|
||||
*.svclog
|
||||
*.scc
|
||||
*.out
|
||||
*.sim
|
||||
|
||||
# Chutzpah Test files
|
||||
_Chutzpah*
|
||||
|
||||
# Visual C++ cache files
|
||||
ipch/
|
||||
*.aps
|
||||
*.ncb
|
||||
*.opendb
|
||||
*.opensdf
|
||||
*.sdf
|
||||
*.cachefile
|
||||
*.VC.db
|
||||
*.VC.VC.opendb
|
||||
|
||||
# Visual Studio profiler
|
||||
*.psess
|
||||
*.vsp
|
||||
*.vspx
|
||||
*.sap
|
||||
|
||||
# Visual Studio Trace Files
|
||||
*.e2e
|
||||
|
||||
# TFS 2012 Local Workspace
|
||||
$tf/
|
||||
|
||||
# Guidance Automation Toolkit
|
||||
*.gpState
|
||||
|
||||
# ReSharper is a .NET coding add-in
|
||||
_ReSharper*/
|
||||
*.[Rr]e[Ss]harper
|
||||
*.DotSettings.user
|
||||
|
||||
# JustCode is a .NET coding add-in
|
||||
.JustCode
|
||||
|
||||
# TeamCity is a build add-in
|
||||
_TeamCity*
|
||||
|
||||
# DotCover is a Code Coverage Tool
|
||||
*.dotCover
|
||||
|
||||
# AxoCover is a Code Coverage Tool
|
||||
.axoCover/*
|
||||
!.axoCover/settings.json
|
||||
|
||||
# Visual Studio code coverage results
|
||||
*.coverage
|
||||
*.coveragexml
|
||||
|
||||
# NCrunch
|
||||
_NCrunch_*
|
||||
.*crunch*.local.xml
|
||||
nCrunchTemp_*
|
||||
|
||||
# MightyMoose
|
||||
*.mm.*
|
||||
AutoTest.Net/
|
||||
|
||||
# Web workbench (sass)
|
||||
.sass-cache/
|
||||
|
||||
# Installshield output folder
|
||||
[Ee]xpress/
|
||||
|
||||
# DocProject is a documentation generator add-in
|
||||
DocProject/buildhelp/
|
||||
DocProject/Help/*.HxT
|
||||
DocProject/Help/*.HxC
|
||||
DocProject/Help/*.hhc
|
||||
DocProject/Help/*.hhk
|
||||
DocProject/Help/*.hhp
|
||||
DocProject/Help/Html2
|
||||
DocProject/Help/html
|
||||
|
||||
# Click-Once directory
|
||||
publish/
|
||||
|
||||
# Publish Web Output
|
||||
*.[Pp]ublish.xml
|
||||
*.azurePubxml
|
||||
# Note: Comment the next line if you want to checkin your web deploy settings,
|
||||
# but database connection strings (with potential passwords) will be unencrypted
|
||||
*.pubxml
|
||||
*.publishproj
|
||||
|
||||
# Microsoft Azure Web App publish settings. Comment the next line if you want to
|
||||
# checkin your Azure Web App publish settings, but sensitive information contained
|
||||
# in these scripts will be unencrypted
|
||||
PublishScripts/
|
||||
|
||||
# NuGet Packages
|
||||
*.nupkg
|
||||
# The packages folder can be ignored because of Package Restore
|
||||
**/[Pp]ackages/*
|
||||
# except build/, which is used as an MSBuild target.
|
||||
!**/[Pp]ackages/build/
|
||||
# Uncomment if necessary however generally it will be regenerated when needed
|
||||
#!**/[Pp]ackages/repositories.config
|
||||
# NuGet v3's project.json files produces more ignorable files
|
||||
*.nuget.props
|
||||
*.nuget.targets
|
||||
|
||||
# Microsoft Azure Build Output
|
||||
csx/
|
||||
*.build.csdef
|
||||
|
||||
# Microsoft Azure Emulator
|
||||
ecf/
|
||||
rcf/
|
||||
|
||||
# Windows Store app package directories and files
|
||||
AppPackages/
|
||||
BundleArtifacts/
|
||||
Package.StoreAssociation.xml
|
||||
_pkginfo.txt
|
||||
*.appx
|
||||
|
||||
# Visual Studio cache files
|
||||
# files ending in .cache can be ignored
|
||||
*.[Cc]ache
|
||||
# but keep track of directories ending in .cache
|
||||
!*.[Cc]ache/
|
||||
|
||||
# Others
|
||||
ClientBin/
|
||||
~$*
|
||||
*~
|
||||
*.dbmdl
|
||||
*.dbproj.schemaview
|
||||
*.jfm
|
||||
*.pfx
|
||||
*.publishsettings
|
||||
orleans.codegen.cs
|
||||
|
||||
# Including strong name files can present a security risk
|
||||
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
|
||||
#*.snk
|
||||
|
||||
# Since there are multiple workflows, uncomment next line to ignore bower_components
|
||||
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
|
||||
#bower_components/
|
||||
|
||||
# RIA/Silverlight projects
|
||||
Generated_Code/
|
||||
|
||||
# Backup & report files from converting an old project file
|
||||
# to a newer Visual Studio version. Backup files are not needed,
|
||||
# because we have git ;-)
|
||||
_UpgradeReport_Files/
|
||||
Backup*/
|
||||
UpgradeLog*.XML
|
||||
UpgradeLog*.htm
|
||||
|
||||
# SQL Server files
|
||||
*.mdf
|
||||
*.ldf
|
||||
*.ndf
|
||||
|
||||
# Business Intelligence projects
|
||||
*.rdl.data
|
||||
*.bim.layout
|
||||
*.bim_*.settings
|
||||
|
||||
# Microsoft Fakes
|
||||
FakesAssemblies/
|
||||
|
||||
# GhostDoc plugin setting file
|
||||
*.GhostDoc.xml
|
||||
|
||||
# Node.js Tools for Visual Studio
|
||||
.ntvs_analysis.dat
|
||||
node_modules/
|
||||
|
||||
# TypeScript v1 declaration files
|
||||
typings/
|
||||
|
||||
# Visual Studio 6 build log
|
||||
*.plg
|
||||
|
||||
# Visual Studio 6 workspace options file
|
||||
*.opt
|
||||
|
||||
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
|
||||
*.vbw
|
||||
|
||||
# Visual Studio LightSwitch build output
|
||||
**/*.HTMLClient/GeneratedArtifacts
|
||||
**/*.DesktopClient/GeneratedArtifacts
|
||||
**/*.DesktopClient/ModelManifest.xml
|
||||
**/*.Server/GeneratedArtifacts
|
||||
**/*.Server/ModelManifest.xml
|
||||
_Pvt_Extensions
|
||||
|
||||
# Paket dependency manager
|
||||
.paket/paket.exe
|
||||
paket-files/
|
||||
|
||||
# FAKE - F# Make
|
||||
.fake/
|
||||
|
||||
# JetBrains Rider
|
||||
.idea/
|
||||
*.sln.iml
|
||||
|
||||
# CodeRush
|
||||
.cr/
|
||||
|
||||
# Python Tools for Visual Studio (PTVS)
|
||||
__pycache__/
|
||||
*.pyc
|
||||
|
||||
# Cake - Uncomment if you are using it
|
||||
# tools/**
|
||||
# !tools/packages.config
|
||||
|
||||
# Tabs Studio
|
||||
*.tss
|
||||
|
||||
# Telerik's JustMock configuration file
|
||||
*.jmconfig
|
||||
|
||||
# BizTalk build output
|
||||
*.btp.cs
|
||||
*.btm.cs
|
||||
*.odx.cs
|
||||
*.xsd.cs
|
||||
|
||||
# OpenCover UI analysis results
|
||||
OpenCover/
|
||||
|
||||
# Azure Stream Analytics local run output
|
||||
ASALocalRun/
|
||||
|
||||
# MSBuild Binary and Structured Log
|
||||
*.binlog
|
||||
|
||||
log_file.txt
|
||||
.metadata/
|
||||
.mxproject
|
||||
.settings/
|
||||
project.ioc
|
||||
mx.scratch
|
||||
*.tilen majerle
|
||||
*.exe
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
" tree-specific vimrc to comply with project coding style
|
||||
" see https://github.com/MaJerle/c-code-style
|
||||
if &ft == "c" || &ft == "cpp"
|
||||
setlocal shiftwidth=4 tabstop=4 softtabstop=4 expandtab autoindent cc=80 foldmethod=indent
|
||||
endif
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
################################################################################
|
||||
# Lightweight GPS (lwgps) CMake support
|
||||
################################################################################
|
||||
# The lwgps library can be configured with a lwgps_opts.h file.
|
||||
# If such a file is used, the user should set the LWGPS_CONFIG_PATH path variable where
|
||||
# the configuration file is located so that is is included properly.
|
||||
#
|
||||
# Other than that, only two steps are necessary to compile and link against LWGPS:
|
||||
# 1. Use add_subdirectory to add the lwgps folder
|
||||
# 2. Link against lwgps with target_link_libraries
|
||||
################################################################################
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(LIB_LWGPS_NAME lwgps)
|
||||
|
||||
add_library(${LIB_LWGPS_NAME})
|
||||
|
||||
add_subdirectory(${LIB_LWGPS_NAME})
|
||||
add_subdirectory(examples)
|
||||
|
||||
# The project CMakeLists can set a LWGPS_CONFIG_PATH path including the lwgps_opts.h file
|
||||
# and add it.
|
||||
if(NOT LWGPS_CONFIG_PATH)
|
||||
message(STATUS "Lightweight GPS configuration path not set.")
|
||||
endif()
|
||||
|
||||
# Extract the absolute path of the provided configuration path
|
||||
if(IS_ABSOLUTE ${LWGPS_CONFIG_PATH})
|
||||
set(LWGPS_CONFIG_PATH_ABSOLUTE ${LWGPS_CONFIG_PATH})
|
||||
else()
|
||||
get_filename_component(LWGPS_CONFIG_PATH_ABSOLUTE
|
||||
${LWGPS_CONFIG_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(${LIB_LWGPS_NAME} PRIVATE
|
||||
${LWGPS_CONFIG_PATH_ABSOLUTE}
|
||||
)
|
||||
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
MIT License
|
||||
|
||||
Copyright (c) 2020 Tilen MAJERLE
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
# Lightweight GPS NMEA parser
|
||||
|
||||
Platform independent GPS NMEA parser for embedded systems.
|
||||
|
||||
<h3>Read first: <a href="http://docs.majerle.eu/projects/lwgps/">Documentation</a></h3>
|
||||
|
||||
## Features
|
||||
|
||||
* Written in ANSI C99
|
||||
* Platform independent, easy to use
|
||||
* Built-in support for 4 GPS statements
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
* Optional ``float`` or ``double`` floating point units
|
||||
* Low-level layer is separated from application layer, thus allows you to add custom communication with GPS device
|
||||
* Works with operating systems
|
||||
* Works with different communication interfaces
|
||||
* User friendly MIT license
|
||||
|
||||
## Contribute
|
||||
|
||||
Fresh contributions are always welcome. Simple instructions to proceed::
|
||||
|
||||
1. Fork Github repository
|
||||
2. Respect [C style & coding rules](https://github.com/MaJerle/c-code-style) used by the library
|
||||
3. Create a pull request to develop branch with new features or bug fixes
|
||||
|
||||
Alternatively you may:
|
||||
|
||||
1. Report a bug
|
||||
2. Ask for a feature request
|
||||
|
||||
## Test
|
||||
|
||||
To build the code and run basic tests on your host::
|
||||
|
||||
cd examples
|
||||
make test
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.28803.452
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lwgps_dev", "lwgps_dev.vcxproj", "{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
Debug|x86 = Debug|x86
|
||||
Release|x64 = Release|x64
|
||||
Release|x86 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x64.ActiveCfg = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x64.Build.0 = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x86.Build.0 = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x64.ActiveCfg = Release|x64
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x64.Build.0 = Release|x64
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x86.ActiveCfg = Release|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
EndGlobalSection
|
||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||
SolutionGuid = {C58E98E3-45C5-48F1-A7AF-BCFEC103DD09}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
149
1.Software/PDR 1.04/3rdparty/lwgps-develop/dev/VisualStudio/lwgps_dev.vcxproj
vendored
Normal file
149
1.Software/PDR 1.04/3rdparty/lwgps-develop/dev/VisualStudio/lwgps_dev.vcxproj
vendored
Normal file
|
|
@ -0,0 +1,149 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>15.0</VCProjectVersion>
|
||||
<ProjectGuid>{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}</ProjectGuid>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<RootNamespace>LwGPS</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<IncludePath>.;..\..\lwgps\src\include;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\..\examples\test_code.c" />
|
||||
<ClCompile Include="..\..\lwgps\src\lwgps\lwgps.c" />
|
||||
<ClCompile Include="main.c" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
23
1.Software/PDR 1.04/3rdparty/lwgps-develop/dev/VisualStudio/lwgps_dev.vcxproj.filters
vendored
Normal file
23
1.Software/PDR 1.04/3rdparty/lwgps-develop/dev/VisualStudio/lwgps_dev.vcxproj.filters
vendored
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup>
|
||||
<Filter Include="Source Files">
|
||||
<UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
|
||||
<Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
|
||||
</Filter>
|
||||
<Filter Include="Source Files\LwGPS">
|
||||
<UniqueIdentifier>{d2afbed4-545f-40e7-bf68-d835c5948bcc}</UniqueIdentifier>
|
||||
</Filter>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\..\lwgps\src\lwgps\lwgps.c">
|
||||
<Filter>Source Files\LwGPS</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="main.c">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\..\examples\test_code.c">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/**
|
||||
* \file lwgps_opts_template.h
|
||||
* \brief LwGPS configuration file
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: $2.1.0$
|
||||
*/
|
||||
#ifndef LWGPS_HDR_OPTS_H
|
||||
#define LWGPS_HDR_OPTS_H
|
||||
|
||||
/* Rename this file to "lwgps_opts.h" for your application */
|
||||
|
||||
/*
|
||||
* Open "include/lwgps/lwgps_opt.h" and
|
||||
* copy & replace here settings you want to change values
|
||||
*/
|
||||
|
||||
#endif /* LWGPS_HDR_OPTS_H */
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* External function */
|
||||
extern void run_tests();
|
||||
|
||||
int
|
||||
main() {
|
||||
lwgps_float_t distance, bearing;
|
||||
run_tests();
|
||||
|
||||
/* Calculate distance and bearing */
|
||||
lwgps_distance_bearing(40.6, -73.7, 48.3, 11.7, &distance, &bearing);
|
||||
|
||||
printf("Distance: %lf meters\r\n", (double)distance);
|
||||
printf("Bearing: %lf degrees\r\n", (double)bearing);
|
||||
|
||||
lwgps_distance_bearing(48.3, 11.7, 40.6, -73.7, &distance, &bearing);
|
||||
printf("Distance: %lf meters\r\n", (double)distance);
|
||||
printf("Bearing: %lf degrees\r\n", (double)bearing);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* JFK: 40.642569, -73.783790 */
|
||||
/* Munich: 48.353962, 11.775114 */
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
# Minimal makefile for Sphinx documentation
|
||||
#
|
||||
|
||||
# You can set these variables from the command line, and also
|
||||
# from the environment for the first two.
|
||||
SPHINXOPTS ?=
|
||||
SPHINXBUILD ?= sphinx-build
|
||||
SOURCEDIR = .
|
||||
BUILDDIR = _build
|
||||
|
||||
# Put it first so that "make" without argument is like "make help".
|
||||
help:
|
||||
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
||||
|
||||
.PHONY: help Makefile
|
||||
|
||||
# Catch-all target: route all unknown targets to Sphinx using the new
|
||||
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
|
||||
%: Makefile
|
||||
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
.. _api_lwgps:
|
||||
|
||||
LwGPS
|
||||
=====
|
||||
|
||||
.. doxygengroup:: LWGPS
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
.. _api_lwgps_opt:
|
||||
|
||||
Configuration
|
||||
=============
|
||||
|
||||
This is the default configuration of the middleware.
|
||||
When any of the settings shall be modified, it shall be done in dedicated application config ``lwgps_opts.h`` file.
|
||||
|
||||
.. note::
|
||||
Check :ref:`getting_started` to create configuration file.
|
||||
|
||||
.. doxygengroup:: LWGPS_OPT
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
.. _api_reference:
|
||||
|
||||
API reference
|
||||
=============
|
||||
|
||||
List of all the modules:
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
:glob:
|
||||
|
||||
*
|
||||
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
# Configuration file for the Sphinx documentation builder.
|
||||
#
|
||||
# This file only contains a selection of the most common options. For a full
|
||||
# list see the documentation:
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html
|
||||
|
||||
# -- Path setup --------------------------------------------------------------
|
||||
|
||||
# If extensions (or modules to document with autodoc) are in another directory,
|
||||
# add these directories to sys.path here. If the directory is relative to the
|
||||
# documentation root, use os.path.abspath to make it absolute, like shown here.
|
||||
#
|
||||
# import os
|
||||
# import sys
|
||||
# sys.path.insert(0, os.path.abspath('.'))
|
||||
from sphinx.builders.html import StandaloneHTMLBuilder
|
||||
import subprocess, os
|
||||
|
||||
# Run doxygen first
|
||||
# read_the_docs_build = os.environ.get('READTHEDOCS', None) == 'True'
|
||||
# if read_the_docs_build:
|
||||
subprocess.call('doxygen doxyfile.doxy', shell=True)
|
||||
# -- Project information -----------------------------------------------------
|
||||
|
||||
project = 'LwGPS'
|
||||
copyright = '2020, Tilen MAJERLE'
|
||||
author = 'Tilen MAJERLE'
|
||||
|
||||
# The full version, including alpha/beta/rc tags
|
||||
version = 'v2.1.0'
|
||||
|
||||
# Try to get branch at which this is running
|
||||
# and try to determine which version to display in sphinx
|
||||
git_branch = ''
|
||||
res = os.popen('git branch').read().strip()
|
||||
for line in res.split("\n"):
|
||||
if line[0] == '*':
|
||||
git_branch = line[1:].strip()
|
||||
|
||||
# Decision for display version
|
||||
try:
|
||||
if git_branch.index('develop') >= 0:
|
||||
version = "latest-develop"
|
||||
except Exception:
|
||||
print("Exception for index check")
|
||||
|
||||
# For debugging purpose
|
||||
print("GIT BRANCH: " + git_branch)
|
||||
print("VERSION: " + version)
|
||||
|
||||
# -- General configuration ---------------------------------------------------
|
||||
|
||||
# Add any Sphinx extension module names here, as strings. They can be
|
||||
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
|
||||
# ones.
|
||||
extensions = [
|
||||
'sphinx.ext.autodoc',
|
||||
'sphinx.ext.intersphinx',
|
||||
'sphinx.ext.autosectionlabel',
|
||||
'sphinx.ext.todo',
|
||||
'sphinx.ext.coverage',
|
||||
'sphinx.ext.mathjax',
|
||||
'sphinx.ext.ifconfig',
|
||||
'sphinx.ext.viewcode',
|
||||
'sphinx_sitemap',
|
||||
|
||||
'breathe',
|
||||
]
|
||||
|
||||
# Add any paths that contain templates here, relative to this directory.
|
||||
templates_path = ['templates']
|
||||
|
||||
# List of patterns, relative to source directory, that match files and
|
||||
# directories to ignore when looking for source files.
|
||||
# This pattern also affects html_static_path and html_extra_path.
|
||||
exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
|
||||
|
||||
highlight_language = 'c'
|
||||
|
||||
# -- Options for HTML output -------------------------------------------------
|
||||
|
||||
# The theme to use for HTML and HTML Help pages. See the documentation for
|
||||
# a list of builtin themes.
|
||||
#
|
||||
html_theme = 'sphinx_rtd_theme'
|
||||
html_theme_options = {
|
||||
'canonical_url': '',
|
||||
'analytics_id': '', # Provided by Google in your dashboard
|
||||
'display_version': True,
|
||||
'prev_next_buttons_location': 'bottom',
|
||||
'style_external_links': False,
|
||||
|
||||
'logo_only': False,
|
||||
|
||||
# Toc options
|
||||
'collapse_navigation': True,
|
||||
'sticky_navigation': True,
|
||||
'navigation_depth': 4,
|
||||
'includehidden': True,
|
||||
'titles_only': False
|
||||
}
|
||||
html_logo = 'static/images/logo.svg'
|
||||
github_url = 'https://github.com/MaJerle/lwgps'
|
||||
html_baseurl = 'https://docs.majerle.eu/projects/lwgps/'
|
||||
|
||||
# Add any paths that contain custom static files (such as style sheets) here,
|
||||
# relative to this directory. They are copied after the builtin static files,
|
||||
# so a file named "default.css" will overwrite the builtin "default.css".
|
||||
html_static_path = ['static']
|
||||
html_css_files = [
|
||||
'css/common.css',
|
||||
'css/custom.css',
|
||||
]
|
||||
html_js_files = [
|
||||
'https://cdnjs.cloudflare.com/ajax/libs/font-awesome/5.15.1/css/all.min.css'
|
||||
]
|
||||
|
||||
master_doc = 'index'
|
||||
|
||||
#
|
||||
# Breathe configuration
|
||||
#
|
||||
#
|
||||
#
|
||||
breathe_projects = {
|
||||
"lwgps": "_build/xml/"
|
||||
}
|
||||
breathe_default_project = "lwgps"
|
||||
breathe_default_members = ('members', 'undoc-members')
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,47 @@
|
|||
.. _examples:
|
||||
|
||||
Examples and demos
|
||||
==================
|
||||
|
||||
There are several basic examples provided with the library.
|
||||
|
||||
Parse block of data
|
||||
^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
In this example, block of data is prepared as big string array and sent to processing function in single shot.
|
||||
Application can then check if GPS signal has been detected as valid and use other data accordingly.
|
||||
|
||||
.. literalinclude:: ../../examples/example.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Minimum example code
|
||||
|
||||
Parse received data from interrupt/DMA
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Second example is a typical use case with interrupts on embedded systems.
|
||||
For each received character, application uses ``ringbuff`` as intermediate buffer.
|
||||
Data are later processed outside interrupt context.
|
||||
|
||||
.. note::
|
||||
For the sake of this example, application *implements* interrupts as function call in *while loop*.
|
||||
|
||||
.. literalinclude:: ../../examples/example_buff.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Example of buffer
|
||||
|
||||
Distance and bearing
|
||||
^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Library provides calculation of distance and bearing between ``2`` coordinates on earth.
|
||||
This is useful if used with autonomnous devices to understand in which direction
|
||||
device has to move to reach end point while knowing start coordinate.
|
||||
|
||||
.. literalinclude:: ../../examples/example_dist_bear.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Distance and bearing calculation
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
.. _getting_started:
|
||||
|
||||
Getting started
|
||||
===============
|
||||
|
||||
.. _download_library:
|
||||
|
||||
Download library
|
||||
^^^^^^^^^^^^^^^^
|
||||
|
||||
Library is primarly hosted on `Github <https://github.com/MaJerle/lwgps>`_.
|
||||
|
||||
* Download latest release from `releases area <https://github.com/MaJerle/lwgps/releases>`_ on Github
|
||||
* Clone `develop` branch for latest development
|
||||
|
||||
Download from releases
|
||||
**********************
|
||||
|
||||
All releases are available on Github `releases area <https://github.com/MaJerle/lwgps/releases>`_.
|
||||
|
||||
Clone from Github
|
||||
*****************
|
||||
|
||||
First-time clone
|
||||
""""""""""""""""
|
||||
|
||||
* Download and install ``git`` if not already
|
||||
* Open console and navigate to path in the system to clone repository to. Use command ``cd your_path``
|
||||
* Clone repository with one of available ``3`` options
|
||||
|
||||
* Run ``git clone --recurse-submodules https://github.com/MaJerle/lwgps`` command to clone entire repository, including submodules
|
||||
* Run ``git clone --recurse-submodules --branch develop https://github.com/MaJerle/lwgps`` to clone `development` branch, including submodules
|
||||
* Run ``git clone --recurse-submodules --branch master https://github.com/MaJerle/lwgps`` to clone `latest stable` branch, including submodules
|
||||
|
||||
* Navigate to ``examples`` directory and run favourite example
|
||||
|
||||
Update cloned to latest version
|
||||
"""""""""""""""""""""""""""""""
|
||||
|
||||
* Open console and navigate to path in the system where your resources repository is. Use command ``cd your_path``
|
||||
* Run ``git pull origin master --recurse-submodules`` command to pull latest changes and to fetch latest changes from submodules
|
||||
* Run ``git submodule foreach git pull origin master`` to update & merge all submodules
|
||||
|
||||
.. note::
|
||||
This is preferred option to use when you want to evaluate library and run prepared examples.
|
||||
Repository consists of multiple submodules which can be automatically downloaded when cloning and pulling changes from root repository.
|
||||
|
||||
|
||||
Add library to project - Generic
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
At this point it is assumed that you have successfully download library, either cloned it or from releases page.
|
||||
|
||||
* Copy ``lwgps`` folder to your project
|
||||
* Add ``lwgps/src/include`` folder to `include path` of your toolchain
|
||||
* Add source files from ``lwgps/src/`` folder to toolchain build
|
||||
* Copy ``lwgps/src/include/lwgps/lwgps_opts_template.h`` to project folder and rename it to ``lwgps_opts.h``
|
||||
* Build the project
|
||||
|
||||
Add library to project - CMake
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Including the library with CMake is very easy.
|
||||
|
||||
* Add the ``lwgps`` folder with ``add_subdirectory``
|
||||
* Copy ``lwgps/src/include/lwgps/lwgps_opts_template.h`` to the project folder and rename it
|
||||
to ``lwgps_opts.h``
|
||||
* Set the ``LWGPS_CONFIG_PATH`` path variable containing the ``lwgps_opts.h`` file
|
||||
in the project ``CMakeLists.txt``
|
||||
* Link your project against the ``lwgps`` library with ``target_link_libraries``
|
||||
* The include directory should be set automatically by CMake
|
||||
|
||||
Configuration file
|
||||
^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Library comes with template config file, which can be modified according to needs.
|
||||
This file shall be named ``lwgps_opts.h`` and its default template looks like the one below.
|
||||
|
||||
.. note::
|
||||
Default configuration template file location: ``lwgps/src/include/lwgps/lwgps_opts_template.h``.
|
||||
File must be renamed to ``lwgps_opts.h`` first and then copied to the project directory (or simply renamed in-place) where compiler
|
||||
include paths have access to it by using ``#include "lwgps_opts.h"``.
|
||||
|
||||
.. tip::
|
||||
Check :ref:`api_lwgps_opt` section for possible configuration settings
|
||||
|
||||
.. literalinclude:: ../../lwgps/src/include/lwgps/lwgps_opts_template.h
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Template options file
|
||||
|
||||
Minimal example code
|
||||
^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Run below example to test and verify library
|
||||
|
||||
.. literalinclude:: ../../examples/example.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Test verification code
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
LwGPS |version| documentation
|
||||
=============================
|
||||
|
||||
Welcome to the documentation for version |version|.
|
||||
|
||||
LwGPS is lightweight, platform independent library to parse NMEA statements from GPS receivers. It is highly optimized for embedded systems.
|
||||
|
||||
.. image:: static/images/logo.svg
|
||||
:align: center
|
||||
|
||||
.. rst-class:: center
|
||||
.. rst-class:: index_links
|
||||
|
||||
:ref:`download_library` :ref:`getting_started` `Open Github <https://github.com/MaJerle/lwgps>`_ `Donate <https://paypal.me/tilz0R>`_
|
||||
|
||||
Features
|
||||
^^^^^^^^
|
||||
|
||||
* Written in ANSI C99
|
||||
* Platform independent, easy to use
|
||||
* Built-in support for 4 GPS statements
|
||||
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
|
||||
* Optional ``float`` or ``double`` floating point units
|
||||
* Low-level layer is separated from application layer, thus allows you to add custom communication with GPS device
|
||||
* Works with operating systems
|
||||
* Works with different communication interfaces
|
||||
* User friendly MIT license
|
||||
|
||||
Requirements
|
||||
^^^^^^^^^^^^
|
||||
|
||||
* C compiler
|
||||
* Driver for receiving data from GPS receiver
|
||||
* Few *kB* of non-volatile memory
|
||||
|
||||
Contribute
|
||||
^^^^^^^^^^
|
||||
|
||||
Fresh contributions are always welcome. Simple instructions to proceed:
|
||||
|
||||
#. Fork Github repository
|
||||
#. Respect `C style & coding rules <https://github.com/MaJerle/c-code-style>`_ used by the library
|
||||
#. Create a pull request to ``develop`` branch with new features or bug fixes
|
||||
|
||||
Alternatively you may:
|
||||
|
||||
#. Report a bug
|
||||
#. Ask for a feature request
|
||||
|
||||
License
|
||||
^^^^^^^
|
||||
|
||||
.. literalinclude:: ../LICENSE
|
||||
|
||||
Table of contents
|
||||
^^^^^^^^^^^^^^^^^
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
||||
self
|
||||
get-started/index
|
||||
user-manual/index
|
||||
api-reference/index
|
||||
examples/index
|
||||
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
@ECHO OFF
|
||||
|
||||
pushd %~dp0
|
||||
|
||||
REM Command file for Sphinx documentation
|
||||
|
||||
if "%SPHINXBUILD%" == "" (
|
||||
set SPHINXBUILD=sphinx-build
|
||||
)
|
||||
set SOURCEDIR=.
|
||||
set BUILDDIR=_build
|
||||
|
||||
if "%1" == "" goto help
|
||||
|
||||
%SPHINXBUILD% >NUL 2>NUL
|
||||
if errorlevel 9009 (
|
||||
echo.
|
||||
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
|
||||
echo.installed, then set the SPHINXBUILD environment variable to point
|
||||
echo.to the full path of the 'sphinx-build' executable. Alternatively you
|
||||
echo.may add the Sphinx directory to PATH.
|
||||
echo.
|
||||
echo.If you don't have Sphinx installed, grab it from
|
||||
echo.http://sphinx-doc.org/
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
goto end
|
||||
|
||||
:help
|
||||
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
|
||||
:end
|
||||
popd
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
breathe>=4.9.1
|
||||
colorama
|
||||
docutils>=0.14
|
||||
sphinx>=2.0.1
|
||||
sphinx_rtd_theme
|
||||
sphinx-tabs
|
||||
sphinxcontrib-svg2pdfconverter
|
||||
sphinx-sitemap
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
/* Center aligned text */
|
||||
.center {
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
/* Paragraph with main links on index page */
|
||||
.index-links {
|
||||
text-align: center;
|
||||
margin-top: 10px;
|
||||
}
|
||||
.index-links a {
|
||||
display: inline-block;
|
||||
border: 1px solid #0E4263;
|
||||
padding: 5px 20px;
|
||||
margin: 2px 5px;
|
||||
background: #2980B9;
|
||||
border-radius: 4px;
|
||||
color: #FFFFFF;
|
||||
}
|
||||
.index-links a:hover, .index-links a:active {
|
||||
background: #0E4263;
|
||||
}
|
||||
|
||||
/* Table header p w/0 margin */
|
||||
.index-links a table thead th {
|
||||
vertical-align: middle;
|
||||
}
|
||||
|
||||
table thead th p {
|
||||
margin: 0;
|
||||
}
|
||||
|
||||
.table-nowrap td {
|
||||
white-space: normal !important;
|
||||
}
|
||||
|
||||
/* Breathe output changes */
|
||||
.breathe-sectiondef.container {
|
||||
background: #f9f9f9;
|
||||
padding: 10px;
|
||||
margin-bottom: 10px;
|
||||
border: 1px solid #efefef;
|
||||
}
|
||||
.breathe-sectiondef.container .breathe-sectiondef-title {
|
||||
background: #2980b9;
|
||||
color: #FFFFFF;
|
||||
padding: 4px;
|
||||
margin: -10px -10px 0 -10px;
|
||||
}
|
||||
.breathe-sectiondef.container .function,
|
||||
.breathe-sectiondef.container .member,
|
||||
.breathe-sectiondef.container .class,
|
||||
.breathe-sectiondef.container .type {
|
||||
border-bottom: 1px solid #efefef;
|
||||
}
|
||||
.breathe-sectiondef.container .function:last-child,
|
||||
.breathe-sectiondef.container .member:last-child,
|
||||
.breathe-sectiondef.container .class:last-child,
|
||||
.breathe-sectiondef.container .type:last-child {
|
||||
border-bottom: none;
|
||||
margin-bottom: 0;
|
||||
}
|
||||
|
||||
/*# sourceMappingURL=common.css.map */
|
||||
2
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/1w_bit_byte.svg
vendored
Normal file
2
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/1w_bit_byte.svg
vendored
Normal file
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 30 KiB |
1
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/1w_bit_byte.xml
vendored
Normal file
1
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/1w_bit_byte.xml
vendored
Normal file
|
|
@ -0,0 +1 @@
|
|||
<mxfile modified="2019-03-17T18:59:38.494Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" version="10.4.9" etag="K-7s2_uXvQkXh2xx6gpu" type="device"><diagram id="7c61e978-b8e5-555b-ca9a-a1ec85429089" name="Page-1">7Ztfb5swFMU/TR4n2RgIPC5d1z5s0rR06rMDJlgBHDlOk+zTz4BpSM2kaupyq6vmITXXf8C/wwPn0MzYTX2803xbfle5qGYByY8z9mUWBDQkzP5pK6e+ktB5X1hrmbtB58JS/hauSFx1L3OxuxholKqM3F4WM9U0IjMXNa61OlwOK1R1edYtXwuvsMx45VcfZW5KV6Vxeu64F3JdulMngdvfimebtVb7xp1vFrCi+/TdNR/WchvdlTxXh1GJ3c7YjVbK9K36eCOqlu2ArZ/39S+9z9etRWNeMyHoJzzxau+2vnxwV2ZOA41uP6KdQWZscSilEcstz9reg9Xf1kpTV/aI2mbFV6JaPGO4UZXStqtRjR2/KFRjnNw0cMfDEMuKMWI/tu7vw23tSWgjjqOS29edULUw+mSHuF4W9TPcLRikDvnhLChzpXIkZehq3N1C6+eFzxhtw5Gcpso8qiuChGoMRzX0qVIkVFM4qrFPNUBClQZwWOc+VoYFawSHNfGxhliwJnBYUx9rhASrvV3BsA6Ps2OuMRauISBX6nOdY+E6B+Q6YQd+IOHKCCBX3xDgsVkJgeM6YQmwGK2QAnKNfK5YrFYYAnJFbLbCOSBXxG4rIoBcEdutiAFyRey3ohiO65CqY/RbUQrIFbHfigNAroj9VhwBcvX91uJkhK2oxn79+vzTui9SiafutWU3gKykGfrpp0epxXlEXNlrW6y0ba3b1oOsZbNuz7frkbTblvVWacMtr8BeH7l/tF+Gb0Q7JOPdcqpoxxl/QSSSv3jjxtiU4v9Lct8Kfkh+ffd/Xc19m7osrSKtDLIW7xN6IatqVM8jkeShre+MVhsx6kmsh4njN5LpFS/Dhzx3rFLyFir5phdN+JVAPjxMmF4s4VcC+fAwYXqxhF8JYFg79ZIRS/iVAoa1w7oYw68UMKxlE6YXS/iVAoa1zDe9aMIvSgDTWjbxb4dY0i9KAONaNvGaEUv8RQlgXssmDByW/IsSwMCW+Z7rIw25vh28ahrCfD/4TXUq9WFIK8pOtS0iGyO03m87wa0IJJdFIbqNt5rtNoO0K9HrLI4i2xurx7tU7mWkwkVSZFORSpwlYlW8Ub79CitFwwmt/yFTsYfnn4R0faPf3bDbPw==</diagram></mxfile>
|
||||
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 8.8 KiB |
|
|
@ -0,0 +1 @@
|
|||
<mxfile modified="2019-03-17T18:28:51.770Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" etag="Lw4IH-C7q6o9UbXhWxg1" version="10.4.9" type="device"><diagram id="oLfy0yLwNNLmw2rXhwVB" name="Page-1">7VpNc9owEP01HMnYkmXsYyBpy6Fpm5CU9tIRtjCaGIsRopD++sq2DJZlkuCYkHaSQ0a7+rL2vd2VNunAwXzzkePF7DMLSdwBVrjpwIsOAD2/J3+niodcgaCfKyJOw1xl7xQ39A9R45RyRUOy1MYJxmJBF7oyYElCAqHpMOdsrQ+bsljfdIEjYihuAhyb2u80FDOltV1/1/GJ0GimtvaAOu8cF4OtXLGc4ZCtSyp42YEDzpjIW/PNgMSp6Qqz5PM+7OndfhgniXjOhC+Mrn6Ovv2KRugD/0a7v4bTUVet8hvHK3Xgz4Nbqfhy3QFuLNftT7hsRWnr9qbfSfdw8Xwhxay3uxWzIbL/9vx6pA4sHgorrmdUkJsFDlJ5LXkix87EPJaSLZt4ucixm9INkZ/bn9I4HrCY8Ww6nHoBCQKpXwrO7kmpZ+IhB1nZzlVrFEcjXJBNSaWs85GwORH8QQ4pei2FlGIq8JS8LgFfjJmVMN8qsSJbtF17h4dsKEgOgMcz4DFMS5LwPOW5lBKWEN2wZEPFuNT+IdvWmfStJibOewovAI8ZfclWPCBP846EmveZ0JRMj2osX+g4ibGgv3WfrUND7fCVUfnFO+Q9pCEPLaQvkZ9HzSr7WGWhrbPvW0hgHhFhLJSRY3vs5nyxTX82CMPZKglTL7tIQX3CM0/iiNKhdEe0TUcEj7ChdTcsNmvuh4n8jHHmfKgQfzQ28QGOmPPtGYx5I55YdSDgVzBt6onAfV1PBH5LoVsxpgjejQiTzr7YqOmZ8KCEA6hUYchzgvxpqWT7Zz3kucByPd9FyAY6ISxwBm0LOo68sqXDvHaIZnvgVYkGzdh0NxiYtzeDfTIYC51y+wm0j3I4plEidYEkC5Gd/TTIU3l7PlcdcxqG6Ya1eUbPRC0kDtvX87jdMxNHHePAsRIHfEY+fjQKHDU5vNWY7zWM+Q56YqFjuyJ4R/tgtCFq6a796mjDljK8/jjrvT/Oqo8zHWcHtnQlNJh3bMIggzB29/vw+tJM1heXd8NBjd4g2KGFlRhPSNzHwX2UJd+CSYqcp3ntuRU/tsyk7dbQy20haUcArybjcLwZXlm3V5zeB5NJ97+FycCkBrm9MEHnjcHkvsNUA5NTzYqnhqn3wixpJrbHiimZsHvqZlLx1m2Kh/HwbVJnyYP/yS5dfktXbGhXK6VHu3TVsumlBfF/gU2nIgnstUWS6kLIP/NKP/6rUualhbhTUmZXIUnXVoYinEqbEJ4mKppEnXL55O2y68nHeeNX/knZVfMXl9FYyl+HVwbNzIpb+/WzF10ebKRHd1DU00oUcZBJkTbqZ/XWNSsq1/+NdaFzNOtKcfePBTnVd/+cAS//Ag==</diagram></mxfile>
|
||||
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 11 KiB |
|
|
@ -0,0 +1 @@
|
|||
<mxfile modified="2019-03-17T18:24:31.030Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" version="10.4.9" etag="GRhl1qzylYBum7KQYHDi" type="device"><diagram id="85174833-9250-f023-2320-fd733ea284ef" name="Page-1">7Vxdd6I6FP01PraLBMLHY9V2pg8zd1Y7HWceKUTNGgQH01Hvr78BgkKigsiHt8WXkgMEOHvvk5ND6EAdLTafQns5/xK42BtAxd0M1PEAQqApKvsTWbaJxQRGYpiFxOUH7Q3P5F+cGBE3vhEXr3LH0SDwKFnmjU7g+9ihOZsdhsE6f9g08PIXXdozLBmeHduTrRPi0jl/CKTs7Z8xmc3TKwOF73m1nd+zMHjz+fUGUJ3Gv2T3wk774sev5rYbrDMm9X6gjsIgoMnWYjPCXuTa1GvJeQ9H9u7uO8Q+LXOCNRk++KOvD5//jP3J2Jj88TXnBqT9/LW9N5w+SHy7dJu6CPvuXeRp1vIDnxmHc7rwWAuwTbwh9CfbVm4Rb/2KWmx7SjxvFHhBGPeiTk0HOw6zr2gY/MaZPa8m0pDCzx5v+OlxY8sbyTkpQMx3w+QusSuhu/cH2HmZkRcHC0zDLTtkvYc5RXmeQTi1hdizKfmb797mbJvtuttd4VtA2IWhwoUBLU5wLgtgqvkuVsFb6GB+Vha3oo4QyndE7XCGqdQR28g89t4U0+IMiiBdYsiX0Qsz/PM0gLrHvDZ8DdnWLNp6eWbAMAfq9mLJmvHem10zPoTtf7l7+i6xbD0nFD8vbSdqr1msyfPMXi0T+U/JBruV6MWfA4cUb84lDT9BwAIanCwZTu0CRJZUO+MhAuUQOxseoyYBg6x8byGq5OLqOmU3HAuikIldCRqYSh57S9BhaUGr6HRHTQvaLGZMPK5FMoujb4E0O1Ei1MT4KksRnmBD7ULUlXc+kpZRqNnpkIu0WwOZOlR009IRAjDPEAPeqkBRNc2E8WFmRf0icUCGrepXl1O2H6ORPBZL7GNio3nKHSfQMcrZHpn5zOYwbmC2cxiJmLB0+o7vWBDXjS54MGjkw0oTUUCTo4B+gHGwsSigNTIcG/1wLA7HRh55TYS0rJxVkO9I1dodjnUkMQbcTB6f7mVFj+9/PI4O2CWGnZtLe/Yr9oa7CW1KJc7OTsZ3VciSwIFU+5Cy9caULc+DepyY7sC14VQivT0ZgeWgyVwRbjOpWdT8lcboqLFPteJWmmtVBaRMeE6iUmFc6So8q7owyRHz7tLh2RKmXYbQUdPh2foAdOqKJZpWE0ukjqB1a2Z+VqucMeQM/ftP1v72+FXijpyUX1mKDZAgQBNJAT71fisptgEl7z69G+/u0p6uvGvJ3j0v3rU5S+ms0qAJqInl3dK1BKEjKS1qOFJZao/22WhXr/yibiu/1qXFiQ+BtjDxrK5tsc7ftrblwkKPdnEArkvbYkdNoy2XJ06h7Xj2akWcDwi4iJNZ19AtdtQ04HW9+E1fESWV5vTd0+G3R1dWggaKcU1cqj5UFJGyYS4Bpc8MSsAt1rQq54FGt3kgUPrUoALcVYtTkrrFjhqH+7zcoIf7MnUX8aZxuEtkBjQktj87WufKYO+SEDuUBFFtjLkgQqJGPtT69l8F5dYAAeU4OS4qngFw6XLaD6G0oqlS5cCqtqy0dIVRD/cpuIWFk9WzZHEFZssFFQD6aun5cFcfR8XF1m1nyaCfFJWA26hrDixOilpXdz8pqqDuymO3qO6W66UA9AXTcoiLQFUumIrUabvIBfqKKXNCt0v0rbpGiyJWNk6mEssM38ck27y6SXaJJXn/gw+YBL92/gETSK/WJ0DnDIe1JUBtFy/gpbNZGbz3t0JVQqm2FaptV4VhP5stsWy96GOgql8V1Qc3a+7/aURy+P4fc6j3/wE=</diagram></mxfile>
|
||||
|
|
@ -0,0 +1 @@
|
|||
<mxfile host="Electron" modified="2020-12-05T00:17:49.072Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/12.3.2 Chrome/78.0.3904.113 Electron/7.1.2 Safari/537.36" etag="J3RPSOEyywhlMJWRwdgH" version="12.3.2" type="device" pages="1"><diagram id="Wsjaadh77UIxB9X1bxos" name="Page-1">jZJNb8MgDIZ/TY6VkpB067Vd10nrqkk59IyCG1AhRJSMdL9+ZDH5UDVpJ/BjY+B9HZGd6g6GNvxDM5BRGrMuIi9RmibxU+aXntwHssnzAVRGMCyaQCG+IZxE2goGt0Wh1Vpa0SxhqesaSrtg1BjtlmUXLZe3NrSCB1CUVD7Ss2CWD/Q5jyf+BqLi4eYkxoyioRjBjVOm3QyRfUR2Rms77FS3A9mLF3QZzr3+kR0fZqC2/zlwWrEMNsodT6rdZpv3izwnK+zyRWWLHz66w2eBL7b3IIPRbc2g75REZOu4sFA0tOyzzhvvGbdKYvpmjb6Ocq09uejaorekj0ctYh88fiS8CoyFbobwYwfQCqy5+xLMkgynCqcsDbGbPEvWaASf+RV8pDgm1dh6UtJvUMwQTqb95majT/Y/</diagram></mxfile>
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
|
||||
<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="166px" height="56px" viewBox="-0.5 -0.5 166 56" content="<mxfile host="Electron" modified="2020-12-05T00:17:54.452Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/12.3.2 Chrome/78.0.3904.113 Electron/7.1.2 Safari/537.36" etag="QrTFHCUJlsVW3_ulGR16" version="12.3.2" type="device" pages="1"><diagram id="Wsjaadh77UIxB9X1bxos" name="Page-1">jZJNb8MgDIZ/TY6VkpB067Vd10nrqkk59IyCG1AhRJSMdL9+ZDH5UDVpJ/BjY+B9HZGd6g6GNvxDM5BRGrMuIi9RmibxU+aXntwHssnzAVRGMCyaQCG+IZxE2goGt0Wh1Vpa0SxhqesaSrtg1BjtlmUXLZe3NrSCB1CUVD7Ss2CWD/Q5jyf+BqLi4eYkxoyioRjBjVOm3QyRfUR2Rms77FS3A9mLF3QZzr3+kR0fZqC2/zlwWrEMNsodT6rdZpv3izwnK+zyRWWLHz66w2eBL7b3IIPRbc2g75REZOu4sFA0tOyzzhvvGbdKYvpmjb6Ocq09uejaorekj0ctYh88fiS8CoyFbobwYwfQCqy5+xLMkgynCqcsDbGbPEvWaASf+RV8pDgm1dh6UtJvUMwQTqb95majT/Y/</diagram></mxfile>"><defs/><g><rect x="3" y="3" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="#000000" stroke-width="6" pointer-events="all"/><g transform="translate(21.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="122" height="41" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 36px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 123px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">LwGPS</div></div></foreignObject><text x="61" y="39" fill="#000000" text-anchor="middle" font-size="36px" font-family="Helvetica">LwGPS</text></switch></g></g></svg>
|
||||
|
After Width: | Height: | Size: 2.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 3.3 KiB |
BIN
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/logo_tm_full.png
vendored
Normal file
BIN
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/static/images/logo_tm_full.png
vendored
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 17 KiB |
15
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/float-double.rst
vendored
Normal file
15
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/float-double.rst
vendored
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
.. _float_double:
|
||||
|
||||
Float/double precision
|
||||
======================
|
||||
|
||||
With configuration of ``GSM_CFG_DOUBLE``, it is possible to enable ``double`` floating point precision.
|
||||
All floating point variables are then configured in *double precision*.
|
||||
|
||||
When configuration is set to ``0``, floating point variables are configured in *single precision* format.
|
||||
|
||||
.. note::
|
||||
Single precision uses less memory in application. As a drawback, application may be a subject of data loss at latter digits.
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
38
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/how-it-works.rst
vendored
Normal file
38
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/how-it-works.rst
vendored
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
.. _how_it_works:
|
||||
|
||||
How it works
|
||||
============
|
||||
|
||||
LwGPS parses raw data formatted as NMEA 0183 statements from GPS receivers. It supports up to ``4`` different statements:
|
||||
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
|
||||
.. tip::
|
||||
By changing different configuration options, it is possible to disable some statements.
|
||||
Check :ref:`api_lwgps_opt` for more information.
|
||||
|
||||
Application must assure to properly receive data from GPS receiver.
|
||||
Usually GPS receivers communicate with host embedded system with UART protocol and output directly formatted NMEA 0183 statements.
|
||||
|
||||
.. note::
|
||||
Application must take care of properly receive data from GPS.
|
||||
|
||||
Application must use :cpp:func:`lwgps_process` function for data processing. Function will:
|
||||
|
||||
* Detect statement type, such as *GPGGA* or *GPGSV*
|
||||
* Parse all the terms of specific statement
|
||||
* Check valid CRC after each statement
|
||||
|
||||
Programmer's model is as following:
|
||||
|
||||
* Application receives data from GPS receiver
|
||||
* Application sends data to :cpp:func:`lwgps_process` function
|
||||
* Application uses processed data to display altitude, latitude, longitude, and other parameters
|
||||
|
||||
Check :ref:`examples` for typical example
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
.. _um:
|
||||
|
||||
User manual
|
||||
===========
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
||||
how-it-works
|
||||
float-double
|
||||
thread-safety
|
||||
tests
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
.. _tests:
|
||||
|
||||
Tests during development
|
||||
========================
|
||||
|
||||
During the development, test check is performed to validate raw NMEA input data vs expected result.
|
||||
|
||||
.. literalinclude:: ../../examples/test_code.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Test code for development
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
16
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/thread-safety.rst
vendored
Normal file
16
1.Software/PDR 1.04/3rdparty/lwgps-develop/docs/user-manual/thread-safety.rst
vendored
Normal file
|
|
@ -0,0 +1,16 @@
|
|||
.. _thread_safety:
|
||||
|
||||
Thread safety
|
||||
=============
|
||||
|
||||
Library tends to be as simple as possible.
|
||||
No specific features have been implemented for thread safety.
|
||||
|
||||
When library is using multi-thread environment and if multi threads tend to access to shared resources,
|
||||
user must resolve it with care, using mutual exclusion.
|
||||
|
||||
.. tip::
|
||||
When single thread is dedicated for GPS processing, no special mutual exclusion is necessary.
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
# gps-nmea-parser examples and tests Makefile
|
||||
# (c) 2020 Sirio, Balmelli Analog & Digital
|
||||
|
||||
TARGETS := \
|
||||
example.exe \
|
||||
example_stat.exe \
|
||||
test_code.exe \
|
||||
test_time.exe
|
||||
|
||||
.PHONY: all clean test
|
||||
all: $(TARGETS)
|
||||
|
||||
clean:
|
||||
@rm -fv $(TARGETS)
|
||||
|
||||
test: $(TARGETS)
|
||||
@for tgt in $(TARGETS); do \
|
||||
echo "\n--- $$tgt ---"; \
|
||||
./$$tgt; \
|
||||
done
|
||||
|
||||
CFLAGS += -Wall \
|
||||
-DDEBUG=1 \
|
||||
-I../lwgps/src/include \
|
||||
-I./
|
||||
|
||||
example.exe: example.c
|
||||
|
||||
example_stat.exe: CFLAGS += -DLWGPS_CFG_STATUS=1
|
||||
example_stat.exe: example_stat.c
|
||||
|
||||
test_code.exe: ../lwgps/src/lwgps/lwgps.c test_code.c ../dev/VisualStudio/main.c
|
||||
|
||||
test_time.exe: CFLAGS += \
|
||||
-DLWGPS_CFG_STATEMENT_PUBX=1 \
|
||||
-DLWGPS_CFG_STATEMENT_PUBX_TIME=1
|
||||
test_time.exe: ../lwgps/src/lwgps/lwgps.c test_time.c ../dev/VisualStudio/main.c
|
||||
|
||||
$(TARGETS) : ../lwgps/src/lwgps/lwgps.c
|
||||
$(CC) -o $@ $(CFLAGS) $^
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/**
|
||||
* This example uses direct processing function
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
|
||||
int
|
||||
main() {
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data));
|
||||
|
||||
/* Print messages */
|
||||
printf("Valid status: %d\r\n", hgps.is_valid);
|
||||
printf("Latitude: %f degrees\r\n", hgps.latitude);
|
||||
printf("Longitude: %f degrees\r\n", hgps.longitude);
|
||||
printf("Altitude: %f meters\r\n", hgps.altitude);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#include "lwgps/lwgps.h"
|
||||
#include "lwrb/lwrb.h"
|
||||
#include <string.h>
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/* GPS buffer */
|
||||
lwrb_t hgps_buff;
|
||||
uint8_t hgps_buff_data[12];
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
* \note This data are used to fake UART receive event on microcontroller
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
static size_t write_ptr;
|
||||
static void uart_irqhandler(void);
|
||||
|
||||
int
|
||||
main() {
|
||||
uint8_t rx;
|
||||
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Create buffer for received data */
|
||||
lwrb_init(&hgps_buff, hgps_buff_data, sizeof(hgps_buff_data));
|
||||
|
||||
while (1) {
|
||||
/* Add new character to buffer */
|
||||
/* Fake UART interrupt handler on host microcontroller */
|
||||
uart_irqhandler();
|
||||
|
||||
/* Process all input data */
|
||||
/* Read from buffer byte-by-byte and call processing function */
|
||||
if (lwrb_get_full(&hgps_buff)) { /* Check if anything in buffer now */
|
||||
while (lwrb_read(&hgps_buff, &rx, 1) == 1) {
|
||||
lwgps_process(&hgps, &rx, 1); /* Process byte-by-byte */
|
||||
}
|
||||
} else {
|
||||
/* Print all data after successful processing */
|
||||
printf("Latitude: %f degrees\r\n", hgps.latitude);
|
||||
printf("Longitude: %f degrees\r\n", hgps.longitude);
|
||||
printf("Altitude: %f meters\r\n", hgps.altitude);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Interrupt handler routing for UART received character
|
||||
* \note This is not real MCU, it is software method, called from main
|
||||
*/
|
||||
static void
|
||||
uart_irqhandler(void) {
|
||||
/* Make interrupt handler as fast as possible */
|
||||
/* Only write to received buffer and process later */
|
||||
if (write_ptr < strlen(gps_rx_data)) {
|
||||
/* Write to buffer only */
|
||||
lwrb_write(&hgps_buff, &gps_rx_data[write_ptr], 1);
|
||||
++write_ptr;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* Distance and bearing results */
|
||||
lwgps_float_t dist, bear;
|
||||
|
||||
/* New York coordinates */
|
||||
lwgps_float_t lat1 = 40.685721;
|
||||
lwgps_float_t lon1 = -73.820465;
|
||||
|
||||
/* Munich coordinates */
|
||||
lwgps_float_t lat2 = 48.150906;
|
||||
lwgps_float_t lon2 = 11.554176;
|
||||
|
||||
/* Go from New York to Munich */
|
||||
/* Calculate distance and bearing related to north */
|
||||
lwgps_distance_bearing(lat1, lon1, lat2, lon2, &dist, &bear);
|
||||
printf("Distance: %f meters\r\n", (float)dist);
|
||||
printf("Initial bearing: %f degrees\r\n", (float)bear);
|
||||
|
||||
/* Go from Munich to New York */
|
||||
/* Calculate distance and bearing related to north */
|
||||
lwgps_distance_bearing(lat2, lon2, lat1, lon1, &dist, &bear);
|
||||
printf("Distance: %f meters\r\n", (float)dist);
|
||||
printf("Initial bearing: %f degrees\r\n", (float)bear);
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
/**
|
||||
* This example tests the callback functionality of lwgps_process()
|
||||
* when the LWGPS_CFG_STATUS flag is set.
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
#if !LWGPS_CFG_STATUS
|
||||
#error "this test must be compiled with -DLWGPS_CFG_STATUS=1"
|
||||
#endif /* !LWGPS_CFG_STATUS */
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
|
||||
const lwgps_statement_t expected[] = {
|
||||
STAT_RMC,
|
||||
STAT_UNKNOWN,
|
||||
STAT_GGA,
|
||||
STAT_GSA,
|
||||
STAT_GSV,
|
||||
STAT_GSV,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_CHECKSUM_FAIL,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_RMC,
|
||||
STAT_UNKNOWN
|
||||
};
|
||||
|
||||
static int err_cnt;
|
||||
|
||||
void
|
||||
callback(lwgps_statement_t res) {
|
||||
static int i;
|
||||
|
||||
if (res != expected[i]) {
|
||||
printf("failed i %d, expected res %d but received %d\n",
|
||||
i, expected[i], res);
|
||||
++err_cnt;
|
||||
}
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
int
|
||||
main() {
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data), callback);
|
||||
|
||||
return err_cnt;
|
||||
}
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
#include "test_common.h"
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"";
|
||||
|
||||
/**
|
||||
* \brief Run the test of raw input data
|
||||
*/
|
||||
void
|
||||
run_tests() {
|
||||
lwgps_init(&hgps); /* Init GPS */
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data));
|
||||
|
||||
/* Run the test */
|
||||
RUN_TEST(!INT_IS_EQUAL(hgps.is_valid, 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.fix, 1));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.fix_mode, 3));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.latitude, 39.1226000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.longitude, -121.0413666666));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.altitude, 646.4000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.course, 360.0000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_p, 1.6000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_h, 1.6000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_v, 1.0000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.speed, 0.0000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.geo_sep, -24.100000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.variation, 15.500000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.sats_in_view, 8));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.sats_in_use, 5));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[0], 2));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[1], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[2], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[3], 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[4], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[5], 9));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[6], 24));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[7], 26));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[8], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[9], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[10], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[11], 0));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 8));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 3));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 1));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 18));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 37));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 30));
|
||||
}
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
#ifndef TEST_COMMON_HDR_H
|
||||
#define TEST_COMMON_HDR_H
|
||||
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#define RUN_TEST(x) do { \
|
||||
if ((x)) { \
|
||||
printf("Test passed on line %u with condition " # x "\r\n", (unsigned)__LINE__); \
|
||||
} else { \
|
||||
printf("Test FAILED on line %u with condition " # x "\r\n", (unsigned)__LINE__ ); \
|
||||
exit(1); \
|
||||
} \
|
||||
} while (0)
|
||||
#define FLT_IS_EQUAL(x, y) (fabs((double)(x) - (double)(y)) < 0.00001)
|
||||
#define INT_IS_EQUAL(x, y) ((int)((x) == (y)))
|
||||
|
||||
#endif /* TEST_COMMON_HDR_H */
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy PUBX TIME packets from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
#include "test_common.h"
|
||||
|
||||
#if !LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
#error "this test must be compiled with -DLWGPS_CFG_STATEMENT_PUBX_TIME=1"
|
||||
#endif /* !LWGPS_CFG_STATEMENT_PUBX_TIME */
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data_A[] = ""
|
||||
"$PUBX,04*37\r\n"
|
||||
"$PUBX,04,073731.00,091202,113851.00,1196,15D,1930035,-2660.664,43*71\r\n"
|
||||
"";
|
||||
const char
|
||||
gps_rx_data_B[] = ""
|
||||
"$PUBX,04,200714.00,230320,158834.00,2098,18,536057,257.043,16*12\r\b"
|
||||
"";
|
||||
|
||||
|
||||
/**
|
||||
* \brief Run the test of raw input data
|
||||
*/
|
||||
void
|
||||
run_tests() {
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process and test block A */
|
||||
lwgps_process(&hgps, gps_rx_data_A, strlen(gps_rx_data_A));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 37));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 31));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 9));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 12));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 2));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.utc_tow, 113851.00));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.utc_wk, 1196));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.leap_sec, 15));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.clk_bias, 1930035));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.clk_drift, -2660.664));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.tp_gran, 43));
|
||||
|
||||
/* Process and test block B */
|
||||
lwgps_process(&hgps, gps_rx_data_B, strlen(gps_rx_data_B));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 20));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 14));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 23));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 3));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 20));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.utc_tow, 158834.00));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.utc_wk, 2098));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.leap_sec, 18));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.clk_bias, 536057));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.clk_drift, 257.043));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.tp_gran, 16));
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
add_subdirectory(src)
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
add_subdirectory(include)
|
||||
add_subdirectory(lwgps)
|
||||
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
target_include_directories(${LIB_LWGPS_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
||||
target_include_directories(${LIB_LWGPS_NAME} PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
3
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/CMakeLists.txt
vendored
Normal file
3
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/CMakeLists.txt
vendored
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
target_sources(${LIB_LWGPS_NAME} PRIVATE
|
||||
lwgps.c
|
||||
)
|
||||
|
|
@ -0,0 +1,291 @@
|
|||
/**
|
||||
* \file lwgps.h
|
||||
* \brief GPS main file
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: v2.1.0
|
||||
*/
|
||||
#ifndef LWGPS_HDR_H
|
||||
#define LWGPS_HDR_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <limits.h>
|
||||
#include "lwgps/lwgps_opt.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif /* __cplusplus */
|
||||
|
||||
/**
|
||||
* \defgroup LWGPS Lightweight GPS NMEA parser
|
||||
* \brief Lightweight GPS NMEA parser
|
||||
* \{
|
||||
*/
|
||||
|
||||
/**
|
||||
* \brief GPS float definition, can be either `float` or `double`
|
||||
* \note Check for \ref LWGPS_CFG_DOUBLE configuration
|
||||
*/
|
||||
#if LWGPS_CFG_DOUBLE || __DOXYGEN__
|
||||
typedef double lwgps_float_t;
|
||||
#else
|
||||
typedef float lwgps_float_t;
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Satellite descriptor
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t num; /*!< Satellite number */
|
||||
uint8_t elevation; /*!< Elevation value */
|
||||
uint16_t azimuth; /*!< Azimuth in degrees */
|
||||
uint8_t snr; /*!< Signal-to-noise ratio */
|
||||
} lwgps_sat_t;
|
||||
|
||||
/**
|
||||
* \brief ENUM of possible GPS statements parsed
|
||||
*/
|
||||
typedef enum {
|
||||
STAT_UNKNOWN = 0, /*!< Unknown NMEA statement */
|
||||
STAT_GGA = 1, /*!< GPGGA statement */
|
||||
STAT_GSA = 2, /*!< GPGSA statement */
|
||||
STAT_GSV = 3, /*!< GPGSV statement */
|
||||
STAT_RMC = 4, /*!< GPRMC statement */
|
||||
STAT_UBX = 5, /*!< UBX statement (uBlox specific) */
|
||||
STAT_UBX_TIME = 6, /*!< UBX TIME statement (uBlox specific) */
|
||||
STAT_CHECKSUM_FAIL = UINT8_MAX /*!< Special case, used when checksum fails */
|
||||
} lwgps_statement_t;
|
||||
|
||||
/**
|
||||
* \brief GPS main structure
|
||||
*/
|
||||
typedef struct {
|
||||
#if LWGPS_CFG_STATEMENT_GPGGA || __DOXYGEN__
|
||||
/* Information related to GPGGA statement */
|
||||
lwgps_float_t latitude; /*!< Latitude in units of degrees */
|
||||
lwgps_float_t longitude; /*!< Longitude in units of degrees */
|
||||
lwgps_float_t altitude; /*!< Altitude in units of meters */
|
||||
lwgps_float_t geo_sep; /*!< Geoid separation in units of meters */
|
||||
uint8_t sats_in_use; /*!< Number of satellites in use */
|
||||
uint8_t fix; /*!< Fix status. `0` = invalid, `1` = GPS fix, `2` = DGPS fix, `3` = PPS fix */
|
||||
uint8_t hours; /*!< Hours in UTC */
|
||||
uint8_t minutes; /*!< Minutes in UTC */
|
||||
uint8_t seconds; /*!< Seconds in UTC */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGGA || __DOXYGEN__ */
|
||||
|
||||
#if LWGPS_CFG_STATEMENT_GPGSA || __DOXYGEN__
|
||||
/* Information related to GPGSA statement */
|
||||
lwgps_float_t dop_h; /*!< Dolution of precision, horizontal */
|
||||
lwgps_float_t dop_v; /*!< Dolution of precision, vertical */
|
||||
lwgps_float_t dop_p; /*!< Dolution of precision, position */
|
||||
uint8_t fix_mode; /*!< Fix mode. `1` = NO fix, `2` = 2D fix, `3` = 3D fix */
|
||||
uint8_t satellites_ids[12]; /*!< List of satellite IDs in use. Valid range is `0` to `sats_in_use` */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSA || __DOXYGEN__ */
|
||||
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV || __DOXYGEN__
|
||||
/* Information related to GPGSV statement */
|
||||
uint8_t sats_in_view; /*!< Number of satellites in view */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV_SAT_DET || __DOXYGEN__
|
||||
lwgps_sat_t sats_in_view_desc[12];
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV_SAT_DET || __DOXYGEN__ */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV || __DOXYGEN__ */
|
||||
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__
|
||||
/* Information related to GPRMC statement */
|
||||
uint8_t is_valid; /*!< GPS valid status */
|
||||
lwgps_float_t speed; /*!< Ground speed in knots */
|
||||
lwgps_float_t course; /*!< Ground coarse */
|
||||
lwgps_float_t variation; /*!< Magnetic variation */
|
||||
uint8_t date; /*!< Fix date */
|
||||
uint8_t month; /*!< Fix month */
|
||||
uint8_t year; /*!< Fix year */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__ */
|
||||
|
||||
#if LWGPS_CFG_STATEMENT_PUBX_TIME || __DOXYGEN__
|
||||
#if !LWGPS_CFG_STATEMENT_GPGGA && !__DOXYGEN__
|
||||
/* rely on time fields from GPGGA if possible */
|
||||
uint8_t hours;
|
||||
uint8_t minutes;
|
||||
uint8_t seconds;
|
||||
#endif /* !LWGPS_CFG_STATEMENT_GPGGA && !__DOXYGEN__ */
|
||||
#if !LWGPS_CFG_STATEMENT_GPRMC && !__DOXYGEN__
|
||||
/* rely on date fields from GPRMC if possible */
|
||||
uint8_t date;
|
||||
uint8_t month;
|
||||
uint8_t year;
|
||||
#endif /* !LWGPS_CFG_STATEMENT_GPRMC && !__DOXYGEN__ */
|
||||
/* fields only available in PUBX_TIME */
|
||||
lwgps_float_t utc_tow; /*!< UTC TimeOfWeek, eg 113851.00 */
|
||||
uint16_t utc_wk; /*!< UTC week number, continues beyond 1023 */
|
||||
uint8_t leap_sec; /*!< UTC leap seconds; UTC + leap_sec = TAI */
|
||||
uint32_t clk_bias; /*!< Receiver clock bias, eg 1930035 */
|
||||
lwgps_float_t clk_drift; /*!< Receiver clock drift, eg -2660.664 */
|
||||
uint32_t tp_gran; /*!< Time pulse granularity, eg 43 */
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME || __DOXYGEN__ */
|
||||
|
||||
#if !__DOXYGEN__
|
||||
struct {
|
||||
lwgps_statement_t stat; /*!< Statement index */
|
||||
char term_str[13]; /*!< Current term in string format */
|
||||
uint8_t term_pos; /*!< Current index position in term */
|
||||
uint8_t term_num; /*!< Current term number */
|
||||
|
||||
uint8_t star; /*!< Star detected flag */
|
||||
|
||||
#if LWGPS_CFG_CRC
|
||||
uint8_t crc_calc; /*!< Calculated CRC string */
|
||||
#endif /* LWGPS_CFG_CRC */
|
||||
|
||||
union {
|
||||
uint8_t dummy; /*!< Dummy byte */
|
||||
#if LWGPS_CFG_STATEMENT_GPGGA
|
||||
struct {
|
||||
lwgps_float_t latitude; /*!< GPS latitude position in degrees */
|
||||
lwgps_float_t longitude; /*!< GPS longitude position in degrees */
|
||||
lwgps_float_t altitude; /*!< GPS altitude in meters */
|
||||
lwgps_float_t geo_sep; /*!< Geoid separation in units of meters */
|
||||
uint8_t sats_in_use; /*!< Number of satellites currently in use */
|
||||
uint8_t fix; /*!< Type of current fix, `0` = Invalid, `1` = GPS fix, `2` = Differential GPS fix */
|
||||
uint8_t hours; /*!< Current UTC hours */
|
||||
uint8_t minutes; /*!< Current UTC minutes */
|
||||
uint8_t seconds; /*!< Current UTC seconds */
|
||||
} gga; /*!< GPGGA message */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSA
|
||||
struct {
|
||||
lwgps_float_t dop_h; /*!< Horizontal dilution of precision */
|
||||
lwgps_float_t dop_v; /*!< Vertical dilution of precision */
|
||||
lwgps_float_t dop_p; /*!< Position dilution of precision */
|
||||
uint8_t fix_mode; /*!< Fix mode, `1` = No fix, `2` = 2D fix, `3` = 3D fix */
|
||||
uint8_t satellites_ids[12]; /*!< IDs of satellites currently in use */
|
||||
} gsa; /*!< GPGSA message */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV
|
||||
struct {
|
||||
uint8_t sats_in_view; /*!< Number of stallites in view */
|
||||
uint8_t stat_num; /*!< Satellite line number during parsing GPGSV data */
|
||||
} gsv; /*!< GPGSV message */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC
|
||||
struct {
|
||||
uint8_t is_valid; /*!< Status whether GPS status is valid or not */
|
||||
uint8_t date; /*!< Current UTC date */
|
||||
uint8_t month; /*!< Current UTC month */
|
||||
uint8_t year; /*!< Current UTC year */
|
||||
lwgps_float_t speed; /*!< Current spead over the ground in knots */
|
||||
lwgps_float_t course; /*!< Current course over ground */
|
||||
lwgps_float_t variation; /*!< Current magnetic variation in degrees */
|
||||
} rmc; /*!< GPRMC message */
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
||||
#if LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
struct {
|
||||
uint8_t hours; /*!< Current UTC hours */
|
||||
uint8_t minutes; /*!< Current UTC minutes */
|
||||
uint8_t seconds; /*!< Current UTC seconds */
|
||||
uint8_t date; /*!< Current UTC date */
|
||||
uint8_t month; /*!< Current UTC month */
|
||||
uint8_t year; /*!< Current UTC year */
|
||||
lwgps_float_t utc_tow; /*!< UTC TimeOfWeek, eg 113851.00 */
|
||||
uint16_t utc_wk; /*!< UTC week number, continues beyond 1023 */
|
||||
uint8_t leap_sec; /*!< UTC leap seconds; UTC + leap_sec = TAI */
|
||||
uint32_t clk_bias; /*!< Receiver clock bias, eg 1930035 */
|
||||
lwgps_float_t clk_drift; /*!< Receiver clock drift, eg -2660.664 */
|
||||
uint32_t tp_gran; /*!< Time pulse granularity, eg 43 */
|
||||
} time; /*!< PUBX TIME message */
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
|
||||
} data; /*!< Union with data for each information */
|
||||
} p; /*!< Structure with private data */
|
||||
#endif /* !__DOXYGEN__ */
|
||||
} lwgps_t;
|
||||
|
||||
/**
|
||||
* \brief List of optional speed transformation from GPS values (in knots)
|
||||
*/
|
||||
typedef enum {
|
||||
/* Metric values */
|
||||
lwgps_speed_kps, /*!< Kilometers per second */
|
||||
lwgps_speed_kph, /*!< Kilometers per hour */
|
||||
lwgps_speed_mps, /*!< Meters per second */
|
||||
lwgps_speed_mpm, /*!< Meters per minute */
|
||||
|
||||
/* Imperial values */
|
||||
lwgps_speed_mips, /*!< Miles per second */
|
||||
lwgps_speed_mph, /*!< Miles per hour */
|
||||
lwgps_speed_fps, /*!< Foots per second */
|
||||
lwgps_speed_fpm, /*!< Foots per minute */
|
||||
|
||||
/* Optimized for runners/joggers */
|
||||
lwgps_speed_mpk, /*!< Minutes per kilometer */
|
||||
lwgps_speed_spk, /*!< Seconds per kilometer */
|
||||
lwgps_speed_sp100m, /*!< Seconds per 100 meters */
|
||||
lwgps_speed_mipm, /*!< Minutes per mile */
|
||||
lwgps_speed_spm, /*!< Seconds per mile */
|
||||
lwgps_speed_sp100y, /*!< Seconds per 100 yards */
|
||||
|
||||
/* Nautical values */
|
||||
lwgps_speed_smph, /*!< Sea miles per hour */
|
||||
} lwgps_speed_t;
|
||||
|
||||
/**
|
||||
* \brief Signature for caller-suplied callback function from gps_process
|
||||
* \param[in] res: statement type of recently parsed statement
|
||||
*/
|
||||
typedef void (*lwgps_process_fn)(lwgps_statement_t res);
|
||||
|
||||
/**
|
||||
* \brief Check if current GPS data contain valid signal
|
||||
* \note \ref LWGPS_CFG_STATEMENT_GPRMC must be enabled and `GPRMC` statement must be sent from GPS receiver
|
||||
* \param[in] _gh: GPS handle
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__
|
||||
#define lwgps_is_valid(_gh) ((_gh)->is_valid)
|
||||
#else
|
||||
#define lwgps_is_valid(_gh) (0)
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__ */
|
||||
|
||||
uint8_t lwgps_init(lwgps_t* gh);
|
||||
#if LWGPS_CFG_STATUS || __DOXYGEN__
|
||||
uint8_t lwgps_process(lwgps_t* gh, const void* data, size_t len, lwgps_process_fn evt_fn);
|
||||
#else /* LWGPS_CFG_STATUS */
|
||||
uint8_t lwgps_process(lwgps_t* gh, const void* data, size_t len);
|
||||
#endif /* !LWGPS_CFG_STATUS */
|
||||
uint8_t lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae, lwgps_float_t loe, lwgps_float_t* d, lwgps_float_t* b);
|
||||
lwgps_float_t lwgps_to_speed(lwgps_float_t sik, lwgps_speed_t ts);
|
||||
|
||||
/**
|
||||
* \}
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* __cplusplus */
|
||||
|
||||
#endif /* LWGPS_HDR_H */
|
||||
183
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/lwgps_opt.h
vendored
Normal file
183
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/lwgps_opt.h
vendored
Normal file
|
|
@ -0,0 +1,183 @@
|
|||
/**
|
||||
* \file lwgps_opt.h
|
||||
* \brief LwGPS options
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: $2.1.0$
|
||||
*/
|
||||
#ifndef LWGPS_HDR_OPT_H
|
||||
#define LWGPS_HDR_OPT_H
|
||||
|
||||
/* Uncomment to ignore user options (or set macro in compiler flags) */
|
||||
/* #define LWGPS_IGNORE_USER_OPTS */
|
||||
|
||||
/* Include application options */
|
||||
#ifndef LWGPS_IGNORE_USER_OPTS
|
||||
#include "lwgps_opts.h"
|
||||
#endif /* LWGPS_IGNORE_USER_OPTS */
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif /* __cplusplus */
|
||||
|
||||
/**
|
||||
* \defgroup LWGPS_OPT Configuration
|
||||
* \brief Default configuration setup
|
||||
* \{
|
||||
*/
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` `double precision` for floating point
|
||||
* values such as latitude, longitude, altitude.
|
||||
*
|
||||
* `double` is used as variable type when enabled, `float` when disabled.
|
||||
*/
|
||||
#ifndef LWGPS_CFG_DOUBLE
|
||||
#define LWGPS_CFG_DOUBLE 1
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` status reporting callback
|
||||
* by \ref lwgps_process
|
||||
*
|
||||
* \note This is an extension, so not enabled by default.
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATUS
|
||||
#define LWGPS_CFG_STATUS 0
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` `GGA` statement parsing.
|
||||
*
|
||||
* \note This statement must be enabled to parse:
|
||||
* - Latitude, Longitude, Altitude
|
||||
* - Number of satellites in use, fix (no fix, GPS, DGPS), UTC time
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_GPGGA
|
||||
#define LWGPS_CFG_STATEMENT_GPGGA 1
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` `GSA` statement parsing.
|
||||
*
|
||||
* \note This statement must be enabled to parse:
|
||||
* - Position/Vertical/Horizontal dilution of precision
|
||||
* - Fix mode (no fix, 2D, 3D fix)
|
||||
* - IDs of satellites in use
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_GPGSA
|
||||
#define LWGPS_CFG_STATEMENT_GPGSA 1
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` `RMC` statement parsing.
|
||||
*
|
||||
* \note This statement must be enabled to parse:
|
||||
* - Validity of GPS signal
|
||||
* - Ground speed in knots and coarse in degrees
|
||||
* - Magnetic variation
|
||||
* - UTC date
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_GPRMC
|
||||
#define LWGPS_CFG_STATEMENT_GPRMC 1
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` `GSV` statement parsing.
|
||||
*
|
||||
* \note This statement must be enabled to parse:
|
||||
* - Number of satellites in view
|
||||
* - Optional details of each satellite in view. See \ref LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_GPGSV
|
||||
#define LWGPS_CFG_STATEMENT_GPGSV 1
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` detailed parsing of each
|
||||
* satellite in view for `GSV` statement.
|
||||
*
|
||||
* \note When this feature is disabled, only number of "satellites in view" is parsed
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
|
||||
#define LWGPS_CFG_STATEMENT_GPGSV_SAT_DET 0
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` parsing and generation
|
||||
* of PUBX (uBlox) messages
|
||||
*
|
||||
* PUBX are a nonstandard ublox-specific extensions,
|
||||
* so disabled by default.
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_PUBX
|
||||
#define LWGPS_CFG_STATEMENT_PUBX 0
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` parsing and generation
|
||||
* of PUBX (uBlox) TIME messages.
|
||||
*
|
||||
* \note TIME messages can be used to obtain:
|
||||
* - UTC time of week
|
||||
* - UTC week number
|
||||
* - Leap seconds (allows conversion to eg. TAI)
|
||||
*
|
||||
* This is a nonstandard ublox-specific extension,
|
||||
* so disabled by default.
|
||||
*
|
||||
* This configure option requires LWGPS_CFG_STATEMENT_PUBX
|
||||
*/
|
||||
#ifndef LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
#define LWGPS_CFG_STATEMENT_PUBX_TIME 0
|
||||
#endif
|
||||
|
||||
/**
|
||||
* \brief Enables `1` or disables `0` CRC calculation and check
|
||||
*
|
||||
* \note When not enabled, CRC check is ignored
|
||||
*/
|
||||
#ifndef LWGPS_CFG_CRC
|
||||
#define LWGPS_CFG_CRC 1
|
||||
#endif
|
||||
|
||||
/* Guard against accidental parser breakage */
|
||||
#if LWGPS_CFG_STATEMENT_PUBX_TIME && !LWGPS_CFG_STATEMENT_PUBX
|
||||
#error LWGPS_CFG_STATEMENT_PUBX must be enabled when enabling LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME && !LWGPS_CFG_STATEMENT_PUBX */
|
||||
|
||||
/**
|
||||
* \}
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* __cplusplus */
|
||||
|
||||
#endif /* LWGPS_HDR_OPT_H */
|
||||
44
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/lwgps_opts_template.h
vendored
Normal file
44
1.Software/PDR 1.04/3rdparty/lwgps-develop/lwgps/src/include/lwgps/lwgps_opts_template.h
vendored
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
/**
|
||||
* \file lwgps_opts_template.h
|
||||
* \brief LwGPS configuration file
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: v2.1.0
|
||||
*/
|
||||
#ifndef LWGPS_HDR_OPTS_H
|
||||
#define LWGPS_HDR_OPTS_H
|
||||
|
||||
/* Rename this file to "lwgps_opts.h" for your application */
|
||||
|
||||
/*
|
||||
* Open "include/lwgps/lwgps_opt.h" and
|
||||
* copy & replace here settings you want to change values
|
||||
*/
|
||||
|
||||
#endif /* LWGPS_HDR_OPTS_H */
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
target_sources(${LIB_LWGPS_NAME} PRIVATE
|
||||
lwgps.c
|
||||
)
|
||||
|
|
@ -0,0 +1,614 @@
|
|||
/**
|
||||
* \file lwgps.c
|
||||
* \brief GPS main file
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: v2.1.0
|
||||
*/
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
#define FLT(x) ((lwgps_float_t)(x))
|
||||
#define D2R(x) FLT(FLT(x) * FLT(0.01745329251994)) /*!< Degrees to radians */
|
||||
#define R2D(x) FLT(FLT(x) * FLT(57.29577951308232))/*!< Radians to degrees */
|
||||
#define EARTH_RADIUS FLT(6371.0) /*!< Earth radius in units of kilometers */
|
||||
|
||||
#if LWGPS_CFG_CRC
|
||||
#define CRC_ADD(_gh, ch) (_gh)->p.crc_calc ^= (uint8_t)(ch)
|
||||
#else
|
||||
#define CRC_ADD(_gh, ch)
|
||||
#endif /* LWGPS_CFG_CRC */
|
||||
#define TERM_ADD(_gh, ch) do { \
|
||||
if ((_gh)->p.term_pos < (sizeof((_gh)->p.term_str) - 1)) { \
|
||||
(_gh)->p.term_str[(_gh)->p.term_pos] = (ch);\
|
||||
(_gh)->p.term_str[++(_gh)->p.term_pos] = 0; \
|
||||
} \
|
||||
} while (0)
|
||||
#define TERM_NEXT(_gh) do { (_gh)->p.term_str[((_gh)->p.term_pos = 0)] = 0; ++(_gh)->p.term_num; } while (0)
|
||||
|
||||
#define CIN(x) ((x) >= '0' && (x) <= '9')
|
||||
#define CIHN(x) (((x) >= '0' && (x) <= '9') || ((x) >= 'a' && (x) <= 'f') || ((x) >= 'A' && (x) <= 'F'))
|
||||
#define CTN(x) ((x) - '0')
|
||||
#define CHTN(x) (((x) >= '0' && (x) <= '9') ? ((x) - '0') : (((x) >= 'a' && (x) <= 'z') ? ((x) - 'a' + 10) : (((x) >= 'A' && (x) <= 'Z') ? ((x) - 'A' + 10) : 0)))
|
||||
|
||||
/**
|
||||
* \brief Parse number as integer
|
||||
* \param[in] gh: GPS handle
|
||||
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
|
||||
* \return Parsed integer
|
||||
*/
|
||||
static int32_t
|
||||
prv_parse_number(lwgps_t* gh, const char* t) {
|
||||
int32_t res = 0;
|
||||
uint8_t minus;
|
||||
|
||||
if (t == NULL) {
|
||||
t = gh->p.term_str;
|
||||
}
|
||||
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
|
||||
|
||||
minus = (*t == '-' ? (++t, 1) : 0);
|
||||
for (; t != NULL && CIN(*t); ++t) {
|
||||
res = 10 * res + CTN(*t);
|
||||
}
|
||||
return minus ? -res : res;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Parse number as double and convert it to \ref lwgps_float_t
|
||||
* \param[in] gh: GPS handle
|
||||
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
|
||||
* \return Parsed double in \ref lwgps_float_t format
|
||||
*/
|
||||
static lwgps_float_t
|
||||
prv_parse_float_number(lwgps_t* gh, const char* t) {
|
||||
lwgps_float_t res;
|
||||
|
||||
if (t == NULL) {
|
||||
t = gh->p.term_str;
|
||||
}
|
||||
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
|
||||
|
||||
#if LWGPS_CFG_DOUBLE
|
||||
res = strtod(t, NULL); /* Parse string to double */
|
||||
#else /* LWGPS_CFG_DOUBLE */
|
||||
res = strtof(t, NULL); /* Parse string to float */
|
||||
#endif /* !LWGPS_CFG_DOUBLE */
|
||||
|
||||
return FLT(res); /* Return casted value, based on float size */
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Parse latitude/longitude NMEA format to double
|
||||
*
|
||||
* NMEA output for latitude is ddmm.sss and longitude is dddmm.sss
|
||||
* \param[in] gh: GPS handle
|
||||
* \return Latitude/Longitude value in degrees
|
||||
*/
|
||||
static lwgps_float_t
|
||||
prv_parse_lat_long(lwgps_t* gh) {
|
||||
lwgps_float_t ll, deg, min;
|
||||
|
||||
ll = prv_parse_float_number(gh, NULL); /* Parse value as double */
|
||||
deg = FLT((int)((int)ll / 100)); /* Get absolute degrees value, interested in integer part only */
|
||||
min = ll - (deg * FLT(100)); /* Get remaining part from full number, minutes */
|
||||
ll = deg + (min / FLT(60.0)); /* Calculate latitude/longitude */
|
||||
|
||||
return ll;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Parse received term
|
||||
* \param[in] gh: GPS handle
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
static uint8_t
|
||||
prv_parse_term(lwgps_t* gh) {
|
||||
if (gh->p.term_num == 0) { /* Check string type */
|
||||
if (0) {
|
||||
#if LWGPS_CFG_STATEMENT_GPGGA
|
||||
} else if (!strncmp(gh->p.term_str, "$GPGGA", 6) || !strncmp(gh->p.term_str, "$GNGGA", 6)) {
|
||||
gh->p.stat = STAT_GGA;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSA
|
||||
} else if (!strncmp(gh->p.term_str, "$GPGSA", 6) || !strncmp(gh->p.term_str, "$GNGSA", 6)) {
|
||||
gh->p.stat = STAT_GSA;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV
|
||||
} else if (!strncmp(gh->p.term_str, "$GPGSV", 6) || !strncmp(gh->p.term_str, "$GNGSV", 6)) {
|
||||
gh->p.stat = STAT_GSV;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC
|
||||
} else if (!strncmp(gh->p.term_str, "$GPRMC", 6) || !strncmp(gh->p.term_str, "$GNRMC", 6)) {
|
||||
gh->p.stat = STAT_RMC;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
||||
#if LWGPS_CFG_STATEMENT_PUBX
|
||||
} else if (!strncmp(gh->p.term_str, "$PUBX", 5)) {
|
||||
gh->p.stat = STAT_UBX;
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX */
|
||||
} else {
|
||||
gh->p.stat = STAT_UNKNOWN; /* Invalid statement for library */
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Start parsing terms */
|
||||
if (gh->p.stat == STAT_UNKNOWN) {
|
||||
#if LWGPS_CFG_STATEMENT_GPGGA
|
||||
} else if (gh->p.stat == STAT_GGA) { /* Process GPGGA statement */
|
||||
switch (gh->p.term_num) {
|
||||
case 1: /* Process UTC time */
|
||||
gh->p.data.gga.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
||||
gh->p.data.gga.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
||||
gh->p.data.gga.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
||||
break;
|
||||
case 2: /* Latitude */
|
||||
gh->p.data.gga.latitude = prv_parse_lat_long(gh); /* Parse latitude */
|
||||
break;
|
||||
case 3: /* Latitude north/south information */
|
||||
if (gh->p.term_str[0] == 'S' || gh->p.term_str[0] == 's') {
|
||||
gh->p.data.gga.latitude = -gh->p.data.gga.latitude;
|
||||
}
|
||||
break;
|
||||
case 4: /* Longitude */
|
||||
gh->p.data.gga.longitude = prv_parse_lat_long(gh); /* Parse longitude */
|
||||
break;
|
||||
case 5: /* Longitude east/west information */
|
||||
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
|
||||
gh->p.data.gga.longitude = -gh->p.data.gga.longitude;
|
||||
}
|
||||
break;
|
||||
case 6: /* Fix status */
|
||||
gh->p.data.gga.fix = (uint8_t)prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 7: /* Satellites in use */
|
||||
gh->p.data.gga.sats_in_use = (uint8_t)prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 9: /* Altitude */
|
||||
gh->p.data.gga.altitude = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 11: /* Altitude above ellipsoid */
|
||||
gh->p.data.gga.geo_sep = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSA
|
||||
} else if (gh->p.stat == STAT_GSA) { /* Process GPGSA statement */
|
||||
switch (gh->p.term_num) {
|
||||
case 2: /* Process fix mode */
|
||||
gh->p.data.gsa.fix_mode = (uint8_t)prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 15: /* Process PDOP */
|
||||
gh->p.data.gsa.dop_p = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 16: /* Process HDOP */
|
||||
gh->p.data.gsa.dop_h = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 17: /* Process VDOP */
|
||||
gh->p.data.gsa.dop_v = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
default:
|
||||
/* Parse satellite IDs */
|
||||
if (gh->p.term_num >= 3 && gh->p.term_num <= 14) {
|
||||
gh->p.data.gsa.satellites_ids[gh->p.term_num - 3] = (uint8_t)prv_parse_number(gh, NULL);
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV
|
||||
} else if (gh->p.stat == STAT_GSV) { /* Process GPGSV statement */
|
||||
switch (gh->p.term_num) {
|
||||
case 2: /* Current GPGSV statement number */
|
||||
gh->p.data.gsv.stat_num = (uint8_t)prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 3: /* Process satellites in view */
|
||||
gh->p.data.gsv.sats_in_view = (uint8_t)prv_parse_number(gh, NULL);
|
||||
break;
|
||||
default:
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
|
||||
if (gh->p.term_num >= 4 && gh->p.term_num <= 19) { /* Check current term number */
|
||||
uint8_t index, term_num = gh->p.term_num - 4; /* Normalize term number from 4-19 to 0-15 */
|
||||
uint16_t value;
|
||||
|
||||
index = ((gh->p.data.gsv.stat_num - 1) << 0x02) + (term_num >> 2); /* Get array index */
|
||||
if (index < sizeof(gh->sats_in_view_desc) / sizeof(gh->sats_in_view_desc[0])) {
|
||||
value = (uint16_t)prv_parse_number(gh, NULL); /* Parse number as integer */
|
||||
switch (term_num & 0x03) {
|
||||
case 0:
|
||||
gh->sats_in_view_desc[index].num = value;
|
||||
break;
|
||||
case 1:
|
||||
gh->sats_in_view_desc[index].elevation = value;
|
||||
break;
|
||||
case 2:
|
||||
gh->sats_in_view_desc[index].azimuth = value;
|
||||
break;
|
||||
case 3:
|
||||
gh->sats_in_view_desc[index].snr = value;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV_SAT_DET */
|
||||
break;
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC
|
||||
} else if (gh->p.stat == STAT_RMC) { /* Process GPRMC statement */
|
||||
switch (gh->p.term_num) {
|
||||
case 2: /* Process valid status */
|
||||
gh->p.data.rmc.is_valid = (gh->p.term_str[0] == 'A');
|
||||
break;
|
||||
case 7: /* Process ground speed in knots */
|
||||
gh->p.data.rmc.speed = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 8: /* Process true ground coarse */
|
||||
gh->p.data.rmc.course = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 9: /* Process date */
|
||||
gh->p.data.rmc.date = (uint8_t)(10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]));
|
||||
gh->p.data.rmc.month = (uint8_t)(10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]));
|
||||
gh->p.data.rmc.year = (uint8_t)(10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]));
|
||||
break;
|
||||
case 10: /* Process magnetic variation */
|
||||
gh->p.data.rmc.variation = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 11: /* Process magnetic variation east/west */
|
||||
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
|
||||
gh->p.data.rmc.variation = -gh->p.data.rmc.variation;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
||||
#if LWGPS_CFG_STATEMENT_PUBX
|
||||
} else if (gh->p.stat == STAT_UBX) { /* Disambiguate generic PUBX statement */
|
||||
if (gh->p.term_str[0] == '0' && gh->p.term_str[1] == '4') {
|
||||
gh->p.stat = STAT_UBX_TIME;
|
||||
}
|
||||
#if LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
} else if (gh->p.stat == STAT_UBX_TIME) { /* Process PUBX (uBlox) TIME statement */
|
||||
switch (gh->p.term_num) {
|
||||
case 2: /* Process UTC time; ignore fractions of seconds */
|
||||
gh->p.data.time.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
||||
gh->p.data.time.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
||||
gh->p.data.time.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
||||
break;
|
||||
case 3: /* Process UTC date */
|
||||
gh->p.data.time.date = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
||||
gh->p.data.time.month = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
||||
gh->p.data.time.year = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
||||
break;
|
||||
case 4: /* Process UTC TimeOfWeek */
|
||||
gh->p.data.time.utc_tow = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 5: /* Process UTC WeekNumber */
|
||||
gh->p.data.time.utc_wk = prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 6: /* Process UTC leap seconds */
|
||||
/*
|
||||
* Accomodate a 2- or 3-digit leap second count
|
||||
* a trailing 'D' means this is the firmware's default value.
|
||||
*/
|
||||
if (gh->p.term_str[2] == 'D' || gh->p.term_str[2] == '\0') {
|
||||
gh->p.data.time.leap_sec = 10 * CTN(gh->p.term_str[0])
|
||||
+ CTN(gh->p.term_str[1]);
|
||||
} else {
|
||||
gh->p.data.time.leap_sec = 100 * CTN(gh->p.term_str[0])
|
||||
+ 10 * CTN(gh->p.term_str[1])
|
||||
+ CTN(gh->p.term_str[2]);
|
||||
}
|
||||
break;
|
||||
case 7: /* Process clock bias */
|
||||
gh->p.data.time.clk_bias = prv_parse_number(gh, NULL);
|
||||
break;
|
||||
case 8: /* Process clock drift */
|
||||
gh->p.data.time.clk_drift = prv_parse_float_number(gh, NULL);
|
||||
break;
|
||||
case 9: /* Process time pulse granularity */
|
||||
gh->p.data.time.tp_gran = prv_parse_number(gh, NULL);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX */
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
#if LWGPS_CFG_CRC
|
||||
/**
|
||||
* \brief Compare calculated CRC with received CRC
|
||||
* \param[in] gh: GPS handle
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
static uint8_t
|
||||
prv_check_crc(lwgps_t* gh) {
|
||||
uint8_t crc;
|
||||
crc = (uint8_t)((CHTN(gh->p.term_str[0]) & 0x0F) << 0x04) | (CHTN(gh->p.term_str[1]) & 0x0F); /* Convert received CRC from string (hex) to number */
|
||||
return gh->p.crc_calc == crc; /* They must match! */
|
||||
}
|
||||
#else
|
||||
#define prv_check_crc(_gh) (1)
|
||||
#endif /* LWGPS_CFG_CRC */
|
||||
|
||||
/**
|
||||
* \brief Copy temporary memory to user memory
|
||||
* \param[in] gh: GPS handle
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
static uint8_t
|
||||
prv_copy_from_tmp_memory(lwgps_t* gh) {
|
||||
if (0) {
|
||||
#if LWGPS_CFG_STATEMENT_GPGGA
|
||||
} else if (gh->p.stat == STAT_GGA) {
|
||||
gh->latitude = gh->p.data.gga.latitude;
|
||||
gh->longitude = gh->p.data.gga.longitude;
|
||||
gh->altitude = gh->p.data.gga.altitude;
|
||||
gh->geo_sep = gh->p.data.gga.geo_sep;
|
||||
gh->sats_in_use = gh->p.data.gga.sats_in_use;
|
||||
gh->fix = gh->p.data.gga.fix;
|
||||
gh->hours = gh->p.data.gga.hours;
|
||||
gh->minutes = gh->p.data.gga.minutes;
|
||||
gh->seconds = gh->p.data.gga.seconds;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSA
|
||||
} else if (gh->p.stat == STAT_GSA) {
|
||||
gh->dop_h = gh->p.data.gsa.dop_h;
|
||||
gh->dop_p = gh->p.data.gsa.dop_p;
|
||||
gh->dop_v = gh->p.data.gsa.dop_v;
|
||||
gh->fix_mode = gh->p.data.gsa.fix_mode;
|
||||
memcpy(gh->satellites_ids, gh->p.data.gsa.satellites_ids, sizeof(gh->satellites_ids));
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
||||
#if LWGPS_CFG_STATEMENT_GPGSV
|
||||
} else if (gh->p.stat == STAT_GSV) {
|
||||
gh->sats_in_view = gh->p.data.gsv.sats_in_view;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
||||
#if LWGPS_CFG_STATEMENT_GPRMC
|
||||
} else if (gh->p.stat == STAT_RMC) {
|
||||
gh->course = gh->p.data.rmc.course;
|
||||
gh->is_valid = gh->p.data.rmc.is_valid;
|
||||
gh->speed = gh->p.data.rmc.speed;
|
||||
gh->variation = gh->p.data.rmc.variation;
|
||||
gh->date = gh->p.data.rmc.date;
|
||||
gh->month = gh->p.data.rmc.month;
|
||||
gh->year = gh->p.data.rmc.year;
|
||||
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
||||
#if LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
} else if (gh->p.stat == STAT_UBX_TIME) {
|
||||
gh->hours = gh->p.data.time.hours;
|
||||
gh->minutes = gh->p.data.time.minutes;
|
||||
gh->seconds = gh->p.data.time.seconds;
|
||||
gh->date = gh->p.data.time.date;
|
||||
gh->month = gh->p.data.time.month;
|
||||
gh->year = gh->p.data.time.year;
|
||||
gh->utc_tow = gh->p.data.time.utc_tow;
|
||||
gh->utc_wk = gh->p.data.time.utc_wk;
|
||||
gh->leap_sec = gh->p.data.time.leap_sec;
|
||||
gh->clk_bias = gh->p.data.time.clk_bias;
|
||||
gh->clk_drift = gh->p.data.time.clk_drift;
|
||||
gh->tp_gran = gh->p.data.time.tp_gran;
|
||||
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Init GPS handle
|
||||
* \param[in] gh: GPS handle structure
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
uint8_t
|
||||
lwgps_init(lwgps_t* gh) {
|
||||
memset(gh, 0x00, sizeof(*gh)); /* Reset structure */
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Process NMEA data from GPS receiver
|
||||
* \param[in] gh: GPS handle structure
|
||||
* \param[in] data: Received data
|
||||
* \param[in] len: Number of bytes to process
|
||||
* \param[in] evt_fn: Event function to notify application layer.
|
||||
* This parameter is available only if \ref LWGPS_CFG_STATUS is enabled
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
uint8_t
|
||||
#if LWGPS_CFG_STATUS || __DOXYGEN__
|
||||
lwgps_process(lwgps_t* gh, const void* data, size_t len, lwgps_process_fn evt_fn) {
|
||||
#else /* LWGPS_CFG_STATUS */
|
||||
lwgps_process(lwgps_t* gh, const void* data, size_t len) {
|
||||
#endif /* !LWGPS_CFG_STATUS */
|
||||
const uint8_t* d = data;
|
||||
|
||||
for (; len > 0; ++d, --len) { /* Process all bytes */
|
||||
if (*d == '$') { /* Check for beginning of NMEA line */
|
||||
memset(&gh->p, 0x00, sizeof(gh->p));/* Reset private memory */
|
||||
TERM_ADD(gh, *d); /* Add character to term */
|
||||
} else if (*d == ',') { /* Term separator character */
|
||||
prv_parse_term(gh); /* Parse term we have currently in memory */
|
||||
CRC_ADD(gh, *d); /* Add character to CRC computation */
|
||||
TERM_NEXT(gh); /* Start with next term */
|
||||
} else if (*d == '*') { /* Start indicates end of data for CRC computation */
|
||||
prv_parse_term(gh); /* Parse term we have currently in memory */
|
||||
gh->p.star = 1; /* STAR detected */
|
||||
TERM_NEXT(gh); /* Start with next term */
|
||||
} else if (*d == '\r') {
|
||||
if (prv_check_crc(gh)) { /* Check for CRC result */
|
||||
/* CRC is OK, in theory we can copy data from statements to user data */
|
||||
prv_copy_from_tmp_memory(gh); /* Copy memory from temporary to user memory */
|
||||
#if LWGPS_CFG_STATUS
|
||||
if (evt_fn != NULL) {
|
||||
evt_fn(gh->p.stat);
|
||||
}
|
||||
} else if (evt_fn != NULL) {
|
||||
evt_fn(STAT_CHECKSUM_FAIL);
|
||||
#endif /* LWGPS_CFG_STATUS */
|
||||
}
|
||||
} else {
|
||||
if (!gh->p.star) { /* Add to CRC only if star not yet detected */
|
||||
CRC_ADD(gh, *d); /* Add to CRC */
|
||||
}
|
||||
TERM_ADD(gh, *d); /* Add character to term */
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Calculate distance and bearing between `2` latitude and longitude coordinates
|
||||
* \param[in] las: Latitude start coordinate, in units of degrees
|
||||
* \param[in] los: Longitude start coordinate, in units of degrees
|
||||
* \param[in] lae: Latitude end coordinate, in units of degrees
|
||||
* \param[in] loe: Longitude end coordinate, in units of degrees
|
||||
* \param[out] d: Pointer to output distance in units of meters
|
||||
* \param[out] b: Pointer to output bearing between start and end coordinate in relation to north in units of degrees
|
||||
* \return `1` on success, `0` otherwise
|
||||
*/
|
||||
uint8_t
|
||||
lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae, lwgps_float_t loe, lwgps_float_t* d, lwgps_float_t* b) {
|
||||
lwgps_float_t df, dfi, a;
|
||||
|
||||
if (d == NULL && b == NULL) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Convert degrees to radians */
|
||||
df = D2R(lae - las);
|
||||
dfi = D2R(loe - los);
|
||||
las = D2R(las);
|
||||
los = D2R(los);
|
||||
lae = D2R(lae);
|
||||
loe = D2R(loe);
|
||||
|
||||
/*
|
||||
* Calculate distance
|
||||
*
|
||||
* Calculated distance is absolute value in meters between 2 points on earth.
|
||||
*/
|
||||
if (d != NULL) {
|
||||
/*
|
||||
* a = sin(df / 2)^2 + cos(las) * cos(lae) * sin(dfi / 2)^2
|
||||
* *d = RADIUS * 2 * atan(sqrt(a) / sqrt(1 - a)) * 1000 (for meters)
|
||||
*/
|
||||
#if LWGPS_CFG_DOUBLE
|
||||
a = FLT(sin(df * 0.5) * sin(df * 0.5) + sin(dfi * 0.5) * sin(dfi * 0.5) * cos(las) * cos(lae));
|
||||
*d = FLT(EARTH_RADIUS * 2.0 * atan2(sqrt(a), sqrt(1.0 - a)) * 1000.0);
|
||||
#else /* LWGPS_CFG_DOUBLE */
|
||||
a = FLT(sinf(df * 0.5f) * sinf(df * 0.5f) + sinf(dfi * 0.5f) * sinf(dfi * 0.5f) * cosf(las) * cosf(lae));
|
||||
*d = FLT(EARTH_RADIUS * 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)) * 1000.0f);
|
||||
#endif /* !LWGPS_CFG_DOUBLE */
|
||||
}
|
||||
|
||||
/*
|
||||
* Calculate bearing
|
||||
*
|
||||
* Bearing is calculated from point 1 to point 2.
|
||||
* Result will tell us in which direction (according to north) we should move,
|
||||
* to reach point 2.
|
||||
*
|
||||
* Example:
|
||||
* Bearing is 0 => move to north
|
||||
* Bearing is 90 => move to east
|
||||
* Bearing is 180 => move to south
|
||||
* Bearing is 270 => move to west
|
||||
*/
|
||||
if (b != NULL) {
|
||||
#if LWGPS_CFG_DOUBLE
|
||||
df = FLT(sin(loe - los) * cos(lae));
|
||||
dfi = FLT(cos(las) * sin(lae) - sin(las) * cos(lae) * cos(loe - los));
|
||||
|
||||
*b = R2D(atan2(df, dfi)); /* Calculate bearing and convert to degrees */
|
||||
#else /* LWGPS_CFG_DOUBLE */
|
||||
df = FLT(sinf(loe - los) * cosf(lae));
|
||||
dfi = FLT(cosf(las) * sinf(lae) - sinf(las) * cosf(lae) * cosf(loe - los));
|
||||
|
||||
*b = R2D(atan2f(df, dfi)); /* Calculate bearing and convert to degrees */
|
||||
#endif /* !LWGPS_CFG_DOUBLE */
|
||||
if (*b < 0) { /* Check for negative angle */
|
||||
*b += FLT(360); /* Make bearing always positive */
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Convert NMEA GPS speed (in knots = nautical mile per hour) to different speed format
|
||||
* \param[in] sik: Speed in knots, received from GPS NMEA statement
|
||||
* \param[in] ts: Target speed to convert to from knots
|
||||
* \return Speed calculated from knots
|
||||
*/
|
||||
lwgps_float_t
|
||||
lwgps_to_speed(lwgps_float_t sik, lwgps_speed_t ts) {
|
||||
switch (ts) {
|
||||
case lwgps_speed_kps:
|
||||
return FLT(sik * FLT(0.000514));
|
||||
case lwgps_speed_kph:
|
||||
return FLT(sik * FLT(1.852));
|
||||
case lwgps_speed_mps:
|
||||
return FLT(sik * FLT(0.5144));
|
||||
case lwgps_speed_mpm:
|
||||
return FLT(sik * FLT(30.87));
|
||||
|
||||
case lwgps_speed_mips:
|
||||
return FLT(sik * FLT(0.0003197));
|
||||
case lwgps_speed_mph:
|
||||
return FLT(sik * FLT(1.151));
|
||||
case lwgps_speed_fps:
|
||||
return FLT(sik * FLT(1.688));
|
||||
case lwgps_speed_fpm:
|
||||
return FLT(sik * FLT(101.3));
|
||||
|
||||
case lwgps_speed_mpk:
|
||||
return FLT(sik * FLT(32.4));
|
||||
case lwgps_speed_spk:
|
||||
return FLT(sik * FLT(1944.0));
|
||||
case lwgps_speed_sp100m:
|
||||
return FLT(sik * FLT(194.4));
|
||||
case lwgps_speed_mipm:
|
||||
return FLT(sik * FLT(52.14));
|
||||
case lwgps_speed_spm:
|
||||
return FLT(sik * FLT(3128.0));
|
||||
case lwgps_speed_sp100y:
|
||||
return FLT(sik * FLT(177.7));
|
||||
|
||||
case lwgps_speed_smph:
|
||||
return FLT(sik * FLT(1.0));
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
|
@ -0,0 +1,8 @@
|
|||
*.o
|
||||
*~
|
||||
*.diff
|
||||
.*.swp
|
||||
minmea
|
||||
tests
|
||||
example
|
||||
*.exe
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
language: c
|
||||
compiler:
|
||||
- gcc
|
||||
- clang
|
||||
install: sudo apt-get install check
|
||||
script: make test
|
||||
notifications:
|
||||
email:
|
||||
on_success: change
|
||||
on_failure: change
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
|
||||
Version 2, December 2004
|
||||
|
||||
Copyright (C) 2004 Sam Hocevar <sam@hocevar.net>
|
||||
|
||||
Everyone is permitted to copy and distribute verbatim or modified
|
||||
copies of this license document, and changing it is allowed as long
|
||||
as the name is changed.
|
||||
|
||||
DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
|
||||
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
|
||||
|
||||
0. You just DO WHAT THE FUCK YOU WANT TO.
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
# Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
# This program is free software. It comes without any warranty, to the extent
|
||||
# permitted by applicable law. You can redistribute it and/or modify it under
|
||||
# the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
# published by Sam Hocevar. See the COPYING file for more details.
|
||||
|
||||
CFLAGS = -g -Wall -Wextra -Werror -std=c99
|
||||
CFLAGS += -D_POSIX_C_SOURCE=199309L -D_BSD_SOURCE -D_DEFAULT_SOURCE -D_DARWIN_C_SOURCE
|
||||
CFLAGS += $(shell pkg-config --cflags check)
|
||||
LDLIBS += $(shell pkg-config --libs check)
|
||||
|
||||
all: scan-build test example
|
||||
@echo "+++ All good."""
|
||||
|
||||
test: tests
|
||||
@echo "+++ Running Check test suite..."
|
||||
./tests
|
||||
|
||||
scan-build: clean
|
||||
@echo "+++ Running Clang Static Analyzer..."
|
||||
scan-build $(MAKE) tests
|
||||
|
||||
clean:
|
||||
$(RM) tests example *.o
|
||||
|
||||
tests: tests.o minmea.o
|
||||
example: example.o minmea.o
|
||||
tests.o: tests.c minmea.h
|
||||
minmea.o: minmea.c minmea.h
|
||||
|
||||
.PHONY: all test scan-build clean
|
||||
|
|
@ -0,0 +1,160 @@
|
|||
# minmea, a lightweight GPS NMEA 0183 parser library
|
||||
|
||||
[](https://travis-ci.org/kosma/minmea)
|
||||
|
||||
Minmea is a minimalistic GPS parser library written in pure C intended for
|
||||
resource-constrained platforms, especially microcontrollers and other embedded
|
||||
systems.
|
||||
|
||||
## Features
|
||||
|
||||
* Written in ISO C99.
|
||||
* No dynamic memory allocation.
|
||||
* No floating point usage in the core library.
|
||||
* Supports both fixed and floating point values.
|
||||
* One source file and one header - can't get any simpler.
|
||||
* Tested under Linux, OS X, Windows and embedded ARM GCC.
|
||||
* Easily extendable to support new sentences.
|
||||
* Complete with a test suite and static analysis.
|
||||
|
||||
## Supported sentences
|
||||
|
||||
* ``RMC`` (Recommended Minimum: position, velocity, time)
|
||||
* ``GGA`` (Fix Data)
|
||||
* ``GSA`` (DOP and active satellites)
|
||||
* ``GLL`` (Geographic Position: Latitude/Longitude)
|
||||
* ``GST`` (Pseudorange Noise Statistics)
|
||||
* ``GSV`` (Satellites in view)
|
||||
* ``VTG`` (Track made good and Ground speed)
|
||||
* ``ZDA`` (Time & Date - UTC, day, month, year and local time zone)
|
||||
|
||||
Adding support for more sentences is trivial; see ``minmea.c`` source. Good documentation
|
||||
on NMEA is at https://gpsd.gitlab.io/gpsd/NMEA.html
|
||||
|
||||
## Compatibility
|
||||
|
||||
Minmea runs out-of-the-box under most Unix-compatible systems. Support for non-Unix systems
|
||||
(including native Windows builds under MSVC) is provided via compatibility headers:
|
||||
|
||||
1. Define `MINMEA_INCLUDE_COMPAT` in the build environment.
|
||||
2. Add appropriate compatibility header from under `compat/` directory as `minmea_compat.h`.
|
||||
|
||||
## Fractional number format
|
||||
|
||||
Internally, minmea stores fractional numbers as pairs of two integers: ``{value, scale}``.
|
||||
For example, a value of ``"-123.456"`` would be parsed as ``{-123456, 1000}``. As this
|
||||
format is quite unwieldy, minmea provides the following convenience functions for converting
|
||||
to either fixed-point or floating-point format:
|
||||
|
||||
* ``minmea_rescale({-123456, 1000}, 10) => -1235``
|
||||
* ``minmea_float({-123456, 1000}) => -123.456``
|
||||
|
||||
The compound type ``struct minmea_float`` uses ``int_least32_t`` internally. Therefore,
|
||||
the coordinate precision is guaranteed to be at least ``[+-]DDDMM.MMMMM`` (five decimal digits)
|
||||
or ±2cm LSB at the equator. Note that GPS modules commonly only provide four deciminal digits
|
||||
(``[+-]DDDMM.MMMM``), which equates to ±20cm (0.0001 minute is 0.0001/60 degrees and one degree
|
||||
is about 111km) at the equator.
|
||||
|
||||
## Coordinate format
|
||||
|
||||
NMEA uses the clunky ``DDMM.MMMM`` format which, honestly, is not good in the internet era.
|
||||
Internally, minmea stores it as a fractional number (see above); for practical uses,
|
||||
the value should be probably converted to the DD.DDDDD floating point format using the
|
||||
following function:
|
||||
|
||||
* ``minmea_tocoord({-375165, 100}) => -37.860832``
|
||||
|
||||
The library doesn't perform this conversion automatically for the following reasons:
|
||||
|
||||
* The conversion is not reversible.
|
||||
* It requires floating point support.
|
||||
* The user might want to perform this conversion later on or retain the original values.
|
||||
|
||||
## Example
|
||||
|
||||
```c
|
||||
char line[MINMEA_MAX_LENGTH];
|
||||
while (fgets(line, sizeof(line), stdin) != NULL) {
|
||||
switch (minmea_sentence_id(line, false)) {
|
||||
case MINMEA_SENTENCE_RMC: {
|
||||
struct minmea_sentence_rmc frame;
|
||||
if (minmea_parse_rmc(&frame, line)) {
|
||||
printf("$RMC: raw coordinates and speed: (%d/%d,%d/%d) %d/%d\n",
|
||||
frame.latitude.value, frame.latitude.scale,
|
||||
frame.longitude.value, frame.longitude.scale,
|
||||
frame.speed.value, frame.speed.scale);
|
||||
printf("$RMC fixed-point coordinates and speed scaled to three decimal places: (%d,%d) %d\n",
|
||||
minmea_rescale(&frame.latitude, 1000),
|
||||
minmea_rescale(&frame.longitude, 1000),
|
||||
minmea_rescale(&frame.speed, 1000));
|
||||
printf("$RMC floating point degree coordinates and speed: (%f,%f) %f\n",
|
||||
minmea_tocoord(&frame.latitude),
|
||||
minmea_tocoord(&frame.longitude),
|
||||
minmea_tofloat(&frame.speed));
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_GGA: {
|
||||
struct minmea_sentence_gga frame;
|
||||
if (minmea_parse_gga(&frame, line)) {
|
||||
printf("$GGA: fix quality: %d\n", frame.fix_quality);
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_GSV: {
|
||||
struct minmea_sentence_gsv frame;
|
||||
if (minmea_parse_gsv(&frame, line)) {
|
||||
printf("$GSV: message %d of %d\n", frame.msg_nr, frame.total_msgs);
|
||||
printf("$GSV: sattelites in view: %d\n", frame.total_sats);
|
||||
for (int i = 0; i < 4; i++)
|
||||
printf("$GSV: sat nr %d, elevation: %d, azimuth: %d, snr: %d dbm\n",
|
||||
frame.sats[i].nr,
|
||||
frame.sats[i].elevation,
|
||||
frame.sats[i].azimuth,
|
||||
frame.sats[i].snr);
|
||||
}
|
||||
} break;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Integration with your project
|
||||
|
||||
Simply add ``minmea.[ch]`` to your project, ``#include "minmea.h"`` and you're
|
||||
good to go.
|
||||
|
||||
## Running unit tests
|
||||
|
||||
Building and running the tests requires the following:
|
||||
|
||||
* Check Framework (https://libcheck.github.io/check/).
|
||||
* Clang Static Analyzer (https://clang-analyzer.llvm.org/).
|
||||
|
||||
If you have both in your ``$PATH``, running the tests should be as simple as
|
||||
typing ``make``.
|
||||
|
||||
## Limitations
|
||||
|
||||
* Only a handful of frames is supported right now.
|
||||
* There's no support for omitting parts of the library from building. As
|
||||
a workaround, use the ``-ffunction-sections -Wl,--gc-sections`` linker flags
|
||||
(or equivalent) to remove the unused functions (parsers) from the final image.
|
||||
* Some systems lack ``timegm``. On these systems, the recommended course of
|
||||
action is to build with ``-Dtimegm=mktime`` which will work correctly as long
|
||||
the system runs in the default ``UTC`` timezone.
|
||||
|
||||
## Bugs
|
||||
|
||||
There are plenty. Report them on GitHub, or - even better - open a pull request.
|
||||
Please write unit tests for any new functions you add - it's fun!
|
||||
|
||||
## Licensing
|
||||
|
||||
Minmea is open source software; see ``COPYING`` for amusement. Email me if the
|
||||
license bothers you and I'll happily re-license under anything else under the sun.
|
||||
|
||||
## Author
|
||||
|
||||
Minmea was written by Kosma Moczek <kosma@kosma.pl> and Patryk Szymczak
|
||||
<patryk.szymczak@gmail.com> at Cloud Your Car, with bugs fixed by countless
|
||||
good people.
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
/*
|
||||
* Copyright © 2017 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
* This program is free software. It comes without any warranty, to the extent
|
||||
* permitted by applicable law. You can redistribute it and/or modify it under
|
||||
* the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
* published by Sam Hocevar. See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#if defined(__TI_ARM__)
|
||||
|
||||
// timespec definition
|
||||
#include <ti/sysbios/posix/types.h>
|
||||
|
||||
#define timegm mktime
|
||||
|
||||
#endif
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
/*
|
||||
* Copyright © 2017 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
* This program is free software. It comes without any warranty, to the extent
|
||||
* permitted by applicable law. You can redistribute it and/or modify it under
|
||||
* the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
* published by Sam Hocevar. See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#if defined(_MSC_VER)
|
||||
|
||||
#if !defined(HAVE_STRUCT_TIMESPEC)
|
||||
struct timespec {
|
||||
time_t tv_sec;
|
||||
long tv_nsec;
|
||||
};
|
||||
#endif
|
||||
|
||||
#define inline __inline
|
||||
#define timegm _mkgmtime
|
||||
|
||||
#endif
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
/*
|
||||
* Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
* This program is free software. It comes without any warranty, to the extent
|
||||
* permitted by applicable law. You can redistribute it and/or modify it under
|
||||
* the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
* published by Sam Hocevar. See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include "minmea.h"
|
||||
|
||||
#define INDENT_SPACES " "
|
||||
|
||||
int main(void)
|
||||
{
|
||||
char line[MINMEA_MAX_LENGTH];
|
||||
while (fgets(line, sizeof(line), stdin) != NULL) {
|
||||
printf("%s", line);
|
||||
switch (minmea_sentence_id(line, false)) {
|
||||
case MINMEA_SENTENCE_RMC: {
|
||||
struct minmea_sentence_rmc frame;
|
||||
if (minmea_parse_rmc(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxRMC: raw coordinates and speed: (%d/%d,%d/%d) %d/%d\n",
|
||||
frame.latitude.value, frame.latitude.scale,
|
||||
frame.longitude.value, frame.longitude.scale,
|
||||
frame.speed.value, frame.speed.scale);
|
||||
printf(INDENT_SPACES "$xxRMC fixed-point coordinates and speed scaled to three decimal places: (%d,%d) %d\n",
|
||||
minmea_rescale(&frame.latitude, 1000),
|
||||
minmea_rescale(&frame.longitude, 1000),
|
||||
minmea_rescale(&frame.speed, 1000));
|
||||
printf(INDENT_SPACES "$xxRMC floating point degree coordinates and speed: (%f,%f) %f\n",
|
||||
minmea_tocoord(&frame.latitude),
|
||||
minmea_tocoord(&frame.longitude),
|
||||
minmea_tofloat(&frame.speed));
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxRMC sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_GGA: {
|
||||
struct minmea_sentence_gga frame;
|
||||
if (minmea_parse_gga(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxGGA: fix quality: %d\n", frame.fix_quality);
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxGGA sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_GST: {
|
||||
struct minmea_sentence_gst frame;
|
||||
if (minmea_parse_gst(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxGST: raw latitude,longitude and altitude error deviation: (%d/%d,%d/%d,%d/%d)\n",
|
||||
frame.latitude_error_deviation.value, frame.latitude_error_deviation.scale,
|
||||
frame.longitude_error_deviation.value, frame.longitude_error_deviation.scale,
|
||||
frame.altitude_error_deviation.value, frame.altitude_error_deviation.scale);
|
||||
printf(INDENT_SPACES "$xxGST fixed point latitude,longitude and altitude error deviation"
|
||||
" scaled to one decimal place: (%d,%d,%d)\n",
|
||||
minmea_rescale(&frame.latitude_error_deviation, 10),
|
||||
minmea_rescale(&frame.longitude_error_deviation, 10),
|
||||
minmea_rescale(&frame.altitude_error_deviation, 10));
|
||||
printf(INDENT_SPACES "$xxGST floating point degree latitude, longitude and altitude error deviation: (%f,%f,%f)",
|
||||
minmea_tofloat(&frame.latitude_error_deviation),
|
||||
minmea_tofloat(&frame.longitude_error_deviation),
|
||||
minmea_tofloat(&frame.altitude_error_deviation));
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxGST sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_GSV: {
|
||||
struct minmea_sentence_gsv frame;
|
||||
if (minmea_parse_gsv(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxGSV: message %d of %d\n", frame.msg_nr, frame.total_msgs);
|
||||
printf(INDENT_SPACES "$xxGSV: sattelites in view: %d\n", frame.total_sats);
|
||||
for (int i = 0; i < 4; i++)
|
||||
printf(INDENT_SPACES "$xxGSV: sat nr %d, elevation: %d, azimuth: %d, snr: %d dbm\n",
|
||||
frame.sats[i].nr,
|
||||
frame.sats[i].elevation,
|
||||
frame.sats[i].azimuth,
|
||||
frame.sats[i].snr);
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxGSV sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_VTG: {
|
||||
struct minmea_sentence_vtg frame;
|
||||
if (minmea_parse_vtg(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxVTG: true track degrees = %f\n",
|
||||
minmea_tofloat(&frame.true_track_degrees));
|
||||
printf(INDENT_SPACES " magnetic track degrees = %f\n",
|
||||
minmea_tofloat(&frame.magnetic_track_degrees));
|
||||
printf(INDENT_SPACES " speed knots = %f\n",
|
||||
minmea_tofloat(&frame.speed_knots));
|
||||
printf(INDENT_SPACES " speed kph = %f\n",
|
||||
minmea_tofloat(&frame.speed_kph));
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxVTG sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_ZDA: {
|
||||
struct minmea_sentence_zda frame;
|
||||
if (minmea_parse_zda(&frame, line)) {
|
||||
printf(INDENT_SPACES "$xxZDA: %d:%d:%d %02d.%02d.%d UTC%+03d:%02d\n",
|
||||
frame.time.hours,
|
||||
frame.time.minutes,
|
||||
frame.time.seconds,
|
||||
frame.date.day,
|
||||
frame.date.month,
|
||||
frame.date.year,
|
||||
frame.hour_offset,
|
||||
frame.minute_offset);
|
||||
}
|
||||
else {
|
||||
printf(INDENT_SPACES "$xxZDA sentence is not parsed\n");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_INVALID: {
|
||||
printf(INDENT_SPACES "$xxxxx sentence is not valid\n");
|
||||
} break;
|
||||
|
||||
default: {
|
||||
printf(INDENT_SPACES "$xxxxx sentence is not parsed\n");
|
||||
} break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,645 @@
|
|||
/*
|
||||
* Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
* This program is free software. It comes without any warranty, to the extent
|
||||
* permitted by applicable law. You can redistribute it and/or modify it under
|
||||
* the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
* published by Sam Hocevar. See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#include "minmea.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <stdarg.h>
|
||||
#include <time.h>
|
||||
|
||||
#define boolstr(s) ((s) ? "true" : "false")
|
||||
|
||||
static int hex2int(char c)
|
||||
{
|
||||
if (c >= '0' && c <= '9')
|
||||
return c - '0';
|
||||
if (c >= 'A' && c <= 'F')
|
||||
return c - 'A' + 10;
|
||||
if (c >= 'a' && c <= 'f')
|
||||
return c - 'a' + 10;
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t minmea_checksum(const char *sentence)
|
||||
{
|
||||
// Support senteces with or without the starting dollar sign.
|
||||
if (*sentence == '$')
|
||||
sentence++;
|
||||
|
||||
uint8_t checksum = 0x00;
|
||||
|
||||
// The optional checksum is an XOR of all bytes between "$" and "*".
|
||||
while (*sentence && *sentence != '*')
|
||||
checksum ^= *sentence++;
|
||||
|
||||
return checksum;
|
||||
}
|
||||
|
||||
bool minmea_check(const char *sentence, bool strict)
|
||||
{
|
||||
uint8_t checksum = 0x00;
|
||||
|
||||
// Sequence length is limited.
|
||||
if (strlen(sentence) > MINMEA_MAX_LENGTH + 3)
|
||||
return false;
|
||||
|
||||
// A valid sentence starts with "$".
|
||||
if (*sentence++ != '$')
|
||||
return false;
|
||||
|
||||
// The optional checksum is an XOR of all bytes between "$" and "*".
|
||||
while (*sentence && *sentence != '*' && isprint((unsigned char) *sentence))
|
||||
checksum ^= *sentence++;
|
||||
|
||||
// If checksum is present...
|
||||
if (*sentence == '*') {
|
||||
// Extract checksum.
|
||||
sentence++;
|
||||
int upper = hex2int(*sentence++);
|
||||
if (upper == -1)
|
||||
return false;
|
||||
int lower = hex2int(*sentence++);
|
||||
if (lower == -1)
|
||||
return false;
|
||||
int expected = upper << 4 | lower;
|
||||
|
||||
// Check for checksum mismatch.
|
||||
if (checksum != expected)
|
||||
return false;
|
||||
} else if (strict) {
|
||||
// Discard non-checksummed frames in strict mode.
|
||||
return false;
|
||||
}
|
||||
|
||||
// The only stuff allowed at this point is a newline.
|
||||
if (*sentence && strcmp(sentence, "\n") && strcmp(sentence, "\r\n"))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static inline bool minmea_isfield(char c) {
|
||||
return isprint((unsigned char) c) && c != ',' && c != '*';
|
||||
}
|
||||
|
||||
bool minmea_scan(const char *sentence, const char *format, ...)
|
||||
{
|
||||
bool result = false;
|
||||
bool optional = false;
|
||||
va_list ap;
|
||||
va_start(ap, format);
|
||||
|
||||
const char *field = sentence;
|
||||
#define next_field() \
|
||||
do { \
|
||||
/* Progress to the next field. */ \
|
||||
while (minmea_isfield(*sentence)) \
|
||||
sentence++; \
|
||||
/* Make sure there is a field there. */ \
|
||||
if (*sentence == ',') { \
|
||||
sentence++; \
|
||||
field = sentence; \
|
||||
} else { \
|
||||
field = NULL; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
while (*format) {
|
||||
char type = *format++;
|
||||
|
||||
if (type == ';') {
|
||||
// All further fields are optional.
|
||||
optional = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!field && !optional) {
|
||||
// Field requested but we ran out if input. Bail out.
|
||||
goto parse_error;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case 'c': { // Single character field (char).
|
||||
char value = '\0';
|
||||
|
||||
if (field && minmea_isfield(*field))
|
||||
value = *field;
|
||||
|
||||
*va_arg(ap, char *) = value;
|
||||
} break;
|
||||
|
||||
case 'd': { // Single character direction field (int).
|
||||
int value = 0;
|
||||
|
||||
if (field && minmea_isfield(*field)) {
|
||||
switch (*field) {
|
||||
case 'N':
|
||||
case 'E':
|
||||
value = 1;
|
||||
break;
|
||||
case 'S':
|
||||
case 'W':
|
||||
value = -1;
|
||||
break;
|
||||
default:
|
||||
goto parse_error;
|
||||
}
|
||||
}
|
||||
|
||||
*va_arg(ap, int *) = value;
|
||||
} break;
|
||||
|
||||
case 'f': { // Fractional value with scale (struct minmea_float).
|
||||
int sign = 0;
|
||||
int_least32_t value = -1;
|
||||
int_least32_t scale = 0;
|
||||
|
||||
if (field) {
|
||||
while (minmea_isfield(*field)) {
|
||||
if (*field == '+' && !sign && value == -1) {
|
||||
sign = 1;
|
||||
} else if (*field == '-' && !sign && value == -1) {
|
||||
sign = -1;
|
||||
} else if (isdigit((unsigned char) *field)) {
|
||||
int digit = *field - '0';
|
||||
if (value == -1)
|
||||
value = 0;
|
||||
if (value > (INT_LEAST32_MAX-digit) / 10) {
|
||||
/* we ran out of bits, what do we do? */
|
||||
if (scale) {
|
||||
/* truncate extra precision */
|
||||
break;
|
||||
} else {
|
||||
/* integer overflow. bail out. */
|
||||
goto parse_error;
|
||||
}
|
||||
}
|
||||
value = (10 * value) + digit;
|
||||
if (scale)
|
||||
scale *= 10;
|
||||
} else if (*field == '.' && scale == 0) {
|
||||
scale = 1;
|
||||
} else if (*field == ' ') {
|
||||
/* Allow spaces at the start of the field. Not NMEA
|
||||
* conformant, but some modules do this. */
|
||||
if (sign != 0 || value != -1 || scale != 0)
|
||||
goto parse_error;
|
||||
} else {
|
||||
goto parse_error;
|
||||
}
|
||||
field++;
|
||||
}
|
||||
}
|
||||
|
||||
if ((sign || scale) && value == -1)
|
||||
goto parse_error;
|
||||
|
||||
if (value == -1) {
|
||||
/* No digits were scanned. */
|
||||
value = 0;
|
||||
scale = 0;
|
||||
} else if (scale == 0) {
|
||||
/* No decimal point. */
|
||||
scale = 1;
|
||||
}
|
||||
if (sign)
|
||||
value *= sign;
|
||||
|
||||
*va_arg(ap, struct minmea_float *) = (struct minmea_float) {value, scale};
|
||||
} break;
|
||||
|
||||
case 'i': { // Integer value, default 0 (int).
|
||||
int value = 0;
|
||||
|
||||
if (field) {
|
||||
char *endptr;
|
||||
value = strtol(field, &endptr, 10);
|
||||
if (minmea_isfield(*endptr))
|
||||
goto parse_error;
|
||||
}
|
||||
|
||||
*va_arg(ap, int *) = value;
|
||||
} break;
|
||||
|
||||
case 's': { // String value (char *).
|
||||
char *buf = va_arg(ap, char *);
|
||||
|
||||
if (field) {
|
||||
while (minmea_isfield(*field))
|
||||
*buf++ = *field++;
|
||||
}
|
||||
|
||||
*buf = '\0';
|
||||
} break;
|
||||
|
||||
case 't': { // NMEA talker+sentence identifier (char *).
|
||||
// This field is always mandatory.
|
||||
if (!field)
|
||||
goto parse_error;
|
||||
|
||||
if (field[0] != '$')
|
||||
goto parse_error;
|
||||
for (int f=0; f<5; f++)
|
||||
if (!minmea_isfield(field[1+f]))
|
||||
goto parse_error;
|
||||
|
||||
char *buf = va_arg(ap, char *);
|
||||
memcpy(buf, field+1, 5);
|
||||
buf[5] = '\0';
|
||||
} break;
|
||||
|
||||
case 'D': { // Date (int, int, int), -1 if empty.
|
||||
struct minmea_date *date = va_arg(ap, struct minmea_date *);
|
||||
|
||||
int d = -1, m = -1, y = -1;
|
||||
|
||||
if (field && minmea_isfield(*field)) {
|
||||
// Always six digits.
|
||||
for (int f=0; f<6; f++)
|
||||
if (!isdigit((unsigned char) field[f]))
|
||||
goto parse_error;
|
||||
|
||||
char dArr[] = {field[0], field[1], '\0'};
|
||||
char mArr[] = {field[2], field[3], '\0'};
|
||||
char yArr[] = {field[4], field[5], '\0'};
|
||||
d = strtol(dArr, NULL, 10);
|
||||
m = strtol(mArr, NULL, 10);
|
||||
y = strtol(yArr, NULL, 10);
|
||||
}
|
||||
|
||||
date->day = d;
|
||||
date->month = m;
|
||||
date->year = y;
|
||||
} break;
|
||||
|
||||
case 'T': { // Time (int, int, int, int), -1 if empty.
|
||||
struct minmea_time *time_ = va_arg(ap, struct minmea_time *);
|
||||
|
||||
int h = -1, i = -1, s = -1, u = -1;
|
||||
|
||||
if (field && minmea_isfield(*field)) {
|
||||
// Minimum required: integer time.
|
||||
for (int f=0; f<6; f++)
|
||||
if (!isdigit((unsigned char) field[f]))
|
||||
goto parse_error;
|
||||
|
||||
char hArr[] = {field[0], field[1], '\0'};
|
||||
char iArr[] = {field[2], field[3], '\0'};
|
||||
char sArr[] = {field[4], field[5], '\0'};
|
||||
h = strtol(hArr, NULL, 10);
|
||||
i = strtol(iArr, NULL, 10);
|
||||
s = strtol(sArr, NULL, 10);
|
||||
field += 6;
|
||||
|
||||
// Extra: fractional time. Saved as microseconds.
|
||||
if (*field++ == '.') {
|
||||
uint32_t value = 0;
|
||||
uint32_t scale = 1000000LU;
|
||||
while (isdigit((unsigned char) *field) && scale > 1) {
|
||||
value = (value * 10) + (*field++ - '0');
|
||||
scale /= 10;
|
||||
}
|
||||
u = value * scale;
|
||||
} else {
|
||||
u = 0;
|
||||
}
|
||||
}
|
||||
|
||||
time_->hours = h;
|
||||
time_->minutes = i;
|
||||
time_->seconds = s;
|
||||
time_->microseconds = u;
|
||||
} break;
|
||||
|
||||
case '_': { // Ignore the field.
|
||||
} break;
|
||||
|
||||
default: { // Unknown.
|
||||
goto parse_error;
|
||||
}
|
||||
}
|
||||
|
||||
next_field();
|
||||
}
|
||||
|
||||
result = true;
|
||||
|
||||
parse_error:
|
||||
va_end(ap);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool minmea_talker_id(char talker[3], const char *sentence)
|
||||
{
|
||||
char type[6];
|
||||
if (!minmea_scan(sentence, "t", type))
|
||||
return false;
|
||||
|
||||
talker[0] = type[0];
|
||||
talker[1] = type[1];
|
||||
talker[2] = '\0';
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
|
||||
{
|
||||
if (!minmea_check(sentence, strict))
|
||||
return MINMEA_INVALID;
|
||||
|
||||
char type[6];
|
||||
if (!minmea_scan(sentence, "t", type))
|
||||
return MINMEA_INVALID;
|
||||
|
||||
if (!strcmp(type+2, "RMC"))
|
||||
return MINMEA_SENTENCE_RMC;
|
||||
if (!strcmp(type+2, "GGA"))
|
||||
return MINMEA_SENTENCE_GGA;
|
||||
if (!strcmp(type+2, "GSA"))
|
||||
return MINMEA_SENTENCE_GSA;
|
||||
if (!strcmp(type+2, "GLL"))
|
||||
return MINMEA_SENTENCE_GLL;
|
||||
if (!strcmp(type+2, "GST"))
|
||||
return MINMEA_SENTENCE_GST;
|
||||
if (!strcmp(type+2, "GSV"))
|
||||
return MINMEA_SENTENCE_GSV;
|
||||
if (!strcmp(type+2, "VTG"))
|
||||
return MINMEA_SENTENCE_VTG;
|
||||
if (!strcmp(type+2, "ZDA"))
|
||||
return MINMEA_SENTENCE_ZDA;
|
||||
|
||||
return MINMEA_UNKNOWN;
|
||||
}
|
||||
|
||||
bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence)
|
||||
{
|
||||
// $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
|
||||
char type[6];
|
||||
char validity;
|
||||
int latitude_direction;
|
||||
int longitude_direction;
|
||||
int variation_direction;
|
||||
if (!minmea_scan(sentence, "tTcfdfdffDfd",
|
||||
type,
|
||||
&frame->time,
|
||||
&validity,
|
||||
&frame->latitude, &latitude_direction,
|
||||
&frame->longitude, &longitude_direction,
|
||||
&frame->speed,
|
||||
&frame->course,
|
||||
&frame->date,
|
||||
&frame->variation, &variation_direction))
|
||||
return false;
|
||||
if (strcmp(type+2, "RMC"))
|
||||
return false;
|
||||
|
||||
frame->valid = (validity == 'A');
|
||||
frame->latitude.value *= latitude_direction;
|
||||
frame->longitude.value *= longitude_direction;
|
||||
frame->variation.value *= variation_direction;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence)
|
||||
{
|
||||
// $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
|
||||
char type[6];
|
||||
int latitude_direction;
|
||||
int longitude_direction;
|
||||
|
||||
if (!minmea_scan(sentence, "tTfdfdiiffcfcf_",
|
||||
type,
|
||||
&frame->time,
|
||||
&frame->latitude, &latitude_direction,
|
||||
&frame->longitude, &longitude_direction,
|
||||
&frame->fix_quality,
|
||||
&frame->satellites_tracked,
|
||||
&frame->hdop,
|
||||
&frame->altitude, &frame->altitude_units,
|
||||
&frame->height, &frame->height_units,
|
||||
&frame->dgps_age))
|
||||
return false;
|
||||
if (strcmp(type+2, "GGA"))
|
||||
return false;
|
||||
|
||||
frame->latitude.value *= latitude_direction;
|
||||
frame->longitude.value *= longitude_direction;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence)
|
||||
{
|
||||
// $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
|
||||
char type[6];
|
||||
|
||||
if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff",
|
||||
type,
|
||||
&frame->mode,
|
||||
&frame->fix_type,
|
||||
&frame->sats[0],
|
||||
&frame->sats[1],
|
||||
&frame->sats[2],
|
||||
&frame->sats[3],
|
||||
&frame->sats[4],
|
||||
&frame->sats[5],
|
||||
&frame->sats[6],
|
||||
&frame->sats[7],
|
||||
&frame->sats[8],
|
||||
&frame->sats[9],
|
||||
&frame->sats[10],
|
||||
&frame->sats[11],
|
||||
&frame->pdop,
|
||||
&frame->hdop,
|
||||
&frame->vdop))
|
||||
return false;
|
||||
if (strcmp(type+2, "GSA"))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence)
|
||||
{
|
||||
// $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$;
|
||||
char type[6];
|
||||
int latitude_direction;
|
||||
int longitude_direction;
|
||||
|
||||
if (!minmea_scan(sentence, "tfdfdTc;c",
|
||||
type,
|
||||
&frame->latitude, &latitude_direction,
|
||||
&frame->longitude, &longitude_direction,
|
||||
&frame->time,
|
||||
&frame->status,
|
||||
&frame->mode))
|
||||
return false;
|
||||
if (strcmp(type+2, "GLL"))
|
||||
return false;
|
||||
|
||||
frame->latitude.value *= latitude_direction;
|
||||
frame->longitude.value *= longitude_direction;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence)
|
||||
{
|
||||
// $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58
|
||||
char type[6];
|
||||
|
||||
if (!minmea_scan(sentence, "tTfffffff",
|
||||
type,
|
||||
&frame->time,
|
||||
&frame->rms_deviation,
|
||||
&frame->semi_major_deviation,
|
||||
&frame->semi_minor_deviation,
|
||||
&frame->semi_major_orientation,
|
||||
&frame->latitude_error_deviation,
|
||||
&frame->longitude_error_deviation,
|
||||
&frame->altitude_error_deviation))
|
||||
return false;
|
||||
if (strcmp(type+2, "GST"))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence)
|
||||
{
|
||||
// $GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74
|
||||
// $GPGSV,3,3,11,22,42,067,42,24,14,311,43,27,05,244,00,,,,*4D
|
||||
// $GPGSV,4,2,11,08,51,203,30,09,45,215,28*75
|
||||
// $GPGSV,4,4,13,39,31,170,27*40
|
||||
// $GPGSV,4,4,13*7B
|
||||
char type[6];
|
||||
|
||||
if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii",
|
||||
type,
|
||||
&frame->total_msgs,
|
||||
&frame->msg_nr,
|
||||
&frame->total_sats,
|
||||
&frame->sats[0].nr,
|
||||
&frame->sats[0].elevation,
|
||||
&frame->sats[0].azimuth,
|
||||
&frame->sats[0].snr,
|
||||
&frame->sats[1].nr,
|
||||
&frame->sats[1].elevation,
|
||||
&frame->sats[1].azimuth,
|
||||
&frame->sats[1].snr,
|
||||
&frame->sats[2].nr,
|
||||
&frame->sats[2].elevation,
|
||||
&frame->sats[2].azimuth,
|
||||
&frame->sats[2].snr,
|
||||
&frame->sats[3].nr,
|
||||
&frame->sats[3].elevation,
|
||||
&frame->sats[3].azimuth,
|
||||
&frame->sats[3].snr
|
||||
)) {
|
||||
return false;
|
||||
}
|
||||
if (strcmp(type+2, "GSV"))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence)
|
||||
{
|
||||
// $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
|
||||
// $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41
|
||||
// $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22
|
||||
// $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F
|
||||
char type[6];
|
||||
char c_true, c_magnetic, c_knots, c_kph, c_faa_mode;
|
||||
|
||||
if (!minmea_scan(sentence, "tfcfcfcfc;c",
|
||||
type,
|
||||
&frame->true_track_degrees,
|
||||
&c_true,
|
||||
&frame->magnetic_track_degrees,
|
||||
&c_magnetic,
|
||||
&frame->speed_knots,
|
||||
&c_knots,
|
||||
&frame->speed_kph,
|
||||
&c_kph,
|
||||
&c_faa_mode))
|
||||
return false;
|
||||
if (strcmp(type+2, "VTG"))
|
||||
return false;
|
||||
// check chars
|
||||
if (c_true != 'T' ||
|
||||
c_magnetic != 'M' ||
|
||||
c_knots != 'N' ||
|
||||
c_kph != 'K')
|
||||
return false;
|
||||
frame->faa_mode = (enum minmea_faa_mode)c_faa_mode;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence)
|
||||
{
|
||||
// $GPZDA,201530.00,04,07,2002,00,00*60
|
||||
char type[6];
|
||||
|
||||
if(!minmea_scan(sentence, "tTiiiii",
|
||||
type,
|
||||
&frame->time,
|
||||
&frame->date.day,
|
||||
&frame->date.month,
|
||||
&frame->date.year,
|
||||
&frame->hour_offset,
|
||||
&frame->minute_offset))
|
||||
return false;
|
||||
if (strcmp(type+2, "ZDA"))
|
||||
return false;
|
||||
|
||||
// check offsets
|
||||
if (abs(frame->hour_offset) > 13 ||
|
||||
frame->minute_offset > 59 ||
|
||||
frame->minute_offset < 0)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_)
|
||||
{
|
||||
if (date->year == -1 || time_->hours == -1)
|
||||
return -1;
|
||||
|
||||
struct tm tm;
|
||||
memset(&tm, 0, sizeof(tm));
|
||||
if (date->year < 80) {
|
||||
tm.tm_year = 2000 + date->year - 1900; // 2000-2079
|
||||
} else if (date->year >= 1900) {
|
||||
tm.tm_year = date->year - 1900; // 4 digit year, use directly
|
||||
} else {
|
||||
tm.tm_year = date->year; // 1980-1999
|
||||
}
|
||||
tm.tm_mon = date->month - 1;
|
||||
tm.tm_mday = date->day;
|
||||
tm.tm_hour = time_->hours;
|
||||
tm.tm_min = time_->minutes;
|
||||
tm.tm_sec = time_->seconds;
|
||||
|
||||
time_t timestamp = timegm(&tm); /* See README.md if your system lacks timegm(). */
|
||||
if (timestamp != (time_t)-1) {
|
||||
ts->tv_sec = timestamp;
|
||||
ts->tv_nsec = time_->microseconds * 1000;
|
||||
return 0;
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,261 @@
|
|||
/*
|
||||
* Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
|
||||
* This program is free software. It comes without any warranty, to the extent
|
||||
* permitted by applicable law. You can redistribute it and/or modify it under
|
||||
* the terms of the Do What The Fuck You Want To Public License, Version 2, as
|
||||
* published by Sam Hocevar. See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#ifndef MINMEA_H
|
||||
#define MINMEA_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
#ifdef MINMEA_INCLUDE_COMPAT
|
||||
#include <minmea_compat.h>
|
||||
#endif
|
||||
|
||||
#define MINMEA_MAX_LENGTH 80
|
||||
|
||||
enum minmea_sentence_id {
|
||||
MINMEA_INVALID = -1,
|
||||
MINMEA_UNKNOWN = 0,
|
||||
MINMEA_SENTENCE_RMC,
|
||||
MINMEA_SENTENCE_GGA,
|
||||
MINMEA_SENTENCE_GSA,
|
||||
MINMEA_SENTENCE_GLL,
|
||||
MINMEA_SENTENCE_GST,
|
||||
MINMEA_SENTENCE_GSV,
|
||||
MINMEA_SENTENCE_VTG,
|
||||
MINMEA_SENTENCE_ZDA,
|
||||
};
|
||||
|
||||
struct minmea_float {
|
||||
int_least32_t value;
|
||||
int_least32_t scale;
|
||||
};
|
||||
|
||||
struct minmea_date {
|
||||
int day;
|
||||
int month;
|
||||
int year;
|
||||
};
|
||||
|
||||
struct minmea_time {
|
||||
int hours;
|
||||
int minutes;
|
||||
int seconds;
|
||||
int microseconds;
|
||||
};
|
||||
|
||||
struct minmea_sentence_rmc {
|
||||
struct minmea_time time;
|
||||
bool valid;
|
||||
struct minmea_float latitude;
|
||||
struct minmea_float longitude;
|
||||
struct minmea_float speed;
|
||||
struct minmea_float course;
|
||||
struct minmea_date date;
|
||||
struct minmea_float variation;
|
||||
};
|
||||
|
||||
struct minmea_sentence_gga {
|
||||
struct minmea_time time;
|
||||
struct minmea_float latitude;
|
||||
struct minmea_float longitude;
|
||||
int fix_quality;
|
||||
int satellites_tracked;
|
||||
struct minmea_float hdop;
|
||||
struct minmea_float altitude; char altitude_units;
|
||||
struct minmea_float height; char height_units;
|
||||
struct minmea_float dgps_age;
|
||||
};
|
||||
|
||||
enum minmea_gll_status {
|
||||
MINMEA_GLL_STATUS_DATA_VALID = 'A',
|
||||
MINMEA_GLL_STATUS_DATA_NOT_VALID = 'V',
|
||||
};
|
||||
|
||||
// FAA mode added to some fields in NMEA 2.3.
|
||||
enum minmea_faa_mode {
|
||||
MINMEA_FAA_MODE_AUTONOMOUS = 'A',
|
||||
MINMEA_FAA_MODE_DIFFERENTIAL = 'D',
|
||||
MINMEA_FAA_MODE_ESTIMATED = 'E',
|
||||
MINMEA_FAA_MODE_MANUAL = 'M',
|
||||
MINMEA_FAA_MODE_SIMULATED = 'S',
|
||||
MINMEA_FAA_MODE_NOT_VALID = 'N',
|
||||
MINMEA_FAA_MODE_PRECISE = 'P',
|
||||
};
|
||||
|
||||
struct minmea_sentence_gll {
|
||||
struct minmea_float latitude;
|
||||
struct minmea_float longitude;
|
||||
struct minmea_time time;
|
||||
char status;
|
||||
char mode;
|
||||
};
|
||||
|
||||
struct minmea_sentence_gst {
|
||||
struct minmea_time time;
|
||||
struct minmea_float rms_deviation;
|
||||
struct minmea_float semi_major_deviation;
|
||||
struct minmea_float semi_minor_deviation;
|
||||
struct minmea_float semi_major_orientation;
|
||||
struct minmea_float latitude_error_deviation;
|
||||
struct minmea_float longitude_error_deviation;
|
||||
struct minmea_float altitude_error_deviation;
|
||||
};
|
||||
|
||||
enum minmea_gsa_mode {
|
||||
MINMEA_GPGSA_MODE_AUTO = 'A',
|
||||
MINMEA_GPGSA_MODE_FORCED = 'M',
|
||||
};
|
||||
|
||||
enum minmea_gsa_fix_type {
|
||||
MINMEA_GPGSA_FIX_NONE = 1,
|
||||
MINMEA_GPGSA_FIX_2D = 2,
|
||||
MINMEA_GPGSA_FIX_3D = 3,
|
||||
};
|
||||
|
||||
struct minmea_sentence_gsa {
|
||||
char mode;
|
||||
int fix_type;
|
||||
int sats[12];
|
||||
struct minmea_float pdop;
|
||||
struct minmea_float hdop;
|
||||
struct minmea_float vdop;
|
||||
};
|
||||
|
||||
struct minmea_sat_info {
|
||||
int nr;
|
||||
int elevation;
|
||||
int azimuth;
|
||||
int snr;
|
||||
};
|
||||
|
||||
struct minmea_sentence_gsv {
|
||||
int total_msgs;
|
||||
int msg_nr;
|
||||
int total_sats;
|
||||
struct minmea_sat_info sats[4];
|
||||
};
|
||||
|
||||
struct minmea_sentence_vtg {
|
||||
struct minmea_float true_track_degrees;
|
||||
struct minmea_float magnetic_track_degrees;
|
||||
struct minmea_float speed_knots;
|
||||
struct minmea_float speed_kph;
|
||||
enum minmea_faa_mode faa_mode;
|
||||
};
|
||||
|
||||
struct minmea_sentence_zda {
|
||||
struct minmea_time time;
|
||||
struct minmea_date date;
|
||||
int hour_offset;
|
||||
int minute_offset;
|
||||
};
|
||||
|
||||
/**
|
||||
* Calculate raw sentence checksum. Does not check sentence integrity.
|
||||
*/
|
||||
uint8_t minmea_checksum(const char *sentence);
|
||||
|
||||
/**
|
||||
* Check sentence validity and checksum. Returns true for valid sentences.
|
||||
*/
|
||||
bool minmea_check(const char *sentence, bool strict);
|
||||
|
||||
/**
|
||||
* Determine talker identifier.
|
||||
*/
|
||||
bool minmea_talker_id(char talker[3], const char *sentence);
|
||||
|
||||
/**
|
||||
* Determine sentence identifier.
|
||||
*/
|
||||
enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict);
|
||||
|
||||
/**
|
||||
* Scanf-like processor for NMEA sentences. Supports the following formats:
|
||||
* c - single character (char *)
|
||||
* d - direction, returned as 1/-1, default 0 (int *)
|
||||
* f - fractional, returned as value + scale (int *, int *)
|
||||
* i - decimal, default zero (int *)
|
||||
* s - string (char *)
|
||||
* t - talker identifier and type (char *)
|
||||
* T - date/time stamp (int *, int *, int *)
|
||||
* Returns true on success. See library source code for details.
|
||||
*/
|
||||
bool minmea_scan(const char *sentence, const char *format, ...);
|
||||
|
||||
/*
|
||||
* Parse a specific type of sentence. Return true on success.
|
||||
*/
|
||||
bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence);
|
||||
bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence);
|
||||
bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence);
|
||||
bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence);
|
||||
bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence);
|
||||
bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence);
|
||||
bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence);
|
||||
bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence);
|
||||
|
||||
/**
|
||||
* Convert GPS UTC date/time representation to a UNIX timestamp.
|
||||
*/
|
||||
int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_);
|
||||
|
||||
/**
|
||||
* Rescale a fixed-point value to a different scale. Rounds towards zero.
|
||||
*/
|
||||
static inline int_least32_t minmea_rescale(struct minmea_float *f, int_least32_t new_scale)
|
||||
{
|
||||
if (f->scale == 0)
|
||||
return 0;
|
||||
if (f->scale == new_scale)
|
||||
return f->value;
|
||||
if (f->scale > new_scale)
|
||||
return (f->value + ((f->value > 0) - (f->value < 0)) * f->scale/new_scale/2) / (f->scale/new_scale);
|
||||
else
|
||||
return f->value * (new_scale/f->scale);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert a fixed-point value to a floating-point value.
|
||||
* Returns NaN for "unknown" values.
|
||||
*/
|
||||
static inline float minmea_tofloat(struct minmea_float *f)
|
||||
{
|
||||
if (f->scale == 0)
|
||||
return NAN;
|
||||
return (float) f->value / (float) f->scale;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert a raw coordinate to a floating point DD.DDD... value.
|
||||
* Returns NaN for "unknown" values.
|
||||
*/
|
||||
static inline float minmea_tocoord(struct minmea_float *f)
|
||||
{
|
||||
if (f->scale == 0)
|
||||
return NAN;
|
||||
int_least32_t degrees = f->value / (f->scale * 100);
|
||||
int_least32_t minutes = f->value % (f->scale * 100);
|
||||
return (float) degrees + (float) minutes / (60 * f->scale);
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* MINMEA_H */
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,31 @@
|
|||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.30804.86
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PDR", "project\PDR.vcxproj", "{28D32532-309F-40EA-9E4A-2D162CC434D2}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
Debug|x86 = Debug|x86
|
||||
Release|x64 = Release|x64
|
||||
Release|x86 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.Build.0 = Debug|x64
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.Build.0 = Debug|Win32
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.ActiveCfg = Release|x64
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.Build.0 = Release|x64
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.ActiveCfg = Release|Win32
|
||||
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
EndGlobalSection
|
||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||
SolutionGuid = {B6378B77-5AB0-4A12-ACE8-2B45F292D7EE}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
#ifndef _PDR_AHRS_H_
|
||||
#define _PDR_AHRS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define AHRS_SAMPLE_FREQ 100 // ARHS算法的传感器采样频率
|
||||
#define IMU_SAMPLING_FREQUENCY 100
|
||||
#define AHRS_KP 0.500
|
||||
#define AHRS_TYPE_GYRO 4
|
||||
#define AHRS_TYPE_ACCE 1
|
||||
#define AHRS_TYPE_MAGN 2
|
||||
#define AHRS_STATUS_RESET_PHASE_0 0x80
|
||||
#define AHRS_STATUS_RESET_PHASE_1 0x40
|
||||
#define AHRS_STATUS_RESET (AHRS_STATUS_RESET_PHASE_0|AHRS_STATUS_RESET_PHASE_1)
|
||||
#define AHRS_STATUS_STABLE 0x20
|
||||
|
||||
/* Struct Declaration --------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
|
||||
void AHRS_Init(void);
|
||||
void ResetARHS(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : fv3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom &&
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void fv3Norm(float* vx, float* vy, float* vz);
|
||||
|
||||
int UpdateAHRS(IMU_t* imu);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // for __AHRS_H
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
#ifndef __FILTER_H__
|
||||
#define __FILTER_H__
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define FILTER_COEF_MAX_LENGTH 71
|
||||
#define FILTER_FIR_COEF_MAX_LENGTH 71
|
||||
|
||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||
typedef struct FILTER { // iir or fir filter
|
||||
int reset; // reset flag
|
||||
int order; // coefficient array length, not filter order
|
||||
double H0; // dc signal gain
|
||||
double b[FILTER_COEF_MAX_LENGTH]; // b coefficient
|
||||
double a[FILTER_COEF_MAX_LENGTH]; // a coefficient
|
||||
double yout[FILTER_COEF_MAX_LENGTH]; // yout buffer
|
||||
double xin[FILTER_COEF_MAX_LENGTH]; // xin buffer
|
||||
} FILTER;
|
||||
|
||||
typedef struct FILTER_FIR { // fir filter
|
||||
int reset; // reset flag
|
||||
int order; // coefficient array length, not filter order
|
||||
double H0; // dc signal gain
|
||||
double b[FILTER_FIR_COEF_MAX_LENGTH]; // b coefficient
|
||||
double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer
|
||||
} FILTER_FIR;
|
||||
|
||||
typedef struct DELAYER { // delayer
|
||||
int reset;
|
||||
int delay; // delay point number
|
||||
double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer
|
||||
} DELAYER;
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
extern void FILTER_Reset(FILTER *f);
|
||||
extern void FILTER_FIR_Reset(FILTER_FIR *f);
|
||||
extern void FILTER_SetCoef(FILTER *f, int order, double *b, double *a, double H0);
|
||||
extern void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0);
|
||||
extern double FILTER_filter(FILTER *f, double x);
|
||||
extern double FILTER_FIR_filter(FILTER_FIR *f, double x);
|
||||
extern void DELAY_reset(DELAYER *d);
|
||||
extern void DELAY_setCoef(DELAYER *d, int delay);
|
||||
extern double DELAY_delay(DELAYER *d, double x);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_kalman.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR 卡尔曼滤波器头文件函数声明以及相关结构体定义
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_KALMAN_H_
|
||||
#define _PDR_KALMAN_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : EKF_Init
|
||||
* Description : 初始化卡尔曼滤波器相关
|
||||
* Date : 2022/09/19
|
||||
*---------------------------------------------------------------------**/
|
||||
void EKF_Init(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : EKFStatePredict
|
||||
* Description : pdr卡尔曼滤波器的状态预测方程
|
||||
* Date : 2022/09/19 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void EKFStatePredict(EKFPara_t* kf, double step_length, PDR_t* g_pdr, int step);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : EKFCalQRMatrix
|
||||
* Description : 计算卡尔曼滤波噪声矩阵
|
||||
* scane_type, 用户所处场景（开阔和非开阔）
|
||||
* nmea_data，NMEA数据结构体
|
||||
* g_pdr，PDR结构体
|
||||
* sys，运动幅度数据结构体
|
||||
* kf，EKF数据结构体
|
||||
* Date : 2022/09/19 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void EKFCalQRMatrix(lct_nmea* nmea_data, PDR_t* g_pdr, classifer* sys, EKFPara_t* kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_ekfStateUpdate
|
||||
* Description : pdr卡尔曼滤波器的状态更新方程
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, classifer* sys, PDR_t* g_pdr,
|
||||
int scene_type);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocFusion
|
||||
* Description : PDR的GNSS和INS融合定位
|
||||
* Date : 2020/07/09 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, classifer* sys, double yaw_bias,
|
||||
EKFPara_t* kf, lct_fs* res);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
#ifndef __PDR_LOCATION_H__
|
||||
#define __PDR_LOCATION_H__
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "pdr_base.h"
|
||||
|
||||
#define HIST_GPS_NUM 8
|
||||
#define ACCURACY_THRES 0.6f // 精度阈值
|
||||
#define YAW_THRES 10.0f // Yaw角范围阈值
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : NavSys_Init
|
||||
* Description : PDR导航系统初始化
|
||||
* Date : 2022/9/16
|
||||
*---------------------------------------------------------------------**/
|
||||
void PDRNav_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : InsLocation
|
||||
* Description : PDR 惯性导航定位
|
||||
* Date : 2022/09/16 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int InsLocation(IMU_t *ss_data, EKFPara_t *kf);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_noGpsPredict
|
||||
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* g_pdr);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : Nmea2Gnss
|
||||
* Description : nmea数据结构转gnss数据
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detectFixMode
|
||||
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
|
||||
* 原始GPS或者不输出
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
|
||||
* 代码的可读性
|
||||
*---------------------------------------------------------------------**/
|
||||
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : GnssCalHeading
|
||||
* Description : 获取GPS偏航角(0 - 360°)
|
||||
* Date : 2022/09/16 logzhan
|
||||
* 代码的可读性
|
||||
*---------------------------------------------------------------------**/
|
||||
double GnssCalHeading(GNSS_t* pgnss);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : calPdrHeadingOffset
|
||||
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_resetSysStatus
|
||||
* Description : PDS 重设系统状态
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void ResetSystemStatus(EKFPara_t* kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocation
|
||||
* Description : PDR GPS融合INS惯性定位
|
||||
* Date : 2021/01/29 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int GnssInsFusionLocation(lct_nmea* nmea_data, EKFPara_t* kf, lct_fs* result);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initGnssInfo
|
||||
* Description : PDS GNSS定位信息初始化
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void InitGnssInfo(void);
|
||||
|
||||
void GnssUpdate(GNSS_t* gps, lct_nmea* nmea);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
#pragma once
|
||||
#ifndef _LOCATION_TOOL_H_
|
||||
#define _LOCATION_TOOL_H_
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
#ifndef PI
|
||||
//#define PI 3.14159265358979323846
|
||||
#endif // !PI
|
||||
|
||||
#ifndef EARTH_RADIUS
|
||||
#define EARTH_RADIUS 6371008.8
|
||||
#endif
|
||||
|
||||
typedef struct DoublePair {
|
||||
double x;
|
||||
double y;
|
||||
} DoublePair;
|
||||
typedef DoublePair PlaneCoordinate;
|
||||
|
||||
typedef struct LatLng{
|
||||
double lat;
|
||||
double lon;
|
||||
} LatLng_t;
|
||||
|
||||
typedef struct DoublePairList{
|
||||
double *x;
|
||||
double *y;
|
||||
size_t length;
|
||||
} DoublePairList;
|
||||
typedef struct LocationList{
|
||||
double *lat;
|
||||
double *lon;
|
||||
size_t length;
|
||||
} LocationList;
|
||||
|
||||
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);
|
||||
DoublePair ProjPoint(DoublePair point, DoublePair linePointA, DoublePair linePointB);
|
||||
|
||||
//PlaneCoordinate WGS84_XYZ(const LatLng lla);
|
||||
double CalculateDistance(LatLng pointA, LatLng pointB);
|
||||
|
||||
#endif // !_LOCATION_TOOL_H_
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef _PDR_MATRIX_H_
|
||||
#define _PDR_MATRIX_H_
|
||||
|
||||
#define N 4
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixTrans
|
||||
* Description : 矩阵转置
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixTrans(double a[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VecMatMultiply
|
||||
* Description : 向量和矩阵相乘 r = b * a
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VecMatMultiply(double a[N], double b[N][N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixMultiply
|
||||
* Description : 矩阵和矩阵相乘 r = a * b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixAdd
|
||||
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorAdd
|
||||
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorAdd(double a[N], double b[N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixSub
|
||||
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorSub
|
||||
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorSub(double a[N], double b[N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixInverse
|
||||
* Description : 求矩阵的逆矩阵
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixInverse(double(*a)[N], double(*a_inv)[N]);
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#ifndef _PARSEDATA_H_
|
||||
#define _PARSEDATA_H_
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
#define NAEA_LAST_TIME 500
|
||||
#define IMU_LAST_COUNT 10
|
||||
|
||||
#define FLOAT_TO_INT 1000000
|
||||
|
||||
/**************************************************************************
|
||||
* Description : NMEA数据和传感器数据解析
|
||||
* : NMEA数据包括RMC,GGA,GSA,GSV,以及安卓定位信息语句
|
||||
传感器数据包括加速度计，陀螺仪，磁力计
|
||||
**************************************************************************/
|
||||
|
||||
char * strtok_ct(char * s, const char * delim);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析惯性传感器数据
|
||||
* Input : pt， 传感器数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
sstp，安卓中传感器类型标识符
|
||||
* Output : imu_p， 惯性传感器结构体
|
||||
**************************************************************************/
|
||||
void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析NMEA数据
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void parseNMEA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GGA协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseGGA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析RMC协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseRMC(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GSV协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void parseGSV(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GSA协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseGSA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : parseLocAccuracy
|
||||
* Description : 解析GPS的Accuracy精度参数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void parseLocAccuracy(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 检测NMEA数据是否解析完整
|
||||
* Input : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void preprocessNMEA(lct_nmea *ln);
|
||||
|
||||
int ParseLineStr(char* line, IMU_t* imu_p, lct_nmea* ln);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : HexToDec
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2022-09-15 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
long HexToDec(char *source);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_int2Hex
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
char * Int2Hex(int a, char *buffer);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_getIndexOfSigns
|
||||
* Description : 返回ch字符在sign数组中的序号
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_getIndexOfSigns(char ch);
|
||||
#endif
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,177 @@
|
|||
#pragma once
|
||||
/******************** (C) COPYRIGHT 2017 Geek************************************
|
||||
* File Name : Steps_Algorithm.h
|
||||
* Department : Sensor Team
|
||||
* Current Version : V1.0
|
||||
* Author : Xiaoyong Li
|
||||
* Date of Issued : 2017.05.8
|
||||
* Comments: step counter algorithm added
|
||||
********************************************************************************
|
||||
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
|
||||
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR REFERENCE OR
|
||||
* EDUCATION. AS A RESULT, Geek SOFTWARE SHALL NOT BE HELD LIABLE FOR ANY
|
||||
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
|
||||
* FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
|
||||
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
|
||||
********************************************************************************
|
||||
* Release Log
|
||||
*
|
||||
* Comments: step counter algorithm released
|
||||
* Version Number : V1.0
|
||||
* Author : Xiaoyong Li
|
||||
* Date of Issued : 2017.05.8
|
||||
*******************************************************************************/
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#ifndef _STEPS_ALGORITHM_H_
|
||||
#define _STEPS_ALGORITHM_H_
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
#define ACC_SAMPLE_RATE_100HZ
|
||||
//#define ACC_SAMPLE_RATE_25HZ
|
||||
|
||||
#ifdef ACC_SAMPLE_RATE_25HZ
|
||||
#define DownResampleTimes 1
|
||||
#define ACC_SAMPLE_TIME 40
|
||||
#define SAMPLE_TIME_MIN 36
|
||||
#else
|
||||
#define DOWN_SAMPLE_TIMES 4
|
||||
#define ACC_SAMPLE_TIME 10 //origin acc sampleFrequency is 100 hz
|
||||
#define GYRO_SAMPLE_TIME 10
|
||||
#define SAMPLE_TIME_MIN 9
|
||||
#endif
|
||||
|
||||
#define SampleFrequency 25
|
||||
#define SampleFrequencyFour 100
|
||||
#define SampleFrequencydouble 50
|
||||
#define SampleFrequencyOPF (37.5l)
|
||||
#define SampleFrequencySix 150
|
||||
#define AvergeFilterOrder 5
|
||||
#define AxisNumber 4
|
||||
#define FiltDifNumber 4
|
||||
#define AxisNumberFiltOut 16 //(AxisNumber * FiltDifNumber)
|
||||
#define AxisNumberStepSaveState 20 //(AxisNumberFiltOut + FiltDifNumber)
|
||||
#define AccelerationNumber 39
|
||||
#define ORI_ACC_SAMPLE_RATE 100 //origin acc sampleFrequency is 100 hz
|
||||
#define WIN_LENGTH_AVE_DEVI 50 //
|
||||
#define PI2A (180/3.1415926)
|
||||
//#define NSTOS 1000000000
|
||||
#define MS2S 1000
|
||||
|
||||
|
||||
#define ACC_INSPECT_TIMEWIDTH 200 //origin acc sampleFrequency is 100 hz
|
||||
|
||||
#define WINDOW_LENGTH_MIN2MAX (ACC_INSPECT_TIMEWIDTH/2 - 1) //25hz , 1 second
|
||||
//#define WINDOW_LENGTH_IN_VEHICLE
|
||||
#define STARTSTEPS 8
|
||||
|
||||
/*#define PLATFORM_ANDROID_QCOM_AP*/
|
||||
/*#define PLATFORM_ANDROID_QCOM_ADSP*/
|
||||
#define PLATFORM_PC_WINDOWS
|
||||
//#define PLATFORM_PC_LINUX
|
||||
|
||||
|
||||
#if defined PLATFORM_ANDROID_QCOM_ADSP
|
||||
#define STEPLIB_MEMSET SNS_OS_MEMSET
|
||||
|
||||
#if 1
|
||||
#define SAM_STEP_LIB_NAME "steplib"
|
||||
#define SAM_STEP_LIB_MSG_0(msg) UMSG(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg)
|
||||
#define SAM_STEP_LIB_MSG_1(msg,p1) UMSG_1(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1)
|
||||
#define SAM_STEP_LIB_MSG_2(msg,p1,p2) UMSG_2(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2)
|
||||
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3) UMSG_3(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3)
|
||||
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4) UMSG_4(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3,p4)
|
||||
#else
|
||||
#define SAM_STEP_LIB_MSG_0(msg)
|
||||
#define SAM_STEP_LIB_MSG_1(msg,p1)
|
||||
#define SAM_STEP_LIB_MSG_2(msg,p1,p2)
|
||||
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3)
|
||||
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4)
|
||||
#endif
|
||||
#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845
|
||||
#define STEPLIB_MEMSET memset
|
||||
#endif
|
||||
#if defined PLATFORM_ANDROID_MTK
|
||||
#define ABS_INT abs_value
|
||||
#else
|
||||
#define ABS_INT abs
|
||||
#endif
|
||||
|
||||
|
||||
typedef struct {
|
||||
unsigned long walkSteps;
|
||||
}stepsRe;
|
||||
|
||||
typedef struct {
|
||||
double walk;
|
||||
double walkRealTime;
|
||||
double nD;
|
||||
}stepsDfine;
|
||||
|
||||
typedef enum {
|
||||
nonSteps = 0,
|
||||
walk,
|
||||
}enumState;
|
||||
|
||||
typedef struct {
|
||||
unsigned char sign;
|
||||
enumState state;
|
||||
unsigned long long stateStartTime;
|
||||
unsigned long long stepsStartTime;
|
||||
}stateNowDfine;
|
||||
|
||||
typedef struct {
|
||||
enumState state;
|
||||
unsigned long long startTimeStamp;
|
||||
}stateDfine;
|
||||
|
||||
typedef struct {
|
||||
int flag;
|
||||
double ax;
|
||||
double ay;
|
||||
double az;
|
||||
}downSample;
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : getPedometerLibVersion
|
||||
* Description : 获取计步器版本
|
||||
* Date : 2020/2/20
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* getPedometerLibVersion(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : initPedometer
|
||||
* Description : PDR 初始化计步器
|
||||
* Date : 2020/2/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Pedometer_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : reinitPedometer
|
||||
* Description : PDR 重新初始化计步器
|
||||
* Date : 2020/2/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void reinitPedometer(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : updatePedometer
|
||||
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void PedometerUpdate(IMU_t* ss_data, unsigned long* step);
|
||||
|
||||
void Steps_State_Detect(enumState* state);
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : detVehicleMode
|
||||
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void detVehicleMode(double ax, double ay, double az);
|
||||
#endif
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
#ifndef _PDR_UTIL_H_
|
||||
#define _PDR_UTIL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#define TRAJ_POINT_COUNT 1000
|
||||
#define MIN_POINT_COUNT 15
|
||||
#define OPT_TRAJ_TIMEOUT 1000
|
||||
|
||||
#define FALIED_LINE 0
|
||||
#define IN_LINE 1
|
||||
#define CLOSE_START 2
|
||||
#define CLOSE_END 3
|
||||
|
||||
typedef struct LatLng{
|
||||
double lat;
|
||||
double lon;
|
||||
} LatLng_t;
|
||||
|
||||
typedef struct TrajSign
|
||||
{
|
||||
int count;
|
||||
double lastTime;
|
||||
double weight;
|
||||
double yaw[TRAJ_POINT_COUNT];
|
||||
double error[TRAJ_POINT_COUNT];
|
||||
double points[TRAJ_POINT_COUNT][2];
|
||||
}TrajSign;
|
||||
|
||||
/**地球相关参数*******************************************************************/
|
||||
typedef struct
|
||||
{
|
||||
double rmh; /* 子午圈曲率半径 */
|
||||
double rnh; /* 卯酉圈曲率半径 */
|
||||
double grav; /* 当地重力加速度 */
|
||||
double lat; /* 当地纬度（rad）*/
|
||||
double wnie[3]; /* 地理系相对惯性系的角速度在导航系分量 */
|
||||
} EarthData_t;
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : mean
|
||||
* Description : 对输入double数组求均值
|
||||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||||
* 不同函数
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double mean(double *x, int n);
|
||||
|
||||
float fmean(float *data, int len);
|
||||
|
||||
float stdf(float* data, int len);
|
||||
|
||||
double meanAngle(double* angle, int len);
|
||||
|
||||
void modAngle(double * angle, double min, double max);
|
||||
|
||||
double pow_i(double num, long long n);
|
||||
|
||||
double pow_f(double num, double m);
|
||||
|
||||
double vivopow(double x, double y);
|
||||
|
||||
void centralization(double *x, double *x_out, int n);
|
||||
|
||||
void getCovMatrix(double *x, double *y, double cov[2][2], int n);
|
||||
|
||||
int Jacobi(double a[][2], double p[][2], int n, double eps, int T);
|
||||
|
||||
|
||||
LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB);
|
||||
|
||||
double CalDistance(LatLng_t pointA, LatLng_t pointB);
|
||||
|
||||
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]);
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_invSqrt
|
||||
* Description : 计算1/sqrt(x)的快速的算法
|
||||
* Date : 2020/6/30 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
float InvSqrt(float x);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_v3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom &&
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_v3Norm(float* vx, float* vy, float* vz);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_writeCsvStr
|
||||
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void WriteCsvStr(FILE* fp_write, char* str);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_utc2hms
|
||||
* Description : 将UTC时间转换为时分秒
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_utc2hms(double utc, double* h, double* m, double* s);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : qnb2att
|
||||
* Description : 四元数转欧拉角
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Qnb2Att(float* q, double* attitude);
|
||||
|
||||
double CalRadianDifferent(double s_dir, double d_dir);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_earthParameter
|
||||
* Description : 根据输入纬度返回该纬度地球相关参数
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
EarthData_t CalEarthParameter(double oriLat);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : Motion2TypeStr
|
||||
* Description : 把用户运动类型转换为字符串输出
|
||||
* Date : 2022/9/16 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* Motion2TypeStr(int type);
|
||||
|
||||
int pdr_min(int param_a, int param_b);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#ifndef _PDR_BUFFER_H_
|
||||
#define _PDR_BUFFER_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define BUFFER_LONG_LENGTH 256
|
||||
#define BUFFER_SHORT_LENGTH 10
|
||||
#define BUFFER_TYPE_STACK 0
|
||||
#define BUFFER_TYPE_QUEUE 1
|
||||
#define BUFFER_NO_ERROR 0
|
||||
#define BUFFER_WRONG_PARAMETER 1
|
||||
#define BUFFER_OUT_OF_MEMORY 2
|
||||
|
||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||
#pragma pack (4)
|
||||
typedef struct BUFFER {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_LONG_LENGTH + 1];
|
||||
} BUFFER;
|
||||
|
||||
typedef struct BUFFER_LONG {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_LONG_LENGTH + 1];
|
||||
} BUFFER_LONG;
|
||||
|
||||
typedef struct BUFFER_SHORT {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_SHORT_LENGTH + 1];
|
||||
} BUFFER_SHORT;
|
||||
#pragma pack ()
|
||||
|
||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
int Buffer_initialize(BUFFER *buffer, const char *name, int type, int length);
|
||||
int Buffer_clear(BUFFER *buffer);
|
||||
int Buffer_count(BUFFER *buffer, int *count);
|
||||
int Buffer_top(BUFFER *buffer, float *value);
|
||||
int Buffer_bottom(BUFFER *buffer, float *value);
|
||||
int Buffer_pop(BUFFER *buffer, float *value);
|
||||
int buffer_push(BUFFER *buffer, float value);
|
||||
int Buffer_get(BUFFER *buffer, float *value, int index);
|
||||
int Buffer_mean(BUFFER *buffer, float *mean);
|
||||
int Buffer_var(BUFFER *buffer, float *var);
|
||||
int Buffer_std(BUFFER *buffer, float *std);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
#ifndef _PDR_GPS_H_
|
||||
#define _PDR_GPS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#ifndef __MUNKRES_H
|
||||
#define __MUNKRES_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------------------------- */
|
||||
#define MUNKRES_NO_ERROR 0
|
||||
#define MUNKRES_EXCESSIVE_MATRIX 1
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
extern int MUNKRES_get_assignment(int *assignment, float* cost, int m, int n);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // for __MUNKRES_H
|
||||
|
|
@ -0,0 +1,156 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_api.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2021.01.26
|
||||
* Comments : PDR 对外部接口
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_API_H_
|
||||
#define _PDR_API_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "pdr_sensor.h"
|
||||
#include "pdr_base.h"
|
||||
#include "Utils.h"
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define IMU_LAST_COUNT 10
|
||||
|
||||
typedef struct _PosFusion{
|
||||
double lat;
|
||||
double lon;
|
||||
double gpsLat;
|
||||
double gpsLon;
|
||||
double t;
|
||||
int vaild;
|
||||
}PosFusion;
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_algorithmInit
|
||||
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Algorithm_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoop
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Input : ss_data， 传感器数据
|
||||
nmea_data, NMEA数据
|
||||
result，定位结果
|
||||
* Output : int, 输出标志位
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int LocationMainLoop(IMU_t* ImuData, lct_nmea* NmeaData, lct_fs* LocFusion,
|
||||
FILE *fp_gps);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoopStr
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/07/03 logzhan
|
||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||
*---------------------------------------------------------------------**/
|
||||
int ParseLineAndUpdate(char* line, lct_fs* result);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initRmc
|
||||
* Description : 初始化RMC结构体
|
||||
* Input : rmc， RMC结构体
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void RMC_Init(lct_nmea_rmc * rmc);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initNmeaFlg
|
||||
* Description : 初始化GPS的NMEA协议的相关标志位
|
||||
* Input : ln， NMEA结构体
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void NmeaFlag_Init(lct_nmea * ln);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : clearNmeaFlg
|
||||
* Description : 清空Nmea标志位,功能和init相同
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void ClearNmeaFlg(lct_nmea * ln);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_saveGnssInfo
|
||||
* Description : 保存GPS相关信息
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void SaveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : GetLIBVersion
|
||||
* Description : pdr库版本号
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* GetLIBVersion(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_closeAlgorithm
|
||||
* Description : 关闭PDR算法,释放资源
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Algorithm_DeInit();
|
||||
|
||||
|
||||
#ifdef _WIN32 // 主要用于支持win32调试使用，用于dll接口导出
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : LibPDR_Init
|
||||
* Description : 初始化PDR算法(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void LibPDR_Init();
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : PDR_StepUpdateGPS
|
||||
* Description : 仿真器按照GPS单步执行(DLL调用)
|
||||
* Date : 2022/9/16 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) PosFusion LibPDR_StepUpdateGPS(int useGpsFlg);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setSimulatorFileCsvPath
|
||||
* Description : 设置仿真文件的路径(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void setSimulatorFileCsvPath(char* path);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : LibPDR_DeInit
|
||||
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void LibPDR_DeInit();
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setRefGpsYaw
|
||||
* Description : 设置GPS参考角度
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void setRefGpsYaw();
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,346 @@
|
|||
/******************** (C) COPYRIGHT 2021 Geek************************************
|
||||
* File Name : pdr_base.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author : yuanlin@vivocom &
|
||||
*
|
||||
*
|
||||
* Date of Issued : 2021.02.01
|
||||
* Comments : PDR 算法基本定义头文件
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_BASE_H_
|
||||
#define _PDR_BASE_H_
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include "stdint.h"
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
/* PDR SYS CFG ---------------------------------------------------------------*/
|
||||
#define PDR_OUTPUT_SMOOTH 0 // 是否开启轨迹平滑
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define PCA_NUM 200
|
||||
#define ACCURACY_ERR_MAX 1000 // GPS的accuracy最大值,一般用于初始化用
|
||||
#define N 4 // 矩阵维数
|
||||
#define OPEN_SKY 1
|
||||
#define MAX_NO_GPS_PREDICT 10 // 无GPS信息状态，最大位置推算数量
|
||||
#define UN_UPDATE 0 // 非更新状态
|
||||
#define DATA_UPDATE 1 // 数据更新
|
||||
#define ON 1
|
||||
#define OFF 0
|
||||
#define PDR_TRUE 1
|
||||
#define PDR_FALSE 0
|
||||
|
||||
#define TYPE_FIX_NONE 0 // PDR修正结果为不输出
|
||||
#define TYPE_FIX_GPS 2 // PDR修正结果为输出原始GPS
|
||||
#define TYPE_FIX_PDR 1 // PDR修正结果为PDR融合
|
||||
|
||||
// PDR携带方式
|
||||
#define UNKNOWN 0
|
||||
#define FORWARD_UP_RIGHT 1 // 臂包：右臂，头朝上，屏朝外；or左臂，头朝上，屏朝内
|
||||
#define BACKWARD_UP_LEFT 2 // 臂包：左臂，头朝上，屏朝外；or右臂，头朝上，屏朝内
|
||||
#define BACKWARD_DOWN_RIGHT 3 // 臂包：右臂，头朝下，屏朝外；or左臂，头朝下，屏朝内
|
||||
#define FORWARD_DOWN_LEFT 4 // 臂包：左臂，头朝下，屏朝外；or右臂，头朝下，屏朝内
|
||||
#define LEFT_UP_FORWARD 5 // 裤兜：头朝上，屏朝前
|
||||
#define RIGHT_UP_BACKWARD 6 // 裤兜：头朝上，屏朝后
|
||||
#define RIGHT_DOWN_FORWARD 7 // 裤兜：头朝下，屏朝前
|
||||
#define LEFT_DOWN_BACKWARD 8 // 裤兜：头朝下，屏朝后
|
||||
|
||||
|
||||
// PDR相关状态
|
||||
#define PDR_NO_ERROR 0
|
||||
#define PDR_OUT_OF_MEMORY 1
|
||||
#define PDR_MOTION_TYPE_STATIC 0
|
||||
#define PDR_MOTION_TYPE_IRREGULAR 1
|
||||
#define PDR_MOTION_TYPE_HANDHELD 2
|
||||
#define PDR_MOTION_TYPE_SWINGING 3
|
||||
#define PDR_STATUS_RESET 0x80
|
||||
#define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40
|
||||
#define PDR_STATUS_PCA_STABLE 0x20
|
||||
#define PDR_STATUS_BIAS_STABLE 0x10
|
||||
#define PDR_STATUS_STABLE (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE)
|
||||
|
||||
// 地球相关常数
|
||||
#define WGS84_RE 6378137 // WGS-84椭球体长半轴
|
||||
#define WGS84_ECCENTR2 6.69437999014e-3 // 第一偏向率的平方，e^2(WGS-84)
|
||||
#define WGS84_OMEGA_E_DOT 7.2921151467e-5 // 地球自转角速度 rad/sec
|
||||
#define WGS84_GRAVITY 9.7803267714 // 地球平均重力加速度 m/s^2
|
||||
|
||||
#define DEG_PER_RADIAN 57.295779513082323 // degrees per radian
|
||||
#define RADIAN_PER_DEG 0.017453292519943 // radians per degree
|
||||
|
||||
#define GPS_SPEED_UNIT 0.5144444 // GPS原始速度转m/s的缩放系数
|
||||
#define PI 3.1415926
|
||||
#define DOUBLE_ZERO 1e-10
|
||||
#define TWO_PI (2.0*PI)
|
||||
#define R2D(x) (x*57.2957795130823)
|
||||
#define D2R(x) (x*3.14159265/180.0)
|
||||
#define USE_BUG_FOR_LOCAL_PARA 1
|
||||
#define BUF_DMT_1 1
|
||||
#define BUF_DMT_2 2
|
||||
|
||||
#define PRINT_ALGO_RUN_TIME 0
|
||||
#define PRINT_CLCT 1
|
||||
#define PATTERN_RECOGNITION_LEN 256 // 缓存256 / SAMPLE_RATE的数量
|
||||
|
||||
#define OPEN_AREA 1 // 开阔区域(GPS信号较好)
|
||||
#define UN_OPEN_AREA 0 // 非开阔区域(GPS信号较弱)
|
||||
|
||||
|
||||
#define DETECTOR_RUN_FREQ 1280 // 用户运动类型检测器检测周期ms
|
||||
#define DETECTOR_TYPE_STATIC 0 // 用户静止
|
||||
#define DETECTOR_TYPE_IRREGULAR 1 // 无规律运动
|
||||
#define DETECTOR_TYPE_HANDHELD 2 // 手持运动
|
||||
#define DETECTOR_TYPE_SWINGING 3 // 摆手运动
|
||||
|
||||
#define ulong unsigned long
|
||||
#define uchar unsigned char
|
||||
|
||||
typedef enum {
|
||||
IS_INITIAL = 0,
|
||||
IS_NORMAL = 1,
|
||||
}pdrStatus;
|
||||
|
||||
|
||||
typedef struct {
|
||||
double xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||
double p_xk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||
double zk[N];
|
||||
double p_pk[N][N];
|
||||
double pk[N][N];
|
||||
double phi[N][N];
|
||||
double hk[N][N];
|
||||
double q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声)
|
||||
double r[N][N]; // 卡尔曼滤波R矩阵(观测噪声)
|
||||
double Kk[N][N];
|
||||
double lambda;
|
||||
double plat;
|
||||
double plon;
|
||||
double initHeading;
|
||||
}EKFPara_t;
|
||||
|
||||
typedef struct{
|
||||
int fnum;
|
||||
int deltaStep;
|
||||
float fsum;
|
||||
float meanTime;
|
||||
double lastTime;
|
||||
}StepPredict;
|
||||
|
||||
typedef struct PDR {
|
||||
uint32_t status; // PDR当前状态
|
||||
uint32_t motionType; // 用户运动类型
|
||||
uint32_t NoGnssCount; // 没有GNSS信息次数
|
||||
pdrStatus sysStatus; // PDR系统状态
|
||||
int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱)
|
||||
int fusionPdrFlg; // 融合PDR位置标志位, 当flg为0时纯输出GPS
|
||||
double pllaInit[3]; // 初始plla坐标
|
||||
double ts; // 时间戳
|
||||
double x0; // 初始北向坐标
|
||||
double y0; // 初始东向坐标
|
||||
// 航向角相关
|
||||
double heading; // PDR方向角
|
||||
double lastHeading; // 上一次PDR方向角
|
||||
double deltaHeading; // 偏航角变化量
|
||||
double insHeadingOffset; // 惯导方向角偏移
|
||||
double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯
|
||||
double gpsHeading; // GPS航向角
|
||||
double lastGpsHeading; // 上一次GPS航向角
|
||||
double trackHeading; // GPS轨迹航向角
|
||||
// 速度相关
|
||||
double gpsSpeed; // GPS速度
|
||||
// 步数相关
|
||||
ulong steps; // 当前步数信息
|
||||
ulong lastSteps; // 上一次的步数
|
||||
ulong lastGpsSteps; // 上一次GPS步数
|
||||
ulong deltaStepsPerGps; // 两次GPS更新之间，步数的变化量
|
||||
float motionFreq; // 运动频率
|
||||
double gyroTime; // 陀螺仪时间
|
||||
float axis_ceofficient[3];
|
||||
float axis_direction[2];
|
||||
float pca_direction[2];
|
||||
float pca_accuracy;
|
||||
float bias_direction[2];
|
||||
float bias_accuracy;
|
||||
float cal_direction[2];
|
||||
} PDR_t;
|
||||
|
||||
typedef struct {
|
||||
int type;
|
||||
float accBuffer[PATTERN_RECOGNITION_LEN];
|
||||
}classifer;
|
||||
|
||||
// 用户运动类型分类器
|
||||
typedef struct DETECTOR {
|
||||
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
||||
uint32_t lastType;
|
||||
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
||||
} DETECTOR_t;
|
||||
|
||||
|
||||
DETECTOR_t *pdr_getDetector(void);
|
||||
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
|
||||
PDR_t* Base_Init(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_resetData
|
||||
* Description : 重置PDR变量
|
||||
* Date : 2020/7/21
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetData(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_computePCA
|
||||
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
|
||||
* Date : 2020/7/21 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void ComputePCA(AHRS_t* ahrs);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : detectorUpdate
|
||||
* Description : 根据新确定的手机携带方式更新g_pdr.status等
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void detectorUpdate(DETECTOR_t* detector);
|
||||
|
||||
|
||||
void gpsUpdateCb(GNSS_t* gps);
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 计算每步所耗时间
|
||||
* Input : stepPredict， 步数预测结构体
|
||||
timestamp，时间戳
|
||||
steps_last，上一次步数
|
||||
steps，本次步数
|
||||
**************************************************************************/
|
||||
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 预测步数
|
||||
* Input : stepPredict， 步数预测结构体
|
||||
timestamp，时间戳
|
||||
steps_last，上一次步数
|
||||
gnssVel，gps速度
|
||||
* Output : int， 步数
|
||||
**************************************************************************/
|
||||
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : stateRecognition
|
||||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void StateRecognition(float *acc, classifer *sys);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
||||
* Input : g_pdr，PDR结构体
|
||||
sys，运动幅度数据结构体
|
||||
gps_yaw，GPS轨迹角
|
||||
G_yaw，GPS方向角
|
||||
ss_data， 传感器数据结构体
|
||||
* Output : double，用户行走方向
|
||||
**************************************************************************/
|
||||
double calPredAngle(PDR_t *g_pdr, classifer *sys, double gps_yaw, double G_yaw, IMU_t *ss_data);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
||||
* Input : g_pdr，PDR结构体
|
||||
steps，本次步数
|
||||
steps_last，上一次步数
|
||||
stepPredict，步数预测结构体
|
||||
pdr_angle，用户行走方向
|
||||
scene_type，GPS用户所处场景（开阔和非开阔）
|
||||
ss_data， 传感器数据结构体
|
||||
pgnss，GPS数据结构体
|
||||
kf，EKF结构体
|
||||
pdr, PDR预测位置成功标志位
|
||||
**************************************************************************/
|
||||
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data,
|
||||
GNSS_t *pgnss, EKFPara_t *kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detStatic
|
||||
* Description : 检测用户是否处于静止状态
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_outputGpsPos
|
||||
* Description : 输出GPS位置
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : resetOutputResult
|
||||
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
|
||||
* 2、GPS值赋值给result
|
||||
* pgnss，GPS数据结构体
|
||||
* plla_init，待重置NED坐标系原点经纬度
|
||||
* x_init，y_init待重置NED坐标系原点的n和e坐标
|
||||
* kf， EKF数据结构体
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detIsCarMode
|
||||
* Description : 识别是否是车载模式
|
||||
* Input : pgnss，GPS数据结构体
|
||||
g_pdr，PDR结构体
|
||||
delSteps， 与上次相比步数的变化量
|
||||
time，计数器
|
||||
* Output : int，车载模式标志位
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int DetectCarMode(GNSS_t *pgnss, PDR_t *g_pdr, unsigned long delSteps, int *time);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detPdrToReset
|
||||
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
|
||||
* 重置PDR到GPS的位置。
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t *g_pdr);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 初始化EKF滤波器
|
||||
* Input : pgnss，GPS数据结构体
|
||||
plla_init，NED坐标系原点经纬度
|
||||
kf， EKF数据结构体
|
||||
x_init，y_init，NED坐标系原点的n和e坐标
|
||||
sys_status，定位系统状态（未初始化和已初始化）
|
||||
* Output : int，初始化标志位
|
||||
result，定位结果结构体
|
||||
**************************************************************************/
|
||||
int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, lct_fs *result);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 设置EKF滤波器的Q和R
|
||||
* Input : scane_type, 用户所处场景（开阔和非开阔）
|
||||
nmea_data，NMEA数据结构体
|
||||
g_pdr，PDR结构体
|
||||
sys，运动幅度数据结构体
|
||||
kf，EKF数据结构体
|
||||
**************************************************************************/
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 计算GPS轨迹角
|
||||
* Input : gpsPos，GPS历史定位点在ECEF下的三维坐标
|
||||
pgnss，GPS数据结构体
|
||||
* Output : double，轨迹角
|
||||
**************************************************************************/
|
||||
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
#pragma once
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
#ifndef __DETECTOR_H
|
||||
#define __DETECTOR_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
#include "pdr_base.h"
|
||||
#include "buffer.h"
|
||||
#include "AHRS.h"
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define DETECTOR_NO_ERROR 0
|
||||
#define DETECTOR_OUT_OF_MEMORY 1
|
||||
|
||||
#define MAG_BUF_LEN 256
|
||||
#define MAG_LEN 6
|
||||
|
||||
/* Struct Declaration --------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
typedef void (*DETECTOR_update_callback)(DETECTOR_t *detector);
|
||||
|
||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||
extern BUFFER_SHORT g_acc_frq_buf[3];
|
||||
extern BUFFER_SHORT g_acc_amp_buf[3];
|
||||
extern BUFFER_SHORT g_gyr_frq_buf[3];
|
||||
extern BUFFER_SHORT g_gyr_amp_buf[3];
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_getDetector
|
||||
* Description : 获取PDR运动分类器的结构体指针
|
||||
* Date : 2020/02/16 logzhan &
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR_t *pdr_getDetector(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : Detector_Init
|
||||
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||
* Date : 2022/09/23 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR_t* Detector_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : DetectorReset
|
||||
* Description : 重置PDR运动分类器
|
||||
* Date : 2022/09/23 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void DetectorReset(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : DetectMotionType
|
||||
* Description : pdr运动类型检测
|
||||
* Date : 2020/7/20
|
||||
*---------------------------------------------------------------------**/
|
||||
int DetectMotionType(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : predict
|
||||
* Description : 判断手机携带方式（静止、手持、摆手等）
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detectorPredict(float* feature);
|
||||
|
||||
void mag_calibration(IMU *imu);
|
||||
void mag_m_trans(float a[MAG_BUF_LEN][3], float r[3][MAG_BUF_LEN]);
|
||||
void mag_min(float a[MAG_BUF_LEN], float *mag_min);
|
||||
void mag_max(float a[MAG_BUF_LEN], float *mag_max);
|
||||
void mag_rand(float r[6]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : DetectorUpdateIMU
|
||||
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
|
||||
* 用户的运动类型
|
||||
* Date : 2022/09/23
|
||||
*---------------------------------------------------------------------**/
|
||||
void DetectorUpdateIMU(IMU_t* imu);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue