Example #1
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) +
Example #2
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;
}