void estimator_update_ir_estim( void ) {
static float last_hspeed_dir;
static uint32_t last_t; /* ms */
static bool_t initialized = FALSE;
static float sum_xy, sum_xx;
if (initialized) {
float dt = (float)(gps_itow - last_t) / 1e3;
if (dt > 0.1) { // Against division by zero
float dpsi = (estimator_hspeed_dir - last_hspeed_dir);
NormRadAngle(dpsi);
estimator_rad = dpsi/dt*NOMINAL_AIRSPEED/G; /* tan linearized */
NormRadAngle(estimator_rad);
estimator_ir = (float)ir_roll;
float absphi = fabs(estimator_rad);
if (absphi < 1.0 && absphi > 0.05 && (- ir_contrast/2 < ir_roll && ir_roll < ir_contrast/2)) {
sum_xy = estimator_rad * estimator_ir + RHO * sum_xy;
sum_xx = estimator_ir * estimator_ir + RHO * sum_xx;
#if defined IR_RAD_OF_IR_MIN_VALUE & defined IR_RAD_OF_IR_MAX_VALUE
float result = sum_xy / sum_xx;
if (result < IR_RAD_OF_IR_MIN_VALUE)
estimator_rad_of_ir = IR_RAD_OF_IR_MIN_VALUE;
else if (result > IR_RAD_OF_IR_MAX_VALUE)
estimator_rad_of_ir = IR_RAD_OF_IR_MAX_VALUE;
else
estimator_rad_of_ir = result;
#else
estimator_rad_of_ir = sum_xy / sum_xx;
#endif
}
}
} else {
initialized = TRUE;
float init_ir2 = ir_contrast;
init_ir2 = init_ir2*init_ir2;
sum_xy = INIT_WEIGHT * estimator_rad_of_ir * init_ir2;
sum_xx = INIT_WEIGHT * init_ir2;
}
last_hspeed_dir = estimator_hspeed_dir;
last_t = gps_itow;
}
/**
* \brief
*
*/
void h_ctl_course_loop ( void ) {
static float last_err;
// Ground path error
float err = estimator_hspeed_dir - h_ctl_course_setpoint;
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
+ h_ctl_course_pgain * speed_depend_nav * err
+ h_ctl_course_dgain * d_err;
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
/**
* \brief
*
*/
void h_ctl_course_loop ( void ) {
static float last_err;
// Ground path error
float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
+ h_ctl_course_pgain * speed_depend_nav * err
+ h_ctl_course_dgain * d_err;
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
float sign_radius = radius > 0 ? 1 : -1;
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
NormRadAngle(trigo_diff);
nav_circle_radians += trigo_diff;
trigo_diff *= - sign_radius;
if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
nav_circle_radians_no_rewind += trigo_diff;
}
}
float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
float dist_carrot = CARROT * NOMINAL_AIRSPEED;
radius += -nav_shift;
float abs_radius = fabs(radius);
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI / 4);
carrot_angle = Max(carrot_angle, M_PI / 16);
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE) {
radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
}
fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
y + sinf(alpha_carrot)*radius_carrot);
nav_in_circle = true;
nav_circle_x = x;
nav_circle_y = y;
nav_circle_radius = radius;
}
//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
desired_x = x;
desired_y = y;
if (nav_mode == NAV_MODE_COURSE) {
h_ctl_course_setpoint = atan2(x - estimator_x, y - estimator_y);
if (h_ctl_course_setpoint < 0.)
h_ctl_course_setpoint += 2 * M_PI;
lateral_mode = LATERAL_MODE_COURSE;
} else {
float diff = atan2(x - estimator_x, y - estimator_y) - estimator_hspeed_dir;
NormRadAngle(diff);
BoundAbs(diff,M_PI/2.);
float s = sin(diff);
h_ctl_roll_setpoint = atan(2 * estimator_hspeed_mod*estimator_hspeed_mod * s * (-h_ctl_course_pgain) / (CARROT * NOMINAL_AIRSPEED * 9.81) );
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
}
//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
desired_x = x;
desired_y = y;
if (nav_mode == NAV_MODE_COURSE) {
h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
if (h_ctl_course_setpoint < 0.)
h_ctl_course_setpoint += 2 * M_PI;
lateral_mode = LATERAL_MODE_COURSE;
} else {
float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(diff);
BoundAbs(diff,M_PI/2.);
float s = sinf(diff);
float speed = *stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
}
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius) {
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x);
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
NormRadAngle(trigo_diff);
nav_circle_radians += trigo_diff;
}
float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y);
float dist_carrot = CARROT*NOMINAL_AIRSPEED;
float sign_radius = radius > 0 ? 1 : -1;
radius += -nav_shift;
float abs_radius = fabs(radius);
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI/4);
carrot_angle = Max(carrot_angle, M_PI/16);
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE)
radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius);
fly_to_xy(x+cos(alpha_carrot)*radius_carrot,
y+sin(alpha_carrot)*radius_carrot);
nav_in_circle = TRUE;
nav_circle_x = x;
nav_circle_y = y;
nav_circle_radius = radius;
}
/**
* \brief
*
*/
void h_ctl_course_loop(void)
{
static float last_err;
// Ground path error
float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
if (
(advance < 1.) && // Path speed is small
(stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
) {
/*
// rough crabangle approximation
float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
float wind_dir = atan2(wind_east,wind_north);
float wind_course = h_ctl_course_setpoint - wind_dir;
NormRadAngle(wind_course);
estimator_hspeed_dir = estimator_psi;
float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
//crab = estimator_hspeed_mod - estimator_psi;
NormRadAngle(crab);
*/
// Heading error
float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
NormRadAngle(herr);
if (advance < -0.5) { //<! moving in the wrong direction / big > 90 degree turn
err = herr;
} else if (advance < 0.) { //<!
err = (-advance) * 2. * herr;
} else {
err = advance * err;
}
// Reset differentiator when switching mode
//if (h_ctl_course_heading_mode == 0)
// last_err = err;
//h_ctl_course_heading_mode = 1;
}
/* else
{
// Reset differentiator when switching mode
if (h_ctl_course_heading_mode == 1)
last_err = err;
h_ctl_course_heading_mode = 0;
}
*/
#endif //STRONG_WIND
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
#ifdef H_CTL_COURSE_SLEW_INCREMENT
/* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
static float h_ctl_course_slew_rate = 0.;
float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
float half_nav_angle_saturation = nav_angle_saturation / 2.;
if (autopilot.launch) { /* prevent accumulator run-up on the ground */
if (err > half_nav_angle_saturation) {
h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate += h_ctl_course_slew_increment;
} else if (err < -half_nav_angle_saturation) {
h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
} else {
h_ctl_course_slew_rate = 0.;
}
}
#endif
float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
#if defined(AGR_CLIMB) && !USE_AIRSPEED
/** limit navigation during extreme altitude changes */
if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {
BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
/* altitude: z-up is positive -> positive error -> too low */
if (v_ctl_altitude_error > 0) {
nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
} else {
nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
}
cmd *= nav_ratio;
}
}
#endif
float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
#ifdef H_CTL_ROLL_SLEW
float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
BoundAbs(diff_roll, h_ctl_roll_slew);
h_ctl_roll_setpoint += diff_roll;
#else
h_ctl_roll_setpoint = roll_setpoint;
#endif
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
/** 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;
}
