From 68f155db184670c0f1e81e2ee7dddae7c5a91070 Mon Sep 17 00:00:00 2001 From: Denis Date: Sat, 18 Nov 2023 17:47:10 +0100 Subject: [PATCH] gyroscope mount matrix --- dev_iio.c | 41 +++++++++++++++++++++++++++++++++++------ dev_iio.h | 2 ++ imu_message.h | 23 ++++++++--------------- logic.c | 8 ++------ logic.h | 12 ++---------- output_dev.c | 8 ++------ virt_ds4.c | 12 ++++++------ 7 files changed, 57 insertions(+), 49 deletions(-) diff --git a/dev_iio.c b/dev_iio.c index 0f8f354..ab0190c 100644 --- a/dev_iio.c +++ b/dev_iio.c @@ -52,6 +52,10 @@ read_file_err: dev_iio_t* dev_iio_create(const char* path) { dev_iio_t *iio = malloc(sizeof(dev_iio_t)); + if (iio == NULL) { + return NULL; + } + iio->anglvel_x_fd = NULL; iio->anglvel_y_fd = NULL; iio->anglvel_z_fd = NULL; @@ -76,6 +80,13 @@ 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] = { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, -1.0} + }; + memcpy(iio->mount_matrix, mm, sizeof(mm)); + const long path_len = strlen(path) + 1; iio->path = malloc(path_len); if (iio->path == NULL) { @@ -380,18 +391,30 @@ int dev_iio_read( return 0; } +static void multiplyMatrixVector(const double matrix[3][3], const double vector[3], double result[3]) { + result[0] = matrix[0][0] * vector[0] + matrix[0][1] * vector[1] + matrix[0][2] * vector[2]; + result[1] = matrix[1][0] * vector[0] + matrix[1][1] * vector[1] + matrix[1][2] * vector[2]; + result[2] = matrix[2][0] * vector[0] + matrix[2][1] * vector[1] + matrix[2][2] * vector[2]; +} + int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { gettimeofday(&out->read_time, NULL); char tmp[128]; + double gyro_in[3]; + double accel_in[3]; + + double gyro_out[3]; + double accel_out[3]; + if (iio->accel_x_fd != NULL) { rewind(iio->accel_x_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_x_fd); if (tmp_read >= 0) { out->accel_x_raw = strtol(&tmp[0], NULL, 10); - out->accel_x_in_m2s = (double)out->accel_x_raw * iio->accel_scale_x; + accel_out[0] = (double)out->accel_x_raw * iio->accel_scale_x; } else { fprintf(stderr, "While reading accel(x): %d\n", tmp_read); return tmp_read; @@ -404,7 +427,7 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_y_fd); if (tmp_read >= 0) { out->accel_y_raw = strtol(&tmp[0], NULL, 10); - out->accel_y_in_m2s = (double)out->accel_y_raw * iio->accel_scale_y; + accel_out[1] = (double)out->accel_y_raw * iio->accel_scale_y; } else { fprintf(stderr, "While reading accel(y): %d\n", tmp_read); return tmp_read; @@ -417,7 +440,7 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_z_fd); if (tmp_read >= 0) { out->accel_z_raw = strtol(&tmp[0], NULL, 10); - out->accel_z_in_m2s = (double)out->accel_z_raw * iio->accel_scale_z; + accel_out[2] = (double)out->accel_z_raw * iio->accel_scale_z; } else { fprintf(stderr, "While reading accel(z): %d\n", tmp_read); return tmp_read; @@ -430,7 +453,7 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_x_fd); if (tmp_read >= 0) { out->gyro_x_raw = strtol(&tmp[0], NULL, 10); - out->gyro_x_in_rad_s = (double)out->gyro_x_raw * iio->anglvel_scale_x; + gyro_in[0] = (double)out->gyro_x_raw * iio->anglvel_scale_x; } else { fprintf(stderr, "While reading anglvel(x): %d\n", tmp_read); return tmp_read; @@ -443,7 +466,7 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_y_fd); if (tmp_read >= 0) { out->gyro_y_raw = strtol(&tmp[0], NULL, 10); - out->gyro_y_in_rad_s = (double)out->gyro_y_raw *iio->anglvel_scale_y; + gyro_in[1] = (double)out->gyro_y_raw *iio->anglvel_scale_y; } else { fprintf(stderr, "While reading anglvel(y): %d\n", tmp_read); return tmp_read; @@ -456,7 +479,7 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_z_fd); if (tmp_read >= 0) { out->gyro_z_raw = strtol(&tmp[0], NULL, 10); - out->gyro_z_in_rad_s = (double)out->gyro_z_raw *iio->anglvel_scale_z; + gyro_in[2] = (double)out->gyro_z_raw *iio->anglvel_scale_z; } else { fprintf(stderr, "While reading anglvel(z): %d\n", tmp_read); return tmp_read; @@ -476,5 +499,11 @@ int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) { } } + multiplyMatrixVector(iio->mount_matrix, gyro_in, gyro_out); + multiplyMatrixVector(iio->mount_matrix, accel_in, accel_out); + + memcpy(out->accel_m2s, accel_out, sizeof(double) * 3); + memcpy(out->gyro_rad_s, gyro_out, sizeof(double) * 3); + return 0; } \ No newline at end of file diff --git a/dev_iio.h b/dev_iio.h index c8d828c..9b72044 100644 --- a/dev_iio.h +++ b/dev_iio.h @@ -42,6 +42,8 @@ typedef struct dev_iio { double temp_scale; double outer_temp_scale; + + double mount_matrix[3][3]; } dev_iio_t; dev_iio_t* dev_iio_create(const char* path); diff --git a/imu_message.h b/imu_message.h index 3c3e828..7da9b0d 100644 --- a/imu_message.h +++ b/imu_message.h @@ -5,24 +5,17 @@ typedef struct imu_message { struct timeval read_time; - double gyro_x_in_rad_s; long gyro_x_raw; - - double gyro_y_in_rad_s; - int16_t gyro_y_raw; - - double gyro_z_in_rad_s; - int16_t gyro_z_raw; + long gyro_y_raw; + long gyro_z_raw; - double accel_x_in_m2s; - int16_t accel_x_raw; - - double accel_y_in_m2s; - int16_t accel_y_raw; - - double accel_z_in_m2s; - int16_t accel_z_raw; + long accel_x_raw; + long accel_y_raw; + long accel_z_raw; int16_t temp_raw; double temp_in_k; + + double gyro_rad_s[3]; // | x, y, z| right-hand-rules -- in rad/s + double accel_m2s[3]; // | x, y, z| positive: right, up, towards player -- in m/s^2 } imu_message_t; \ No newline at end of file diff --git a/logic.c b/logic.c index f6d07f1..a51b543 100644 --- a/logic.c +++ b/logic.c @@ -18,12 +18,8 @@ int logic_create(logic_t *const logic) { logic->gamepad.option = 0; logic->gamepad.share = 0; logic->gamepad.center = 0; - logic->gamepad.gyro_x = 0; - logic->gamepad.gyro_y = 0; - logic->gamepad.gyro_z = 0; - logic->gamepad.accel_x = 0; - logic->gamepad.accel_y = 0; - logic->gamepad.accel_z = 0; + memset(logic->gamepad.gyro, 0, sizeof(logic->gamepad.gyro)); + memset(logic->gamepad.accel, 0, sizeof(logic->gamepad.accel)); const int mutex_creation_res = pthread_mutex_init(&logic->gamepad_mutex, NULL); if (mutex_creation_res != 0) { diff --git a/logic.h b/logic.h index d83dec6..e261671 100644 --- a/logic.h +++ b/logic.h @@ -29,16 +29,8 @@ typedef struct gamepad_status { struct timeval last_motion_time; - int16_t gyro_x; // follows right-hand-rules - int16_t gyro_y; // follows right-hand-rules - int16_t gyro_z; // follows right-hand-rules - - int16_t accel_x; // positive: right - int16_t accel_y; // positive: up - int16_t accel_z; // positive: towards player - - - //uint8_t + double gyro[3]; // | x, y, z| right-hand-rules -- in rad/s + double accel[3]; // | x, y, z| positive: right, up, towards player -- in m/s^2 } gamepad_status_t; diff --git a/output_dev.c b/output_dev.c index 790dfc5..caef27d 100644 --- a/output_dev.c +++ b/output_dev.c @@ -740,12 +740,8 @@ static void handle_msg(output_dev_t *const out_dev, message_t *const msg) { const int upd_beg_res = logic_begin_status_update(out_dev->logic); if (upd_beg_res == 0) { out_dev->logic->gamepad.last_motion_time = msg->data.imu.read_time; - out_dev->logic->gamepad.accel_x = msg->data.imu.accel_x_raw; - out_dev->logic->gamepad.accel_y = msg->data.imu.accel_y_raw; - out_dev->logic->gamepad.accel_z = msg->data.imu.accel_z_raw; - out_dev->logic->gamepad.gyro_x = msg->data.imu.gyro_x_raw; - out_dev->logic->gamepad.gyro_y = msg->data.imu.gyro_y_raw; - out_dev->logic->gamepad.gyro_z = msg->data.imu.gyro_z_raw; + memcpy(out_dev->logic->gamepad.gyro, msg->data.imu.gyro_rad_s, sizeof(double) * 3); + memcpy(out_dev->logic->gamepad.accel, msg->data.imu.accel_m2s, sizeof(double) * 3); logic_end_status_update(out_dev->logic); diff --git a/virt_ds4.c b/virt_ds4.c index 68cd5b1..51176b4 100644 --- a/virt_ds4.c +++ b/virt_ds4.c @@ -561,12 +561,12 @@ 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, }; - const int16_t g_x = gs.gyro_x / 2; - const int16_t g_y = gs.gyro_y / 2; - const int16_t g_z = gs.gyro_z / 2; - const int16_t a_x = gs.accel_x / 2; - const int16_t a_y = gs.accel_y / 2; - const int16_t a_z = gs.accel_z / 2; + const int16_t g_x = gs.gyro[0] / (double)2.0; + const int16_t g_y = gs.gyro[1] / (double)2.0; + const int16_t g_z = gs.gyro[2] / (double)2.0; + const int16_t a_x = gs.accel[0] / (double)2.0; + const int16_t a_y = gs.accel[1] / (double)2.0; + const int16_t a_z = gs.accel[2] / (double)2.0; buf[0] = 0x01; // [00] report ID (0x01) buf[1] = ((uint64_t)((int64_t)gs.joystick_positions[0][0] + (int64_t)32768) >> (uint64_t)8); // L stick, X axis