diff --git a/src/psmove.c b/src/psmove.c index 3285947d..d6d07c56 100644 --- a/src/psmove.c +++ b/src/psmove.c @@ -1455,7 +1455,7 @@ psmove_get_temperature(PSMove *move) } float -psmove_get_temperature_in_celsius(PSMove *move) +_psmove_temperature_to_celsius(int raw_value) { /** * The Move uses this table in Debug mode. Even though the resulting values @@ -1475,9 +1475,6 @@ psmove_get_temperature_in_celsius(PSMove *move) 0xCC1, 0xCD8, 0xCF0, 0xD06, 0xD1C, 0xD31, 0xD46, 0xD5A, }; - psmove_return_val_if_fail(move != NULL, 0.0); - - int raw_value = psmove_get_temperature(move); int i; for (i = 0; i < 80; i++) { @@ -1489,6 +1486,16 @@ psmove_get_temperature_in_celsius(PSMove *move) return 70.0f; } +float +psmove_get_temperature_in_celsius(PSMove *move) +{ + psmove_return_val_if_fail(move != NULL, 0.0); + + int raw_value = psmove_get_temperature(move); + + return _psmove_temperature_to_celsius(raw_value); +} + unsigned char psmove_get_trigger(PSMove *move) { diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index 03e236a1..b887a4d1 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -115,6 +115,25 @@ psmove_calibration_decode(char *data, int offset) return (low | (high << 8)) - 0x8000; } +unsigned int +psmove_calibration_decode_12bits(char *data, int offset) +{ + unsigned char low = data[offset] & 0xFF; + unsigned char high = (data[offset+1]) & 0xFF; + return low | (high << 8); +} + +float +psmove_calibration_decode_float(char *data, int offset) +{ + uint32_t v = (data[offset] & 0xFF) + | ((data[offset+1] & 0xFF) << 8) + | ((data[offset+2] & 0xFF) << 16) + | ((data[offset+3] & 0xFF) << 24); + return *((float *) &v); +} + + void psmove_calibration_parse_usb(PSMoveCalibration *calibration) { @@ -122,48 +141,69 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) char *data = calibration->usb_calibration; int orientation; int x, y, z, t; + float fx, fy, fz; printf("\n"); /* https://github.com/nitsch/moveonpc/wiki/Calibration-data */ + t = psmove_calibration_decode_12bits(data, 0x02); + printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); for (orientation=0; orientation<6; orientation++) { x = psmove_calibration_decode(data, 0x04 + 6*orientation); y = psmove_calibration_decode(data, 0x04 + 6*orientation + 2); z = psmove_calibration_decode(data, 0x04 + 6*orientation + 4); - printf("# Orientation #%d: (%5d | %5d | %5d)\n", orientation, x, y, z); + printf("# Orientation #%d: (%5d | %5d | %5d)\n", orientation, x, y, z); } printf("\n"); + t = psmove_calibration_decode_12bits(data, 0x42); + printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); for (orientation=0; orientation<3; orientation++) { x = psmove_calibration_decode(data, 0x46 + 8*orientation); y = psmove_calibration_decode(data, 0x46 + 8*orientation + 2); z = psmove_calibration_decode(data, 0x46 + 8*orientation + 4); - printf("# Gyro %c, 80 rpm: (%5d | %5d | %5d)\n", "XYZ"[orientation], x, y, z); + printf("# Gyro %c, 80 rpm: (%5d | %5d | %5d)\n", "XYZ"[orientation], x, y, z); } printf("\n"); - t = psmove_calibration_decode(data, 0x28); + t = psmove_calibration_decode_12bits(data, 0x28); x = psmove_calibration_decode(data, 0x2a); y = psmove_calibration_decode(data, 0x2a + 2); z = psmove_calibration_decode(data, 0x2a + 4); - printf("# Temperature at 0x28: (%5d)\n", t); - printf("# Vector at 0x2a: (%5d | %5d | %5d)\n", x, y, z); + printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); + printf("# Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); - t = psmove_calibration_decode(data, 0x30); + t = psmove_calibration_decode_12bits(data, 0x30); x = psmove_calibration_decode(data, 0x32); y = psmove_calibration_decode(data, 0x32 + 2); z = psmove_calibration_decode(data, 0x32 + 4); - printf("# Temperature at 0x30: (%5d)\n", t); - printf("# Vector at 0x32: (%5d | %5d | %5d)\n", x, y, z); + printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); + printf("# Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)\n", x, y, z); + + printf("\n"); + + t = psmove_calibration_decode_12bits(data, 0x5c); + fx = psmove_calibration_decode_float(data, 0x5e); + fy = psmove_calibration_decode_float(data, 0x5e + 4); + fz = psmove_calibration_decode_float(data, 0x5e + 8); + printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); + printf("# Vector @0x5e: (%f | %f | %f)\n", fx, fy, fz); + + fx = psmove_calibration_decode_float(data, 0x6a); + fy = psmove_calibration_decode_float(data, 0x6a + 4); + fz = psmove_calibration_decode_float(data, 0x6a + 8); + printf("# Vector @0x6a: (%f | %f | %f)\n", fx, fy, fz); printf("\n"); - printf("# byte at 0x3F: %02x\n", (unsigned char) data[0x3F]); + printf("# byte @0x3f: 0x%02x\n", (unsigned char) data[0x3f]); + printf("# float @0x76: %f\n", psmove_calibration_decode_float(data, 0x76)); + printf("# float @0x7a: %f\n", psmove_calibration_decode_float(data, 0x7a)); } void diff --git a/src/psmove_private.h b/src/psmove_private.h index 2057085a..817cd5a2 100644 --- a/src/psmove_private.h +++ b/src/psmove_private.h @@ -145,6 +145,12 @@ ADDCALL _psmove_get_device_path(PSMove *move); ADDAPI int ADDCALL _psmove_get_calibration_blob(PSMove *move, char **dest, size_t *size); +/** + * [PRIVATE API] Translate a raw temperature value to degrees Celsius + **/ +ADDAPI float +ADDCALL _psmove_temperature_to_celsius(int temperature); + /* A Bluetooth address. */ typedef unsigned char PSMove_Data_BTAddr[6];