void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
{
#if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */
struct LlaCoor_f in_f;
in_f.lon = RAD_OF_EM7DEG((float)in->lon);
in_f.lat = RAD_OF_EM7DEG((float)in->lat);
in_f.alt = M_OF_MM((float)in->alt);
/* calls the floating point transformation */
struct EcefCoor_f out_f;
ecef_of_lla_f(&out_f, &in_f);
/* convert the output to fixed point */
out->x = (int32_t)CM_OF_M(out_f.x);
out->y = (int32_t)CM_OF_M(out_f.y);
out->z = (int32_t)CM_OF_M(out_f.z);
#else // use double precision by default
/* convert our input to floating point */
struct LlaCoor_d in_d;
in_d.lon = RAD_OF_EM7DEG((double)in->lon);
in_d.lat = RAD_OF_EM7DEG((double)in->lat);
in_d.alt = M_OF_MM((double)in->alt);
/* calls the floating point transformation */
struct EcefCoor_d out_d;
ecef_of_lla_d(&out_d, &in_d);
/* convert the output to fixed point */
out->x = (int32_t)CM_OF_M(out_d.x);
out->y = (int32_t)CM_OF_M(out_d.y);
out->z = (int32_t)CM_OF_M(out_d.z);
#endif
}
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
}
/***************************************************************************************
*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, <pdef, &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(<p_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, <pdef, &fdm->ecef_pos);
fdm->lla_pos_pprz = pos;
ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, <pdef, &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
}
void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
/* convert our input to floating point */
struct LlaCoor_d in_d;
in_d.lon = RAD_OF_EM7RAD((double)in->lon);
in_d.lat = RAD_OF_EM7RAD((double)in->lat);
in_d.alt = M_OF_MM((double)in->alt);
/* calls the floating point transformation */
struct EcefCoor_d out_d;
ecef_of_lla_d(&out_d, &in_d);
/* convert the output to fixed point */
out->x = (int32_t)CM_OF_M(out_d.x);
out->y = (int32_t)CM_OF_M(out_d.y);
out->z = (int32_t)CM_OF_M(out_d.z);
}
/** Convert a LLA to ECEF.
* @param[out] out ECEF in cm
* @param[in] in LLA in degrees*1e7 and mm above ellipsoid
*/
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
{
#if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */
struct LlaCoor_f in_f;
LLA_FLOAT_OF_BFP(in_f, *in);
/* calls the floating point transformation */
struct EcefCoor_f out_f;
ecef_of_lla_f(&out_f, &in_f);
/* convert the output to fixed point */
ECEF_BFP_OF_REAL(*out, out_f);
#else // use double precision by default
/* convert our input to floating point */
struct LlaCoor_d in_d;
LLA_DOUBLE_OF_BFP(in_d, *in);
/* calls the floating point transformation */
struct EcefCoor_d out_d;
ecef_of_lla_d(&out_d, &in_d);
/* convert the output to fixed point */
ECEF_BFP_OF_REAL(*out, out_d);
#endif
}
/** 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 EcefCoor_d tracking_ecef;
struct LlaCoor_d tracking_lla;
tracking_lla.lat = atof(argv[++i]);
tracking_lla.lon = atof(argv[++i]);
tracking_lla.alt = atof(argv[++i]);
ecef_of_lla_d(&tracking_ecef, &tracking_lla);
ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
}
// 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);
}
}
