130 lines
5.1 KiB
C
130 lines
5.1 KiB
C
#ifndef NAVEQUATIONS_H
|
|
#define NAVEQUATIONS_H
|
|
|
|
#include "gps.h"
|
|
#include "gps/gps_ip_defines.h"
|
|
#include "gps/gps_nav_epoch.h"
|
|
#include "gps/matrixLib.h"
|
|
|
|
typedef struct gps_nav_solution {
|
|
double ecef_m[3];
|
|
double clock_bias_m;
|
|
double latitude_deg;
|
|
double longitude_deg;
|
|
double height_m;
|
|
double covariance_enu_m2[3][3];
|
|
double hacc_m;
|
|
double vacc_m;
|
|
double gdop;
|
|
double pdop;
|
|
double hdop;
|
|
double vdop;
|
|
double tdop;
|
|
double residual_rms_m;
|
|
double max_abs_residual_m;
|
|
double postfit_residual_m[GPS_TRCK_IP_COUNT];
|
|
double elevation_deg[GPS_TRCK_IP_COUNT];
|
|
double measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
|
double full_postfit_residual_m[GPS_TRCK_IP_COUNT];
|
|
double full_elevation_deg[GPS_TRCK_IP_COUNT];
|
|
double full_measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
|
double weighted_residual_rms_m;
|
|
double weighted_residual_sum;
|
|
double chi_square_threshold;
|
|
double chi_square_ratio;
|
|
double full_chi_square_ratio;
|
|
double postfit_position_correction_m;
|
|
double postfit_clock_correction_m;
|
|
double full_postfit_position_correction_m;
|
|
double full_postfit_clock_correction_m;
|
|
double quality_score;
|
|
unsigned int available_satellite_count;
|
|
unsigned int used_satellite_count;
|
|
unsigned int tracked_satellite_count;
|
|
unsigned int degrees_of_freedom;
|
|
uint32_t used_row_mask;
|
|
uint32_t excluded_row_mask;
|
|
uint64_t candidate_cycles;
|
|
uint64_t full_solution_cycles;
|
|
uint64_t fde_cycles;
|
|
uint64_t total_wls_cycles;
|
|
unsigned int iteration_count;
|
|
unsigned int fde_trial_count;
|
|
bool candidate_valid;
|
|
bool residual_test_valid;
|
|
bool chi_square_pass;
|
|
bool fde_applied;
|
|
bool solution_valid;
|
|
} gps_nav_solution_t;
|
|
|
|
void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr);
|
|
void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities);
|
|
double check_t(long double tkl);
|
|
void subVect(double* y, const double* x1, const double* x2);
|
|
double normVect(const double* x);
|
|
void minimum_element(double* minimum, const double * vec, const int ActiveSatellitesNumber);
|
|
bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
|
|
mat_type * az, mat_type * el,
|
|
const unsigned char* ura_indices,
|
|
const ionoutc_t* ionoutc, double gps_tow,
|
|
gps_nav_solution_t* solution_out);
|
|
void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot);
|
|
void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
|
void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
|
double tropo(double sinel, double hsta);
|
|
double SquareRoot(double number);
|
|
void cart2geo(double X, double Y, double Z, int i, t_phi_Lambda_h * phi_Lambda_h);
|
|
int findUtmZone(double latitude, double longitude);
|
|
|
|
//typedef struct{
|
|
// mat_type* absoluteSamples;
|
|
// mat_type* txTimeVec;
|
|
// mat_type* pseudoRanges;
|
|
// mat_type* az;
|
|
// mat_type* el;
|
|
//}matc_variable_t; // matc class variable encapsulation
|
|
// ***HEADER VARIABLES*** ( class variables that MAPPED TO THE HEADER GLOBAL VARIABLES )
|
|
//extern matc_variable_t* m_matc_variable;
|
|
//t_topocentData topcent;
|
|
//extern unsigned int nmbOfIterations;
|
|
//extern char subframeID; //TODO where is this variable get initialized ?
|
|
//extern unsigned int number_of_satellites; //TODO it will be replaced with subframeID; (represents same thing. # of captured satellites)
|
|
//extern mat_type* pos; // position vector.
|
|
//extern mat_type* posEq; // ?
|
|
//extern t_phi_Lambda_h phi_lambda_h;
|
|
|
|
|
|
|
|
|
|
|
|
//double* mat(int n, int m);
|
|
////double absoluteSamples[6] = {66564551, 66525117,66589459,66666672,66633917,66700000};
|
|
//double travelTime[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
|
//double pseudoRanges[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
|
//double az[8] = {0,0,0,0,0,0,0,0};
|
|
//double el[8] = {0,0,0,0,0,0,0,0};
|
|
//double pos[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
|
|
//double CorrectedPos[6] = {0,0,0,0,0,0};
|
|
//double Positions[MAX_SAT][POSITION_VECTOR_COL];
|
|
|
|
//double subGpsTime(gpstime_t g1, gpstime_t g0);
|
|
//gpstime_t incGpsTime(gpstime_t g0, double dt);
|
|
//void ecef2neu(const double* xyz, double t[3][3], double* neu);
|
|
//void ltcmat(const double* llh, double t[3][3]);
|
|
//void neu2azel(double* azel, const double* neu);
|
|
//void gps2date(const gpstime_t* g, datetime_t* t);
|
|
//void llh2xyz(const double* llh, double* xyz);
|
|
|
|
|
|
//double dotProd(const double* x1, const double* x2);
|
|
|
|
//void xyz2llh(const double* xyz, double* llh);
|
|
//double ionosphericDelay(const ionoutc_t* ionoutc, gpstime_t g, double* llh, double* azel);
|
|
|
|
//int findUtmZone(double latitude, double longitude);
|
|
//void CART2UTM(double X, double Y, double Z, int zone);
|
|
|
|
|
|
|
|
#endif
|