@Override public void timestampedDataReceived( long curr_system_timestamp, long curr_sensor_timestamp, Object kind) { if (timestamped && (kind.getClass() == AHRS.DeviceDataType.class)) { double process_value; switch (timestamped_src) { case YAW: process_value = navx_device.getYaw(); break; case PITCH: process_value = navx_device.getPitch(); break; case ROLL: process_value = navx_device.getRoll(); break; case COMPASS_HEADING: process_value = navx_device.getCompassHeading(); break; case FUSED_HEADING: process_value = navx_device.getFusedHeading(); break; case LINEAR_ACCEL_X: process_value = navx_device.getWorldLinearAccelX(); break; case LINEAR_ACCEL_Y: process_value = navx_device.getWorldLinearAccelY(); break; case LINEAR_ACCEL_Z: process_value = navx_device.getWorldLinearAccelZ(); break; default: process_value = 0.0; break; } int num_missed_samples = 0; /* TODO */ last_system_timestamp = curr_system_timestamp; last_sensor_timestamp = curr_sensor_timestamp; double output = this.stepController(process_value, num_missed_samples); synchronized (sync_event) { sync_event.notify(); } } }