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; }