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; } } }