PSMove_3AxisVector
psmove_orientation_get_gyroscope_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
float omega_x, omega_y, omega_z;
psmove_get_gyroscope_frame(orientation_state->move, frame, &omega_x, &omega_y, &omega_z);
// Apply the "identity pose" transform
PSMove_3AxisVector omega = psmove_3axisvector_xyz(omega_x, omega_y, omega_z);
omega= psmove_3axisvector_apply_transform(&omega, &orientation_state->sensor_transform);
return omega;
}
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;
}
void
PSMoveAPI::update()
{
for (auto &c: controllers) {
if (c->connected && !c->sent_connect) {
if (receiver->connect != nullptr) {
// Send initial connect event
receiver->connect(&c->controller, user_data);
}
c->sent_connect = true;
}
if (!c->connected && !c->sent_disconnect) {
if (receiver->disconnect != nullptr) {
// Send disconnect event
receiver->disconnect(&c->controller, user_data);
}
c->sent_disconnect = true;
}
if (!c->connected) {
// Done with this controller (TODO: Can we check reconnect?)
continue;
}
auto read_move = c->read_move();
if (read_move == nullptr) {
// We don't have a handle that supports reading (USB-only connection);
// we call update exactly once (no new data will be reported), so that
// the update method can change the LED and rumble values
if (receiver->update != nullptr) {
receiver->update(&c->controller, user_data);
}
} else {
while (psmove_poll(read_move)) {
if (receiver->update != nullptr) {
int previous = c->controller.buttons;
c->controller.buttons = psmove_get_buttons(read_move);
c->controller.pressed = c->controller.buttons & ~previous;
c->controller.released = previous & ~c->controller.buttons;
c->controller.trigger = float(psmove_get_trigger(read_move)) / 255.f;
psmove_get_accelerometer_frame(read_move, Frame_SecondHalf,
&c->controller.accelerometer.x,
&c->controller.accelerometer.y,
&c->controller.accelerometer.z);
psmove_get_gyroscope_frame(read_move, Frame_SecondHalf,
&c->controller.gyroscope.x,
&c->controller.gyroscope.y,
&c->controller.gyroscope.z);
psmove_get_magnetometer_vector(read_move,
&c->controller.magnetometer.x,
&c->controller.magnetometer.y,
&c->controller.magnetometer.z);
c->controller.battery = psmove_get_battery(read_move);
receiver->update(&c->controller, user_data);
}
}
}
auto write_move = c->write_move();
if (write_move != nullptr) {
psmove_set_leds(write_move,
uint32_t(255 * c->controller.color.r),
uint32_t(255 * c->controller.color.g),
uint32_t(255 * c->controller.color.b));
psmove_set_rumble(write_move, uint32_t(255 * c->controller.rumble));
if (psmove_update_leds(write_move) == Update_Failed) {
if (read_move != nullptr && write_move != read_move) {
// Disconnected from USB, but we read from Bluetooth, so we
// can just give up the USB connection and use Bluetooth
psmove_disconnect(c->move_usb), c->move_usb = nullptr;
// Now that USB is gone, clear the controller's USB flag
c->controller.usb = 0;
} else {
c->connected = false;
}
}
}
}
}
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;
}
}
}
