NeroReflex/ROGueENEMY
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity
ROGueENEMY/dev_iio.c

637 lines
21 KiB
C
Raw Blame History

#include "dev_iio.h"
#include <stdlib.h>
static char* read_file(const char* base_path, const char *file) {
char* res = NULL;
char* fdir = NULL;
long len = 0;
len = strlen(base_path) + strlen(file) + 1;
fdir = malloc(len);
if (fdir == NULL) {
fprintf(stderr, "Cannot allocate %ld bytes for device path, device skipped.\n", len);
goto read_file_err;
}
strcpy(fdir, base_path);
strcat(fdir, file);
if (access(fdir, F_OK) == 0) {
FILE* fp = fopen(fdir, "r");
if (fp != NULL) {
fseek(fp, 0L, SEEK_END);
len = ftell(fp);
rewind(fp);
len += 1;
res = malloc(len);
if (res != NULL) {
unsigned long read_bytes = fread(res, 1, len, fp);
printf("Read %lu bytes from file %s\n", read_bytes, fdir);
} else {
fprintf(stderr, "Cannot allocate %ld bytes for %s content.\n", len, fdir);
}
fclose(fp);
} else {
fprintf(stderr, "Cannot open file %s.\n", fdir);
}
} else {
fprintf(stderr, "File %s does not exists.\n", fdir);
}
free(fdir);
fdir = NULL;
read_file_err:
return res;
}
int write_file(const char* base_path, const char *file, const void* buf, size_t buf_sz) {
char* fdir = NULL;
int res = 0;
const size_t len = strlen(base_path) + strlen(file) + 1;
fdir = malloc(len);
if (fdir == NULL) {
fprintf(stderr, "Cannot allocate %ld bytes for device path, device skipped.\n", len);
goto write_file_err;
}
strcpy(fdir, base_path);
strcat(fdir, file);
if (access(fdir, F_OK) == 0) {
FILE* fp = fopen(fdir, "w");
if (fp != NULL) {
res = fwrite(buf, 1, buf_sz, fp);
if (res >= buf_sz) {
printf("Written %d bytes to file %s\n", res, fdir);
} else {
fprintf(stderr, "Cannot write to %s: %d.\n", fdir, res);
}
fclose(fp);
} else {
fprintf(stderr, "Cannot open file %s.\n", fdir);
}
} else {
fprintf(stderr, "File %s does not exists.\n", fdir);
}
free(fdir);
fdir = NULL;
write_file_err:
return res;
}
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;
iio->accel_x_fd = NULL;
iio->accel_y_fd = NULL;
iio->accel_z_fd = NULL;
iio->temp_fd = NULL;
iio->accel_scale_x = 0.0f;
iio->accel_scale_y = 0.0f;
iio->accel_scale_z = 0.0f;
iio->anglvel_scale_x = 0.0f;
iio->anglvel_scale_y = 0.0f;
iio->anglvel_scale_z = 0.0f;
iio->temp_scale = 0.0f;
iio->outer_accel_scale_x = ACCEL_SCALE;
iio->outer_accel_scale_y = ACCEL_SCALE;
iio->outer_accel_scale_z = ACCEL_SCALE;
iio->outer_anglvel_scale_x = GYRO_SCALE;
iio->outer_anglvel_scale_y = GYRO_SCALE;
iio->outer_anglvel_scale_z = GYRO_SCALE;
iio->outer_temp_scale = 0.0;
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}
};
*/
// 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;
iio->path = malloc(path_len);
if (iio->path == NULL) {
fprintf(stderr, "Cannot allocate %ld bytes for device name, device skipped.\n", path_len);
free(iio);
iio = NULL;
goto dev_iio_create_err;
}
strcpy(iio->path, path);
// ============================================= DEVICE NAME ================================================
iio->name = read_file(iio->path, "/name");
if (iio->name == NULL) {
fprintf(stderr, "Unable to read iio device name.\n");
free(iio);
iio = NULL;
goto dev_iio_create_err;
} else {
int idx = strlen(iio->name) - 1;
if ((iio->name[idx] == '\n') || ((iio->name[idx] == '\t'))) {
iio->name[idx] = '\0';
}
}
// ==========================================================================================================
// ========================================== in_anglvel_scale ==============================================
{
const char* preferred_scale = LSB_PER_RAD_S_2000_DEG_S_STR;
const char *scale_main_file = "/in_anglvel_scale";
char* const anglvel_scale = read_file(iio->path, scale_main_file);
if (anglvel_scale != NULL) {
iio->anglvel_scale_x = iio->anglvel_scale_y = iio->anglvel_scale_z = strtod(anglvel_scale, NULL);
free((void*)anglvel_scale);
if (write_file(iio->path, scale_main_file, preferred_scale, strlen(preferred_scale)) >= 0) {
iio->anglvel_scale_x = iio->anglvel_scale_y = iio->anglvel_scale_z = LSB_PER_RAD_S_2000_DEG_S;
printf("anglvel scale changed to %f for device %s\n", iio->anglvel_scale_x, iio->name);
} else {
fprintf(stderr, "Unable to set preferred in_anglvel_scale for device %s.\n", iio->name);
}
} else {
// TODO: what about if those are split in in_anglvel_{x,y,z}_scale?
fprintf(stderr, "Unable to read in_anglvel_scale from path %s%s.\n", iio->path, scale_main_file);
free(iio);
iio = NULL;
goto dev_iio_create_err;
}
}
// ==========================================================================================================
// =========================================== in_accel_scale ===============================================
{
const char* preferred_scale = LSB_PER_16G_STR;
const char *scale_main_file = "/in_accel_scale";
char* const accel_scale = read_file(iio->path, scale_main_file);
if (accel_scale != NULL) {
iio->accel_scale_x = iio->accel_scale_y = iio->accel_scale_z = strtod(accel_scale, NULL);
free((void*)accel_scale);
if (write_file(iio->path, scale_main_file, preferred_scale, strlen(preferred_scale)) >= 0) {
iio->accel_scale_x = iio->accel_scale_y = iio->accel_scale_z = LSB_PER_16G;
printf("accel scale changed to %f for device %s\n", iio->accel_scale_x, iio->name);
} else {
fprintf(stderr, "Unable to set preferred in_accel_scale for device %s.\n", iio->name);
}
} else {
// TODO: what about if those are plit in in_accel_{x,y,z}_scale?
fprintf(stderr, "Unable to read in_accel_scale file from path %s%s.\n", iio->path, scale_main_file);
free(iio);
iio = NULL;
goto dev_iio_create_err;
}
}
// ==========================================================================================================
// ============================================= temp_scale =================================================
{
const char *scale_main_file = "/in_temp_scale";
char* const accel_scale = read_file(iio->path, scale_main_file);
if (accel_scale != NULL) {
iio->temp_scale = strtod(accel_scale, NULL);
free((void*)accel_scale);
} else {
fprintf(stderr, "Unable to read in_temp_scale file from path %s%s.\n", iio->path, scale_main_file);
free(iio);
iio = NULL;
goto dev_iio_create_err;
}
}
// ==========================================================================================================
// ============================================ samplig_freq ================================================
{
const char* const preferred_samplig_freq = "800.000000";
const size_t preferred_samplig_freq_len = strlen(preferred_samplig_freq);
if (write_file(iio->path, "/in_accel_sampling_frequency", preferred_samplig_freq, preferred_samplig_freq_len) >= 0) {
printf("Accel sampling frequency changed to %s\n", preferred_samplig_freq);
} else {
fprintf(stderr, "Could not change accel sampling frequency\n");
}
if (write_file(iio->path, "/in_anglvel_sampling_frequency", preferred_samplig_freq, preferred_samplig_freq_len) >= 0) {
printf("Gyro sampling frequency changed to %s\n", preferred_samplig_freq);
} else {
fprintf(stderr, "Could not change gyro sampling frequency\n");
}
}
// ==========================================================================================================
const size_t tmp_sz = path_len + 128 + 1;
char* const tmp = malloc(tmp_sz);
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_accel_x_raw");
iio->accel_x_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_accel_y_raw");
iio->accel_y_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_accel_z_raw");
iio->accel_z_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_anglvel_x_raw");
iio->anglvel_x_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_anglvel_y_raw");
iio->anglvel_y_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_anglvel_z_raw");
iio->anglvel_z_fd = fopen(tmp, "r");
memset(tmp, 0, tmp_sz);
strcat(tmp, iio->path);
strcat(tmp, "/in_temp_raw");
iio->temp_fd = fopen(tmp, "r");
free(tmp);
printf(
"anglvel scale: x=%f, y=%f, z=%f | accel scale: x=%f, y=%f, z=%f\n",
iio->anglvel_scale_x,
iio->anglvel_scale_y,
iio->anglvel_scale_z,
iio->accel_scale_x,
iio->accel_scale_y,
iio->accel_scale_z
);
// give time to change the scale
sleep(4);
dev_iio_create_err:
return iio;
}
void dev_iio_destroy(dev_iio_t* iio) {
fclose(iio->accel_x_fd);
fclose(iio->accel_y_fd);
fclose(iio->accel_z_fd);
fclose(iio->anglvel_x_fd);
fclose(iio->anglvel_y_fd);
fclose(iio->anglvel_z_fd);
fclose(iio->temp_fd);
free(iio->name);
free(iio->path);
free(iio);
}
const char* dev_iio_get_name(const dev_iio_t* iio) {
return iio->name;
}
const char* dev_iio_get_path(const dev_iio_t* iio) {
return iio->path;
}
int dev_iio_read(
const dev_iio_t *const iio,
struct input_event *const buf,
size_t buf_sz,
uint32_t *const buf_out
) {
*buf_out = 0;
char tmp[128];
struct timeval now = {0};
if (*buf_out == buf_sz) {
return -ENOMEM;
} else 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) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->accel_scale_x;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_X;
ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_x);
} else {
fprintf(stderr, "While reading accel(x): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
if (*buf_out == buf_sz) {
return -ENOMEM;
} else if (iio->accel_y_fd != NULL) {
rewind(iio->accel_y_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_y_fd);
if (tmp_read >= 0) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->accel_scale_y;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_Y;
ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_y);
} else {
fprintf(stderr, "While reading accel(y): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
if (*buf_out == buf_sz) {
return -ENOMEM;
} else if (iio->accel_z_fd != NULL) {
rewind(iio->accel_z_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_z_fd);
if (tmp_read >= 0) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->accel_scale_z;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_Z;
ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_z);
} else {
fprintf(stderr, "While reading accel(z): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
if (*buf_out == buf_sz) {
return -ENOMEM;
} else if (iio->anglvel_x_fd != NULL) {
rewind(iio->anglvel_x_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_x_fd);
if (tmp_read >= 0) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->anglvel_scale_x;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_RX;
ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_x);
} else {
fprintf(stderr, "While reading anglvel(x): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
if (*buf_out == buf_sz) {
return -ENOMEM;
} else if (iio->anglvel_y_fd != NULL) {
rewind(iio->anglvel_y_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_y_fd);
if (tmp_read >= 0) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->anglvel_scale_y;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_RY;
ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_y);
} else {
fprintf(stderr, "While reading anglvel(y): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
if (*buf_out == buf_sz) {
return -ENOMEM;
} else if (iio->anglvel_z_fd != NULL) {
rewind(iio->anglvel_z_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_z_fd);
if (tmp_read >= 0) {
gettimeofday(&now, NULL);
const double val = strtod(&tmp[0], NULL);
const double val_in_m2s = val * iio->anglvel_scale_z;
struct input_event* ev = &buf[*buf_out];
ev->time = now;
ev->type = EV_ABS;
ev->code = ABS_RZ;
ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_z);
} else {
fprintf(stderr, "While reading anglvel(z): %d\n", tmp_read);
return tmp_read;
}
++(*buf_out);
}
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[1][0] * vector[1] + matrix[2][0] * vector[2];
result[1] = matrix[0][1] * vector[0] + matrix[1][1] * vector[1] + matrix[2][1] * vector[2];
result[2] = matrix[0][2] * vector[0] + matrix[1][2] * vector[1] + matrix[2][2] * vector[2];
}
int dev_iio_read_imu(const dev_iio_t *const iio, imu_message_t *const out) {
struct timeval read_time;
gettimeofday(&read_time, NULL);
out->flags = 0x00000000U;
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);
accel_in[0] = (double)out->accel_x_raw * iio->accel_scale_x;
if ((out->flags & IMU_MESSAGE_FLAGS_ACCEL) == 0) {
out->accel_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ACCEL;
}
} else {
fprintf(stderr, "While reading accel(x): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->accel_y_fd != NULL) {
rewind(iio->accel_y_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
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);
accel_in[1] = (double)out->accel_y_raw * iio->accel_scale_y;
if ((out->flags & IMU_MESSAGE_FLAGS_ACCEL) == 0) {
out->accel_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ACCEL;
}
} else {
fprintf(stderr, "While reading accel(y): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->accel_z_fd != NULL) {
rewind(iio->accel_z_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
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);
accel_in[2] = (double)out->accel_z_raw * iio->accel_scale_z;
if ((out->flags & IMU_MESSAGE_FLAGS_ACCEL) == 0) {
out->accel_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ACCEL;
}
} else {
fprintf(stderr, "While reading accel(z): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->anglvel_x_fd != NULL) {
rewind(iio->anglvel_x_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
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);
gyro_in[0] = (double)out->gyro_x_raw * iio->anglvel_scale_x;
if ((out->flags & IMU_MESSAGE_FLAGS_ANGLVEL) == 0) {
out->gyro_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ANGLVEL;
}
} else {
fprintf(stderr, "While reading anglvel(x): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->anglvel_y_fd != NULL) {
rewind(iio->anglvel_y_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
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);
gyro_in[1] = (double)out->gyro_y_raw *iio->anglvel_scale_y;
if ((out->flags & IMU_MESSAGE_FLAGS_ANGLVEL) == 0) {
out->gyro_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ANGLVEL;
}
} else {
fprintf(stderr, "While reading anglvel(y): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->anglvel_z_fd != NULL) {
rewind(iio->anglvel_z_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
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);
gyro_in[2] = (double)out->gyro_z_raw *iio->anglvel_scale_z;
if ((out->flags & IMU_MESSAGE_FLAGS_ANGLVEL) == 0) {
out->gyro_read_time = read_time;
out->flags |= IMU_MESSAGE_FLAGS_ANGLVEL;
}
} else {
fprintf(stderr, "While reading anglvel(z): %d\n", tmp_read);
return tmp_read;
}
}
if (iio->temp_fd != NULL) {
rewind(iio->temp_fd);
memset((void*)&tmp[0], 0, sizeof(tmp));
const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->temp_fd);
if (tmp_read >= 0) {
out->temp_raw = strtol(&tmp[0], NULL, 10);
out->temp_in_k = (double)out->temp_raw *iio->temp_scale;
} else {
fprintf(stderr, "While reading temp: %d\n", tmp_read);
return tmp_read;
}
}
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;
}
Reference in a new issue View git blame Copy permalink