void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct EcefCoor_f in_f; in_f.x = M_OF_CM((float)in->x); in_f.y = M_OF_CM((float)in->y); in_f.z = M_OF_CM((float)in->z); /* calls the floating point transformation */ struct LlaCoor_f out_f; lla_of_ecef_f(&out_f, &in_f); /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_f.lon)); out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_f.lat)); out->alt = (int32_t)MM_OF_M(out_f.alt); #else // use double precision by default /* convert our input to floating point */ struct EcefCoor_d in_d; in_d.x = M_OF_CM((double)in->x); in_d.y = M_OF_CM((double)in->y); in_d.z = M_OF_CM((double)in->z); /* calls the floating point transformation */ struct LlaCoor_d out_d; lla_of_ecef_d(&out_d, &in_d); /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon)); out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat)); out->alt = (int32_t)MM_OF_M(out_d.alt); #endif }
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) { if (time < gps->next_update) return; /* * simulate speed sensor */ struct DoubleVect3 cur_speed_reading; VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel); /* add a gaussian noise */ double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel); /* * simulate position sensor */ /* compute gps error readings */ struct DoubleVect3 pos_error; VECT3_COPY(pos_error, gps->pos_bias_initial); /* add a gaussian noise */ double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev); /* update random walk bias and add it to error*/ double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.); VECT3_ADD(pos_error, gps->pos_bias_random_walk_value); /* add error to current pos reading */ struct DoubleVect3 cur_pos_reading; VECT3_COPY(cur_pos_reading, fdm.ecef_pos); VECT3_ADD(cur_pos_reading, pos_error); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos); /* * simulate lla pos */ /* convert current ecef reading to lla */ struct LlaCoor_d cur_lla_reading; lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos); double cur_hmsl_reading = fdm.hmsl; UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl); gps->next_update += NPS_GPS_DT; gps->data_available = TRUE; }
void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) { /* convert our input to floating point */ struct EcefCoor_d in_d; in_d.x = M_OF_CM((double)in->x); in_d.y = M_OF_CM((double)in->y); in_d.z = M_OF_CM((double)in->z); /* calls the floating point transformation */ struct LlaCoor_d out_d; lla_of_ecef_d(&out_d, &in_d); /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon)); out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat)); out->alt = (int32_t)MM_OF_M(out_d.alt); }
void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) { /* store the origin of the tangeant plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_d(&def->lla, &def->ecef); /* store the rotation matrix */ const double sin_lat = sin(def->lla.lat); const double cos_lat = cos(def->lla.lat); const double sin_lon = sin(def->lla.lon); const double cos_lon = cos(def->lla.lon); def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0.; def->ltp_of_ecef.m[3] = -sin_lat*cos_lon; def->ltp_of_ecef.m[4] = -sin_lat*sin_lon; def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = cos_lat*cos_lon; def->ltp_of_ecef.m[7] = cos_lat*sin_lon; def->ltp_of_ecef.m[8] = sin_lat; }
/** Convert a ECEF to LLA. * @param[out] out LLA in degrees*1e7 and mm above ellipsoid * @param[in] in ECEF in cm */ void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct EcefCoor_f in_f; ECEF_FLOAT_OF_BFP(in_f, *in); /* calls the floating point transformation */ struct LlaCoor_f out_f; lla_of_ecef_f(&out_f, &in_f); /* convert the output to fixed point */ LLA_BFP_OF_REAL(*out, out_f); #else // use double precision by default /* convert our input to floating point */ struct EcefCoor_d in_d; ECEF_DOUBLE_OF_BFP(in_d, *in); /* calls the floating point transformation */ struct LlaCoor_d out_d; lla_of_ecef_d(&out_d, &in_d); /* convert the output to fixed point */ LLA_BFP_OF_REAL(*out, out_d); #endif }
/** 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; }
/** 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; }