📄 gps_nav.cpp
字号:
//---------------------------------------------------------------------------
//Copyright (C) 2003,2004 Krzysztof Kamieniecki (krys@kamieniecki.com)
/*
This file is part of kkGPS.
kkGPS is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
kkGPS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with kkGPS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
#include "gps_nav.h"
#include <fstream>
//debug#include <iostream>
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//GPS standard constants
//WGS 84 value of earth's universal gravitational parameter
f64 const k_mu = 3.986005e14; //meter^3 / second^2
//speed of light
f64 const k_c = 2.99792458e8; //meter / second
//WGS 84 value of the earth's rotation rate
f64 const k_dot_omega_e = 7.2921151467e-5; //radians / second
//GPS standard Pi
f64 const k_pi = 3.1415926535898;
//GPS radians per semi circle
f64 const k_semi_circle_radians = k_pi;
//GPS inital guess on satellite distance
//satellite orbital radius - earth average radius
f64 const k_satellite_orbital_radius_m = 26560000 - 6368000;
//
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
void
Invert4x4Matrix( f64 *mat, f64 *dst)
{
f64 const a00 = mat[4*0 + 0];
f64 const a01 = mat[4*0 + 1];
f64 const a02 = mat[4*0 + 2];
f64 const a03 = mat[4*0 + 3];
f64 const a10 = mat[4*1 + 0];
f64 const a11 = mat[4*1 + 1];
f64 const a12 = mat[4*1 + 2];
f64 const a13 = mat[4*1 + 3];
f64 const a20 = mat[4*2 + 0];
f64 const a21 = mat[4*2 + 1];
f64 const a22 = mat[4*2 + 2];
f64 const a23 = mat[4*2 + 3];
f64 const a30 = mat[4*3 + 0];
f64 const a31 = mat[4*3 + 1];
f64 const a32 = mat[4*3 + 2];
f64 const a33 = mat[4*3 + 3];
#define det2x2(A00,A01,A10,A11) \
(A00 * A11 - A01 * A10)
#define det3x3(A00,A01,A02,A10,A11,A12,A20,A21,A22) \
(A00 * det2x2(A11,A12,A21,A22) - A01 * det2x2(A10,A12,A20,A22) + A02 * det2x2(A10,A11,A20,A21))
f64 const r00 = det3x3(a11,a12,a13,a21,a22,a23,a31,a32,a33);
f64 const r01 = -det3x3(a10,a12,a13,a20,a22,a23,a30,a32,a33);
f64 const r02 = det3x3(a10,a11,a13,a20,a21,a23,a30,a31,a33);
f64 const r03 = -det3x3(a10,a11,a12,a20,a21,a22,a30,a31,a32);
f64 const r10 = -det3x3(a01,a02,a03,a21,a22,a23,a31,a32,a33);
f64 const r11 = det3x3(a00,a02,a03,a20,a22,a23,a30,a32,a33);
f64 const r12 = -det3x3(a00,a01,a03,a20,a21,a23,a30,a31,a33);
f64 const r13 = det3x3(a00,a01,a02,a20,a21,a22,a30,a31,a32);
f64 const r20 = det3x3(a01,a02,a03,a11,a12,a13,a31,a32,a33);
f64 const r21 = -det3x3(a00,a02,a03,a10,a12,a13,a30,a32,a33);
f64 const r22 = det3x3(a00,a01,a03,a10,a11,a13,a30,a31,a33);
f64 const r23 = -det3x3(a00,a01,a02,a10,a11,a12,a30,a31,a32);
f64 const r30 = -det3x3(a01,a02,a03,a11,a12,a13,a21,a22,a23);
f64 const r31 = det3x3(a00,a02,a03,a10,a12,a13,a20,a22,a23);
f64 const r32 = -det3x3(a00,a01,a03,a10,a11,a13,a20,a21,a23);
f64 const r33 = det3x3(a00,a01,a02,a10,a11,a12,a20,a21,a22);
f64 const det = a00 * r00 + a01 * r01 + a02 * r02 + a03 * r03;
f64 const invDet = (0.0 == det)?(0.0):(1.0 / det);
dst[4*0 + 0] = invDet * r00;
dst[4*0 + 1] = invDet * r10;
dst[4*0 + 2] = invDet * r20;
dst[4*0 + 3] = invDet * r30;
dst[4*1 + 0] = invDet * r01;
dst[4*1 + 1] = invDet * r11;
dst[4*1 + 2] = invDet * r21;
dst[4*1 + 3] = invDet * r31;
dst[4*2 + 0] = invDet * r02;
dst[4*2 + 1] = invDet * r12;
dst[4*2 + 2] = invDet * r22;
dst[4*2 + 3] = invDet * r32;
dst[4*3 + 0] = invDet * r03;
dst[4*3 + 1] = invDet * r13;
dst[4*3 + 2] = invDet * r23;
dst[4*3 + 3] = invDet * r33;
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
f64
limitTimeToValidRange(
f64 const inTime)
{
if(inTime < -302400) return inTime + 604800;
if(inTime > 302400) return inTime - 604800;
return inTime;
}
//---------------------------------------------------------------------------
template<typename TYPE_>
TYPE_
parseMsgDataI32Helper(
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
TYPE_ value;
u32 const msbFirstBit = 24 - inMsbFirstBit;
u32 const msbLastBit = 24 - inMsbLastBit;
//shift left to put FirstBit in Msb bit location so that if value is a
//signed integer the sign will be saved
value = inMsbWord << (31 - msbFirstBit);
//bring value back to lsb
value >>= (31 - msbFirstBit) + msbLastBit;
if(inLsbFirstBit && inLsbLastBit)
{
u32 const lsbWidth = (inLsbLastBit - inLsbFirstBit + 1);
u32 const lsbMask = ~(0xFFFFFFFF << lsbWidth);
u32 const lsbOffset = 24 - inLsbLastBit;
value <<= lsbWidth;
value |= (inLsbWord >> lsbOffset) & lsbMask;
}
return value;
}
//---------------------------------------------------------------------------
template<typename TYPE_>
f64
parseMsgDataF64Helper(
s32 const inScale,
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
return std::ldexp(
static_cast<f64>(parseMsgDataI32Helper<TYPE_>(
inMsbWord,inMsbFirstBit,inMsbLastBit,
inLsbWord,inLsbFirstBit,inLsbLastBit)),
inScale);
}
//---------------------------------------------------------------------------
s32
parseMsgDataS32(
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
return parseMsgDataI32Helper<s32>(
inMsbWord,inMsbFirstBit,inMsbLastBit,inLsbWord,inLsbFirstBit,inLsbLastBit);
}
//---------------------------------------------------------------------------
u32
parseMsgDataU32(
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
return parseMsgDataI32Helper<u32>(
inMsbWord,inMsbFirstBit,inMsbLastBit,inLsbWord,inLsbFirstBit,inLsbLastBit);
}
//---------------------------------------------------------------------------
f64
parseMsgDataSF64(
s32 const inScale,
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
return parseMsgDataF64Helper<s32>(
inScale,
inMsbWord,inMsbFirstBit,inMsbLastBit,inLsbWord,inLsbFirstBit,inLsbLastBit);
}
//---------------------------------------------------------------------------
f64
parseMsgDataUF64(
s32 const inScale,
u32 const inMsbWord,
u32 const inMsbFirstBit,
u32 const inMsbLastBit,
u32 const inLsbWord,
u32 const inLsbFirstBit,
u32 const inLsbLastBit)
{
return parseMsgDataF64Helper<u32>(
inScale,
inMsbWord,inMsbFirstBit,inMsbLastBit,inLsbWord,inLsbFirstBit,inLsbLastBit);
}
//---------------------------------------------------------------------------
bool
SubFrameIDGet(
SubFrameRaw const& inSubFrame,
s32& ouSubFrameID,
s32& ouPage)
{
ouSubFrameID = 0;
ouPage = 0;
u32 dataWord;
if(!checkWordParity(inSubFrame.words_[1],dataWord))
return false;
ouSubFrameID = (dataWord >> 2) & 0x7;
switch(ouSubFrameID)
{
case 1:
case 2:
case 3:
return true;
case 4:
case 5:
//not implemented
return false;
default:
return false;
}
}
//---------------------------------------------------------------------------
#define M_DUMP_VALUE(V_) result << #V_ << ": " << V_ << std::endl
//---------------------------------------------------------------------------
//SubFrameBase::
//---------------------------------------------------------------------------
bool
SubFrameBase::parse(
SubFrameRaw const& inSubFrame)
{
u32 dataWord1;
if(!checkWordParity(inSubFrame.words_[0],dataWord1))
return false;
u32 dataWord2;
if(!checkWordParity(inSubFrame.words_[1],dataWord2))
return false;
//sub-frame data
telemetryMessage_ = parseMsgDataU32(dataWord1,9,22);
truncatedTowCount_ = parseMsgDataU32(dataWord2,1,17);
alertFlag_ = parseMsgDataU32(dataWord2,18,18);
antiSpoofFlag_ = parseMsgDataU32(dataWord2,19,19);
subFrameId_ = parseMsgDataU32(dataWord2,20,22);
return true;
}
//---------------------------------------------------------------------------
std::string
SubFrameBase::debugDump()
{
std::stringstream result;
M_DUMP_VALUE(telemetryMessage_);
M_DUMP_VALUE(truncatedTowCount_);
M_DUMP_VALUE(alertFlag_);
M_DUMP_VALUE(antiSpoofFlag_);
M_DUMP_VALUE(subFrameId_);
return result.str();
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//SubFrame1::
//---------------------------------------------------------------------------
bool
SubFrame1::parse(
SubFrameRaw const& inSubFrame)
{
if(!SubFrameBase::parse(inSubFrame))
return false;
if(1 != subFrameId_)
return false;
u32 dataWord3;
if(!checkWordParity(inSubFrame.words_[2],dataWord3))
return false;
u32 dataWord4;
if(!checkWordParity(inSubFrame.words_[3],dataWord4))
return false;
u32 dataWord7;
if(!checkWordParity(inSubFrame.words_[6],dataWord7))
return false;
u32 dataWord8;
if(!checkWordParity(inSubFrame.words_[7],dataWord8))
return false;
u32 dataWord9;
if(!checkWordParity(inSubFrame.words_[8],dataWord9))
return false;
u32 dataWord10;
if(!checkWordParity(inSubFrame.words_[9],dataWord10))
return false;
//sub-frame data
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -