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