void test_read_performance(PSMove *move, enum TestMode mode) {
int round, max_rounds = 3;
float sum = 0.;
/* Only enable rate limiting if we test for smart updates */
psmove_set_rate_limiting(move, mode == SMART_UPDATES);
if (mode == STATIC_UPDATES) {
psmove_set_leds(move, 0, 255, 0);
}
for (round=0; round<max_rounds; round++) {
long packet_loss = 0;
long started = psmove_util_get_ticks();
long reads = 0;
int old_sequence = -1, sequence;
while (reads < 1000) {
if (mode != NO_UPDATES) {
if (mode != STATIC_UPDATES) {
psmove_set_leds(move, reads%255, reads%255, reads%255);
}
psmove_update_leds(move);
}
if (reads % 20 == 0) {
fprintf(stderr, "\r%c", "-\\|/"[(reads/20)%4]);
}
while (!(sequence = psmove_poll(move))) {}
if (old_sequence != -1) {
if (sequence != ((old_sequence % 16) + 1)) {
packet_loss++;
/*fprintf(stderr, " %d->%d", old_sequence, sequence);*/
fputc('x', stderr);
}
}
old_sequence = sequence;
reads++;
}
fputc('\r', stderr);
long diff = psmove_util_get_ticks() - started;
double reads_per_second = 1000. * (double)reads / (double)diff;
printf("%ld reads in %ld ms = %f reads/sec "
"(%ldx seq jump = %.2f %%)\n",
reads, diff, reads_per_second, packet_loss,
100. * (double)packet_loss / (double)reads);
sum += reads_per_second;
}
printf("=====\n");
printf("Mean over %d rounds: %f reads/sec\n", max_rounds,
sum/(double)max_rounds);
}
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;
}
//-- 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;
}
IplImage *
camera_control_query_frame(CameraControl* cc)
{
IplImage* result;
#if defined(CAMERA_CONTROL_USE_CL_DRIVER)
// assign buffer-pointer to address of buffer
cvGetRawData(cc->frame4ch, &cc->pCapBuffer, 0, 0);
CLEyeCameraGetFrame(cc->camera, cc->pCapBuffer, 2000);
// convert 4ch image to 3ch image
const int from_to[] = { 0, 0, 1, 1, 2, 2 };
const CvArr** src = (const CvArr**) &cc->frame4ch;
CvArr** dst = (CvArr**) &cc->frame3ch;
cvMixChannels(src, 1, dst, 1, from_to, 3);
result = cc->frame3ch;
#else
long start = psmove_util_get_ticks();
result = cvQueryFrame(cc->capture);
psmove_DEBUG("cvQueryFrame: %ld ms\n", psmove_util_get_ticks() - start);
#endif
#if defined(PSMOVE_USE_DEINTERLACE)
/**
* Dirty hack follows:
* - Clone image
* - Hack internal variables to make an image of all odd lines
**/
IplImage *tmp = cvCloneImage(result);
tmp->imageData += tmp->widthStep; // odd lines
tmp->widthStep *= 2;
tmp->height /= 2;
/**
* Use nearest-neighbor to be faster. In my tests, this does not
* cause a speed disadvantage, and tracking quality is still good.
*
* This will scale the half-height image "tmp" to the original frame
* size by doubling lines (so we can still do normal circle tracking).
**/
cvResize(tmp, result, CV_INTER_NN);
/**
* Need to revert changes in tmp from above, otherwise the call
* to cvReleaseImage would cause a crash.
**/
tmp->height = result->height;
tmp->widthStep = result->widthStep;
tmp->imageData -= tmp->widthStep; // odd lines
cvReleaseImage(&tmp);
#endif
// undistort image
if (cc->mapx && cc->mapy) {
cvRemap(result, cc->frame3chUndistort,
cc->mapx, cc->mapy,
CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS,
cvScalarAll(0));
result = cc->frame3chUndistort;
}
return result;
}
//-- 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;
}
void
psmove_orientation_update(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
int frame_half;
long now = psmove_util_get_ticks();
if (now - orientation_state->sample_freq_measure_start >= 1000)
{
float measured = ((float)orientation_state->sample_freq_measure_count) /
((float)(now-orientation_state->sample_freq_measure_start))*1000.f;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation_state->sample_freq = measured;
orientation_state->sample_freq_measure_start = now;
orientation_state->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation_state->sample_freq_measure_count += 2;
Eigen::Quaternionf quaternion_backup = orientation_state->quaternion;
float deltaT = 1.f / fmax(orientation_state->sample_freq, SAMPLE_FREQUENCY); // time delta = 1/frequency
for (frame_half=0; frame_half<2; frame_half++)
{
switch (orientation_state->fusion_type)
{
case OrientationFusion_None:
break;
case OrientationFusion_MadgwickIMU:
{
// Get the sensor data transformed by the sensor_transform
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_imu_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z));
}
break;
case OrientationFusion_MadgwickMARG:
{
PSMove_3AxisVector m = psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a = psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega = psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
_psmove_orientation_fusion_madgwick_marg_update(orientation_state, deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
case OrientationFusion_ComplementaryMARG:
{
PSMove_3AxisVector m=
psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_complementary_marg_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
}
if (!psmove_quaternion_is_valid(orientation_state->quaternion))
{
psmove_DEBUG("Orientation is NaN!");
orientation_state->quaternion = quaternion_backup;
}
}
}
void
psmove_orientation_update(PSMoveOrientation *orientation)
{
psmove_return_if_fail(orientation != NULL);
int frame;
long now = psmove_util_get_ticks();
if (now - orientation->sample_freq_measure_start >= 1000) {
float measured = ((float)orientation->sample_freq_measure_count) /
((float)(now-orientation->sample_freq_measure_start))*1000.;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation->sample_freq = measured;
orientation->sample_freq_measure_start = now;
orientation->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation->sample_freq_measure_count += 2;
psmove_get_magnetometer_vector(orientation->move,
&orientation->output[6],
&orientation->output[7],
&orientation->output[8]);
float q0 = orientation->quaternion[0];
float q1 = orientation->quaternion[1];
float q2 = orientation->quaternion[2];
float q3 = orientation->quaternion[3];
for (frame=0; frame<2; frame++) {
psmove_get_accelerometer_frame(orientation->move, frame,
&orientation->output[0],
&orientation->output[1],
&orientation->output[2]);
psmove_get_gyroscope_frame(orientation->move, frame,
&orientation->output[3],
&orientation->output[4],
&orientation->output[5]);
#if defined(PSMOVE_WITH_MADGWICK_AHRS)
MadgwickAHRSupdate(orientation->quaternion,
orientation->sample_freq,
0.5f,
/* Gyroscope */
orientation->output[3],
orientation->output[5],
-orientation->output[4],
/* Accelerometer */
orientation->output[0],
orientation->output[2],
-orientation->output[1],
/* Magnetometer */
orientation->output[6],
orientation->output[8],
orientation->output[7]
);
#else
psmove_DEBUG("Built without Madgwick AHRS - no orientation tracking");
return;
#endif
if (isnan(orientation->quaternion[0]) ||
isnan(orientation->quaternion[1]) ||
isnan(orientation->quaternion[2]) ||
isnan(orientation->quaternion[3])) {
psmove_DEBUG("Orientation is NaN!");
orientation->quaternion[0] = q0;
orientation->quaternion[1] = q1;
orientation->quaternion[2] = q2;
orientation->quaternion[3] = q3;
}
}
}