void stateCalcPositionEcef_i(void) {
if (bit_is_set(state.pos_status, POS_ECEF_I))
return;
if (bit_is_set(state.pos_status, POS_ECEF_F)) {
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_I)) {
/// @todo check if resolution is good enough
ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
}
else if (bit_is_set(state.pos_status, POS_NED_F)) {
/* transform ned_f to ecef_f, set status bit, then convert to int */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f to ecef_f, set status bit, then convert to int */
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
}
else if (bit_is_set(state.pos_status, POS_LLA_I)) {
ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i);
}
else {
/* could not get this representation, set errno */
//struct EcefCoor_i _ecef_zero = {0};
//return _ecef_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ECEF_I);
}
void gps_sim_hitl_event(void)
{
if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
SysTimeTimerStart(gps_sim_hitl_timer);
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (state.ned_initialized_i) {
if (!autopilot_in_flight) {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
INT_VECT2_ZERO(guidance_h.ref.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
gv_set_ref(0, 0, 0);
guidance_v_z_ref = 0;
guidance_v_zd_ref = 0;
guidance_v_zdd_ref = 0;
}
struct NedCoor_i ned_c;
ned_c.x = guidance_h.ref.pos.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ned_c.y = guidance_h.ref.pos.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c);
gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z;
gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
ned_c.x = guidance_h.ref.speed.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ned_c.y = guidance_h.ref.speed.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c);
gps.fix = GPS_FIX_3D;
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
gps_available = true;
}
else {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
gv_set_ref(0, 0, 0);
}
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
}
}
void stateCalcPositionLla_i(void) {
if (bit_is_set(state.pos_status, POS_LLA_I))
return;
if (bit_is_set(state.pos_status, POS_LLA_F)) {
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
}
else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> lla_f, set status bit, then convert to int */
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_F)) {
/* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_I)) {
/* transform ned_i -> ecef_i -> lla_i, set status bits */
/// @todo check if resolution is enough
ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
}
else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> lla_f -> lla_i, set status bits */
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else {
/* could not get this representation, set errno */
//struct LlaCoor_i _lla_zero = {0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_I);
}
int main()
{
struct LtpDef_i ref;
struct EcefCoor_i ecef_ref;
ecef_ref.x = -241887298;
ecef_ref.y = 538037726;
ecef_ref.z = 241732905;
ltp_def_from_ecef_i(&ref,&ecef_ref);
struct EcefCoor_i pos1;
pos1.x = -241887285;
pos1.y = 538037716;
pos1.z = 241732895;
struct NedCoor_i result1;
ned_of_ecef_pos_i(&result1,&ref,&ecef_ref);
printf("x %f, ",POS_FLOAT_OF_BFP(result1.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result1.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result1.z));
struct NedCoor_i result;
ned_of_ecef_pos_i(&result,&ref,&pos1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
struct NedCoor_i ned_tar1, ned_tar2;
ned_tar1.x = 100 + POS_FLOAT_OF_BFP(result.x) * 100;
ned_tar1.y = POS_FLOAT_OF_BFP(result.y) * 100;
ned_tar1.z = 100 + POS_FLOAT_OF_BFP(result.z) * 100;
ned_tar2.x = -300;
ned_tar2.y = 0;
ned_tar2.z = 100;
struct EcefCoor_i ecef_tar1, ecef_tar2;
ecef_of_ned_point_i(&ecef_tar1,&ref,&ned_tar1);
ecef_of_ned_point_i(&ecef_tar2,&ref,&ned_tar2);
printf("ecef_tar1 x %d y %d z %d\n",ecef_tar1.x,ecef_tar1.y,ecef_tar1.z);
printf("ecef_tar2 x %d y %d z %d\n",ecef_tar2.x,ecef_tar2.y,ecef_tar2.z);
ned_of_ecef_pos_i(&result,&ref,&ecef_tar1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_pos_i(&result,&ref,&ecef_tar2);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_point_i(&result,&ref,&ecef_tar1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_point_i(&result,&ref,&ecef_tar2);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
return 0;
}
