From b1f2497ea53894fed9230319d17531f6c94f37c0 Mon Sep 17 00:00:00 2001 From: Denis Date: Sun, 19 Nov 2023 18:34:40 +0100 Subject: [PATCH] Add useful info --- .gitignore | 3 ++- dev_iio.c | 32 +++++++++++++++++++++++++++-- dev_iio.h | 2 ++ virt_ds4.c | 59 ++++++++++++++++++++++++++++++++++++++++++++++++------ 4 files changed, 87 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 674305f..f71946a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build /rogue_enemy *.o -/depends \ No newline at end of file +/depends +/debug \ No newline at end of file diff --git a/dev_iio.c b/dev_iio.c index 3767898..081685c 100644 --- a/dev_iio.c +++ b/dev_iio.c @@ -115,11 +115,23 @@ dev_iio_t* dev_iio_create(const char* path) { iio->outer_anglvel_scale_z = GYRO_SCALE; iio->outer_temp_scale = 0.0; - double mm[3][3] = { + double mm[3][3] = + /* + // this is the testing but "wrong" mount matrix + { {0.0f, 0.0f, -1.0f}, {0.0f, 1.0f, 0.0f}, - {1.0f, 0.0f, 0.0f} + {-1.0f, 0.0f, 0.0f} }; + */ + // this is the correct matrix: + { + {-1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, -1.0} + }; + + // store the mount matrix memcpy(iio->mount_matrix, mm, sizeof(mm)); const long path_len = strlen(path) + 1; @@ -217,6 +229,22 @@ dev_iio_t* dev_iio_create(const char* path) { } // ========================================================================================================== + // ============================================ sampling_rate ================================================ + { + char* const accel_scale = read_file(iio->path, "/in_temp_scale"); + if (accel_scale != NULL) { + iio->temp_scale = strtod(accel_scale, NULL); + free((void*)accel_scale); + } else { + fprintf(stderr, "Unable to read in_accel_scale file from path %s%s.\n", iio->path, "/in_accel_scale"); + + free(iio); + iio = NULL; + goto dev_iio_create_err; + } + } + // ========================================================================================================== + const size_t tmp_sz = path_len + 128 + 1; char* const tmp = malloc(tmp_sz); diff --git a/dev_iio.h b/dev_iio.h index 9b72044..1f07d05 100644 --- a/dev_iio.h +++ b/dev_iio.h @@ -44,6 +44,8 @@ typedef struct dev_iio { double outer_temp_scale; double mount_matrix[3][3]; + + double sampling_rate_hz; } dev_iio_t; dev_iio_t* dev_iio_create(const char* path); diff --git a/virt_ds4.c b/virt_ds4.c index 547015b..77c49f1 100644 --- a/virt_ds4.c +++ b/virt_ds4.c @@ -414,7 +414,7 @@ static int event(int fd) uhid_write(fd, &firmware_info_response); } else if (ev.u.get_report.rnum == 0x02) { // dualshock4_get_calibration_data - const struct uhid_event firmware_info_response = { + struct uhid_event firmware_info_response = { .type = UHID_GET_REPORT_REPLY, .u = { .get_report_reply = { @@ -422,17 +422,57 @@ static int event(int fd) .id = ev.u.get_report.id, .err = 0, .data = { - 0x02, 0xf9, 0xff, 0x09, 0x00, 0xf9, 0xff, 0xfe, - 0x22, 0xf4, 0xdc, 0xbb, 0x22, 0x59, 0xdd, 0x89, - 0x22, 0x68, 0xdd, 0x1c, 0x02, 0x1c, 0x02, 0xd3, - 0x20, 0x07, 0xdf, 0xbf, 0x20, 0xaa, 0xe0, 0xbc, - 0x1e, 0x86, 0xe0, 0x06, 0x00, + 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x06, 0x00, } } } }; + uint16_t gyro_pitch_bias = 0xfff9; + uint16_t gyro_yaw_bias = 0x0009; + uint16_t gyro_roll_bias = 0xfeff; + uint16_t gyro_pitch_plus = 0x2200; + uint16_t gyro_pitch_minus = 0xdcf4; + uint16_t gyro_yaw_plus = 0x22bb; + uint16_t gyro_yaw_minus = 0xdd59; + uint16_t gyro_roll_plus = 0x2289; + uint16_t gyro_roll_minus = 0xdd68; + uint16_t gyro_speed_plus = 0x021c /* 540 */; // speed_2x = (gyro_speed_plus + gyro_speed_minus) = 1080; + uint16_t gyro_speed_minus = 0x021c /* 540 */; // speed_2x = (gyro_speed_plus + gyro_speed_minus) = 1080; + uint16_t acc_x_plus = 0x20d3; + uint16_t acc_x_minus = 0xdf07; + uint16_t acc_y_plus = 0x20bf; + uint16_t acc_y_minus = 0xe0aa; + uint16_t acc_z_plus = 0x1ebc; + uint16_t acc_z_minus = 0xe086; + + // bias in kernel is 0 (embedded constant) + // speed_2x = speed_2x*DS_GYRO_RES_PER_DEG_S; calculated by the kernel will be 1080. + // As a consequence sens_numer (for every axis) is 1080*1024. + // that number will be 1105920 + memcpy((void*)&firmware_info_response.u.get_report_reply.data[1], (const void*)&gyro_pitch_bias, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[3], (const void*)&gyro_yaw_bias, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[5], (const void*)&gyro_roll_bias, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[7], (const void*)&gyro_pitch_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[9], (const void*)&gyro_pitch_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[11], (const void*)&gyro_yaw_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[13], (const void*)&gyro_yaw_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[15], (const void*)&gyro_roll_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[17], (const void*)&gyro_roll_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[19], (const void*)&gyro_speed_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[21], (const void*)&gyro_speed_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[23], (const void*)&acc_x_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[25], (const void*)&acc_x_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[27], (const void*)&acc_y_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[29], (const void*)&acc_y_minus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[31], (const void*)&acc_z_plus, sizeof(int16_t)); + memcpy((void*)&firmware_info_response.u.get_report_reply.data[33], (const void*)&acc_z_minus, sizeof(int16_t)); + uhid_write(fd, &firmware_info_response); } @@ -561,6 +601,13 @@ static int send_data(int fd, logic_t *const logic, uint8_t counter) { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; + /* + * kernel will do: + * int calib_data = mult_frac(ds4->gyro_calib_data[i].sens_numer, raw_data, ds4->gyro_calib_data[i].sens_denom); + * input_report_abs(ds4->sensors, ds4->gyro_calib_data[i].abs_code, calib_data); + * + * as we know sens_numer is 0, hence calib_data is zero. + */ const int16_t g_x = (gs.gyro[0]) / LSB_PER_RAD_S_2000_DEG_S; const int16_t g_y = (gs.gyro[1]) / LSB_PER_RAD_S_2000_DEG_S; const int16_t g_z = (gs.gyro[2]) / LSB_PER_RAD_S_2000_DEG_S;