163 lines
4.2 KiB
C
163 lines
4.2 KiB
C
|
#include "accel_monitor.h"
|
||
|
|
||
|
#include <stdio.h>
|
||
|
#include <stdlib.h>
|
||
|
#include <string.h>
|
||
|
|
||
|
static char* sensor_get_axis_path(char const* const base, char axis)
|
||
|
{
|
||
|
// TODO: Be smarter about the buffer size
|
||
|
char buffer[128];
|
||
|
snprintf(buffer, sizeof(buffer), "%s/in_accel_%c_raw", base, axis);
|
||
|
return strdup(buffer);
|
||
|
}
|
||
|
|
||
|
static int monitor_thread(void* data)
|
||
|
{
|
||
|
struct accel_monitor* monitor = data;
|
||
|
|
||
|
while (!monitor->monitoring_thread_is_ready)
|
||
|
;
|
||
|
|
||
|
char* x_axis_path = sensor_get_axis_path(monitor->sysfs_device_path, 'x');
|
||
|
FILE* x_axis_fp = fopen(x_axis_path, "r");
|
||
|
free(x_axis_path);
|
||
|
|
||
|
char* y_axis_path = sensor_get_axis_path(monitor->sysfs_device_path, 'y');
|
||
|
FILE* y_axis_fp = fopen(y_axis_path, "r");
|
||
|
free(y_axis_path);
|
||
|
|
||
|
char* z_axis_path = sensor_get_axis_path(monitor->sysfs_device_path, 'z');
|
||
|
FILE* z_axis_fp = fopen(z_axis_path, "r");
|
||
|
free(z_axis_path);
|
||
|
|
||
|
while (!monitor->monitoring_thread_should_exit) {
|
||
|
fseek(x_axis_fp, 0, SEEK_SET);
|
||
|
fseek(y_axis_fp, 0, SEEK_SET);
|
||
|
fseek(z_axis_fp, 0, SEEK_SET);
|
||
|
|
||
|
int16_t x_axis;
|
||
|
int16_t y_axis;
|
||
|
int16_t z_axis;
|
||
|
|
||
|
fscanf(x_axis_fp, "%hd\n", &x_axis);
|
||
|
fscanf(y_axis_fp, "%hd\n", &y_axis);
|
||
|
fscanf(z_axis_fp, "%hd\n", &z_axis);
|
||
|
|
||
|
if (mtx_lock(&monitor->current_data_lock) != thrd_success) {
|
||
|
fprintf(stderr,
|
||
|
"Can't lock data from data monitoring thread. Something is "
|
||
|
"going wrong. Giving up on monitoring sensor data.\n");
|
||
|
break;
|
||
|
}
|
||
|
|
||
|
monitor->current_data.x = x_axis;
|
||
|
monitor->current_data.y = y_axis;
|
||
|
monitor->current_data.z = z_axis;
|
||
|
|
||
|
if (mtx_unlock(&monitor->current_data_lock) != thrd_success) {
|
||
|
fprintf(stderr,
|
||
|
"Can't unlock data from data monitoring thread. Something "
|
||
|
"is going wrong. Giving up on monitoring sensor data.\n");
|
||
|
break;
|
||
|
}
|
||
|
|
||
|
monitor->data_is_ready = true;
|
||
|
}
|
||
|
|
||
|
fclose(x_axis_fp);
|
||
|
fclose(y_axis_fp);
|
||
|
fclose(z_axis_fp);
|
||
|
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
struct accel_monitor* accel_start_monitor(
|
||
|
char const* const sysfs_device_path)
|
||
|
{
|
||
|
mtx_t current_data_lock;
|
||
|
if (mtx_init(¤t_data_lock, mtx_plain) != thrd_success) return 0;
|
||
|
|
||
|
if (mtx_lock(¤t_data_lock) != thrd_success) return 0;
|
||
|
|
||
|
struct accel_monitor* monitor = calloc(1, sizeof(struct accel_monitor));
|
||
|
|
||
|
monitor->monitoring_thread_is_ready = false;
|
||
|
monitor->data_is_ready = false;
|
||
|
|
||
|
thrd_t monitoring_thread;
|
||
|
if (thrd_create(&monitoring_thread, &monitor_thread, monitor) !=
|
||
|
thrd_success) {
|
||
|
free(monitor);
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
monitor->sysfs_device_path = sysfs_device_path;
|
||
|
monitor->monitoring_thread = monitoring_thread;
|
||
|
monitor->current_data_lock = current_data_lock;
|
||
|
|
||
|
monitor->current_data.x = 0;
|
||
|
monitor->current_data.y = 0;
|
||
|
monitor->current_data.z = 0;
|
||
|
|
||
|
monitor->monitoring_thread_should_exit = false;
|
||
|
|
||
|
if (mtx_unlock(&monitor->current_data_lock) != thrd_success)
|
||
|
fprintf(stderr,
|
||
|
"Unable to unlock the data after initialisation. We are in UB "
|
||
|
"territory. Something must've went very wrong\n");
|
||
|
|
||
|
monitor->monitoring_thread_is_ready = true;
|
||
|
|
||
|
return monitor;
|
||
|
}
|
||
|
|
||
|
static bool axis_is_near_0(int16_t axis)
|
||
|
{
|
||
|
return (axis > -500) && (axis < 500);
|
||
|
}
|
||
|
|
||
|
static bool axis_is_mostly_positive(int16_t axis)
|
||
|
{
|
||
|
return axis >= 500;
|
||
|
}
|
||
|
|
||
|
static bool axis_is_mostly_negative(int16_t axis)
|
||
|
{
|
||
|
return axis <= -500;
|
||
|
}
|
||
|
|
||
|
enum accel_rotation accel_get_current_rotation(
|
||
|
struct accel_monitor* monitor)
|
||
|
{
|
||
|
if (mtx_lock(&monitor->current_data_lock) != thrd_success)
|
||
|
return ACCEL_ROTATION_NO_CHANGE;
|
||
|
struct accel_data data = monitor->current_data;
|
||
|
if (mtx_unlock(&monitor->current_data_lock) != thrd_success)
|
||
|
fprintf(stderr, "Can't unlock data, this is going to cause issues.\n");
|
||
|
|
||
|
if (axis_is_near_0(data.x)) {
|
||
|
if (axis_is_mostly_positive(data.y)) return ACCEL_ROTATION_0DEG;
|
||
|
if (axis_is_mostly_negative(data.y)) return ACCEL_ROTATION_180DEG;
|
||
|
}
|
||
|
|
||
|
if (axis_is_mostly_positive(data.x)) return ACCEL_ROTATION_90DEG;
|
||
|
|
||
|
if (axis_is_mostly_negative(data.x)) return ACCEL_ROTATION_270DEG;
|
||
|
|
||
|
return ACCEL_ROTATION_NO_CHANGE;
|
||
|
}
|
||
|
|
||
|
void accel_stop_monitor(struct accel_monitor* monitor)
|
||
|
{
|
||
|
monitor->monitoring_thread_should_exit = true;
|
||
|
|
||
|
if (thrd_join(monitor->monitoring_thread, NULL) != thrd_success)
|
||
|
fprintf(stderr,
|
||
|
"Can't join monitoring thread. The program may not exit "
|
||
|
"cleanly.\n");
|
||
|
|
||
|
mtx_destroy(&monitor->current_data_lock);
|
||
|
free(monitor);
|
||
|
}
|