Ejemplo n.º 1
0
/** 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;
}
Ejemplo n.º 2
0
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);
}
Ejemplo n.º 3
0
/** 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;
}