/** The transmitter periodic function */
gboolean timeout_transmit_callback(gpointer data)
{
int i;
// Loop trough all the available rigidbodies (TODO: optimize)
for (i = 0; i < MAX_RIGIDBODIES; i++) {
// Check if ID's are correct
if (rigidBodies[i].id >= MAX_RIGIDBODIES) {
fprintf(stderr,
"Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n",
rigidBodies[i].id, MAX_RIGIDBODIES - 1);
exit(EXIT_FAILURE);
}
// Check if we want to transmit (follow) this rigid
if (aircrafts[rigidBodies[i].id].ac_id == 0) {
continue;
}
// When we don track anymore and timeout or start tracking
if (rigidBodies[i].nSamples < 1
&& aircrafts[rigidBodies[i].id].connected
&& (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
aircrafts[rigidBodies[i].id].connected = FALSE;
fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
} else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
// Check if we still track the rigid
if (rigidBodies[i].nSamples < 1) {
continue;
}
// Update the last tracked
aircrafts[rigidBodies[i].id].connected = TRUE;
aircrafts[rigidBodies[i].id].lastSample = natnet_latency;
// Defines to make easy use of paparazzi math
struct EnuCoor_d pos, speed;
struct EcefCoor_d ecef_pos;
struct LlaCoor_d lla_pos;
struct DoubleQuat orient;
struct DoubleEulers orient_eulers;
// Add the Optitrack angle to the x and y positions
pos.x = cos(tracking_offset_angle) * rigidBodies[i].x - sin(tracking_offset_angle) * rigidBodies[i].y;
pos.y = sin(tracking_offset_angle) * rigidBodies[i].x + cos(tracking_offset_angle) * rigidBodies[i].y;
pos.z = rigidBodies[i].z;
// Convert the position to ecef and lla based on the Optitrack LTP
ecef_of_enu_point_d(&ecef_pos , &tracking_ltp , &pos);
lla_of_ecef_d(&lla_pos, &ecef_pos);
// Check if we have enough samples to estimate the velocity
rigidBodies[i].nVelocityTransmit++;
if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
// Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples)
double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) /
((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time;
rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time;
rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
// Add the Optitrack angle to the x and y velocities
speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x - sin(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x + cos(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.z = rigidBodies[i].vel_z;
// Conver the speed to ecef based on the Optitrack LTP
ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel , &tracking_ltp , &speed);
}
// Copy the quaternions and convert to euler angles for the heading
orient.qi = rigidBodies[i].qw;
orient.qx = rigidBodies[i].qx;
orient.qy = rigidBodies[i].qy;
orient.qz = rigidBodies[i].qz;
double_eulers_of_quat(&orient_eulers, &orient);
// Calculate the heading by adding the Natnet offset angle and normalizing it
double heading = -orient_eulers.psi + 90.0 / 57.6 -
tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
NormRadAngle(heading);
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id,
aircrafts[rigidBodies[i].id].ac_id
, rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
/* Construct time of time of week (tow) */
time_t now;
time(&now);
struct tm *ts = localtime(&now);
uint32_t tow = (ts->tm_wday - 1)*(24*60*60*1000) + ts->tm_hour*(60*60*1000) + ts->tm_min*(60*1000) + ts->tm_sec*1000;
// Transmit the REMOTE_GPS packet on the ivy bus (either small or big)
if (small_packets) {
/* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into
* a single integer. The z axis is considered unsigned and only the latter 10 LSBs are
* used.
*/
uint32_t pos_xyz = 0;
// check if position within limits
if (fabs(pos.x * 100.) < pow(2, 10)) {
pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
} else {
fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x);
pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm
}
if (fabs(pos.y * 100.) < pow(2, 10)) {
pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
} else {
fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y);
pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm
}
if (pos.z * 100. < pow(2, 10) && pos.z > 0.) {
pos_xyz |= (((uint32_t)(fabs(pos.z) * 100.0)) & 0x3FF); // bits 9-0 z position in cm
} else if (pos.z > 0.) {
fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z);
pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm
}
// printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z);
/* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into
* a single integer.
*/
uint32_t speed_xyz = 0;
// check if speed within limits
if (fabs(speed.x * 100) < pow(2, 10)) {
speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
} else {
fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x);
speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
}
if (fabs(speed.y * 100) < pow(2, 10)) {
speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
} else {
fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y);
speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
}
if (fabs(speed.z * 100) < pow(2, 9)) {
speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s
} else {
fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z);
speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s
}
/* The gps_small msg should always be less than 20 bytes including the pprz header of 6 bytes
* This is primarily due to the maximum packet size of the bluetooth msgs of 19 bytes
* increases the probability that a complete message will be accepted
*/
IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d",
(int16_t)(heading * 10000), // int16_t heading in rad*1e4 (2 bytes)
pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes)
speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes)
tow, // uint32_t time of day
aircrafts[rigidBodies[i].id].ac_id); // uint8 rigid body ID (1 byte)
} else {
IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
(int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM
(int)(DegOfRad(lla_pos.lat) * 10000000.0), //int32 LLA latitude in deg*1e7
(int)(DegOfRad(lla_pos.lon) * 10000000.0), //int32 LLA longitude in deg*1e7
(int)(lla_pos.alt * 1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in cm/s
(int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in cm/s
(int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in cm/s
tow,
(int)(heading * 10000000.0)); //int32 Course in rad*1e7
}
if (must_log) {
if (log_exists == 0) {
fp = fopen(nameOfLogfile, "w");
log_exists = 1;
}
if (fp == NULL) {
printf("I couldn't open file for writing.\n");
exit(0);
} else {
struct timeval cur_time;
gettimeofday(&cur_time, NULL);
fprintf(fp, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
(int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM
(int)(DegOfRad(lla_pos.lat) * 1e7), //int32 LLA latitude in deg*1e7
(int)(DegOfRad(lla_pos.lon) * 1e7), //int32 LLA longitude in deg*1e7
(int)(lla_pos.alt*1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in cm/s
(int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in cm/s
(int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in cm/s
(int)(heading * 10000000.0), //int32 Course in rad*1e7
(int)cur_time.tv_sec,
(int)cur_time.tv_usec);
}
}
// Reset the velocity differentiator if we calculated the velocity
if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
rigidBodies[i].vel_x = 0;
rigidBodies[i].vel_y = 0;
rigidBodies[i].vel_z = 0;
rigidBodies[i].nVelocitySamples = 0;
rigidBodies[i].totalVelocitySamples = 0;
rigidBodies[i].nVelocityTransmit = 0;
}
rigidBodies[i].nSamples = 0;
}
return TRUE;
}
void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) {
struct EnuCoor_d enu;
ENU_OF_TO_NED(enu, *ned);
ecef_of_enu_point_d(ecef, def, &enu);
}
/** The transmitter periodic function */
gboolean timeout_transmit_callback(gpointer data) {
int i;
// Loop trough all the available rigidbodies (TODO: optimize)
for(i = 0; i < MAX_RIGIDBODIES; i++) {
// Check if ID's are correct
if(rigidBodies[i].id >= MAX_RIGIDBODIES) {
fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1);
exit(EXIT_FAILURE);
}
// Check if we want to transmit (follow) this rigid
if(aircrafts[rigidBodies[i].id].ac_id == 0)
continue;
// When we don track anymore and timeout or start tracking
if(rigidBodies[i].nSamples < 1
&& aircrafts[rigidBodies[i].id].connected
&& (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
aircrafts[rigidBodies[i].id].connected = FALSE;
fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
// Check if we still track the rigid
if(rigidBodies[i].nSamples < 1)
continue;
// Update the last tracked
aircrafts[rigidBodies[i].id].connected = TRUE;
aircrafts[rigidBodies[i].id].lastSample = natnet_latency;
// Defines to make easy use of paparazzi math
struct EnuCoor_d pos, speed;
struct EcefCoor_d ecef_pos;
struct LlaCoor_d lla_pos;
struct DoubleQuat orient;
struct DoubleEulers orient_eulers;
// Add the Optitrack angle to the x and y positions
pos.x = cos(tracking_offset_angle) * rigidBodies[i].x + sin(tracking_offset_angle) * rigidBodies[i].y;
pos.y = sin(tracking_offset_angle) * rigidBodies[i].x - cos(tracking_offset_angle) * rigidBodies[i].y;
pos.z = rigidBodies[i].z;
// Convert the position to ecef and lla based on the Optitrack LTP
ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos);
lla_of_ecef_d(&lla_pos, &ecef_pos);
// Check if we have enough samples to estimate the velocity
rigidBodies[i].nVelocityTransmit++;
if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
// Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples)
double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) /
((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time;
rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time;
rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
// Add the Optitrack angle to the x and y velocities
speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x + sin(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x - cos(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.z = rigidBodies[i].vel_z;
// Conver the speed to ecef based on the Optitrack LTP
ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed);
}
// Copy the quaternions and convert to euler angles for the heading
orient.qi = rigidBodies[i].qw;
orient.qx = rigidBodies[i].qx;
orient.qy = rigidBodies[i].qy;
orient.qz = rigidBodies[i].qz;
DOUBLE_EULERS_OF_QUAT(orient_eulers, orient);
// Calculate the heading by adding the Natnet offset angle and normalizing it
double heading = -orient_eulers.psi-tracking_offset_angle;
NormRadAngle(heading);
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id
, rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
// Transmit the REMOTE_GPS packet on the ivy bus
IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x*100.0), //int32 ECEF X in CM
(int)(ecef_pos.y*100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z*100.0), //int32 ECEF Z in CM
(int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7
(int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7
(int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s
(int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s
(int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s
0,
(int)(heading*10000000.0)); //int32 Course in rad*1e7
// Reset the velocity differentiator if we calculated the velocity
if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
rigidBodies[i].vel_x = 0;
rigidBodies[i].vel_y = 0;
rigidBodies[i].vel_z = 0;
rigidBodies[i].nVelocitySamples = 0;
rigidBodies[i].totalVelocitySamples = 0;
rigidBodies[i].nVelocityTransmit = 0;
}
rigidBodies[i].nSamples = 0;
}
return TRUE;
}
