int main(int argc, char* argv[]) { PSMove *move; if (psmove_count_connected() == 0) { printf("No controllers connected.\n"); return 2; } move = psmove_connect(); if (move == NULL) { printf("Cannot connect to controller.\n"); return 3; } printf("Serial: %s\n", psmove_get_serial(move)); if (psmove_connection_type(move) == Conn_USB) { _psmove_get_firmware(move); } else { printf("Please connect the controller via USB.\n"); } psmove_disconnect(move); return 0; }
PSMoveCalibration * psmove_calibration_new(PSMove *move) { PSMove_Data_BTAddr addr; char *serial; int i; PSMoveCalibration *calibration = (PSMoveCalibration*)calloc(1, sizeof(PSMoveCalibration)); calibration->move = move; if (psmove_connection_type(move) == Conn_USB) { _psmove_read_btaddrs(move, NULL, &addr); serial = _psmove_btaddr_to_string(addr); } else { serial = psmove_get_serial(move); } if (!serial) { psmove_CRITICAL("Could not determine serial from controller"); free(calibration); return NULL; } for (i=0; i<strlen(serial); i++) { if (serial[i] == ':') { serial[i] = '_'; } } char *template = malloc(strlen(serial) +
PSMoveAPI::PSMoveAPI(EventReceiver *receiver, void *user_data) : receiver(receiver) , user_data(user_data) , controllers() { std::map<std::string, std::vector<PSMove *>> moves; int n = psmove_count_connected(); for (int i=0; i<n; i++) { PSMove *move = psmove_connect_by_id(i); char *tmp = psmove_get_serial(move); std::string serial(tmp); free(tmp); moves[serial].emplace_back(move); } int i = 0; for (auto &kv: moves) { if (kv.second.size() == 2) { // Have two handles for this controller (USB + Bluetooth) controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], kv.second[1])); } else if (kv.second.size() == 1) { // Have only one handle for this controller controllers.emplace_back(new ControllerGlue(i++, kv.first, kv.second[0], nullptr)); } else { // FATAL } } }
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; }
int main(int argc, char* argv[]) { PSMove *moves[MAX_CONTROLLERS]; int controllers = psmove_count_connected(); int i; int quit = 0; for (i=0; i<controllers; i++) { moves[i] = psmove_connect_by_id(i); if (moves[i] == NULL) { fprintf(stderr, "Could not connect to controller #%d.\n", i); return EXIT_FAILURE; } } while (!quit) { for (i=0; i<controllers; i++) { int x, y, z; int seq = psmove_poll(moves[i]); if (seq) { int which; if (psmove_get_buttons(moves[i]) & Btn_PS) { quit = 1; break; } seq -= 1; for (which=0; which<2; which++) { printf("%d %s PSMOVE seq=%-2d", i, psmove_get_serial(moves[i]), seq*2+which); psmove_get_half_frame(moves[i], Sensor_Accelerometer, which, &x, &y, &z); printf(" aX=%-6d aY=%-6d aZ=%-6d", x, y, z); psmove_get_half_frame(moves[i], Sensor_Gyroscope, which, &x, &y, &z); printf(" gX=%-6d gY=%-6d gZ=%-6d", x, y, z); psmove_get_magnetometer(moves[i], &x, &y, &z); /* The y value of the magnetometer is inverted in linmctool */ printf(" mX=%-5d mY=%-5d mZ=%-5d", x, -y, z); printf("\n"); } } } } for (i=0; i<controllers; i++) { psmove_disconnect(moves[i]); } return 0; }
void move_daemon::dump_devices() { printf("%d connected\n", count()); #ifdef PSMOVE_DEBUG for (psmove_dev *dev: devs) { char *serial = psmove_get_serial(dev->move); printf("Device %d: %s\n", dev->assigned_id, serial); free(serial); } #endif fflush(stdout); }
int main(int argc, char* argv[]) { PSMove *move; int i; if (psmove_count_connected() == 0) { printf("No controllers connected.\n"); return 1; } move = psmove_connect(); if (move == NULL) { printf("Cannot connect to controller.\n"); return 2; } printf("Connection established.\n"); printf("Serial: %s\n", psmove_get_serial(move)); if (psmove_connection_type(move) != Conn_Bluetooth) { printf("Controller must be connected via Bluetooth.\n"); return 3; } PSMove_Data_AuthChallenge challenge = { 0x02, 0x00, 0x1D, 0x91, 0xE5, 0x81, 0x30, 0x6A, 0x22, 0x9A, 0xAB, 0x2E, 0x80, 0xB4, 0xED, 0x2E, 0xDE, 0x40, 0x0A, 0xF0, 0x02, 0xB0, 0x42, 0x8B, 0x01, 0x41, 0xB2, 0xA4, 0x3D, 0xE7, 0xD4, 0xBF, 0x05, 0x92 }; challenge[0] = 0; send_and_receive(move, &challenge); challenge[0] = 1; send_and_receive(move, &challenge); for (i = 1; i < 0x10; i++) { challenge[0] = 0; challenge[1] = (unsigned char)i; send_and_receive(move, &challenge); } memset(challenge, 0, sizeof(challenge)); send_and_receive(move, &challenge); psmove_disconnect(move); return 0; }
//-- 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; }
PSMoveCalibration * psmove_calibration_new(PSMove *move) { PSMove_Data_BTAddr addr; char *serial; int i; PSMoveCalibration *calibration = (PSMoveCalibration*)calloc(1, sizeof(PSMoveCalibration)); calibration->move = move; if (psmove_connection_type(move) == Conn_USB) { _psmove_read_btaddrs(move, NULL, &addr); serial = _psmove_btaddr_to_string(addr); } else { serial = psmove_get_serial(move); } if (!serial) { psmove_CRITICAL("Could not determine serial from controller"); free(calibration); return NULL; } for (i = 0; i<(int)strlen(serial); i++) { if (serial[i] == ':') { serial[i] = '_'; } } size_t calibration_filename_length = strlen(serial) + strlen(PSMOVE_CALIBRATION_EXTENSION) + 1; char *calibration_filename = (char *)malloc(calibration_filename_length); strcpy(calibration_filename, serial); strcat(calibration_filename, PSMOVE_CALIBRATION_EXTENSION); calibration->filename = psmove_util_get_file_path(calibration_filename); calibration->system_filename = psmove_util_get_system_file_path(calibration_filename); free(calibration_filename); free(serial); /* Try to load the calibration data from disk, or from USB */ psmove_calibration_load(calibration); if (!psmove_calibration_supported(calibration)) { if (psmove_connection_type(move) == Conn_USB) { psmove_DEBUG("Storing calibration from USB\n"); psmove_calibration_read_from_usb(calibration); psmove_calibration_save(calibration); } } /* Pre-calculate values used for mapping input */ if (psmove_calibration_supported(calibration)) { /* Accelerometer reading (high/low) for each axis */ int axlow, axhigh, aylow, ayhigh, azlow, azhigh; psmove_calibration_get_usb_accel_values(calibration, &axlow, &axhigh, &aylow, &ayhigh, &azlow, &azhigh); /** * * Calculation of accelerometer mapping (as factor of gravity, 1g): * * 2 * (raw - low) * calibrated = ---------------- - 1 * (high - low) * * with: * * raw .... Raw sensor reading * low .... Raw reading at -1g * high ... Raw reading at +1g * * Now define: * * 2 * f = -------------- * (high - low) * * And combine constants: * * c = - (f * low) - 1 * * Then we get: * * calibrated = f * raw + c * **/ /* Accelerometer factors "f" */ calibration->ax = 2.f / (float)(axhigh - axlow); calibration->ay = 2.f / (float)(ayhigh - aylow); calibration->az = 2.f / (float)(azhigh - azlow); /* Accelerometer constants "c" */ calibration->bx = - (calibration->ax * (float)axlow) - 1.f; calibration->by = - (calibration->ay * (float)aylow) - 1.f; calibration->bz = - (calibration->az * (float)azlow) - 1.f; /** * Calculation of gyroscope mapping (in radiant per second): * * raw * calibrated = -------- * 2 PI * rpm60 * * 60 * rpm80 * rpm60 = ------------ * 80 * * with: * * raw ..... Raw sensor reading * rpm80 ... Sensor reading at 80 RPM (from calibration blob) * rpm60 ... Sensor reading at 60 RPM (1 rotation per second) * * Or combined: * * 80 * raw * 2 PI * calibrated = ----------------- * 60 * rpm80 * * Now define: * * 2 * PI * 80 * f = ------------- * 60 * rpm80 * * Then we get: * * calibrated = f * raw * **/ int gx80, gy80, gz80; psmove_calibration_get_usb_gyro_values(calibration, &gx80, &gy80, &gz80); float factor = (float)(2.0 * M_PI * 80.0) / 60.0; calibration->gx = factor / (float)gx80; calibration->gy = factor / (float)gy80; calibration->gz = factor / (float)gz80; } else { /* No calibration data - pass-through input data */ calibration->ax = 1.f; calibration->ay = 1.f; calibration->az = 1.f; calibration->bx = 0.f; calibration->by = 0.f; calibration->bz = 0.f; calibration->gx = 1.f; calibration->gy = 1.f; calibration->gz = 1.f; } return calibration; }
int main(int argc, char* argv[]) { PSMove *move; enum PSMove_Connection_Type ctype; int i; if (!psmove_init(PSMOVE_CURRENT_VERSION)) { fprintf(stderr, "PS Move API init failed (wrong version?)\n"); exit(1); } i = psmove_count_connected(); printf("Connected controllers: %d\n", i); move = psmove_connect(); if (move == NULL) { printf("Could not connect to default Move controller.\n" "Please connect one via USB or Bluetooth.\n"); exit(1); } char *serial = psmove_get_serial(move); printf("Serial: %s\n", serial); free(serial); ctype = psmove_connection_type(move); switch (ctype) { case Conn_USB: printf("Connected via USB.\n"); break; case Conn_Bluetooth: printf("Connected via Bluetooth.\n"); break; case Conn_Unknown: printf("Unknown connection type.\n"); break; } for (i=0; i<10; i++) { psmove_set_leds(move, 0, 255*(i%3==0), 0); psmove_set_rumble(move, 255*(i%2)); psmove_update_leds(move); psmove_usleep(10000 * (i % 10)); } for (i=250; i>=0; i-=5) { psmove_set_leds(move, i, i, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); } /* Enable rate limiting for LED updates */ psmove_set_rate_limiting(move, 1); psmove_set_leds(move, 0, 0, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); while (ctype != Conn_USB && !(psmove_get_buttons(move) & Btn_PS)) { int res = psmove_poll(move); if (res) { if (psmove_get_buttons(move) & Btn_TRIANGLE) { printf("Triangle pressed, with trigger value: %d\n", psmove_get_trigger(move)); psmove_set_rumble(move, psmove_get_trigger(move)); } else { psmove_set_rumble(move, 0x00); } psmove_set_leds(move, 0, 0, psmove_get_trigger(move)); int x, y, z; psmove_get_accelerometer(move, &x, &y, &z); printf("accel: %5d %5d %5d\n", x, y, z); psmove_get_gyroscope(move, &x, &y, &z); printf("gyro: %5d %5d %5d\n", x, y, z); psmove_get_magnetometer(move, &x, &y, &z); printf("magnetometer: %5d %5d %5d\n", x, y, z); printf("buttons: %x\n", psmove_get_buttons(move)); int battery = psmove_get_battery(move); if (battery == Batt_CHARGING) { printf("battery charging\n"); } else if (battery == Batt_CHARGING_DONE) { printf("battery fully charged (on charger)\n"); } else if (battery >= Batt_MIN && battery <= Batt_MAX) { printf("battery level: %d / %d\n", battery, Batt_MAX); } else { printf("battery level: unknown (%x)\n", battery); } printf("raw temperature: %d\n", psmove_get_temperature(move)); printf("celsius temperature: %f\n", psmove_get_temperature_in_celsius(move)); psmove_update_leds(move); } } psmove_disconnect(move); psmove_shutdown(); return 0; }
void moved_server::handle_request() { struct sockaddr_in si_other; socklen_t si_len = sizeof(si_other); psmove_dev *dev = NULL; int send_response = 0; int request_id = -1, device_id = -1; unsigned char request[MOVED_SIZE_REQUEST] = {0}; unsigned char response[MOVED_SIZE_READ_RESPONSE] = {0}; int bytes_received = recvfrom(socket, (char*)request, sizeof(request), 0, (struct sockaddr *)&si_other, &si_len); assert(bytes_received != -1); // Client disconnected if (bytes_received == 0) return; request_id = request[0]; device_id = request[1]; for (psmove_dev *dev: devs) { if (dev->assigned_id == device_id) { break; } } if (dev != NULL && dev->assigned_id != device_id) { dev = NULL; } switch (request_id) { case MOVED_REQ_COUNT_CONNECTED: response[0] = (unsigned char)moved->count(); send_response = 1; break; case MOVED_REQ_WRITE: if (dev != NULL) { dev->set_output(request+2); } else { printf("Cannot write to device %d.\n", device_id); } break; case MOVED_REQ_READ: if (dev != NULL) { _psmove_read_data(dev->move, dev->input, sizeof(dev->input)); memcpy(response, dev->input, sizeof(dev->input)); } else { printf("Cannot read from device %d.\n", device_id); } send_response = 1; break; case MOVED_REQ_SERIAL: if (dev != NULL) { char *serial = psmove_get_serial(dev->move); memcpy(response, serial, strlen(serial)+1); free(serial); } else { printf("Cannot read from device %d.\n", device_id); } send_response = 1; break; default: printf("Unsupported call: %x - ignoring.\n", request_id); return; } /* Some requests need a response - send it here */ if (send_response) { int result = sendto(socket, (char*)response, sizeof(response), 0, (struct sockaddr *)&si_other, si_len); (void)result; assert(result != -1); } }