2019-09-05 00:35:18 -06:00
# define _LARGEFILE64_SOURCE
2019-08-06 04:57:06 -06:00
# include <sys/types.h>
2019-08-07 07:39:45 -06:00
# include <sys/time.h>
# include <map>
2019-08-06 04:57:06 -06:00
# include <sys/stat.h>
# include <fcntl.h>
# include <termios.h>
# include <stdio.h>
# include <unistd.h>
# include <strings.h>
# include <stdlib.h>
# include <string>
# include <stdint.h>
# include "ubx.hh"
2019-08-30 17:08:56 -06:00
# include "navmon.hh"
2019-08-06 04:57:06 -06:00
# include <iostream>
2019-08-07 17:22:46 -06:00
# include <fstream>
2019-08-06 04:57:06 -06:00
# include <string.h>
2019-08-07 07:39:45 -06:00
# include "fmt/format.h"
# include "fmt/printf.h"
2019-08-09 07:58:52 -06:00
# include "bits.hh"
# include "galileo.hh"
# include <arpa/inet.h>
# include "navmon.pb.h"
2019-08-12 07:54:16 -06:00
# include "gps.hh"
2019-08-26 07:58:01 -06:00
# include "glonass.hh"
# include "beidou.hh"
# include "CLI/CLI.hpp"
2019-09-23 06:23:03 -06:00
# include <thread>
# include <mutex>
# include "comboaddress.hh"
# include "swrappers.hh"
# include "sclasses.hh"
2019-12-28 15:19:40 -07:00
# include "githash.h"
2019-09-23 06:23:03 -06:00
2019-10-15 06:51:39 -06:00
bool doDEBUG { false } ;
bool doLOGFILE { false } ;
2019-09-23 06:23:03 -06:00
struct timespec g_gnssutc ;
uint16_t g_galwn ;
2019-08-06 04:57:06 -06:00
using namespace std ;
2019-09-23 06:23:03 -06:00
uint16_t g_srcid { 0 } ;
2019-11-03 12:50:50 -07:00
int g_fixtype { - 1 } ;
2019-11-26 11:12:37 -07:00
double g_speed { - 1 } ;
2019-09-23 06:23:03 -06:00
2019-11-03 12:50:50 -07:00
static int getBaudrate ( int baud )
{
if ( baud = = 115200 )
return B115200 ;
else if ( baud = = 57600 )
return B57600 ;
else if ( baud = = 38400 )
return B38400 ;
else if ( baud = = 19200 )
return B19200 ;
else if ( baud = = 9600 )
return B9600 ;
else
throw std : : runtime_error ( " Unknown baudrate " + to_string ( baud ) ) ;
}
2019-08-09 13:42:28 -06:00
2019-11-03 12:50:50 -07:00
static int g_baudval ;
2019-08-06 04:57:06 -06:00
size_t writen2 ( int fd , const void * buf , size_t count )
{
const char * ptr = ( char * ) buf ;
const char * eptr = ptr + count ;
ssize_t res ;
while ( ptr ! = eptr ) {
res = : : write ( fd , ptr , eptr - ptr ) ;
if ( res < 0 ) {
throw runtime_error ( " failed in writen2: " + string ( strerror ( errno ) ) ) ;
}
else if ( res = = 0 )
throw EofException ( ) ;
ptr + = ( size_t ) res ;
}
return count ;
}
2019-08-09 07:58:52 -06:00
/* inav schedule:
1 ) Find plausible start time of next cycle
Current cycle : TOW - ( TOW % 30 )
Next cycle : TOW - ( TOW % 30 ) + 30
2019-09-28 13:30:12 -06:00
E1 :
t n w
0 1 : 2 tow % 30 = = 1
2 2 : 4 tow % 30 = = 3
4 3 : 6 TOW 5 - > set startTow , startTowFresh
6 4 : 7 / 9 7
8 5 : 8 / 10 9
10 6 : 0 WN / TOW 11
12 7 : 0 WN / TOW 13
14 8 : 0 WN / TOW 15
16 9 : 0 WN / TOW 17
18 10 : 0 WN / TOW 19
20 11 : 1 21
22 12 : 3 23
24 13 : 5 WN / TOW 25
26 14 : 0 WN / TOW 27
28 15 : 0 WN / TOW 29
E5b - 1 :
2019-08-09 07:58:52 -06:00
t n w
2019-09-28 13:30:12 -06:00
0 1 : 1 ( 2 / 2 ) tow % 30 = = 0
2 2 : 3 tow % 30 = = 2
4 3 : 5 WN / TOW 4 - > set startTow , startTowFresh
6 4 : 7 / 9
2019-08-09 07:58:52 -06:00
8 5 : 8 / 10
2019-09-28 13:30:12 -06:00
10 6 : 0 WN / TOW
2019-08-09 07:58:52 -06:00
12 7 : 0 WN / TOW
14 8 : 0 WN / TOW
16 9 : 0 WN / TOW
18 10 : 0 WN / TOW
2019-09-28 13:30:12 -06:00
20 11 : 2
22 12 : 4
24 13 : 6 TOW
2019-08-09 07:58:52 -06:00
26 14 : 0 WN / TOW
2019-09-28 13:30:12 -06:00
28 15 : 1 ( 1 / 2 ) WN / TOW
2019-08-09 07:58:52 -06:00
*/
2019-08-07 07:39:45 -06:00
class UBXMessage
{
public :
2019-08-09 07:58:52 -06:00
struct BadChecksum { } ;
2019-08-07 07:39:45 -06:00
explicit UBXMessage ( basic_string_view < uint8_t > src )
{
d_raw = src ;
if ( d_raw . size ( ) < 6 )
throw std : : runtime_error ( " Partial UBX message " ) ;
uint16_t csum = calcUbxChecksum ( getClass ( ) , getType ( ) , d_raw . substr ( 6 , d_raw . size ( ) - 8 ) ) ;
if ( csum ! = d_raw . at ( d_raw . size ( ) - 2 ) + 256 * d_raw . at ( d_raw . size ( ) - 1 ) )
2019-08-09 07:58:52 -06:00
throw BadChecksum ( ) ;
2019-08-07 07:39:45 -06:00
}
uint8_t getClass ( ) const
{
return d_raw . at ( 2 ) ;
}
uint8_t getType ( ) const
{
return d_raw . at ( 3 ) ;
}
std : : basic_string < uint8_t > getPayload ( ) const
{
return d_raw . substr ( 6 , d_raw . size ( ) - 8 ) ;
}
std : : basic_string < uint8_t > d_raw ;
} ;
2019-08-12 07:54:16 -06:00
bool g_fromFile { false } ;
2019-09-05 02:19:43 -06:00
# if !defined(O_LARGEFILE)
# define O_LARGEFILE 0
# endif
2019-11-22 06:32:25 -07:00
std : : pair < UBXMessage , struct timeval > getUBXMessage ( int fd , double * timeout )
2019-08-07 07:39:45 -06:00
{
2019-10-15 06:51:39 -06:00
static int logfile = 0 ;
if ( doLOGFILE ) {
if ( ! logfile & & ! g_fromFile ) {
2019-09-05 00:35:18 -06:00
logfile = open ( " ./logfile " , O_WRONLY | O_CREAT | O_APPEND | O_LARGEFILE , 0600 ) ;
2019-10-15 06:51:39 -06:00
if ( ! logfile )
throw std : : runtime_error ( " Failed to open logfile for writing " ) ;
}
2019-08-12 07:54:16 -06:00
}
2019-08-07 07:39:45 -06:00
uint8_t marker [ 2 ] = { 0 } ;
for ( ; ; ) {
marker [ 0 ] = marker [ 1 ] ;
2019-11-22 06:32:25 -07:00
int res = readn2Timeout ( fd , marker + 1 , 1 , timeout ) ;
2019-08-12 07:54:16 -06:00
2019-08-07 07:39:45 -06:00
if ( res < 0 )
throw EofException ( ) ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" marker now: "<< (int)marker[0]<<" " <<(int)marker[1]<<endl; }
2019-08-07 07:39:45 -06:00
if ( marker [ 0 ] = = 0xb5 & & marker [ 1 ] = = 0x62 ) { // bingo
struct timeval tv ;
gettimeofday ( & tv , 0 ) ;
basic_string < uint8_t > msg ;
msg . append ( marker , 2 ) ; // 0,1
uint8_t b [ 4 ] ;
2019-11-22 06:32:25 -07:00
readn2Timeout ( fd , b , 4 , timeout ) ;
2019-08-07 07:39:45 -06:00
msg . append ( b , 4 ) ; // class, type, len1, len2
uint16_t len = b [ 2 ] + 256 * b [ 3 ] ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" Got class "<<(int)msg[2]<<" type "<<(int)msg[3]<<", len = "<<len<<endl; }
2019-08-07 07:39:45 -06:00
uint8_t buffer [ len + 2 ] ;
2019-11-22 06:32:25 -07:00
res = readn2Timeout ( fd , buffer , len + 2 , timeout ) ;
2019-08-12 07:54:16 -06:00
msg . append ( buffer , len + 2 ) ; // checksum
2019-10-15 06:51:39 -06:00
if ( doLOGFILE ) {
if ( ! g_fromFile )
writen2 ( logfile , msg . c_str ( ) , msg . size ( ) ) ;
}
2019-08-09 07:58:52 -06:00
return make_pair ( UBXMessage ( msg ) , tv ) ;
2019-08-07 07:39:45 -06:00
}
}
}
UBXMessage waitForUBX ( int fd , int seconds , uint8_t ubxClass , uint8_t ubxType )
{
2019-11-22 06:32:25 -07:00
double timeout = seconds ;
for ( ; ; ) {
auto [ msg , tv ] = getUBXMessage ( fd , & timeout ) ;
2019-08-09 07:58:52 -06:00
( void ) tv ;
2019-08-26 07:58:01 -06:00
2019-08-07 07:39:45 -06:00
if ( msg . getClass ( ) = = ubxClass & & msg . getType ( ) = = ubxType ) {
return msg ;
}
2019-08-26 07:58:01 -06:00
else
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got: " < < ( int ) msg . getClass ( ) < < " " < < ( int ) msg . getType ( ) < < " while waiting for " < < ( int ) ubxClass < < " " < < ( int ) ubxType < < endl ; }
2019-08-07 07:39:45 -06:00
}
throw std : : runtime_error ( " Did not get response on time " ) ;
}
2019-11-03 12:50:50 -07:00
UBXMessage sendAndWaitForUBX ( int fd , int seconds , basic_string_view < uint8_t > msg , uint8_t ubxClass , uint8_t ubxType )
{
for ( int n = 3 ; n ; - - n ) {
writen2 ( fd , & msg [ 0 ] , msg . size ( ) ) ;
try {
return waitForUBX ( fd , seconds , ubxClass , ubxType ) ;
}
catch ( . . . ) {
if ( n = = 1 )
throw ;
cerr < < " Retransmit " < < endl ;
}
}
2019-12-20 12:08:14 -07:00
// we actually never get here, but if you remove this line, we get a warning
// and people can't stop nagging us about the warning..
return waitForUBX ( fd , seconds , ubxClass , ubxType ) ;
2019-11-03 12:50:50 -07:00
}
2019-08-26 07:58:01 -06:00
bool waitForUBXAckNack ( int fd , int seconds , int ubxClass , int ubxType )
2019-08-07 07:39:45 -06:00
{
2019-11-22 06:32:25 -07:00
double timeout = seconds ;
for ( ; ; ) {
auto [ msg , tv ] = getUBXMessage ( fd , & timeout ) ;
2019-08-09 07:58:52 -06:00
( void ) tv ;
2019-08-26 07:58:01 -06:00
if ( msg . getClass ( ) ! = 5 | | ! ( msg . getType ( ) = = 0 | | msg . getType ( ) = = 1 ) ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got: " < < ( int ) msg . getClass ( ) < < " " < < ( int ) msg . getType ( ) < < " while waiting for ack/nack of " < < ubxClass < < " " < < ubxType < < endl ; }
2019-08-26 07:58:01 -06:00
continue ;
}
const auto & payload = msg . getPayload ( ) ;
if ( payload . size ( ) ! = 2 ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < " Wrong payload size for ack/nack: " < < payload . size ( ) < < endl ; }
2019-08-26 07:58:01 -06:00
continue ;
}
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got an " < < ( msg . getType ( ) ? " ack " : " nack " ) < < " for " < < ( int ) payload [ 0 ] < < " " < < ( int ) payload [ 1 ] < < " while waiting for " < < ubxClass < < " " < < ubxType < < endl ; }
2019-08-26 07:58:01 -06:00
if ( msg . getClass ( ) = = 0x05 & & msg . getType ( ) = = 0x01 & & payload [ 0 ] = = ubxClass & & payload [ 1 ] = = ubxType ) {
2019-08-07 07:39:45 -06:00
return true ;
}
2019-08-26 07:58:01 -06:00
else if ( msg . getClass ( ) = = 0x05 & & msg . getType ( ) = = 0x00 & & payload [ 0 ] = = ubxClass & & payload [ 1 ] = = ubxType ) {
2019-08-07 07:39:45 -06:00
return false ;
}
}
2019-08-26 07:58:01 -06:00
throw std : : runtime_error ( " Did not get ACK/NACK response for class " + to_string ( ubxClass ) + " type " + to_string ( ubxType ) + " on time " ) ;
2019-08-07 07:39:45 -06:00
}
2019-11-03 12:50:50 -07:00
bool sendAndWaitForUBXAckNack ( int fd , int seconds , basic_string_view < uint8_t > msg , uint8_t ubxClass , uint8_t ubxType )
{
for ( int n = 3 ; n ; - - n ) {
writen2 ( fd , & msg [ 0 ] , msg . size ( ) ) ;
try {
return waitForUBXAckNack ( fd , seconds , ubxClass , ubxType ) ;
}
catch ( . . . ) {
if ( n = = 1 )
throw ;
cerr < < " Retransmit " < < endl ;
}
}
return false ;
}
2019-09-23 06:23:03 -06:00
class NMMSender
{
struct Destination
{
int fd { - 1 } ;
ComboAddress dst { " 0.0.0.0:0 " } ;
std : : string fname ;
deque < string > queue ;
std : : mutex mut ;
void emitNMM ( const NavMonMessage & nmm ) ;
} ;
public :
void addDestination ( int fd )
{
auto d = std : : make_unique < Destination > ( ) ;
d - > fd = fd ;
d_dests . push_back ( std : : move ( d ) ) ;
}
void addDestination ( const ComboAddress & dest )
{
auto d = std : : make_unique < Destination > ( ) ;
d - > dst = dest ;
d_dests . push_back ( std : : move ( d ) ) ;
}
void launch ( )
{
for ( auto & d : d_dests ) {
if ( d - > dst . sin4 . sin_port ! = 0 ) {
thread t ( & NMMSender : : sendTCPThread , this , d . get ( ) ) ;
t . detach ( ) ;
}
}
}
void sendTCPThread ( Destination * d )
{
for ( ; ; ) {
try {
Socket s ( d - > dst . sin4 . sin_family , SOCK_STREAM ) ;
SocketCommunicator sc ( s ) ;
sc . setTimeout ( 3 ) ;
sc . connect ( d - > dst ) ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Connected to " < < d - > dst . toStringWithPort ( ) < < endl ; }
2019-09-23 06:23:03 -06:00
for ( ; ; ) {
std : : string msg ;
{
std : : lock_guard < std : : mutex > mut ( d - > mut ) ;
if ( ! d - > queue . empty ( ) ) {
msg = d - > queue . front ( ) ;
}
}
if ( ! msg . empty ( ) ) {
sc . writen ( msg ) ;
2019-11-22 01:02:16 -07:00
std : : lock_guard < std : : mutex > mut ( d - > mut ) ;
2019-09-23 06:23:03 -06:00
d - > queue . pop_front ( ) ;
}
else usleep ( 100000 ) ;
}
}
catch ( std : : exception & e ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending thread for " < < d - > dst . toStringWithPort ( ) < < " had error: " < < e . what ( ) < < endl ; }
2019-09-23 06:23:03 -06:00
{
std : : lock_guard < std : : mutex > mut ( d - > mut ) ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " There are now " < < d - > queue . size ( ) < < " messages queued for " < < d - > dst . toStringWithPort ( ) < < endl ; }
2019-09-23 06:23:03 -06:00
}
sleep ( 1 ) ;
}
catch ( . . . ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending thread for " < < d - > dst . toStringWithPort ( ) < < " had error " ; }
2019-09-23 06:23:03 -06:00
{
std : : lock_guard < std : : mutex > mut ( d - > mut ) ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < " There are now " < < d - > queue . size ( ) < < " messages queued for " < < d - > dst . toStringWithPort ( ) < < endl ; }
2019-09-23 06:23:03 -06:00
}
sleep ( 1 ) ;
}
}
}
void emitNMM ( const NavMonMessage & nmm ) ;
private :
vector < unique_ptr < Destination > > d_dests ;
} ;
void NMMSender : : emitNMM ( const NavMonMessage & nmm )
{
for ( auto & d : d_dests ) {
d - > emitNMM ( nmm ) ;
}
}
void NMMSender : : Destination : : emitNMM ( const NavMonMessage & nmm )
2019-08-09 07:58:52 -06:00
{
string out ;
nmm . SerializeToString ( & out ) ;
2019-08-29 08:46:03 -06:00
string msg ( " bert " ) ;
2019-08-09 07:58:52 -06:00
uint16_t len = htons ( out . size ( ) ) ;
2019-08-29 08:46:03 -06:00
msg . append ( ( char * ) & len , 2 ) ;
msg . append ( out ) ;
2019-09-23 06:23:03 -06:00
if ( dst . sin4 . sin_port ) {
std : : lock_guard < std : : mutex > l ( mut ) ;
queue . push_back ( msg ) ;
}
else
writen2 ( fd , msg . c_str ( ) , msg . size ( ) ) ;
2019-08-09 07:58:52 -06:00
}
2019-08-07 07:39:45 -06:00
2019-09-23 06:23:03 -06:00
2019-11-03 12:50:50 -07:00
void enableUBXMessageOnPort ( int fd , uint8_t ubxClass , uint8_t ubxType , uint8_t port , uint8_t rate = 1 )
2019-08-07 07:39:45 -06:00
{
2019-08-26 07:58:01 -06:00
for ( int n = 0 ; n < 5 ; + + n ) {
try {
2019-11-03 12:50:50 -07:00
basic_string < uint8_t > payload ( { ubxClass , ubxType , 0 , 0 , 0 , 0 , 0 , 0 } ) ;
if ( port > 6 )
throw std : : runtime_error ( " Port number out of range (>6) " ) ;
2019-11-10 13:57:40 -07:00
payload [ 2 + port ] = rate ;
2019-11-03 12:50:50 -07:00
auto msg = buildUbxMessage ( 0x06 , 0x01 , payload ) ;
if ( sendAndWaitForUBXAckNack ( fd , 2 , msg , 0x06 , 0x01 ) )
2019-08-26 07:58:01 -06:00
return ;
else
throw std : : runtime_error ( " Got NACK enabling UBX message " + to_string ( ( int ) ubxClass ) + " " + to_string ( ( int ) ubxType ) ) ;
}
catch ( TimeoutError & te ) {
2019-11-03 12:50:50 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Had " < < n < < " th timeout in enableUBXMessageOnPort " < < endl ; }
2019-08-26 07:58:01 -06:00
continue ;
}
}
throw TimeoutError ( ) ;
2019-08-07 07:39:45 -06:00
}
2019-08-06 04:57:06 -06:00
2019-10-15 06:51:39 -06:00
bool isPresent ( string_view fname )
{
struct stat sb ;
if ( stat ( & fname [ 0 ] , & sb ) < 0 )
return false ;
return true ;
}
2019-08-09 14:49:52 -06:00
bool isCharDevice ( string_view fname )
{
struct stat sb ;
if ( stat ( & fname [ 0 ] , & sb ) < 0 )
return false ;
return ( sb . st_mode & S_IFMT ) = = S_IFCHR ;
}
2019-08-26 07:58:01 -06:00
void readSome ( int fd )
2019-08-09 14:49:52 -06:00
{
2019-11-22 06:32:25 -07:00
2019-08-26 07:58:01 -06:00
for ( int n = 0 ; n < 5 ; + + n ) {
2019-11-22 06:32:25 -07:00
double timeout = 1 ;
try {
auto [ msg , timestamp ] = getUBXMessage ( fd , & timeout ) ;
( void ) timestamp ;
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Read some init: " < < ( int ) msg . getClass ( ) < < " " < < ( int ) msg . getType ( ) < < endl ; }
if ( msg . getClass ( ) = = 0x4 )
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " " < < string ( ( char * ) msg . getPayload ( ) . c_str ( ) , msg . getPayload ( ) . size ( ) ) < < endl ; }
}
catch ( TimeoutError & te ) {
cerr < < " Timeout " < < endl ;
}
2019-08-09 14:49:52 -06:00
}
2019-11-22 06:32:25 -07:00
2019-08-26 07:58:01 -06:00
}
struct termios g_oldtio ;
2019-09-05 02:19:43 -06:00
int initFD ( const char * fname , bool doRTSCTS )
2019-08-26 07:58:01 -06:00
{
2019-08-09 07:58:52 -06:00
int fd ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD() " < < endl ; }
if ( ! isPresent ( fname ) ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD - " < < fname < < " - not present " < < endl ; }
throw runtime_error ( " Opening file " + string ( fname ) ) ;
}
2019-08-26 07:58:01 -06:00
if ( string ( fname ) ! = " stdin " & & string ( fname ) ! = " /dev/stdin " & & isCharDevice ( fname ) ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD - open( " < < fname < < " ) " < < endl ; }
2019-08-26 07:58:01 -06:00
fd = open ( fname , O_RDWR | O_NOCTTY ) ;
if ( fd < 0 ) {
throw runtime_error ( " Opening file " + string ( fname ) ) ;
}
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD - open successful " < < endl ; }
2019-08-26 07:58:01 -06:00
struct termios newtio ;
if ( tcgetattr ( fd , & g_oldtio ) ) { /* save current port settings */
perror ( " tcgetattr " ) ;
exit ( - 1 ) ;
}
2019-08-12 07:54:16 -06:00
2019-08-09 07:58:52 -06:00
bzero ( & newtio , sizeof ( newtio ) ) ;
2019-10-15 06:51:39 -06:00
newtio . c_cflag = CS8 | CLOCAL | CREAD ;
2019-11-03 12:50:50 -07:00
if ( doRTSCTS ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " will enable RTSCTS " < < endl ; }
2019-09-05 02:19:43 -06:00
newtio . c_cflag | = CRTSCTS ;
2019-11-03 12:50:50 -07:00
}
2019-08-09 07:58:52 -06:00
newtio . c_iflag = IGNPAR ;
newtio . c_oflag = 0 ;
/* set input mode (non-canonical, no echo,...) */
newtio . c_lflag = 0 ;
newtio . c_cc [ VTIME ] = 0 ; /* inter-character timer unused */
2019-11-22 06:32:25 -07:00
newtio . c_cc [ VMIN ] = 4 ; /* blocking read until 5 chars received */
2019-08-09 07:58:52 -06:00
2019-11-03 12:50:50 -07:00
cfsetspeed ( & newtio , g_baudval ) ;
if ( tcsetattr ( fd , TCSANOW , & newtio ) ) {
2019-08-26 07:58:01 -06:00
perror ( " tcsetattr " ) ;
exit ( - 1 ) ;
2019-08-09 07:58:52 -06:00
}
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD - tty set " < < endl ; }
2019-08-26 07:58:01 -06:00
}
else {
g_fromFile = true ;
2019-08-09 07:58:52 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " initFD - open( " < < fname < < " ) - from file " < < endl ; }
2019-08-26 07:58:01 -06:00
fd = open ( fname , O_RDONLY ) ;
if ( fd < 0 )
throw runtime_error ( " Opening file " + string ( fname ) ) ;
}
return fd ;
}
// ubxtool device srcid
int main ( int argc , char * * argv )
{
2019-12-28 15:19:40 -07:00
time_t starttime = time ( 0 ) ;
2019-08-26 07:58:01 -06:00
GOOGLE_PROTOBUF_VERIFY_VERSION ;
CLI : : App app ( " ubxtool " ) ;
2019-08-09 07:58:52 -06:00
2019-09-05 02:19:43 -06:00
2019-09-23 06:23:03 -06:00
bool doGPS { true } , doGalileo { true } , doGlonass { false } , doBeidou { true } , doReset { false } , doWait { true } , doRTSCTS { true } , doSBAS { false } ;
2019-11-26 11:12:37 -07:00
bool doFakeFix { false } ;
2019-11-03 12:50:50 -07:00
bool doKeepNMEA { false } ;
2019-09-23 06:23:03 -06:00
bool doSTDOUT = false ;
2019-09-05 02:19:43 -06:00
# ifdef OpenBSD
doRTSCTS = false ;
# endif
2019-09-23 06:23:03 -06:00
vector < string > destinations ;
string portName ;
2019-11-10 13:57:40 -07:00
int ubxport = 3 ;
2019-11-03 12:50:50 -07:00
int baudrate = 115200 ;
2019-12-04 13:49:06 -07:00
unsigned int fuzzPositionMeters = 0 ;
2019-12-27 12:37:45 -07:00
string owner ;
string remark ;
2019-11-03 12:50:50 -07:00
2019-09-23 06:23:03 -06:00
app . add_option ( " --destination,-d " , destinations , " Send output to this IPv4/v6 address " ) ;
2019-09-03 13:00:44 -06:00
app . add_flag ( " --wait " , doWait , " Wait a bit, do not try to read init messages " ) ;
2019-08-29 15:17:26 -06:00
app . add_flag ( " --reset " , doReset , " Reset UBX device " ) ;
2019-08-26 07:58:01 -06:00
app . add_flag ( " --beidou,-c " , doBeidou , " Enable BeiDou reception " ) ;
app . add_flag ( " --gps,-g " , doGPS , " Enable GPS reception " ) ;
app . add_flag ( " --glonass,-r " , doGlonass , " Enable Glonass reception " ) ;
app . add_flag ( " --galileo,-e " , doGalileo , " Enable Galileo reception " ) ;
2019-09-09 10:09:07 -06:00
app . add_flag ( " --sbas,-s " , doSBAS , " Enable SBAS (EGNOS/WAAS/etc) reception " ) ;
2019-09-05 02:19:43 -06:00
app . add_option ( " --rtscts " , doRTSCTS , " Set hardware handshaking " ) ;
2019-09-23 06:23:03 -06:00
app . add_flag ( " --stdout " , doSTDOUT , " Emit output to stdout " ) ;
2019-11-09 13:59:23 -07:00
app . add_option ( " --port,-p " , portName , " Device or file to read serial from " ) - > required ( ) ;
2019-11-03 12:50:50 -07:00
app . add_option ( " --station " , g_srcid , " Station id " ) ;
2019-11-10 13:57:40 -07:00
app . add_option ( " --ubxport,-u " , ubxport , " UBX port to enable messages on (usb=3) " ) ;
2019-11-03 12:50:50 -07:00
app . add_option ( " --baud,-b " , baudrate , " Baudrate for serial connection " ) ;
app . add_flag ( " --keep-nmea,-k " , doKeepNMEA , " Don't disable NMEA " ) ;
2019-11-26 11:12:37 -07:00
app . add_flag ( " --fake-fix " , doFakeFix , " Inject locally generated fake fix data " ) ;
2019-12-04 13:49:06 -07:00
app . add_option ( " --fuzz-position,-f " , fuzzPositionMeters , " Fuzz position by this many meters " ) ;
2019-12-27 12:37:45 -07:00
app . add_option ( " --owner,-o " , owner , " Name/handle/nick of owner/operator " ) ;
app . add_option ( " --remark " , remark , " Remark for this station " ) ;
2019-11-03 12:50:50 -07:00
2019-10-15 06:51:39 -06:00
app . add_flag ( " --debug " , doDEBUG , " Display debug information " ) ;
app . add_flag ( " --logfile " , doLOGFILE , " Create logfile " ) ;
2019-08-26 07:58:01 -06:00
try {
app . parse ( argc , argv ) ;
} catch ( const CLI : : Error & e ) {
return app . exit ( e ) ;
}
2019-12-27 12:37:45 -07:00
2019-11-03 12:50:50 -07:00
g_baudval = getBaudrate ( baudrate ) ;
2019-08-26 07:58:01 -06:00
2019-11-09 13:59:23 -07:00
if ( ! ( doGPS | | doGalileo | | doGlonass | | doBeidou ) ) {
cerr < < " Enable at least one of --gps, --galileo, --glonass, --beidou " < < endl ;
return EXIT_FAILURE ;
}
2019-09-23 06:23:03 -06:00
int fd = initFD ( portName . c_str ( ) , doRTSCTS ) ;
2019-08-26 07:58:01 -06:00
2019-12-20 05:12:53 -07:00
bool version9 = false ;
if ( doFakeFix ) // hack
version9 = true ;
bool m8t = false ;
2019-12-27 12:37:45 -07:00
string hwversion ;
string swversion ;
string mods ;
string serialno ;
2019-12-20 05:12:53 -07:00
2019-08-26 07:58:01 -06:00
if ( ! g_fromFile ) {
bool doInit = true ;
if ( doInit ) {
2019-09-03 13:00:44 -06:00
if ( doWait )
sleep ( 2 ) ;
else
readSome ( fd ) ;
2019-08-26 07:58:01 -06:00
std : : basic_string < uint8_t > msg ;
2019-08-29 15:17:26 -06:00
if ( doReset ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending a soft reset " < < endl ; }
2019-11-03 12:50:50 -07:00
msg = buildUbxMessage ( 0x06 , 0x04 , { 0x00 , 0x00 , 0x01 , 0x00 } ) ; // soft reset
2019-08-29 15:17:26 -06:00
writen2 ( fd , msg . c_str ( ) , msg . size ( ) ) ;
usleep ( 100000 ) ;
close ( fd ) ;
for ( int n = 0 ; n < 20 ; + + n ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Waiting for device to come back " < < endl ; }
2019-08-29 15:17:26 -06:00
try {
2019-09-23 06:23:03 -06:00
fd = initFD ( portName . c_str ( ) , doRTSCTS ) ;
2019-08-29 15:17:26 -06:00
readSome ( fd ) ;
2019-08-26 07:58:01 -06:00
}
2019-08-29 15:17:26 -06:00
catch ( . . . )
{
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Not yet back " < < endl ; }
2019-08-29 15:17:26 -06:00
usleep ( 400000 ) ;
continue ;
}
break ;
}
2019-08-26 07:58:01 -06:00
}
2019-09-09 17:25:13 -06:00
2019-11-03 12:50:50 -07:00
2019-09-09 17:25:13 -06:00
msg = buildUbxMessage ( 0x0a , 0x04 , { } ) ;
2019-11-03 12:50:50 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending version query " < < endl ; }
UBXMessage um1 = sendAndWaitForUBX ( fd , 1 , msg , 0x0a , 0x04 ) ; // ask for version
2019-12-27 12:37:45 -07:00
swversion = ( const char * ) um1 . getPayload ( ) . c_str ( ) ;
hwversion = ( const char * ) um1 . getPayload ( ) . c_str ( ) + 30 ;
cerr < < humanTimeNow ( ) < < " swVersion: " < < swversion < < endl ;
cerr < < humanTimeNow ( ) < < " hwVersion: " < < hwversion < < endl ;
2019-12-27 12:51:00 -07:00
for ( unsigned int n = 0 ; 40 + 30 * n < um1 . getPayload ( ) . size ( ) ; + + n ) {
string line = ( const char * ) um1 . getPayload ( ) . c_str ( ) + 40 + 30 * n ;
cerr < < humanTimeNow ( ) < < " Extended info: " < < line < < endl ;
if ( line . find ( " F9 " ) ! = string : : npos )
version9 = true ;
if ( line . find ( " M8T " ) ! = string : : npos ) {
m8t = true ;
}
if ( line . find ( " MOD= " ) ! = string : : npos )
mods + = line . substr ( 4 ) ;
// timing: MOD=NEO-M8T-0
}
if ( doDEBUG & & m8t ) { cerr < < humanTimeNow ( ) < < " Detected timing module " < < endl ; }
2019-12-27 12:37:45 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending serial number query " < < endl ; }
msg = buildUbxMessage ( 0x27 , 0x03 , { } ) ;
um1 = sendAndWaitForUBX ( fd , 1 , msg , 0x27 , 0x03 ) ; // ask for serial
serialno = fmt : : sprintf ( " %02x%02x%02x%02x%02x " ,
um1 . getPayload ( ) [ 4 ] ,
um1 . getPayload ( ) [ 5 ] ,
um1 . getPayload ( ) [ 6 ] ,
um1 . getPayload ( ) [ 7 ] ,
um1 . getPayload ( ) [ 8 ] ) ;
cerr < < humanTimeNow ( ) < < " Serial number " < < serialno < < endl ;
2019-09-16 06:33:06 -06:00
if ( version9 )
2019-10-15 06:51:39 -06:00
cerr < < humanTimeNow ( ) < < " Detected version U-Blox 9 " < < endl ;
2019-11-03 12:50:50 -07:00
usleep ( 50000 ) ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending GNSS query " < < endl ; }
2019-08-26 07:58:01 -06:00
msg = buildUbxMessage ( 0x06 , 0x3e , { } ) ;
2019-11-03 12:50:50 -07:00
um1 = sendAndWaitForUBX ( fd , 1 , msg , 0x06 , 0x3e ) ; // query GNSS
2019-08-26 07:58:01 -06:00
auto payload = um1 . getPayload ( ) ;
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) {
cerr < < humanTimeNow ( ) < < " GNSS status, got " < < ( int ) payload [ 3 ] < < " rows: " < < endl ;
for ( uint8_t n = 0 ; n < payload [ 3 ] ; + + n ) {
cerr < < humanTimeNow ( ) < < " GNSSID " < < ( int ) payload [ 4 + 8 * n ] < < " enabled " < < ( int ) payload [ 8 + 8 * n ] < < " minTrk " < < ( int ) payload [ 5 + 8 * n ] < < " maxTrk " < < ( int ) payload [ 6 + 8 * n ] < < " " < < ( int ) payload [ 8 + 8 * n ] < < " " < < ( int ) payload [ 9 + 8 * n ] < < " " < < " " < < ( int ) payload [ 10 + 8 * n ] < < " " < < ( int ) payload [ 11 + 8 * n ] < < endl ;
}
2019-08-26 07:58:01 -06:00
}
2019-11-03 12:50:50 -07:00
try {
2019-08-26 07:58:01 -06:00
if ( waitForUBXAckNack ( fd , 2 , 0x06 , 0x3e ) ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got ACK for our poll of GNSS settings " < < endl ; }
2019-08-26 07:58:01 -06:00
}
2019-11-03 12:50:50 -07:00
} catch ( . . . ) {
cerr < < " Got timeout waiting for ack of poll, no problem " < < endl ;
}
2019-09-16 06:33:06 -06:00
if ( ! version9 ) {
// ver RO maxch cfgs
msg = buildUbxMessage ( 0x06 , 0x3e , { 0x00 , 0x00 , 0xff , 0x06 ,
// GPS min max res x1 x2 x3, x4
0x00 , 0x04 , 0x08 , 0 , doGPS , 0x00 , 0x01 , 0x00 ,
// SBAS min max rex x1 x2 x3 x4
0x01 , 0x03 , 0x04 , 0 , doSBAS , 0x00 , 0x01 , 0x00 ,
// BEI min max res x1 x2 x3, x4
0x03 , 0x04 , 0x08 , 0 , doBeidou , 0x00 , 0x01 , 0x00 ,
// ??? min max res x1 x2 x3, x4
0x05 , 0x04 , 0x08 , 0 , 0 , 0x00 , 0x01 , 0x00 ,
2019-08-26 07:58:01 -06:00
2019-09-16 06:33:06 -06:00
// GAL min max res x1 x2 x3, x4
2019-09-23 12:17:15 -06:00
0x02 , 0x08 , 0x0A , 0 , doGalileo , 0x00 , 0x01 , 0x00 ,
2019-09-16 06:33:06 -06:00
// GLO min max res x1 x2 x3, x4
0x06 , 0x06 , 0x08 , 0 , doGlonass , 0x00 , 0x01 , 0x00
2019-08-12 07:54:16 -06:00
2019-09-16 06:33:06 -06:00
} ) ;
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending GNSS setting, GPS: " < < doGPS < < " , Galileo: " < < doGalileo < < " , BeiDou: " < < doBeidou < < " , GLONASS: " < < doGlonass < < " , SBAS: " < < doSBAS < < endl ; }
2019-08-26 07:58:01 -06:00
2019-11-03 12:50:50 -07:00
if ( sendAndWaitForUBXAckNack ( fd , 2 , msg , 0x06 , 0x3e ) ) { // GNSS setting
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got ack on GNSS setting " < < endl ; }
}
2019-09-16 06:33:06 -06:00
else {
2019-11-17 15:12:52 -07:00
cerr < < humanTimeNow ( ) < < " Got nack on GNSS setting " < < endl ;
exit ( - 1 ) ;
}
}
2019-12-12 03:23:46 -07:00
else { // UBX-CFG-VALSET
2019-11-17 15:12:52 -07:00
msg = buildUbxMessage ( 0x06 , 0x8a , { 0x00 , 0x01 , 0x00 , 0x00 ,
0x1f , 0x00 , 0x31 , 0x10 , doGPS ,
0x01 , 0x00 , 0x31 , 0x10 , doGPS ,
0x03 , 0x00 , 0x31 , 0x10 , doGPS ,
0x21 , 0x00 , 0x31 , 0x10 , doGalileo ,
0x07 , 0x00 , 0x31 , 0x10 , doGalileo ,
0x0a , 0x00 , 0x31 , 0x10 , doGalileo ,
0x22 , 0x00 , 0x31 , 0x10 , doBeidou ,
0x0d , 0x00 , 0x31 , 0x10 , doBeidou ,
0x0e , 0x00 , 0x31 , 0x10 , doBeidou ,
0x25 , 0x00 , 0x31 , 0x10 , doGlonass ,
0x18 , 0x00 , 0x31 , 0x10 , doGlonass ,
0x1a , 0x00 , 0x31 , 0x10 , doGlonass
} ) ;
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Sending F9P GNSS setting, GPS: " < < doGPS < < " , Galileo: " < < doGalileo < < " , BeiDou: " < < doBeidou < < " , GLONASS: " < < doGlonass < < " , SBAS: " < < doSBAS < < endl ; }
if ( sendAndWaitForUBXAckNack ( fd , 2 , msg , 0x06 , 0x8a ) ) { // GNSS setting, F9 stylee
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got ack on F9P GNSS setting " < < endl ; }
}
else {
cerr < < humanTimeNow ( ) < < " Got nack on F9P GNSS setting " < < endl ;
2019-09-16 06:33:06 -06:00
exit ( - 1 ) ;
}
2019-08-26 07:58:01 -06:00
}
2019-09-16 06:33:06 -06:00
2019-09-09 10:09:07 -06:00
if ( doSBAS ) {
// "on" "*.*" ign
2019-11-03 12:50:50 -07:00
msg = buildUbxMessage ( 0x06 , 0x16 , { 0x01 , 0x07 , 0x03 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ; // enable SBAS
if ( sendAndWaitForUBXAckNack ( fd , 10 , msg , 0x06 , 0x16 ) ) { // enable SBAS
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got ack on SBAS setting " < < endl ; }
}
2019-09-09 10:09:07 -06:00
else {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got nack on SBAS setting " < < endl ; }
2019-09-09 10:09:07 -06:00
exit ( - 1 ) ;
}
}
2019-11-03 12:50:50 -07:00
if ( ! doKeepNMEA ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Disabling NMEA " < < endl ; }
2019-11-12 08:30:35 -07:00
msg = buildUbxMessage ( 0x06 , 0x00 , { ( unsigned char ) ( ubxport ) , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x03 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ;
2019-11-03 12:50:50 -07:00
if ( sendAndWaitForUBXAckNack ( fd , 10 , msg , 0x06 , 0x00 ) ) { // disable NMEA
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " NMEA disabled " < < endl ; }
}
else {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got NACK disabling NMEA " < < endl ; }
}
2019-10-15 06:51:39 -06:00
}
2019-11-03 12:50:50 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Polling port settings " < < endl ; } // UBX-CFG-PRT, 0x03 == USB
2019-11-12 08:30:35 -07:00
msg = buildUbxMessage ( 0x06 , 0x00 , { ( unsigned char ) ( ubxport ) } ) ;
2019-08-26 07:58:01 -06:00
2019-11-03 12:50:50 -07:00
UBXMessage um = sendAndWaitForUBX ( fd , 4 , msg , 0x06 , 0x00 ) ; // UBX-CFG-PRT
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) {
2019-11-03 12:50:50 -07:00
cerr < < humanTimeNow ( ) < < " Protocol settings on port: " < < endl ;
2019-10-15 06:51:39 -06:00
for ( const auto & c : um . getPayload ( ) )
cerr < < ( int ) c < < " " ;
cerr < < endl ;
}
2019-11-03 12:50:50 -07:00
try {
if ( waitForUBXAckNack ( fd , 2 , 0x06 , 0x00 ) ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got ACK for our poll of port protocol settings " < < endl ; }
}
} catch ( . . . ) {
cerr < < " Got timeout waiting for ack of port protocol poll, no problem " < < endl ;
2019-10-15 06:51:39 -06:00
}
2019-11-03 12:50:50 -07:00
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-RXM-RLM " < < endl ; } // SAR
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x02 , 0x59 , ubxport ) ; // UBX-RXM-RLM
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-RXM-RAWX " < < endl ; } // RF doppler
2019-12-20 05:12:53 -07:00
enableUBXMessageOnPort ( fd , 0x02 , 0x15 , ubxport , 8 ) ; // RXM-RAWX
2019-12-27 12:37:45 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-NAV-CLOCK " < < endl ; } // clock details
enableUBXMessageOnPort ( fd , 0x01 , 0x22 , ubxport , 16 ) ; // UBX-NAV-CLOCK
2019-12-20 05:12:53 -07:00
if ( ! version9 & & ! m8t ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling debugging data " < < endl ; } // RF doppler
enableUBXMessageOnPort ( fd , 0x03 , 0x10 , ubxport , 1 ) ;
}
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-RXM-SFRBX " < < endl ; } // raw navigation frames
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x02 , 0x13 , ubxport ) ; // SFRBX
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-NAV-POSECEF " < < endl ; } // position
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x01 , 0x01 , ubxport , 8 ) ; // POSECEF
2019-09-16 06:33:06 -06:00
if ( version9 ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-NAV-SIG " < < endl ; } // satellite reception details
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x01 , 0x43 , ubxport , 8 ) ; // NAV-SIG
2019-12-28 05:48:34 -07:00
/*
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-RXM-MEASX " < < endl ; } // satellite reception details
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x02 , 0x14 , ubxport , 1 ) ; // RXM-MEASX
2019-12-28 05:48:34 -07:00
*/
2019-09-16 06:33:06 -06:00
}
else {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-NAV-SAT " < < endl ; } // satellite reception details
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x01 , 0x35 , ubxport , 8 ) ; // NAV-SAT
2019-09-16 06:33:06 -06:00
}
2019-08-26 07:58:01 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Enabling UBX-NAV-PVT " < < endl ; } // position, velocity, time fix
2019-11-03 12:50:50 -07:00
enableUBXMessageOnPort ( fd , 0x01 , 0x07 , ubxport , 1 ) ; // NAV-PVT we use this to get timing
2019-08-26 07:58:01 -06:00
}
2019-08-09 07:58:52 -06:00
}
2019-08-07 07:39:45 -06:00
2019-08-06 04:57:06 -06:00
/* goal: isolate UBX messages, ignore everyting else.
The challenge is that we might sometimes hit the 0xb5 0x62 marker
2019-08-09 07:58:52 -06:00
in the middle of a message , which will cause us to possibly jump over valid messages
2019-08-06 04:57:06 -06:00
2019-08-09 07:58:52 -06:00
This program sits on the serial link and therefore has the best timestamps .
At least Galileo messages sometimes rely on the timestamp - of - receipt , but the promise
is that such messages are sent at whole second intervals .
We therefore perform a layering violation and peer into the message to see
what timestamp it claims to have , so that we can set subsequent timestamps correctly .
*/
2019-08-07 07:39:45 -06:00
2019-08-11 02:43:29 -06:00
int curCycleTOW { - 1 } ; // means invalid
2019-09-23 06:23:03 -06:00
signal ( SIGPIPE , SIG_IGN ) ;
NMMSender ns ;
for ( const auto & s : destinations )
ns . addDestination ( ComboAddress ( s , 29603 ) ) ;
if ( doSTDOUT )
ns . addDestination ( 1 ) ;
ns . launch ( ) ;
2019-10-15 06:51:39 -06:00
cerr < < humanTimeNow ( ) < < " Entering main loop " < < endl ;
2019-08-06 04:57:06 -06:00
for ( ; ; ) {
2019-08-09 07:58:52 -06:00
try {
2019-11-22 06:32:25 -07:00
auto [ msg , timestamp ] = getUBXMessage ( fd , nullptr ) ;
2019-08-09 07:58:52 -06:00
( void ) timestamp ;
auto payload = msg . getPayload ( ) ;
2019-09-23 06:23:03 -06:00
2019-08-09 07:58:52 -06:00
if ( msg . getClass ( ) = = 0x01 & & msg . getType ( ) = = 0x07 ) { // UBX-NAV-PVT
struct PVT
{
uint32_t itow ;
uint16_t year ;
uint8_t month ; // jan = 1
uint8_t day ;
uint8_t hour ; // 24
uint8_t min ;
uint8_t sec ;
uint8_t valid ;
uint32_t tAcc ;
int32_t nano ;
uint8_t fixtype ;
2019-11-03 12:50:50 -07:00
uint8_t flags ;
uint8_t flags2 ;
uint8_t numsv ;
2019-11-26 11:12:37 -07:00
// 1e-7 mm mm
int32_t lon , lat , height , hMSL ;
// mm mm
uint32_t hAcc , vAcc ;
// mm/s mm/s mm/s mm/s
int32_t velN , velE , velD , gSpeed ; // millimeters
2019-08-09 07:58:52 -06:00
} __attribute__ ( ( packed ) ) ;
PVT pvt ;
memcpy ( & pvt , & payload [ 0 ] , sizeof ( pvt ) ) ;
2019-11-26 11:12:37 -07:00
// cerr << "Ground speed: "<<pvt.gSpeed<<", "<<pvt.velN<<" "<<pvt.velE<<" "<<pvt.velD << endl;
2019-11-03 12:50:50 -07:00
g_fixtype = pvt . fixtype ;
2019-11-26 11:12:37 -07:00
g_speed = pvt . gSpeed / 1000.0 ;
2019-11-03 12:50:50 -07:00
2019-08-09 07:58:52 -06:00
struct tm tm ;
memset ( & tm , 0 , sizeof ( tm ) ) ;
tm . tm_year = pvt . year - 1900 ;
tm . tm_mon = pvt . month - 1 ;
tm . tm_mday = pvt . day ;
tm . tm_hour = pvt . hour ;
tm . tm_min = pvt . min ;
tm . tm_sec = pvt . sec ;
uint32_t satt = timegm ( & tm ) ;
2019-08-26 07:58:01 -06:00
// double satutc = timegm(&tm) + pvt.nano/1000000000.0; // negative is no problem here
2019-08-09 07:58:52 -06:00
if ( pvt . nano < 0 ) {
pvt . sec - - ;
satt - - ;
pvt . nano + = 1000000000 ;
}
2019-10-15 06:51:39 -06:00
if ( ! g_gnssutc . tv_sec ) {
2019-10-15 10:12:18 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Got initial timestamp: " < < humanTime ( satt ) < < endl ; }
}
2019-09-23 06:23:03 -06:00
g_gnssutc . tv_sec = satt ;
g_gnssutc . tv_nsec = pvt . nano ;
continue ;
}
2019-10-15 10:12:18 -06:00
2019-11-26 11:12:37 -07:00
if ( ! doFakeFix ) {
if ( ! g_gnssutc . tv_sec ) {
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Ignoring message with class " < < ( int ) msg . getClass ( ) < < " and type " < < ( int ) msg . getType ( ) < < " : have not yet received a timestamp " < < endl ; }
continue ;
}
}
else {
auto oldtime = g_gnssutc ;
clock_gettime ( CLOCK_REALTIME , & g_gnssutc ) ;
if ( oldtime . tv_sec ! = g_gnssutc . tv_sec ) {
NavMonMessage nmm ;
nmm . set_type ( NavMonMessage : : ObserverPositionType ) ;
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
nmm . set_sourceid ( g_srcid ) ;
2019-12-20 05:12:53 -07:00
// 3924698.1158 301124.8036 5001904.9952
nmm . mutable_op ( ) - > set_x ( 3924698.1158 ) ;
nmm . mutable_op ( ) - > set_y ( 301124.8036 ) ;
nmm . mutable_op ( ) - > set_z ( 5001904.9952 ) ;
2019-11-26 11:12:37 -07:00
nmm . mutable_op ( ) - > set_acc ( 3.14 ) ;
ns . emitNMM ( nmm ) ;
}
2019-08-07 07:39:45 -06:00
}
2019-09-23 06:23:03 -06:00
if ( msg . getClass ( ) = = 0x02 & & msg . getType ( ) = = 0x15 ) { // RAWX, the doppler stuff
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" Got "<<(int)payload[11] <<" measurements "<<endl; }
2019-08-09 08:50:26 -06:00
double rcvTow ;
2019-08-10 08:03:45 -06:00
memcpy ( & rcvTow , & payload [ 0 ] , 8 ) ;
uint16_t rcvWn = payload [ 8 ] + 256 * payload [ 9 ] ;
2019-12-12 03:23:46 -07:00
bool clkReset = payload [ 12 ] & 0x02 ;
2019-08-09 08:50:26 -06:00
for ( int n = 0 ; n < payload [ 11 ] ; + + n ) {
double prMes ;
double cpMes ;
float doppler ;
memcpy ( & prMes , & payload [ 16 + 32 * n ] , 8 ) ;
memcpy ( & cpMes , & payload [ 24 + 32 * n ] , 8 ) ;
memcpy ( & doppler , & payload [ 32 + 32 * n ] , 4 ) ;
int gnssid = payload [ 36 + 32 * n ] ;
int sv = payload [ 37 + 32 * n ] ;
2019-09-16 06:33:06 -06:00
int sigid = 0 ;
if ( version9 ) {
sigid = payload [ 38 + 32 * n ] ;
if ( gnssid = = 2 & & sigid = = 6 ) // they separate out I and Q, but the rest of UBX doesn't
sigid = 5 ; // so map it back
if ( gnssid = = 2 & & sigid = = 0 ) // they separate out I and Q, but the rest of UBX doesn't
sigid = 1 ; // so map it back
}
else if ( gnssid = = 2 ) { // version 8 defaults galileo to E1B
sigid = 1 ;
}
2019-08-09 08:50:26 -06:00
uint16_t locktimems ;
memcpy ( & locktimems , & payload [ 40 + 32 * n ] , 2 ) ;
2019-11-26 11:12:37 -07:00
uint8_t cno = payload [ 42 + 32 * n ] ;
uint8_t prStddev = payload [ 43 + 32 * n ] & 0xf ;
uint8_t cpStddev = payload [ 44 + 32 * n ] & 0xf ;
uint8_t doStddev = payload [ 45 + 32 * n ] & 0xf ;
2019-12-12 03:23:46 -07:00
uint8_t trkStat = payload [ 46 + 32 * n ] & 0xf ;
2019-08-09 08:50:26 -06:00
NavMonMessage nmm ;
nmm . set_type ( NavMonMessage : : RFDataType ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-12 07:54:16 -06:00
nmm . set_sourceid ( g_srcid ) ;
2019-08-09 08:50:26 -06:00
nmm . mutable_rfd ( ) - > set_gnssid ( gnssid ) ;
nmm . mutable_rfd ( ) - > set_gnsssv ( sv ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_rfd ( ) - > set_sigid ( sigid ) ;
2019-08-09 08:50:26 -06:00
nmm . mutable_rfd ( ) - > set_rcvtow ( rcvTow ) ;
2019-08-10 08:03:45 -06:00
nmm . mutable_rfd ( ) - > set_rcvwn ( rcvWn ) ;
2019-08-09 08:50:26 -06:00
nmm . mutable_rfd ( ) - > set_doppler ( doppler ) ;
nmm . mutable_rfd ( ) - > set_carrierphase ( cpMes ) ;
nmm . mutable_rfd ( ) - > set_pseudorange ( prMes ) ;
nmm . mutable_rfd ( ) - > set_prstd ( ldexp ( 0.01 , prStddev ) ) ;
nmm . mutable_rfd ( ) - > set_dostd ( ldexp ( 0.002 , doStddev ) ) ;
nmm . mutable_rfd ( ) - > set_cpstd ( cpStddev * 0.4 ) ;
nmm . mutable_rfd ( ) - > set_locktimems ( locktimems ) ;
2019-11-26 11:12:37 -07:00
nmm . mutable_rfd ( ) - > set_cno ( cno ) ;
2019-12-12 03:23:46 -07:00
nmm . mutable_rfd ( ) - > set_prvalid ( trkStat & 1 ) ;
nmm . mutable_rfd ( ) - > set_cpvalid ( trkStat & 2 ) ;
nmm . mutable_rfd ( ) - > set_halfcycvalid ( trkStat & 4 ) ;
nmm . mutable_rfd ( ) - > set_subhalfcyc ( trkStat & 8 ) ;
nmm . mutable_rfd ( ) - > set_clkreset ( clkReset ) ;
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-09 08:50:26 -06:00
}
}
2019-08-09 07:58:52 -06:00
else if ( msg . getClass ( ) = = 0x01 & & msg . getType ( ) = = 0x01 ) { // POSECF
struct pos
{
uint32_t iTOW ;
int32_t ecefX ;
int32_t ecefY ;
int32_t ecefZ ;
uint32_t pAcc ;
} ;
pos p ;
memcpy ( & p , payload . c_str ( ) , sizeof ( pos ) ) ;
2019-12-04 13:49:06 -07:00
if ( fuzzPositionMeters ) {
p . ecefX - = ( p . ecefX % ( fuzzPositionMeters * 100 ) ) ;
p . ecefY - = ( p . ecefY % ( fuzzPositionMeters * 100 ) ) ;
p . ecefZ - = ( p . ecefZ % ( fuzzPositionMeters * 100 ) ) ;
}
2019-10-15 06:51:39 -06:00
/*
if ( doDEBUG ) {
cerr < < humanTimeNow ( ) < < " Position: ( " < < p . ecefX / 100000.0 < < " , "
2019-08-09 07:58:52 -06:00
< < p . ecefY / 100000.0 < < " , "
2019-10-15 06:51:39 -06:00
< < p . ecefZ / 100000.0 < < " ) +- " < < p . pAcc < < " cm " < < endl ;
}
*/
2019-08-09 07:58:52 -06:00
2019-08-15 13:49:53 -06:00
// g_ourpos = {p.ecefX/100.0, p.ecefY/100.0, p.ecefZ/100.0};
2019-08-12 07:54:16 -06:00
2019-08-09 07:58:52 -06:00
NavMonMessage nmm ;
nmm . set_type ( NavMonMessage : : ObserverPositionType ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-12-04 13:49:06 -07:00
nmm . set_sourceid ( g_srcid ) ;
2019-08-09 07:58:52 -06:00
nmm . mutable_op ( ) - > set_x ( p . ecefX / 100.0 ) ;
nmm . mutable_op ( ) - > set_y ( p . ecefY / 100.0 ) ;
nmm . mutable_op ( ) - > set_z ( p . ecefZ / 100.0 ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_op ( ) - > set_acc ( p . pAcc / 100.0 ) ;
2019-11-26 11:12:37 -07:00
if ( g_speed > = 0.0 )
nmm . mutable_op ( ) - > set_groundspeed ( g_speed ) ;
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-09 07:58:52 -06:00
}
else if ( msg . getClass ( ) = = 2 & & msg . getType ( ) = = 0x13 ) { // SFRBX
2019-08-10 09:00:06 -06:00
// order: 2, 4, 6, 7/9, 8/10, 0, 0, 0, 0, 0, 1, 3, 5, 0, 0
// * * * * * * *
// tow
2019-08-09 13:42:28 -06:00
try {
pair < int , int > id = make_pair ( payload [ 0 ] , payload [ 1 ] ) ;
2019-09-16 06:33:06 -06:00
int sigid = payload [ 2 ] ;
static set < tuple < int , int , int > > svseen ;
2019-08-29 15:17:26 -06:00
static time_t lastStat ;
2019-09-16 06:33:06 -06:00
svseen . insert ( { id . first , id . second , payload [ 2 ] } ) ;
2019-08-29 15:17:26 -06:00
if ( time ( 0 ) - lastStat > 30 ) {
2019-11-03 12:50:50 -07:00
cerr < < humanTimeNow ( ) < < " src " < < g_srcid < < " (fix: " < < g_fixtype < < " ) currently receiving: " ;
2019-08-29 15:17:26 -06:00
for ( auto & s : svseen ) {
2019-09-16 06:33:06 -06:00
cerr < < get < 0 > ( s ) < < " , " < < get < 1 > ( s ) < < " @ " < < get < 2 > ( s ) < < " " ;
2019-08-29 15:17:26 -06:00
}
cerr < < endl ;
lastStat = time ( 0 ) ;
svseen . clear ( ) ;
}
2019-09-16 06:33:06 -06:00
if ( id . first = = 0 & & ! sigid ) { // can only parse the old stuff
2019-08-12 07:54:16 -06:00
NavMonMessage nmm ;
nmm . set_type ( NavMonMessage : : GPSInavType ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-12 07:54:16 -06:00
nmm . set_sourceid ( g_srcid ) ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" GPS frame, numwords: "<<(int)payload[4]<<", version: "<<(int)payload[6]<<endl; }
2019-08-18 02:48:15 -06:00
static int wn , tow ;
2019-08-26 07:58:01 -06:00
auto gpsframe = getGPSFromSFRBXMsg ( payload ) ;
2019-08-12 07:54:16 -06:00
auto cond = getCondensedGPSMessage ( gpsframe ) ;
2019-08-18 02:48:15 -06:00
auto frameno = getbitu ( & cond [ 0 ] , 24 + 19 , 3 ) ;
2019-09-16 06:33:06 -06:00
if ( frameno = = 1 & & sigid = = 0 )
2019-08-18 02:48:15 -06:00
wn = 2048 + getbitu ( & cond [ 0 ] , 2 * 24 , 10 ) ;
if ( ! wn )
continue ; // can't file this yet
2019-08-12 07:54:16 -06:00
tow = 1.5 * ( getbitu ( & cond [ 0 ] , 24 , 17 ) * 4 ) ;
nmm . mutable_gpsi ( ) - > set_gnsswn ( wn ) ; // XXX this sucks
2019-09-16 06:33:06 -06:00
nmm . mutable_gpsi ( ) - > set_sigid ( sigid ) ;
2019-08-12 07:54:16 -06:00
nmm . mutable_gpsi ( ) - > set_gnsstow ( tow ) ; // "with 6 second increments" -- needs to be adjusted
nmm . mutable_gpsi ( ) - > set_gnssid ( id . first ) ;
nmm . mutable_gpsi ( ) - > set_gnsssv ( id . second ) ;
nmm . mutable_gpsi ( ) - > set_contents ( string ( ( char * ) gpsframe . c_str ( ) , gpsframe . size ( ) ) ) ;
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-12 07:54:16 -06:00
continue ;
}
2019-09-28 13:30:12 -06:00
else if ( id . first = = 2 ) { // GALILEO
2019-08-26 07:58:01 -06:00
auto inav = getInavFromSFRBXMsg ( payload ) ;
unsigned int wtype = getbitu ( & inav [ 0 ] , 0 , 6 ) ;
2019-09-23 06:23:03 -06:00
2019-08-26 07:58:01 -06:00
uint32_t satTOW ;
int msgTOW { 0 } ;
2019-09-23 06:23:03 -06:00
if ( getTOWFromInav ( inav , & satTOW , & g_galwn ) ) { // 0, 6, 5
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" "<<wtype<<" sv "<<id.second<<" tow "<<satTOW << " % 30 = "<< satTOW % 30<<", implied start of cycle: "<<(satTOW - (satTOW %30)) <<endl; }
2019-08-26 07:58:01 -06:00
msgTOW = satTOW ;
curCycleTOW = satTOW - ( satTOW % 30 ) ;
2019-08-09 07:58:52 -06:00
}
2019-08-26 07:58:01 -06:00
else {
if ( curCycleTOW < 0 ) // did not yet have a start of cycle
continue ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" "<<wtype<<" sv "<<id.second<<" tow "; }
2019-09-28 13:30:12 -06:00
if ( sigid = = 5 ) {
if ( wtype = = 2 ) {
msgTOW = curCycleTOW + 20 ;
}
else if ( wtype = = 4 ) {
msgTOW = curCycleTOW + 22 ;
} // next have '6' which sets TOW
else if ( wtype = = 7 | | wtype = = 9 ) {
msgTOW = curCycleTOW + 6 ;
}
else if ( wtype = = 8 | | wtype = = 10 ) {
msgTOW = curCycleTOW + 8 ;
}
else if ( wtype = = 1 ) {
msgTOW = curCycleTOW + 30 ;
}
else if ( wtype = = 3 ) {
msgTOW = curCycleTOW + 32 ;
}
else { // dummy
if ( id . second ! = 20 ) // known broken XXX
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " galileo E " < < id . second < < " what kind of wtype is this: " < < wtype < < endl ; }
2019-09-28 13:30:12 -06:00
continue ;
}
2019-08-26 07:58:01 -06:00
}
2019-09-28 13:30:12 -06:00
else {
if ( wtype = = 2 ) {
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" infered to be 1 "<<curCycleTOW + 31<<endl; }
2019-09-28 13:30:12 -06:00
msgTOW = curCycleTOW + 31 ;
}
else if ( wtype = = 4 ) {
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" infered to be 3 "<<curCycleTOW + 33<<endl; }
2019-09-28 13:30:12 -06:00
msgTOW = curCycleTOW + 33 ;
} // next have '6' which sets TOW
else if ( wtype = = 7 | | wtype = = 9 ) {
msgTOW = curCycleTOW + 7 ;
}
else if ( wtype = = 8 | | wtype = = 10 ) {
msgTOW = curCycleTOW + 9 ;
}
else if ( wtype = = 1 ) {
msgTOW = curCycleTOW + 21 ;
}
else if ( wtype = = 3 ) {
msgTOW = curCycleTOW + 23 ;
}
2019-10-01 07:03:30 -06:00
else { // dummy
if ( id . second ! = 20 ) // known broken XXX
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " galileo E " < < id . second < < " what kind of wtype is this: " < < wtype < < endl ; }
2019-10-01 07:03:30 -06:00
continue ;
}
2019-08-26 07:58:01 -06:00
}
2019-08-09 07:58:52 -06:00
}
2019-08-26 07:58:01 -06:00
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
nmm . set_type ( NavMonMessage : : GalileoInavType ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-26 07:58:01 -06:00
2019-09-23 06:23:03 -06:00
nmm . mutable_gi ( ) - > set_gnsswn ( g_galwn ) ;
2019-09-16 06:33:06 -06:00
2019-08-26 07:58:01 -06:00
nmm . mutable_gi ( ) - > set_gnsstow ( msgTOW ) ;
nmm . mutable_gi ( ) - > set_gnssid ( id . first ) ;
nmm . mutable_gi ( ) - > set_gnsssv ( id . second ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_gi ( ) - > set_sigid ( sigid ) ;
2019-08-26 07:58:01 -06:00
nmm . mutable_gi ( ) - > set_contents ( ( const char * ) & inav [ 0 ] , inav . size ( ) ) ;
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-26 07:58:01 -06:00
}
else if ( id . first = = 3 ) {
auto gstr = getGlonassFromSFRBXMsg ( payload ) ;
auto cond = getCondensedBeidouMessage ( gstr ) ;
static map < int , BeidouMessage > bms ;
auto & bm = bms [ id . second ] ;
uint8_t pageno ;
2019-08-29 15:17:26 -06:00
bm . parse ( cond , & pageno ) ;
2019-08-26 07:58:01 -06:00
if ( bm . wn < 0 ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " BeiDou C " < < id . second < < " WN not yet known, not yet emitting message " < < endl ; }
2019-08-09 13:42:28 -06:00
continue ;
2019-08-09 07:58:52 -06:00
}
2019-08-26 07:58:01 -06:00
NavMonMessage nmm ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-29 08:46:03 -06:00
nmm . set_sourceid ( g_srcid ) ;
if ( id . second > 5 ) {
// this **HARDCODES** that C01,02,03,04,05 emit D2 messages!
nmm . set_type ( NavMonMessage : : BeidouInavTypeD1 ) ;
2019-09-23 06:23:03 -06:00
nmm . mutable_bid1 ( ) - > set_gnsswn ( bm . wn ) ; // only sent in word 1!!
2019-08-29 08:46:03 -06:00
nmm . mutable_bid1 ( ) - > set_gnsstow ( bm . sow ) ;
nmm . mutable_bid1 ( ) - > set_gnssid ( id . first ) ;
nmm . mutable_bid1 ( ) - > set_gnsssv ( id . second ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_bid1 ( ) - > set_sigid ( sigid ) ;
2019-08-29 08:46:03 -06:00
nmm . mutable_bid1 ( ) - > set_contents ( string ( ( char * ) gstr . c_str ( ) , gstr . size ( ) ) ) ;
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-29 08:46:03 -06:00
}
else {
2019-09-23 06:23:03 -06:00
// not sending this: we can't even get the week number right!
/*
2019-08-29 08:46:03 -06:00
nmm . set_type ( NavMonMessage : : BeidouInavTypeD2 ) ;
nmm . mutable_bid2 ( ) - > set_gnsswn ( bm . wn ) ;
nmm . mutable_bid2 ( ) - > set_gnsstow ( bm . sow ) ;
nmm . mutable_bid2 ( ) - > set_gnssid ( id . first ) ;
nmm . mutable_bid2 ( ) - > set_gnsssv ( id . second ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_bid2 ( ) - > set_sigid ( sigid ) ;
2019-08-29 08:46:03 -06:00
nmm . mutable_bid2 ( ) - > set_contents ( string ( ( char * ) gstr . c_str ( ) , gstr . size ( ) ) ) ;
2019-09-23 06:23:03 -06:00
*/
2019-08-29 08:46:03 -06:00
}
2019-09-23 06:23:03 -06:00
2019-08-26 07:58:01 -06:00
continue ;
2019-08-09 07:58:52 -06:00
}
2019-08-26 07:58:01 -06:00
else if ( id . first = = 6 ) {
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" SFRBX from GLONASS "<<id.second<<" @ frequency "<<(int)payload[3]<<", msg of "<<(int)payload[4]<< " words"<<endl; }
2019-08-26 07:58:01 -06:00
auto gstr = getGlonassFromSFRBXMsg ( payload ) ;
2019-09-02 08:13:49 -06:00
/*
2019-08-26 07:58:01 -06:00
static map < int , GlonassMessage > gms ;
GlonassMessage & gm = gms [ id . second ] ;
int strno = gm . parse ( gstr ) ;
2019-09-02 08:13:49 -06:00
*/
2019-08-30 09:56:41 -06:00
if ( id . second ! = 255 ) {
NavMonMessage nmm ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-30 09:56:41 -06:00
nmm . set_sourceid ( g_srcid ) ;
nmm . set_type ( NavMonMessage : : GlonassInavType ) ;
nmm . mutable_gloi ( ) - > set_freq ( payload [ 3 ] ) ;
nmm . mutable_gloi ( ) - > set_gnssid ( id . first ) ;
nmm . mutable_gloi ( ) - > set_gnsssv ( id . second ) ;
2019-09-16 06:33:06 -06:00
nmm . mutable_gloi ( ) - > set_sigid ( sigid ) ;
2019-08-30 09:56:41 -06:00
nmm . mutable_gloi ( ) - > set_contents ( string ( ( char * ) gstr . c_str ( ) , gstr . size ( ) ) ) ;
2019-09-16 06:33:06 -06:00
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-08-30 09:56:41 -06:00
}
2019-08-26 07:58:01 -06:00
}
2019-09-23 06:23:03 -06:00
else if ( id . first = = 1 ) { // SBAS
2019-11-03 12:50:50 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " SBAS " < < id . second < < " frame, numwords: " < < ( int ) payload [ 4 ] < < " , version: " < < ( int ) payload [ 6 ] < < " , totsize " < < payload . size ( ) < < endl ; }
auto sbas = getSBASFromSFRBXMsg ( payload ) ;
cerr < < " Preamble: " < < ( int ) sbas [ 0 ] < < " , type " < < getbitu ( & sbas [ 0 ] , 8 , 6 ) < < endl ;
2019-09-23 06:23:03 -06:00
}
2019-08-26 07:58:01 -06:00
else
2019-10-15 06:51:39 -06:00
; // if (doDEBUG) { cerr<<humanTimeNow()<<" SFRBX from unsupported GNSSID/sigid combination "<<id.first<<", sv "<<id.second<<", sigid "<<sigid<<", "<<payload.size()<<" bytes"<<endl; }
2019-10-15 10:12:18 -06:00
2019-08-09 11:09:47 -06:00
}
catch ( CRCMismatch & cm ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Had CRC mismatch! " < < endl ; }
2019-08-09 11:09:47 -06:00
}
2019-12-04 13:49:06 -07:00
2019-08-09 07:58:52 -06:00
}
else if ( msg . getClass ( ) = = 1 & & msg . getType ( ) = = 0x35 ) { // UBX-NAV-SAT
2019-09-16 06:33:06 -06:00
// if(version9) // we have UBX-NAV-SIG
// continue;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" Info for "<<(int) payload[5]<<" svs: \n"; }
2019-08-09 07:58:52 -06:00
for ( unsigned int n = 0 ; n < payload [ 5 ] ; + + n ) {
2019-08-12 07:54:16 -06:00
int gnssid = payload [ 8 + 12 * n ] ;
int sv = payload [ 9 + 12 * n ] ;
2019-08-09 07:58:52 -06:00
2019-08-12 07:54:16 -06:00
auto el = ( int ) ( char ) payload [ 11 + 12 * n ] ;
auto azi = ( ( int ) payload [ 13 + 12 * n ] * 256 + payload [ 12 + 12 * n ] ) ;
auto db = ( int ) payload [ 10 + 12 * n ] ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<"gnssid "<<gnssid<<" sv "<<sv<<" el "<<el<<endl; }
2019-08-12 07:54:16 -06:00
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-08-12 07:54:16 -06:00
nmm . set_type ( NavMonMessage : : ReceptionDataType ) ;
nmm . mutable_rd ( ) - > set_gnssid ( gnssid ) ;
nmm . mutable_rd ( ) - > set_gnsssv ( sv ) ;
nmm . mutable_rd ( ) - > set_db ( db ) ;
nmm . mutable_rd ( ) - > set_el ( el ) ;
nmm . mutable_rd ( ) - > set_azi ( azi ) ;
nmm . mutable_rd ( ) - > set_prres ( * ( ( int16_t * ) ( payload . c_str ( ) + 14 + 12 * n ) ) * 0.1 ) ;
2019-12-28 05:48:34 -07:00
2019-09-23 06:23:03 -06:00
uint32_t status ;
memcpy ( & status , & payload [ 16 + 12 * n ] , 4 ) ;
2019-12-28 05:48:34 -07:00
nmm . mutable_rd ( ) - > set_qi ( status & 7 ) ;
nmm . mutable_rd ( ) - > set_used ( status & 8 ) ;
/*
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) {
cerr < < humanTimeNow ( ) < < " " < < gnssid < < " , " < < sv < < " : " ;
2019-12-28 05:48:34 -07:00
cerr < < " used " < < ( ( status & 8 ) = = 8 ) ;
2019-10-15 06:51:39 -06:00
cerr < < " qualityind " < < ( status & 7 ) ;
cerr < < " db " < < db < < " el " < < el ;
cerr < < " health " < < ( status & ( 1 < < 5 ) ) ;
cerr < < " sbasCorr " < < ( status & ( 1 < < 16 ) ) ;
cerr < < " prRes " < < nmm . rd ( ) . prres ( ) ;
cerr < < " orbsrc " < < ( ( status > > 8 ) & 7 ) ;
cerr < < " eph-avail " < < ! ! ( status & ( 1 < < 11 ) ) ;
cerr < < " alm-avail " < < ! ! ( status & ( 1 < < 12 ) ) ;
cerr < < endl ;
}
2019-09-23 06:23:03 -06:00
*/
ns . emitNMM ( nmm ) ;
2019-08-09 07:58:52 -06:00
}
2019-08-07 17:22:46 -06:00
}
2019-09-16 06:33:06 -06:00
else if ( msg . getClass ( ) = = 1 & & msg . getType ( ) = = 0x43 ) { // UBX-NAV-SIG
for ( unsigned int n = 0 ; n < payload [ 5 ] ; + + n ) {
int gnssid = payload [ 8 + 16 * n ] ;
int sv = payload [ 9 + 16 * n ] ;
2019-12-28 05:48:34 -07:00
int qi = payload [ 15 + 16 * n ] ;
uint16_t sigflags ;
memcpy ( & sigflags , & payload [ 18 + 16 * n ] , 2 ) ;
2019-09-16 06:33:06 -06:00
int sigid = 0 ;
2019-12-28 05:48:34 -07:00
if ( version9 ) { // we only use this on version9 right now tho
2019-09-16 06:33:06 -06:00
sigid = payload [ 10 + 16 * n ] ;
if ( gnssid = = 2 & & sigid = = 6 ) // they separate out I and Q, but the rest of UBX doesn't
sigid = 5 ; // so map it back
if ( gnssid = = 2 & & sigid = = 0 ) // they separate out I and Q, but the rest of UBX doesn't
sigid = 1 ; // so map it back
}
else if ( gnssid = = 2 ) { // version 8 defaults galileo to E1B
sigid = 1 ;
}
auto db = ( int ) payload [ 14 + 16 * n ] ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<"gnssid "<<gnssid<<" sv "<<sv<<" el "<<el<<endl; }
2019-09-16 06:33:06 -06:00
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-09-16 06:33:06 -06:00
nmm . set_type ( NavMonMessage : : ReceptionDataType ) ;
nmm . mutable_rd ( ) - > set_gnssid ( gnssid ) ;
nmm . mutable_rd ( ) - > set_gnsssv ( sv ) ;
nmm . mutable_rd ( ) - > set_db ( db ) ;
nmm . mutable_rd ( ) - > set_prres ( * ( ( int16_t * ) ( payload . c_str ( ) + 12 + 16 * n ) ) * 0.1 ) ; // ENDIANISM
nmm . mutable_rd ( ) - > set_sigid ( sigid ) ;
nmm . mutable_rd ( ) - > set_el ( 0 ) ;
nmm . mutable_rd ( ) - > set_azi ( 0 ) ;
2019-12-28 05:48:34 -07:00
nmm . mutable_rd ( ) - > set_qi ( qi ) ;
nmm . mutable_rd ( ) - > set_used ( sigflags & 8 ) ;
2019-09-16 06:33:06 -06:00
2019-09-23 06:23:03 -06:00
ns . emitNMM ( nmm ) ;
2019-09-16 06:33:06 -06:00
}
2019-09-22 08:42:23 -06:00
}
else if ( msg . getClass ( ) = = 1 & & msg . getType ( ) = = 0x30 ) { // UBX-NAV-SVINFO
2019-09-16 06:33:06 -06:00
2019-12-27 12:37:45 -07:00
}
else if ( msg . getClass ( ) = = 1 & & msg . getType ( ) = = 0x22 ) { // UBX-NAV-CLOCK
struct NavClock
{
uint32_t iTowMS ;
int32_t clkBNS ;
int32_t clkDNS ;
uint32_t tAcc ;
uint32_t fAcc ;
} nc ;
memcpy ( & nc , & payload [ 0 ] , sizeof ( nc ) ) ;
2019-12-27 12:51:00 -07:00
// cerr<<"Clock offset "<< nc.clkBNS<<" nanoseconds, drift "<< nc.clkDNS<<" nanoseconds/second, accuracy " << nc.tAcc<<" ns, frequency accuracy "<<nc.fAcc << " ps/s"<<endl;
// cerr<<"hwversion "<<hwversion<<" swversion "<< swversion <<" mods "<< mods <<" serialno "<<serialno<<endl;
2019-12-27 12:37:45 -07:00
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
nmm . set_type ( NavMonMessage : : ObserverDetailsType ) ;
nmm . mutable_od ( ) - > set_vendor ( " Ublox " ) ;
nmm . mutable_od ( ) - > set_hwversion ( hwversion ) ;
nmm . mutable_od ( ) - > set_swversion ( swversion ) ;
nmm . mutable_od ( ) - > set_serialno ( serialno ) ;
nmm . mutable_od ( ) - > set_modules ( mods ) ;
nmm . mutable_od ( ) - > set_clockoffsetns ( nc . clkBNS ) ;
nmm . mutable_od ( ) - > set_clockoffsetdriftns ( nc . clkDNS ) ;
nmm . mutable_od ( ) - > set_clockaccuracyns ( nc . tAcc ) ;
nmm . mutable_od ( ) - > set_freqaccuracyps ( nc . fAcc ) ;
nmm . mutable_od ( ) - > set_owner ( owner ) ;
nmm . mutable_od ( ) - > set_remark ( remark ) ;
2019-12-28 15:19:40 -07:00
extern const char * g_gitHash ;
nmm . mutable_od ( ) - > set_recvgithash ( g_gitHash ) ;
nmm . mutable_od ( ) - > set_uptime ( time ( 0 ) - starttime ) ;
2019-12-27 12:37:45 -07:00
ns . emitNMM ( nmm ) ;
2019-09-16 06:33:06 -06:00
}
2019-09-22 08:42:23 -06:00
else if ( msg . getClass ( ) = = 0x02 & & msg . getType ( ) = = 0x14 ) { // UBX-RXM-MEASX
2019-10-15 10:12:18 -06:00
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" Got RXM-MEASX for "<<(int)payload[34]<<" satellites, r0 "<< (int)payload[30]<<" r1 " <<(int)payload[31]<<endl; }
2019-09-22 08:42:23 -06:00
for ( unsigned int n = 0 ; n < payload [ 34 ] ; + + n ) {
uint16_t wholeChips ;
uint16_t fracChips ;
2019-11-03 12:50:50 -07:00
// uint8_t intCodePhase = payload[64+24*n];
2019-09-22 08:42:23 -06:00
uint32_t codePhase ;
memcpy ( & wholeChips , & payload [ 56 + 24 * n ] , 2 ) ;
memcpy ( & fracChips , & payload [ 58 + 24 * n ] , 2 ) ;
memcpy ( & codePhase , & payload [ 60 + 24 * n ] , 4 ) ;
2019-10-15 06:51:39 -06:00
// if (doDEBUG) { cerr<<humanTimeNow()<<" "<<(int)payload[44+24*n]<<","<<(int)payload[45+24*n]<<" whole-chips "<<wholeChips<<" frac-chips "<<fracChips<<" int-code-phase " <<(int)intCodePhase <<" frac-code-phase "<<ldexp(codePhase, -21) << " mpath " << (int)payload[47+24*n] << " r1 " << (int)payload[66+24*n] << " r2 " <<(int)payload[67+24*n]<<endl; }
2019-09-22 08:42:23 -06:00
}
}
else if ( msg . getClass ( ) = = 0x02 & & msg . getType ( ) = = 0x59 ) { // UBX-RXM-RLM
int type = ( int ) payload [ 1 ] ;
int sv = ( int ) payload [ 2 ] ;
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
2019-09-23 06:23:03 -06:00
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
2019-09-22 08:42:23 -06:00
nmm . set_type ( NavMonMessage : : SARResponseType ) ;
nmm . mutable_sr ( ) - > set_gnssid ( 2 ) ; // Galileo only for now
nmm . mutable_sr ( ) - > set_gnsssv ( sv ) ;
nmm . mutable_sr ( ) - > set_sigid ( 0 ) ; // we should fill this in later
nmm . mutable_sr ( ) - > set_type ( payload [ 1 ] ) ;
nmm . mutable_sr ( ) - > set_identifier ( string ( ( char * ) payload . c_str ( ) + 4 , 8 ) ) ;
nmm . mutable_sr ( ) - > set_code ( payload [ 12 ] ) ;
nmm . mutable_sr ( ) - > set_params ( string ( ( char * ) payload . c_str ( ) + 13 , payload . size ( ) - 14 ) ) ;
string hexstring ;
for ( int n = 0 ; n < 15 ; + + n )
hexstring + = fmt : : sprintf ( " %x " , ( int ) getbitu ( payload . c_str ( ) , 36 + 4 * n , 4 ) ) ;
2019-10-15 10:12:18 -06:00
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " " < < humanTime ( g_gnssutc . tv_sec ) < < " SAR RLM type " < < type < < " from gal sv " < < sv < < " beacon " < < hexstring < < " code " < < ( int ) payload [ 12 ] < < " params " < < payload [ 12 ] + 256 * payload [ 13 ] < < endl ; }
2019-09-22 08:42:23 -06:00
// wk.emitLine(sv, "SAR "+hexstring);
// cout<<"SAR: sv = "<< (int)msg[2] <<" ";
// for(int n=4; n < 12; ++n)
// fmt::printf("%02x", (int)msg[n]);
// for(int n = 0; n < 15; ++n)
// fmt::printf("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4));
// cout << " Type: "<< (int) msg[12] <<"\n";
// cout<<"Parameter: (len = "<<msg.length()<<") ";
// for(unsigned int n = 13; n < msg.length(); ++n)
// fmt::printf("%02x ", (int)msg[n]);
// cout<<"\n";
}
2019-12-20 05:12:53 -07:00
else if ( msg . getClass ( ) = = 39 & & msg . getType ( ) = = 0 ) {
NavMonMessage nmm ;
nmm . set_sourceid ( g_srcid ) ;
nmm . set_localutcseconds ( g_gnssutc . tv_sec ) ;
nmm . set_localutcnanoseconds ( g_gnssutc . tv_nsec ) ;
nmm . set_type ( NavMonMessage : : DebuggingType ) ;
nmm . mutable_dm ( ) - > set_type ( 0 ) ;
nmm . mutable_dm ( ) - > set_payload ( string ( ( char * ) & payload [ 0 ] , payload . size ( ) ) ) ;
ns . emitNMM ( nmm ) ;
}
2019-09-22 08:42:23 -06:00
else
2019-12-20 05:12:53 -07:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Unknown UBX message of class " < < ( int ) msg . getClass ( ) < < " and type " < < ( int ) msg . getType ( ) < < " of " < < payload . size ( ) < < " bytes " < < endl ; }
2019-08-07 17:22:46 -06:00
2019-08-09 07:58:52 -06:00
// writen2(1, payload.d_raw.c_str(),msg.d_raw.size());
}
catch ( UBXMessage : : BadChecksum & e ) {
2019-10-15 06:51:39 -06:00
if ( doDEBUG ) { cerr < < humanTimeNow ( ) < < " Bad UBX checksum, skipping message " < < endl ; }
2019-08-09 07:58:52 -06:00
}
2019-12-02 06:41:06 -07:00
catch ( EofException & em ) {
cerr < < " EOF, break " < < endl ;
break ;
}
2019-08-07 07:39:45 -06:00
}
2019-08-12 07:54:16 -06:00
if ( ! g_fromFile )
2019-08-26 07:58:01 -06:00
tcsetattr ( fd , TCSANOW , & g_oldtio ) ;
2019-08-06 04:57:06 -06:00
}