C++ (Cpp) ecef_of_ned_vect_d Examples

Example #1

File: test_libeknav_4.cpp Project: Paolo-Maffei/lxyppc-tetrix

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(&current_ltp, &pos_0_ecef_pprz);
	ecef_of_ned_vect_d(&ref_dir_ecef, &current_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;
}

Example #2

File: libeknav_from_log.cpp Project: 2770862886/paparazzi

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(&current_ltp, &pos_0_ecef_pprz);
	ecef_of_ned_vect_d(&ref_dir_ecef, &current_ltp, &ref_dir_ned);
	
	reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
}

Example #3

File: nps_fdm_crrcsim.c Project: 2seasuav/paparuzzi

/***************************************************************************************
 *decode the gps data packet
 ***************************************************************************************/
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
{
  /* gps velocity (1e2 m/s to  m/s */
  struct NedCoor_d vel;
  vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2;
  vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2;
  vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2;
  fdm->ltp_ecef_vel = vel;
  ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, &ltpdef, &vel);

  /* No airspeed from CRRCSIM?
   * use ground speed for now, since we also don't know wind
   */
  struct DoubleVect3 ltp_airspeed;
  VECT3_COPY(ltp_airspeed, vel);
  fdm.airspeed = double_vect3_norm(&ltp_airspeed);

  /* gps position (1e7 deg to rad and 1e3 m to m) */
  struct LlaCoor_d pos;
  pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9;
  pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9;
  pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3;

  pos.lat += ltpdef.lla.lat;
  pos.lon += ltpdef.lla.lon;
  pos.alt += ltpdef.lla.alt;

  fdm->lla_pos = pos;
  ecef_of_lla_d(&fdm->ecef_pos, &pos);
  fdm->hmsl = pos.alt - NAV_MSL0 / 1000.;

  fdm->pressure = pprz_isa_pressure_of_altitude(fdm->hmsl);

  /* gps time */
  fdm->time = (double)UShortOfBuf(buffer, 27);

  /* in LTP pprz */
  ned_of_ecef_point_d(&fdm->ltpprz_pos, &ltpdef, &fdm->ecef_pos);
  fdm->lla_pos_pprz = pos;
  ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, &ltpdef, &fdm->ecef_ecef_vel);

#if NPS_CRRCSIM_DEBUG
  printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n",
         57.3 * fdm->lla_pos.lat,
         57.3 * fdm->lla_pos.lon,
         fdm->lla_pos.alt,
         fdm->ltp_ecef_vel.x,
         fdm->ltp_ecef_vel.y,
         fdm->ltp_ecef_vel.z,
         fdm->time);
#endif
}