void test_ned_to_ecef_to_ned( void ) {
#if 0
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(<p_def, &ref_coor);
struct EcefCoor_d ecef_p1 = ref_coor;
struct NedCoor_d ned_p1;
ned_of_ecef_point_d(&ned_p1, <p_def, &ecef_p1);
printf("ecef to ned : (%f,%f,%f) -> (%f,%f,%f)\n",
ecef_p1.x, ecef_p1.y, ecef_p1.z,
ned_p1.x, ned_p1.y, ned_p1.z );
struct EcefCoor_d ecef_p2;
ecef_of_ned_point_d(&ecef_p2, <p_def, &ned_p1);
printf("ned to ecef : (%f,%f,%f) -> (%f,%f,%f)\n",
ned_p1.x, ned_p1.y, ned_p1.z,
ecef_p2.x, ecef_p2.y, ecef_p2.z);
printf("\n");
#endif
}
static void test_doubles(void) {
printf("\n--- enu_of_ecef double ---\n");
// struct LlaCoor_f ref_coor;
// ref_coor.lat = RAD_OF_DEG(43.605278);
// ref_coor.lon = RAD_OF_DEG(1.442778);
// ref_coor.alt = 180.0;
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(<p_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
struct EcefCoor_d my_ecef_point = ref_coor;
struct EnuCoor_d my_enu_point;
enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
my_enu_point.x, my_enu_point.y, my_enu_point.z );
printf("\n");
}
static void set_reference_direction(void){
struct NedCoor_d ref_dir_ned;
struct EcefCoor_d pos_0_ecef_pprz,
ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
struct LtpDef_d current_ltp;
VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz);
ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned);
// THIS SOMEWHERE ELSE!
DoubleQuat initial_orientation;
FLOAT_QUAT_ZERO(initial_orientation);
ENU_NED_transformation(current_ltp.ltp_of_ecef);
DOUBLE_QUAT_OF_RMAT(initial_orientation, current_ltp.ltp_of_ecef);
ins.avg_state.orientation = DOUBLEQUAT_AS_QUATERNIOND(initial_orientation);
// THIS SOMEWHERE ELSE! (END)
// old transformation:
//struct DoubleRMat ned2ecef;
//NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m);
//RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
//reference_direction = Vector3d(1, 0, 0);
std::cout <<"reference direction NED : " << VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl;
std::cout <<"reference direction ECEF: " << reference_direction.transpose() << std::endl;
}
/** Transformation **/
Quaterniond ecef2body_from_pprz_ned2body(Vector3d ecef_pos, struct DoubleQuat q_ned2body){
Quaterniond ecef2body;
struct LtpDef_d current_ltp;
struct EcefCoor_d ecef_pos_pprz;
struct DoubleQuat q_ecef2enu,
q_ecef2ned,
q_ecef2body;
VECTOR_AS_VECT3(ecef_pos_pprz, ecef_pos);
ltp_def_from_ecef_d(¤t_ltp, &ecef_pos_pprz);
DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned);
//FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
// a = ecef b = ned c = body
FLOAT_QUAT_COMP_INV_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
#ifdef EKNAV_FROM_LOG_DEBUG
printf("Right after initialization:\n");
DISPLAY_DOUBLE_QUAT("\t ned2body quaternion:", q_ned2body);
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ned2body);
DISPLAY_DOUBLE_QUAT("\tecef2enu quaternion:", q_ecef2enu);
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2enu);
DISPLAY_DOUBLE_QUAT("\tecef2ned quaternion:", q_ecef2ned);
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2ned);
DISPLAY_DOUBLE_QUAT("\tecef2body quaternion:", q_ecef2body);
DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2body);
#endif /* EKNAV_FROM_LOG_DEBUG */
return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
}
static void init_ltp(void)
{
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = (NAV_ALT0 + NAV_MSL0) / 1000.;
struct EcefCoor_d ecef_nav0;
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_d(<pdef, &ecef_nav0);
fdm.ltp_g.x = 0.;
fdm.ltp_g.y = 0.;
fdm.ltp_g.z = 0.; // accel data are already with the correct format
#ifdef AHRS_H_X
#pragma message "Using magnetic field as defined in airframe file."
fdm.ltp_h.x = AHRS_H_X;
fdm.ltp_h.y = AHRS_H_Y;
fdm.ltp_h.z = AHRS_H_Z;
#else
fdm.ltp_h.x = 0.4912;
fdm.ltp_h.y = 0.1225;
fdm.ltp_h.z = 0.8624;
#endif
}
static void set_reference_direction(void){
struct NedCoor_d ref_dir_ned;
struct EcefCoor_d pos_0_ecef_pprz,
ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz);
ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned);
reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
}
int main(int argc, char **argv)
{
// Set the default tracking system position and angle
struct EcefCoor_d tracking_ecef;
//alt 45 m because of ellipsoid altitude in Delft
tracking_ecef.x = 3924331.5;
tracking_ecef.y = 300361.7;
tracking_ecef.z = 5002197.1;
tracking_offset_angle = 33.0 / 57.6;
ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
// Parse the options from cmdline
parse_options(argc, argv);
printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat),
DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
// Create the network connections
printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n",
natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving
udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);
udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB
printf_debug("Starting NatNet command socket (server address: %s, command port: %d)\n", natnet_addr, natnet_cmd_port);
udp_socket_create(&natnet_cmd, natnet_addr, natnet_cmd_port, 0, 1);
udp_socket_set_recvbuf(&natnet_cmd, 0x100000); // 1MB
// Create the Ivy Client
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0);
IvyStart(ivy_bus);
// Create the main timers
printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n",
freq_transmit, min_velocity_samples);
g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL);
GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd);
g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP,
sample_data, NULL);
// Run the main loop
g_main_loop_run(ml);
return 0;
}
/** Parse the options from the commandline */
static void parse_options(int argc, char **argv)
{
int i, count_ac = 0;
for (i = 1; i < argc; ++i) {
// Print help
if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
print_help(argv[0]);
exit(0);
}
// Set the verbosity level
if (strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) {
check_argcount(argc, argv, i, 1);
verbose = atoi(argv[++i]);
}
// Set an rigid body to ivy ac_id
else if (strcmp(argv[i], "-ac") == 0) {
check_argcount(argc, argv, i, 2);
int rigid_id = atoi(argv[++i]);
uint8_t ac_id = atoi(argv[++i]);
if (rigid_id >= MAX_RIGIDBODIES) {
fprintf(stderr, "Rigid body ID must be less then %d (MAX_RIGIDBODIES)\n\n", MAX_RIGIDBODIES);
print_help(argv[0]);
exit(EXIT_FAILURE);
}
aircrafts[rigid_id].ac_id = ac_id;
count_ac++;
}
// See if we want to log to a file
else if (strcmp(argv[i], "-log") == 0) {
check_argcount(argc, argv, i, 1);
nameOfLogfile = argv[++i];
must_log = 1;
}
// Set the NatNet multicast address
else if (strcmp(argv[i], "-multicast_addr") == 0) {
check_argcount(argc, argv, i, 1);
natnet_multicast_addr = argv[++i];
}
// Set the NatNet server ip address
else if (strcmp(argv[i], "-server") == 0) {
check_argcount(argc, argv, i, 1);
natnet_addr = argv[++i];
}
// Set the NatNet server version
else if (strcmp(argv[i], "-version") == 0) {
check_argcount(argc, argv, i, 1);
float version = atof(argv[++i]);
natnet_major = (uint8_t)version;
natnet_minor = (uint8_t)(version * 10.0) % 10;
}
// Set the NatNet server data port
else if (strcmp(argv[i], "-data_port") == 0) {
check_argcount(argc, argv, i, 1);
natnet_data_port = atoi(argv[++i]);
}
// Set the NatNet server command port
else if (strcmp(argv[i], "-cmd_port") == 0) {
check_argcount(argc, argv, i, 1);
natnet_cmd_port = atoi(argv[++i]);
}
// Set the Tracking system position in ECEF
else if (strcmp(argv[i], "-ecef") == 0) {
check_argcount(argc, argv, i, 3);
struct EcefCoor_d tracking_ecef;
tracking_ecef.x = atof(argv[++i]);
tracking_ecef.y = atof(argv[++i]);
tracking_ecef.z = atof(argv[++i]);
ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
}
// Set the tracking system position in LLA
else if (strcmp(argv[i], "-lla") == 0) {
check_argcount(argc, argv, i, 3);
struct LlaCoor_d tracking_lla;
tracking_lla.lat = atof(argv[++i]);
tracking_lla.lon = atof(argv[++i]);
tracking_lla.alt = atof(argv[++i]);
ltp_def_from_lla_d(&tracking_ltp, &tracking_lla);
}
// Set the tracking system offset angle in degrees
else if (strcmp(argv[i], "-offset_angle") == 0) {
check_argcount(argc, argv, i, 1);
tracking_offset_angle = atof(argv[++i]);
}
// Set the transmit frequency
else if (strcmp(argv[i], "-tf") == 0) {
check_argcount(argc, argv, i, 1);
freq_transmit = atoi(argv[++i]);
}
// Set the minimum amount of velocity samples for the differentiator
else if (strcmp(argv[i], "-vel_samples") == 0) {
check_argcount(argc, argv, i, 1);
min_velocity_samples = atoi(argv[++i]);
}
// Set to use small packets
else if (strcmp(argv[i], "-small") == 0) {
small_packets = TRUE;
}
// Set the ivy bus
else if (strcmp(argv[i], "-ivy_bus") == 0) {
check_argcount(argc, argv, i, 1);
ivy_bus = argv[++i];
}
// Unknown option
else {
fprintf(stderr, "Unknown option %s\r\n\r\n", argv[i]);
print_help(argv[0]);
exit(0);
}
}
// Check if at least one aircraft is set
if (count_ac < 1) {
fprintf(stderr, "You must specify at least one aircraft (-ac <rigid_id> <ac_id>)\n\n");
print_help(argv[0]);
exit(EXIT_FAILURE);
}
}
static void print_estimator_state(double time) {
#if FILTER_OUTPUT_IN_NED
struct LtpDef_d current_ltp;
struct EcefCoor_d pos_ecef,
cur_pos_ecef,
cur_vel_ecef;
struct NedCoor_d pos_ned,
vel_ned;
struct DoubleQuat q_ecef2body,
q_ecef2enu,
q_enu2body,
q_ned2enu,
q_ned2body;
VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
ltp_def_from_ecef_d(¤t_ltp, &pos_ecef);
ned_of_ecef_point_d(&pos_ned, ¤t_ltp, &cur_pos_ecef);
ned_of_ecef_vect_d(&vel_ned, ¤t_ltp, &cur_vel_ecef);
int32_t xdd = 0;
int32_t ydd = 0;
int32_t zdd = 0;
int32_t xd = vel_ned.x/0.0000019073;
int32_t yd = vel_ned.y/0.0000019073;
int32_t zd = vel_ned.z/0.0000019073;
int32_t x = pos_ned.x/0.0039;
int32_t y = pos_ned.y/0.0039;
int32_t z = pos_ned.z/0.0039;
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
-ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^*
FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body) // q_ned2body = q_enu2body * q_ned2enu
struct FloatEulers e;
FLOAT_EULERS_OF_QUAT(e, q_ned2body);
#if PRINT_EULER_NED
printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI);
#endif
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)),
sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)),
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#else
int32_t xdd = 0;
int32_t ydd = 0;
int32_t zdd = 0;
int32_t xd = ins.avg_state.velocity(0)/0.0000019073;
int32_t yd = ins.avg_state.velocity(1)/0.0000019073;
int32_t zd = ins.avg_state.velocity(2)/0.0000019073;
int32_t x = ins.avg_state.position(0)/0.0039;
int32_t y = ins.avg_state.position(1)/0.0039;
int32_t z = ins.avg_state.position(2)/0.0039;
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
struct FloatQuat q_ecef2body;
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
struct FloatEulers e_ecef2body;
FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)),
sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)),
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#endif
}
