From 8475f5f055445fbff8b996bdace3a60b000934b6 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 28 Jul 2018 14:10:32 +0200 Subject: [PATCH 1/5] Make conversion from raw temperature to degrees Celsius reusable --- src/psmove.c | 15 +++++++++++---- src/psmove_private.h | 6 ++++++ 2 files changed, 17 insertions(+), 4 deletions(-) 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_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]; From dd702fbfc8b72549a01650e72b6e899e08995fe4 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 28 Jul 2018 14:27:27 +0200 Subject: [PATCH 2/5] List converted temperatures in dump of calibration data --- src/psmove_calibration.c | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index 03e236a1..eb4e7338 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -115,6 +115,14 @@ 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); +} + void psmove_calibration_parse_usb(PSMoveCalibration *calibration) { @@ -127,6 +135,8 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) /* 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); @@ -136,6 +146,8 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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); @@ -145,20 +157,20 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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("# Temperature at 0x28: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); printf("# Vector at 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("# Temperature at 0x30: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); printf("# Vector at 0x32: (%5d | %5d | %5d)\n", x, y, z); printf("\n"); From 82dd1d2c6f267e4ba71ac9f7333e9ea0b8c636e9 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 28 Jul 2018 14:39:09 +0200 Subject: [PATCH 3/5] Label 0 rpm gyro readings in calibration dump --- src/psmove_calibration.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index eb4e7338..067b46cf 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -161,8 +161,8 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) x = psmove_calibration_decode(data, 0x2a); y = psmove_calibration_decode(data, 0x2a + 2); z = psmove_calibration_decode(data, 0x2a + 4); - printf("# Temperature at 0x28: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(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 (at 0x2a): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); @@ -170,8 +170,8 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) x = psmove_calibration_decode(data, 0x32); y = psmove_calibration_decode(data, 0x32 + 2); z = psmove_calibration_decode(data, 0x32 + 4); - printf("# Temperature at 0x30: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(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 (at 0x32): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); From 6b99245af1d1b65f11b1bc7866e0e31509c89c84 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 28 Jul 2018 15:07:17 +0200 Subject: [PATCH 4/5] List floats in calibration dump --- src/psmove_calibration.c | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index 067b46cf..4ca9cf73 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -123,6 +123,17 @@ psmove_calibration_decode_12bits(char *data, int offset) 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) { @@ -130,6 +141,7 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) char *data = calibration->usb_calibration; int orientation; int x, y, z, t; + float fx, fy, fz; printf("\n"); @@ -175,7 +187,23 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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 at 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 at 0x6a: (%f | %f | %f)\n", fx, fy, fz); + + printf("\n"); + printf("# byte at 0x3F: %02x\n", (unsigned char) data[0x3F]); + printf("# float at 0x76: %f\n", psmove_calibration_decode_float(data, 0x76)); + printf("# float at 0x7a: %f\n", psmove_calibration_decode_float(data, 0x7a)); } void From 771db454ca375e4d59c91b346b86bdb6420f8143 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 28 Jul 2018 15:12:05 +0200 Subject: [PATCH 5/5] Fix output formatting in calibration dump --- src/psmove_calibration.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index 4ca9cf73..b887a4d1 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -153,7 +153,7 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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"); @@ -164,7 +164,7 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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"); @@ -174,7 +174,7 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) y = psmove_calibration_decode(data, 0x2a + 2); z = psmove_calibration_decode(data, 0x2a + 4); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); - printf("# Gyro, 0 rpm (at 0x2a): (%5d | %5d | %5d)\n", x, y, z); + printf("# Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); @@ -183,7 +183,7 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) y = psmove_calibration_decode(data, 0x32 + 2); z = psmove_calibration_decode(data, 0x32 + 4); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); - printf("# Gyro, 0 rpm (at 0x32): (%5d | %5d | %5d)\n", x, y, z); + printf("# Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); @@ -192,18 +192,18 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) 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 at 0x5e: (%f | %f | %f)\n", fx, fy, fz); + 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 at 0x6a: (%f | %f | %f)\n", fx, fy, fz); + printf("# Vector @0x6a: (%f | %f | %f)\n", fx, fy, fz); printf("\n"); - printf("# byte at 0x3F: %02x\n", (unsigned char) data[0x3F]); - printf("# float at 0x76: %f\n", psmove_calibration_decode_float(data, 0x76)); - printf("# float at 0x7a: %f\n", psmove_calibration_decode_float(data, 0x7a)); + 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