Exemple #1
0
/*
  receive an update from the FDM
  This is a blocking function
 */
void JSBSim::recv_fdm(const struct sitl_input &input)
{
    FGNetFDM fdm;
    check_stdout();
    while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) {
        send_servos(input);
        check_stdout();
    }
    fdm.ByteSwap();

    accel_body = Vector3f(fdm.A_X_pilot, fdm.A_Y_pilot, fdm.A_Z_pilot) * FEET_TO_METERS;

    double p, q, r;
    SITL::convert_body_frame(degrees(fdm.phi), degrees(fdm.theta),
                             degrees(fdm.phidot), degrees(fdm.thetadot), degrees(fdm.psidot),
                             &p, &q, &r);
    gyro = Vector3f(p, q, r);

    velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS;
    location.lat = degrees(fdm.latitude) * 1.0e7;
    location.lng = degrees(fdm.longitude) * 1.0e7;
    location.alt = fdm.agl*100 + home.alt;
    dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
    airspeed = fdm.vcas * FEET_TO_METERS;

    // assume 1kHz for now
    time_now_us += 1000;
}
/*
  receive an update from the FDM
  This is a blocking function
 */
void last_letter::recv_fdm(const struct sitl_input &input)
{
    fdm_packet pkt;

    /*
      we re-send the servo packet every 0.1 seconds until we get a
      reply. This allows us to cope with some packet loss to the FDM
     */
    while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
        send_servos(input);
    }

    accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
    gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
    velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
    location.lat = pkt.latitude * 1.0e7;
    location.lng = pkt.longitude * 1.0e7;
    location.alt = pkt.altitude*1.0e2;
    dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);

    airspeed = pkt.airspeed;
    airspeed_pitot = pkt.airspeed;

    
    // auto-adjust to last_letter frame rate
    uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us;
    time_now_us += deltat_us;

    if (deltat_us < 1.0e4 && deltat_us > 0) {
        adjust_frame_time(1.0e6/deltat_us);
    }
    last_timestamp_us = pkt.timestamp_us;
}
Exemple #3
0
/*
  receive an update from the FDM
  This is a blocking function
 */
void Gazebo::recv_fdm(const struct sitl_input &input)
{
    fdm_packet pkt;

    /*
      we re-send the servo packet every 0.1 seconds until we get a
      reply. This allows us to cope with some packet loss to the FDM
     */
    while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
        send_servos(input);
    }

    // get imu stuff
    accel_body = Vector3f(pkt.imu_linear_acceleration_xyz[0],
                          pkt.imu_linear_acceleration_xyz[1],
                          pkt.imu_linear_acceleration_xyz[2]);

    gyro = Vector3f(pkt.imu_angular_velocity_rpy[0],
                    pkt.imu_angular_velocity_rpy[1],
                    pkt.imu_angular_velocity_rpy[2]);

    // compute dcm from imu orientation
    Quaternion quat(pkt.imu_orientation_quat[0],
                    pkt.imu_orientation_quat[1],
                    pkt.imu_orientation_quat[2],
                    pkt.imu_orientation_quat[3]);
    quat.rotation_matrix(dcm);

    double speedN =  pkt.velocity_xyz[0];
    double speedE =  pkt.velocity_xyz[1];
    double speedD =  pkt.velocity_xyz[2];
    velocity_ef = Vector3f(speedN, speedE, speedD);

    position = Vector3f(pkt.position_xyz[0],
                        pkt.position_xyz[1],
                        pkt.position_xyz[2]);


    // auto-adjust to simulation frame rate
    double deltat = pkt.timestamp - last_timestamp;
    time_now_us += deltat * 1.0e6;

    if (deltat < 0.01 && deltat > 0) {
        adjust_frame_time(1.0/deltat);
    }
    last_timestamp = pkt.timestamp;

    /* copied below from iris_ros.py */
    /*
    bearing = to_degrees(atan2(position.y, position.x));
    distance = math.sqrt(self.position.x**2 + self.position.y**2)
    (self.latitude, self.longitude) = util.gps_newpos(
      self.home_latitude, self.home_longitude, bearing, distance)
    self.altitude  = self.home_altitude - self.position.z
    velocity_body = self.dcm.transposed() * self.velocity
    self.accelerometer = self.accel_body.copy()
    */
}
Exemple #4
0
/*
  update the Gazebo simulation by one time step
 */
void Gazebo::update(const struct sitl_input &input)
{
    send_servos(input);
    recv_fdm(input);
    update_position();

    // update magnetic field
    update_mag_field_bf();
}
Exemple #5
0
/*
  update the last_letter simulation by one time step
 */
void last_letter::update(const struct sitl_input &input)
{
    send_servos(input);
    recv_fdm(input);
    sync_frame_time();

    update_position();
    // update magnetic field
    update_mag_field_bf();
}
Exemple #6
0
/*
  update the JSBSim simulation by one time step
 */
void JSBSim::update(const struct sitl_input &input)
{
    while (!initialised) {
        if (!create_templates() ||
            !start_JSBSim() ||
            !open_control_socket() ||
            !open_fdm_socket()) {
            time_now_us = 1;
            return;
        }
        initialised = true;
    }
    send_servos(input);
    recv_fdm(input);
    adjust_frame_time(1000);
    sync_frame_time();
    drain_control_socket();
}
/*
  receive an update from the FDM
  This is a blocking function
 */
void CRRCSim::recv_fdm(const struct sitl_input &input)
{
    fdm_packet pkt;

    /*
      we re-send the servo packet every 0.1 seconds until we get a
      reply. This allows us to cope with some packet loss to the FDM
     */
    while (!sock.recv(&pkt, sizeof(pkt), 100)) {
        send_servos(input);
    }

    accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
    gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
    velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);

    Location loc1, loc2;
    loc2.lat = pkt.latitude * 1.0e7;
    loc2.lng = pkt.longitude * 1.0e7;
    memset(&loc1, 0, sizeof(loc1));
    Vector2f posdelta = location_diff(loc1, loc2);
    position.x = posdelta.x;
    position.y = posdelta.y;
    position.z = -pkt.altitude;

    dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);

    // auto-adjust to crrcsim frame rate
    double deltat = pkt.timestamp - last_timestamp;
    time_now_us += deltat * 1.0e6;

    if (deltat < 0.01 && deltat > 0) {
        adjust_frame_time(1.0/deltat);
    }
    last_timestamp = pkt.timestamp;
}
/*
  update the last_letter simulation by one time step
 */
void last_letter::update(const struct sitl_input &input)
{
    send_servos(input);
    recv_fdm(input);
    sync_frame_time();
}
/*
  update the CRRCSim simulation by one time step
 */
void CRRCSim::update(const struct sitl_input &input)
{
    send_servos(input);
    recv_fdm(input);
    update_position();
}