/** 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 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; }