void copy_gps_reading(void)
{
static vector v_r_ecef, v_v_ecef;
Current_state.gps = gps;
Current_state.gps.time_since_reading = 0;
v_r_ecef[0] = (float)Current_state.gps.x / 1000;
v_r_ecef[1] = (float)Current_state.gps.y / 1000;
v_r_ecef[2] = (float)Current_state.gps.z / 1000;
v_v_ecef[0] = (float)Current_state.gps.v_x / 1000;
v_v_ecef[1] = (float)Current_state.gps.v_y / 1000;
v_v_ecef[2] = (float)Current_state.gps.v_z / 1000;
get_seconds_since_equinox();
ecef2eci(v_r_ecef, v_r);
ecef2eci(v_v_ecef, v_v);
v_sat[0] = (((float)Current_state.gps.lat) / 10000000) * (M_PI / 180);
v_sat[1] = (((float)Current_state.gps.lon) / 10000000) * (M_PI / 180);
GPS_done = 0;
}
void copy_gps_reading(void)
{
static vector v_r_ecef, v_v_ecef;
// Current_state.gps = gps;
Current_state.gps.time_since_reading = 0;
//increase a zero at end
//int32_t test = -181930228;
v_r_ecef[0] = ((float)Current_state.gps.x)/100;//((float)test/100);//
v_r_ecef[1] = ((float)Current_state.gps.y) / 100;//((float)test/100);//
v_r_ecef[2] = ((float)Current_state.gps.z) / 100;//((float)test/100);//
v_v_ecef[0] = (float)Current_state.gps.v_x / 1;
v_v_ecef[1] = (float)Current_state.gps.v_y / 1;
v_v_ecef[2] = (float)Current_state.gps.v_z / 1;
get_seconds_since_equinox();//check
r_ecef_ash[0] = v_r_ecef[0];
r_ecef_ash[1] = v_r_ecef[1];
r_ecef_ash[2] = v_r_ecef[2];
ecef2eci(v_r_ecef, v_r);
ecef2eci(v_v_ecef, v_v);
int32_t send;
/*for (int i=0;i<3;i++)
{
//if(send[i]<0)
//send[i]=(int16_t)(-1*(v_v_ecef[i]/1));
//else
send[i]=(int16_t)((abs(v_v[i])));
}*/
/*
uint8_t a,b,c,d;
for (int i=0;i<3;i++)
{
send=(int32_t)(v_r_ecef[i]*100);
a = (int8_t)send;
b = (int8_t)(send>>8);
c = (int8_t)(send>>16);
d = (int8_t)(send>>24);
transmit_UART0(a);
transmit_UART0(b);
transmit_UART0(c);
transmit_UART0(d);
}
*/
v_sat[0] = (((float)Current_state.gps.lat) / 10000000) * (M_PI / 180);// check scale factor
v_sat[1] = (((float)Current_state.gps.lon) / 10000000) * (M_PI / 180);
GPS_done = 0;
}
void sgp_get_acceleration(vector v_g)// only j2 perturbations taken
{
vector v_r_ecef, v_g_ecef;
float R, R2, R3, R4;
//eci2ecef(v_r, v_r_ecef);//see change
R = vector_norm(r_ecef_ash); //
R2 = pow(R, 2);
R2 = (1.5 * J2 * R_E2) / R2;
R3 = pow(R, 3);
R4 = pow(R, 4);
R4 = (7.5 * J2 * pow(r_ecef_ash[2],2) * R_E2) / R4; //
v_g_ecef[0] = (-1 * GM * r_ecef_ash[0] * (1 + R2 - R4)) / R3;//
v_g_ecef[1] = (-1 * GM * r_ecef_ash[1] * (1 + R2 - R4)) / R3;//
v_g_ecef[2] = (-1 * GM * r_ecef_ash[2] * (1 + 3 * R2 - R4)) / R3;//
ecef2eci(v_g_ecef, v_g);
/* uint8_t sent[3];
for (int i=0;i<2;i=i+1)
{
sent[i] = (uint8_t)((v_g_ecef[i]));
transmit_UART0(sent[i]);
}*/
}
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 copy_gps_reading(void)
{
static vector v_r_ecef, v_v_ecef;
v_r_ecef[0] = (float)1 / 1000;
v_r_ecef[1] = (float)1 / 1000;
v_r_ecef[2] = (float)1 / 1000;
v_v_ecef[0] = (float)1 / 1000;
v_v_ecef[1] = (float)1 / 1000;
v_v_ecef[2] = (float)1 / 1000;
get_seconds_since_equinox();
ecef2eci(v_r_ecef, v_r);
ecef2eci(v_v_ecef, v_v);
v_r[0] = -7178521.3196791;
v_r[1] = -525365.237135614;
v_r[2] = -14623.7085998278;
v_v[0] = -89.2053675056426;
v_v[1] = 1118.96667576478;
v_v[2] = 7357.30907922713;
}
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);
}
void sgp_get_acceleration(vector v_g)
{
vector v_r_ecef, v_g_ecef;
float R, R2, R3, R4;
eci2ecef(v_r, v_r_ecef);
R = vector_norm(v_r_ecef);
R2 = pow(R, 2);
R2 = (1.5 * J2 * R_E2) / R2;
R3 = pow(R, 3);
R4 = pow(R, 4);
R4 = (7.5 * J2 * v_r_ecef[2] * R_E2) / R4;
v_g_ecef[0] = (-1 * GM * v_r_ecef[0] * (1 + R2 + R4)) / R3;
v_g_ecef[1] = (-1 * GM * v_r_ecef[1] * (1 + R2 + R4)) / R3;
v_g_ecef[2] = (-1 * GM * v_r_ecef[2] * (1 + 3 * R2 + R4)) / R3;
ecef2eci(v_g_ecef, v_g);
}
