PSMoveOrientation * psmove_orientation_new(PSMove *move) { psmove_return_val_if_fail(move != NULL, NULL); if (!psmove_has_calibration(move)) { psmove_DEBUG("Can't create orientation - no calibration!\n"); return NULL; } PSMoveOrientation *orientation = calloc(1, sizeof(PSMoveOrientation)); orientation->move = move; /* Initial sampling frequency */ orientation->sample_freq = 120.0f; /* Measurement */ orientation->sample_freq_measure_start = psmove_util_get_ticks(); orientation->sample_freq_measure_count = 0; /* Initial quaternion */ orientation->quaternion[0] = 1.f; orientation->quaternion[1] = 0.f; orientation->quaternion[2] = 0.f; orientation->quaternion[3] = 0.f; return orientation; }
int pair(const char *custom_addr) { if (!psmove_init(PSMOVE_CURRENT_VERSION)) { fprintf(stderr, "PS Move API init failed (wrong version?)\n"); exit(1); } int count = psmove_count_connected(); int i; PSMove *move; int result = 0; printf("Connected controllers: %d\n", count); for (i=0; i<count; i++) { move = psmove_connect_by_id(i); if (move == NULL) { printf("Error connecting to PSMove #%d\n", i+1); result = 1; continue; } if (psmove_connection_type(move) != Conn_Bluetooth) { printf("PSMove #%d connected via USB.\n", i+1); int result = 0; if (custom_addr != NULL) { result = psmove_pair_custom(move, custom_addr); } else { result = psmove_pair(move); } if (result) { printf("Pairing of #%d succeeded!\n", i+1); char *serial = psmove_get_serial(move); printf("Controller address: %s\n", serial); free(serial); } else { printf("Pairing of #%d failed.\n", i+1); } if (psmove_has_calibration(move)) { printf("Calibration data available and saved.\n"); } else { printf("Error reading/loading calibration data.\n"); } } else { printf("Ignoring non-USB PSMove #%d\n", i+1); } psmove_disconnect(move); } psmove_shutdown(); return result; }
void run() { PSMove *move; /* 6 values = ax, ay, az, gx, gy, gz (accel + gyro) */ float gx, gy, gz; int i = 0; qreal rate = 1.; assert((move = psmove_connect()) != NULL); assert(psmove_connection_type(move) == Conn_Bluetooth); assert(psmove_has_calibration(move)); while (true) { if (psmove_poll(move)) { psmove_get_gyroscope_frame(move, Frame_SecondHalf, &gx, &gy, &gz); /** * Playback rate is calculated based on the RPM of the * controller on the Z axis and the normal * playback rate of the turntable (see above). **/ rate = RAD_PER_S_TO_RPM(gz) / TURNTABLE_RPM; /* Rate-limiting the updates of the playback rate */ if (i % READ_FRAME_UPDATE_RATE == 0) { /** * On Linux, the rate must not be negative. It might be * possible that on Mac OS X or Windows, the rate can * also be negative (depends on the backend). **/ if (rate > 0 && rate != player->playbackRate()) { qint64 pos = player->position(); player->setPlaybackRate(rate); player->setPosition(pos); } } i++; } } psmove_disconnect(move); }
int main(int argc, char* argv[]) { PSMove *move; if (!psmove_init(PSMOVE_CURRENT_VERSION)) { fprintf(stderr, "PS Move API init failed (wrong version?)\n"); exit(1); } move = psmove_connect(); if (move == NULL) { fprintf(stderr, "Could not connect to controller.\n"); return EXIT_FAILURE; } assert(psmove_has_calibration(move)); if (psmove_connection_type(move) == Conn_Bluetooth) { float ax, ay, az, gx, gy, gz; while (1) { int res = psmove_poll(move); if (res) { psmove_get_accelerometer_frame(move, Frame_SecondHalf, &ax, &ay, &az); psmove_get_gyroscope_frame(move, Frame_SecondHalf, &gx, &gy, &gz); printf("A: %5.2f %5.2f %5.2f ", ax, ay, az); printf("G: %6.2f %6.2f %6.2f ", gx, gy, gz); printf("\n"); } } } psmove_disconnect(move); psmove_shutdown(); return EXIT_SUCCESS; }
//-- public methods ----- PSMoveOrientation * psmove_orientation_new(PSMove *move) { psmove_return_val_if_fail(move != NULL, NULL); if (!psmove_has_calibration(move)) { psmove_DEBUG("Can't create orientation - no calibration!\n"); return NULL; } PSMoveOrientation *orientation_state = (PSMoveOrientation *)calloc(1, sizeof(PSMoveOrientation)); orientation_state->move = move; /* Initial sampling frequency */ orientation_state->sample_freq = SAMPLE_FREQUENCY; /* Measurement */ orientation_state->sample_freq_measure_start = psmove_util_get_ticks(); orientation_state->sample_freq_measure_count = 0; /* Initial quaternion */ orientation_state->quaternion.setIdentity(); orientation_state->reset_quaternion.setIdentity(); /* Initialize data specific to the selected filter */ psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_ComplementaryMARG); /* Set the transform used re-orient the calibration data used by the orientation fusion algorithm */ psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_laying_flat); /* Set the transform used re-orient the sensor data used by the orientation fusion algorithm */ psmove_orientation_set_sensor_data_transform(orientation_state, k_psmove_sensor_transform_opengl); return orientation_state; }
//-- public methods ---- int main(int arg, char** args) { if (!psmove_init(PSMOVE_CURRENT_VERSION)) { fprintf(stderr, "PS Move API init failed (wrong version?)\n"); exit(1); } int i; int count = psmove_count_connected(); printf("Connected controllers for calibration: %d\n", count); for (i=0; i<count; i++) { PSMove *move = psmove_connect_by_id(i); if (move == NULL) { printf("Failed to open PS Move #%d\n", i); continue; } // Make sure accelerometer is calibrated // Otherwise we can't tell if the controller is sitting upright if (psmove_has_calibration(move)) { if (psmove_connection_type(move) == Conn_Bluetooth) { float old_range = 0; enum eCalibrationState calibration_state = Calibration_MeasureBExtents; // Reset existing magnetometer calibration state psmove_reset_magnetometer_calibration(move); printf("Calibrating PS Move #%d\n", i); printf("[Step 1 of 2: Measuring extents of the magnetometer]\n"); printf("Rotate the controller in all directions.\n"); fflush(stdout); while (calibration_state == Calibration_MeasureBExtents) { if (psmove_poll(move)) { unsigned int pressed, released; psmove_get_button_events(move, &pressed, &released); /* This call updates min/max values if exceeding previously stored values */ psmove_get_magnetometer_3axisvector(move, NULL); /* Returns the minimum delta (max-min) across dimensions (x, y, z) */ float range = psmove_get_magnetometer_calibration_range(move); /* Update the LEDs to indicate progress */ int percentage = (int)(100.f * range / LED_RANGE_TARGET); if (percentage > 100) { percentage = 100; } else if (percentage < 0) { percentage = 0; } psmove_set_leds( move, (unsigned char)((255 * (100 - percentage)) / 100), (unsigned char)((255 * percentage) / 100), 0); psmove_update_leds(move); if (pressed & Btn_MOVE) { psmove_set_leds(move, 0, 0, 0); psmove_update_leds(move); // Move on to the calibration_state = Calibration_WaitForGravityAlignment; } else if (range > old_range) { old_range = range; printf("\rPress the MOVE button when value stops changing: %.1f", range); fflush(stdout); } } } printf("\n\n[Step 2 of 2: Measuring default magnetic field direction]\n"); printf("Stand the controller on a level surface with the Move button facing you.\n"); printf("This will be the default orientation of the move controller.\n"); printf("Measurement will start once the controller is aligned with gravity and stable.\n"); fflush(stdout); //TODO: Wait for accelerometer stabilization and button press // Sample the magnetometer field for several frames and average // Store as the default magnetometer direction while (calibration_state != Calibration_Complete) { int stable_start_time = psmove_util_get_ticks(); PSMove_3AxisVector identity_pose_average_m_vector = *k_psmove_vector_zero; int sample_count = 0; while (calibration_state == Calibration_WaitForGravityAlignment) { if (psmove_poll(move)) { if (is_move_stable_and_aligned_with_gravity(move)) { int current_time = psmove_util_get_ticks(); int stable_duration = (current_time - stable_start_time); if ((current_time - stable_start_time) >= STABILIZE_WAIT_TIME_MS) { calibration_state = Calibration_MeasureBDirection; } else { printf("\rStable for: %dms/%dms ", stable_duration, STABILIZE_WAIT_TIME_MS); } } else { stable_start_time = psmove_util_get_ticks(); printf("\rMove Destabilized! Waiting for stabilization."); } } } printf("\n\nMove stabilized. Starting magnetometer sampling.\n"); while (calibration_state == Calibration_MeasureBDirection) { if (psmove_poll(move)) { if (is_move_stable_and_aligned_with_gravity(move)) { PSMove_3AxisVector m; psmove_get_magnetometer_3axisvector(move, &m); psmove_3axisvector_normalize_with_default(&m, k_psmove_vector_zero); identity_pose_average_m_vector = psmove_3axisvector_add(&identity_pose_average_m_vector, &m); sample_count++; if (sample_count > DESIRED_MAGNETOMETER_SAMPLE_COUNT) { float N = (float)sample_count; // The average magnetometer direction was recorded while the controller // was in the cradle pose identity_pose_average_m_vector = psmove_3axisvector_divide_by_scalar_unsafe(&identity_pose_average_m_vector, N); psmove_set_magnetometer_calibration_direction(move, &identity_pose_average_m_vector); calibration_state = Calibration_Complete; } else { printf("\rMagnetometer Sample: %d/%d ", sample_count, DESIRED_MAGNETOMETER_SAMPLE_COUNT); } } else { calibration_state = Calibration_WaitForGravityAlignment; printf("\rMove Destabilized! Waiting for stabilization.\n"); } } } } psmove_save_magnetometer_calibration(move); } else { printf("Ignoring non-Bluetooth PS Move #%d\n", i); } } else { char *serial= psmove_get_serial(move); printf("\nController #%d has bad calibration file (accelerometer values won't be correct).\n", i); if (serial != NULL) { printf("Please delete %s.calibration and re-run \"psmove pair\" with the controller plugged into usb.", serial); free(serial); } else { printf("Please re-run \"psmove pair\" with the controller plugged into usb."); } } printf("\nFinished PS Move #%d\n", i); psmove_disconnect(move); } return 0; }