void magnetic_field_estimator(vector v_B_o)
{
float time_in_years = 2015 + (float)seconds_since_pivot / SECONDS_IN_YEAR;
vector v_temp, v_r_lla, v_B_ned, v_B_eci;
eci2ecef(v_r, v_temp);
ecef2lla(v_temp, v_r_lla);// LLA is need
///* Save LLA vector for use in communications check routine
copy_vector(v_r_lla, v_sat);// why is this required when ,lat long alt coming from GPS
igrf(v_r_lla, time_in_years, 8, v_B_ned);// need to check at the end
ned2ecef(v_B_ned, v_r_lla, v_temp);
ecef2eci(v_temp, v_B_eci);
eci2orbit(v_r, v_v, v_B_eci, v_B_o);
scalar_into_vector(v_B_o, 1e-9); // igrf gives in nT
int8_t sen,sen1;
int16_t st;
/*for (int i=0;i<3;i=i+1)
{
//sen = ((int8_t)((lambda))/2);
st =(int16_t)(v_B_eci[i]/10);
sen = (int8_t)st;
sen1 = (int8_t)(st>>8);
transmit_UART0(sen);
transmit_UART0(sen1);
}*/
}
void magnetic_field_estimator(vector v_B_o)
{
float time_in_years = 2010 + (float)seconds_since_pivot / SECONDS_IN_YEAR;
vector v_temp, v_r_lla, v_B_ned, v_B_eci;
eci2ecef(v_r, v_temp);
ecef2lla(v_temp, v_r_lla);
igrf(v_r_lla, time_in_years, 8, v_B_ned);
ned2ecef(v_B_ned, v_r_lla, v_temp);
ecef2eci(v_temp, v_B_eci);
eci2orbit(v_r, v_v, v_B_eci, v_B_o);
scalar_into_vector(v_B_o, 1e-9);
}
void magnetic_field_estimator(vector v_B_o)
{
float time_in_years = 2010 + (float)seconds_since_pivot / SECONDS_IN_YEAR;
vector v_temp, v_r_lla, v_B_ned, v_B_eci;
eci2ecef(v_r, v_temp);
ecef2lla(v_temp, v_r_lla);
///* Save LLA vector for use in communications check routine
copy_vector(v_r_lla, v_sat);
igrf(v_r_lla, time_in_years, 8, v_B_ned);
ned2ecef(v_B_ned, v_r_lla, v_temp);
ecef2eci(v_temp, v_B_eci);
eci2orbit(v_r, v_v, v_B_eci, v_B_o);
scalar_into_vector(v_B_o, 1e-9);
}
static void parse_gps( struct gps *gpsData_ptr ){
double old_GPS_TOW;
double sig_X, sig_Y, sig_Z, sig_VX, sig_VY, sig_VZ;
uint8_t finesteering_ok, solution_ok;
MATRIX ecef_mat = mat_creat(3,1,ZERO_MATRIX);
MATRIX lla_mat = mat_creat(3,1,ZERO_MATRIX);
MATRIX ned_mat = mat_creat(3,1,ZERO_MATRIX);
switch(localBuffer[5]*256 + localBuffer[4] ){
case 241: // parse BESTXYZ
endian_swap(localBuffer,14,2); // GPS Week No
endian_swap(localBuffer,16,4); // GPS_TOW
endian_swap(localBuffer,28,4); // Solution Status
endian_swap(localBuffer,28+8,8); // X
endian_swap(localBuffer,28+16,8); // Y
endian_swap(localBuffer,28+24,8); // Z
endian_swap(localBuffer,28+32,4); // stdevXe
endian_swap(localBuffer,28+36,4); // stdevYe
endian_swap(localBuffer,28+40,4); // stdevZe
endian_swap(localBuffer,28+52,8); // Vx
endian_swap(localBuffer,28+60,8); // Vy
endian_swap(localBuffer,28+68,8); // Vz
endian_swap(localBuffer,28+76,4); // stdevVx
endian_swap(localBuffer,28+80,4); // stdevVy
endian_swap(localBuffer,28+84,4); // stdevVz
gpsData_ptr->GPS_week = *((uint16_t *)(&localBuffer[14]));
gpsData_ptr->satVisible = (uint16_t)(localBuffer[28+105]);
old_GPS_TOW = gpsData_ptr->GPS_TOW;
gpsData_ptr->GPS_TOW = (double) *((uint32_t *)(&localBuffer[16])) / 1000.0;
// convert positions from ECEF to LLA
gpsData_ptr->Xe = *((double *)(&localBuffer[28+8]));
gpsData_ptr->Ye = *((double *)(&localBuffer[28+16]));
gpsData_ptr->Ze = *((double *)(&localBuffer[28+24]));
ecef_mat[0][0] = gpsData_ptr->Xe;
ecef_mat[1][0] = gpsData_ptr->Ye;
ecef_mat[2][0] = gpsData_ptr->Ze;
if(sqrt(gpsData_ptr->Xe*gpsData_ptr->Xe + gpsData_ptr->Ye*gpsData_ptr->Ye + gpsData_ptr->Ze*gpsData_ptr->Ze) < 1e-3) {
lla_mat[0][0] = 0.0;
lla_mat[1][0] = 0.0;
lla_mat[2][0] = 0.0;
} else {
lla_mat = ecef2lla(ecef_mat,lla_mat);
}
gpsData_ptr->lat = lla_mat[0][0]*R2D;
gpsData_ptr->lon = lla_mat[1][0]*R2D;
gpsData_ptr->alt = lla_mat[2][0];
// convert velocities from ECEF to NED
gpsData_ptr->Ue = *((double *)(&localBuffer[28+52]));
gpsData_ptr->Ve = *((double *)(&localBuffer[28+60]));
gpsData_ptr->We = *((double *)(&localBuffer[28+68]));
ecef_mat[0][0] = gpsData_ptr->Ue;
ecef_mat[1][0] = gpsData_ptr->Ve;
ecef_mat[2][0] = gpsData_ptr->We;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->vn = ned_mat[0][0];
gpsData_ptr->ve = ned_mat[1][0];
gpsData_ptr->vd = ned_mat[2][0];
// convert stdev position from ECEF to NED
sig_X = (double) *((float *)(&localBuffer[28+32]));
sig_Y = (double) *((float *)(&localBuffer[28+36]));
sig_Z = (double) *((float *)(&localBuffer[28+40]));
ecef_mat[0][0] = sig_X;
ecef_mat[1][0] = sig_Y;
ecef_mat[2][0] = sig_Z;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->sig_N = ned_mat[0][0];
gpsData_ptr->sig_E = ned_mat[1][0];
gpsData_ptr->sig_D = ned_mat[2][0];
// convert stdev velocities from ECEF to NED
sig_VX = (double) *((float *)(&localBuffer[28+76]));
sig_VY = (double) *((float *)(&localBuffer[28+80]));
sig_VZ = (double) *((float *)(&localBuffer[28+84]));
ecef_mat[0][0] = sig_VX;
ecef_mat[1][0] = sig_VY;
ecef_mat[2][0] = sig_VZ;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->sig_vn = ned_mat[0][0];
gpsData_ptr->sig_ve = ned_mat[1][0];
gpsData_ptr->sig_vd = ned_mat[2][0];
//fprintf(stderr,"Stat:%d \n",(*((uint32_t *)(&localBuffer[28]))));
//fprintf(stderr,"Time:%d \n",(*((uint8_t *)(&localBuffer[13]))));
//endian_swap(localBuffer,20,4);
//fprintf(stderr,"Rx:%08X \n",(*((uint32_t *)(&localBuffer[20]))));
solution_ok = ((*((uint32_t *)(&localBuffer[28]))) == 0);
//finesteering_ok = ( (*((uint8_t *)(&localBuffer[13]))) == 160 ) || ( (*((uint8_t *)(&localBuffer[13]))) == 170 ) || ( (*((uint8_t *)(&localBuffer[13]))) == 180 );
finesteering_ok=1;
if(solution_ok & finesteering_ok) {
if(fabs(gpsData_ptr->GPS_TOW - old_GPS_TOW) > 1e-3) {
// Check that this is a new data (no GPS outage)
gpsData_ptr->navValid = 0; // Light the GPS LOCK in Ground Station
gpsData_ptr->newData = 1; // Execute measurement update
}
else gpsData_ptr->navValid = 1; // Turn off the GPS LOCK light in Ground station
} // Also, no measurement update will occur since newData is not set to 1
else gpsData_ptr->navValid = 1; // Turn off the GPS LOCK light in Ground station
// Also, no measurement update will occur since newData is not set to 1
break;
}
mat_free(ecef_mat);
mat_free(lla_mat);
mat_free(ned_mat);
}
