spacecruft
/
galmon
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

move to WGS84 ellipsoid lat/lon calculations, adjust to sources that do not provide a fix, add ground speed, log location/speed in influxdb

pull/51/head
bert hubert 2019-11-26 19:12:37 +01:00
parent c7479a7b8d
commit 5d1ef42e4e
8 changed files with 182 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Makefile
View File

@ -22,17 +22,17 @@ clean:
rm -f ext/fmt-5.2.1/src/format.o
navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o
navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY)
reporter: reporter.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o
reporter: reporter.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY)
navdump: navdump.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o
navdump: navdump.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -pthread -lprotobuf
navdisplay: navdisplay.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o
navdisplay: navdisplay.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o osen.o
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -pthread -lprotobuf -lncurses
@ -52,10 +52,10 @@ tlecatch: tlecatch.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc))
navmon.pb.cc: navmon.proto
protoc --cpp_out=./ navmon.proto
ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o navmon.o ephemeris.o $(SIMPLESOCKETS)
ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o navmon.o ephemeris.o $(SIMPLESOCKETS) osen.o
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -lprotobuf -pthread
testrunner: navmon.pb.o testrunner.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o ephemeris.o sp3.o
testrunner: navmon.pb.o testrunner.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o ephemeris.o sp3.o osen.o
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -lprotobuf
check: testrunner

26
ephemeris.cc
View File

@ -1,5 +1,6 @@
#include "ephemeris.hh"
#include "minivec.hh"
#include <tuple>
/* | t0e tow | - > tow - t0e, <3.5 days, so ok
| t0e tow | -> tow - t0e > 3.5 days, so
@ -22,6 +23,8 @@ int ephAge(int tow, int t0e)
return diff;
}
// all axes start at earth center of gravity
// x-axis is on equator, 0 longitude
// y-axis is on equator, 90 longitude
@ -29,27 +32,8 @@ int ephAge(int tow, int t0e)
// https://en.wikipedia.org/wiki/ECEF#/media/File:Ecef.png
std::pair<double,double> getLongLat(double x, double y, double z)
{
Point core{0,0,0};
Point LatLon0{1,0,0};
Point pos{x, y, z};
Point proj{x, y, 0}; // in equatorial plane now
Vector flat(core, proj);
Vector toLatLon0{core, LatLon0};
double longitude = acos( toLatLon0.inner(flat) / (toLatLon0.length() * flat.length()));
if(y < 0)
longitude *= -1;
Vector toUs{core, pos};
double inp = flat.inner(toUs) / (toUs.length() * flat.length());
if(inp > 1.0 && inp < 1.0000001) // this happens because of rounding errors
inp=1.0;
double latitude = acos( inp);
if(z < 0)
latitude *= -1;
return std::make_pair(longitude, latitude);
auto ret = ecefToWGS84(x, y, z);
return {std::get<1>(ret), std::get<0>(ret)};
}

2
ephemeris.hh
View File

@ -1,6 +1,8 @@
#pragma once
#include "minivec.hh"
#include <iostream>
// lat, lon, height (rad, rad, meters)
std::tuple<double, double, double> ecefToWGS84(double x, double y, double z);
int ephAge(int tow, int t0e);

3
navdump.cc
View File

@ -642,6 +642,9 @@ try
cout<<" dop-std " << nmm.rfd().dostd();
cout<<" cp-std " << nmm.rfd().cpstd();
cout<<" locktime-ms " <<nmm.rfd().locktimems();
if(nmm.rfd().has_cno()) {
cout<<" cno-db " <<nmm.rfd().cno();
}
cout<<endl;
}

2
navmon.proto
View File

@ -90,6 +90,7 @@ message NavMonMessage {
required double cpStd = 10;
required double prStd = 11;
optional uint32 sigid = 12;
optional uint32 cno = 13;
}
message ObserverPosition {
@ -97,6 +98,7 @@ message NavMonMessage {
required double y = 2;
required double z = 3;
required double acc = 4;
optional double groundSpeed = 5;
}
message SARResponse {

61
navparse.cc
View File

@ -35,6 +35,7 @@ using namespace std;
struct ObserverPosition
{
Point pos;
double groundSpeed{-1};
time_t lastSeen{0};
};
std::map<int, ObserverPosition> g_srcpos;
@ -342,6 +343,38 @@ struct InfluxPusher
checkSend();
}
void addValueObserver(int src, string_view name, const initializer_list<pair<const char*, double>>& values, time_t t)
{
if(d_mute)
return;
if(t > 2200000000) {
cerr<<"Unable to store item "<<name<<" for observer "<<src<<": time out of range "<<t<<endl;
return;
}
for(const auto& p : values) {
if(isnan(p.second))
return;
}
d_buffer+= string(name)+",src="+to_string(src);
d_buffer+= " ";
bool lefirst=true;
for(const auto& v : values) {
if(!lefirst) {
d_buffer +=",";
}
lefirst=false;
d_buffer += string(v.first) + "="+to_string(v.second);
}
d_buffer += " " + to_string(t* 1000000000)+"\n";
checkSend();
}
template<typename T>
void addValue(const SatID& id, string_view name, const T& value, std::optional<int> src = std::optional<int>())
@ -526,7 +559,7 @@ void addHeaders(h2o_req_t* req)
int main(int argc, char** argv)
try
{
feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW );
// feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW );
// g_tles.parseFile("active.txt");
@ -850,12 +883,13 @@ try
obj["longitude"] = longlat.first;
obj["latitude"] = longlat.second;
obj["last-seen"] = src.second.lastSeen;
obj["ground-speed"] = src.second.groundSpeed;
auto svstats = g_statskeeper.get();
nlohmann::json svs = nlohmann::json::object();
for(const auto& sv : svstats) {
if(auto iter = sv.second.perrecv.find(src.first); iter != sv.second.perrecv.end()) {
if(iter->second.db > 0 && time(0) - iter->second.t < 120) {
if(iter->second.db > 0 && time(0) - iter->second.t < 120) {
nlohmann::json svo = nlohmann::json::object();
svo["db"] = iter->second.db;
@ -1679,14 +1713,35 @@ try
g_srcpos[nmm.sourceid()].pos.x = nmm.op().x();
g_srcpos[nmm.sourceid()].pos.y = nmm.op().y();
g_srcpos[nmm.sourceid()].pos.z = nmm.op().z();
idb.addValueObserver(nmm.sourceid(), "accfix", nmm.op().acc(), nmm.localutcseconds());
if(nmm.op().has_groundspeed()) {
g_srcpos[nmm.sourceid()].groundSpeed = nmm.op().groundspeed();
}
// idb.addValueObserver(nmm.sourceid(), "accfix", nmm.op().acc(), nmm.localutcseconds());
auto latlonh = ecefToWGS84(nmm.op().x(), nmm.op().y(), nmm.op().z());
idb.addValueObserver(nmm.sourceid(), "fix", {
{"x", nmm.op().x()},
{"y", nmm.op().y()},
{"z", nmm.op().z()},
{"lat", 180.0*std::get<0>(latlonh)/M_PI},
{"lon", 180.0*std::get<1>(latlonh)/M_PI},
{"h", std::get<2>(latlonh)},
{"acc", nmm.op().acc()},
{"groundspeed", nmm.op().has_groundspeed() ? nmm.op().groundspeed() : -1.0}
},
nmm.localutcseconds());
}
else if(nmm.type() == NavMonMessage::RFDataType) {
SatID id{nmm.rfd().gnssid(), nmm.rfd().gnsssv(), nmm.rfd().sigid()};
if(id.gnss==2 && id.sigid == 0) // old ubxtool output gets this wrong
id.sigid = 1;
// some sources ONLY have RFDatatype & not ReceptionDataType
g_svstats[id].perrecv[nmm.sourceid()].db = nmm.rfd().cno();
idb.addValue(id, nanoTime(0, nmm.rfd().rcvwn(), nmm.rfd().rcvtow()), "rfdata",
{{"carrierphase", nmm.rfd().carrierphase()},

60
osen.cc 100644
View File

@ -0,0 +1,60 @@
#include <tuple>
#include <math.h>
/* gratefully adopted from http://danceswithcode.net/engineeringnotes/geodetic_to_ecef/geodetic_to_ecef.html
which in turn is based on the excellent work from
https://hal.archives-ouvertes.fr/hal-01704943/document
*/
// lat, lon, height (rad, rad, meters)
std::tuple<double, double, double> ecefToWGS84(double x, double y, double z)
{
constexpr double a = 6378137.0; //WGS-84 semi-major axis
constexpr double e2 = 6.6943799901377997e-3; //WGS-84 first eccentricity squared
constexpr double a1 = 4.2697672707157535e+4; //a1 = a*e2
constexpr double a2 = 1.8230912546075455e+9; //a2 = a1*a1
constexpr double a3 = 1.4291722289812413e+2; //a3 = a1*e2/2
constexpr double a4 = 4.5577281365188637e+9; //a4 = 2.5*a2
constexpr double a5 = 4.2840589930055659e+4; //a5 = a1+a3
constexpr double a6 = 9.9330562000986220e-1; //a6 = 1-e2
double zp,w2,w,r2,r,s2,c2,s,c,ss;
double g,rg,rf,u,v,m,f,p;
std::tuple<double, double, double> geo; //Results go here (Lat, Lon, Altitude)
zp = abs( z );
w2 = x*x + y*y;
w = sqrt( w2 );
r2 = w2 + z*z;
r = sqrt( r2 );
std::get<1>(geo) = atan2( y, x ); //Lon (final)
s2 = z*z/r2;
c2 = w2/r2;
u = a2/r;
v = a3 - a4/r;
if( c2 > 0.3 ){
s = ( zp/r )*( 1.0 + c2*( a1 + u + s2*v )/r );
std::get<0>(geo) = asin( s ); //Lat
ss = s*s;
c = sqrt( 1.0 - ss );
}
else{
c = ( w/r )*( 1.0 - s2*( a5 - u - c2*v )/r );
std::get<0>(geo) = acos( c ); //Lat
ss = 1.0 - c*c;
s = sqrt( ss );
}
g = 1.0 - e2*ss;
rg = a/sqrt( g );
rf = a6*rg;
u = w - rg*c;
v = zp - rf*s;
f = c*u + s*v;
m = c*v - s*u;
p = m/( rf/g + f );
std::get<0>(geo) = std::get<0>(geo) + p; //Lat
std::get<2>(geo) = f + m*p/2.0; //Altitude
if( z < 0.0 ){
std::get<0>(geo) *= -1.0; //Lat
}
return( geo ); //Return Lat, Lon, Altitude in that order
}

54
ubxtool.cc
View File

@ -42,6 +42,7 @@ using namespace std;
uint16_t g_srcid{0};
int g_fixtype{-1};
double g_speed{-1};
static int getBaudrate(int baud)
{
@ -524,6 +525,7 @@ int main(int argc, char** argv)
bool doGPS{true}, doGalileo{true}, doGlonass{false}, doBeidou{true}, doReset{false}, doWait{true}, doRTSCTS{true}, doSBAS{false};
bool doFakeFix{false};
bool doKeepNMEA{false};
bool doSTDOUT=false;
#ifdef OpenBSD
@ -550,6 +552,7 @@ int main(int argc, char** argv)
app.add_option("--ubxport,-u", ubxport, "UBX port to enable messages on (usb=3)");
app.add_option("--baud,-b", baudrate, "Baudrate for serial connection");
app.add_flag("--keep-nmea,-k", doKeepNMEA, "Don't disable NMEA");
app.add_flag("--fake-fix", doFakeFix, "Inject locally generated fake fix data");
app.add_flag("--debug", doDEBUG, "Display debug information");
app.add_flag("--logfile", doLOGFILE, "Create logfile");
@ -820,12 +823,21 @@ int main(int argc, char** argv)
uint8_t flags;
uint8_t flags2;
uint8_t numsv;
// 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
} __attribute__((packed));
PVT pvt;
memcpy(&pvt, &payload[0], sizeof(pvt));
// cerr << "Ground speed: "<<pvt.gSpeed<<", "<<pvt.velN<<" "<<pvt.velE<<" "<<pvt.velD << endl;
g_fixtype = pvt.fixtype;
g_speed = pvt.gSpeed / 1000.0;
struct tm tm;
memset(&tm, 0, sizeof(tm));
@ -851,10 +863,31 @@ int main(int argc, char** argv)
g_gnssutc.tv_nsec = pvt.nano;
continue;
}
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;
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);
// ECEF 3919766.490000, 300647.060000, 5005731.330000
nmm.mutable_op()->set_x(3919766);
nmm.mutable_op()->set_y(300647);
nmm.mutable_op()->set_z(5005731);
nmm.mutable_op()->set_acc(3.14);
ns.emitNMM( nmm);
}
}
@ -888,10 +921,11 @@ int main(int argc, char** argv)
uint16_t locktimems;
memcpy(&locktimems, &payload[40+32*n], 2);
uint8_t prStddev = payload[43+23*n] & 0xf;
uint8_t cpStddev = payload[44+23*n] & 0xf;
uint8_t doStddev = payload[45+23*n] & 0xf;
// uint8_t trkStat = payload[46+23*n] & 0xf;
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;
// uint8_t trkStat = payload[46+32*n] & 0xf;
NavMonMessage nmm;
nmm.set_type(NavMonMessage::RFDataType);
@ -912,6 +946,7 @@ int main(int argc, char** argv)
nmm.mutable_rfd()->set_dostd(ldexp(0.002, doStddev));
nmm.mutable_rfd()->set_cpstd(cpStddev*0.4);
nmm.mutable_rfd()->set_locktimems(locktimems);
nmm.mutable_rfd()->set_cno(cno);
ns.emitNMM( nmm);
}
}
@ -945,6 +980,9 @@ int main(int argc, char** argv)
nmm.mutable_op()->set_y(p.ecefY /100.0);
nmm.mutable_op()->set_z(p.ecefZ /100.0);
nmm.mutable_op()->set_acc(p.pAcc /100.0);
if(g_speed >= 0.0)
nmm.mutable_op()->set_groundspeed(g_speed);
ns.emitNMM( nmm);
}
else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX