mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
Antoine Drouin
85 lines
2.7 KiB
C
85 lines
2.7 KiB
C
#include "booz_joystick.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <sys/types.h>
|
|
#include <glib.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#include <sys/ioctl.h>
|
|
#include <linux/joystick.h>
|
|
|
|
double booz_joystick_value[JS_NB_AXIS];
|
|
//const double booz_joystick_neutral[JS_NB_AXIS] = { 112., 113., 112., 112., 112., 112., 30.};
|
|
//const double booz_joystick_max[JS_NB_AXIS] = { 1., 224., 224., 224., 224., 224., 223.};
|
|
//const double booz_joystick_min[JS_NB_AXIS] = { 224., 1., 1., 1., 1., 1., 30.};
|
|
|
|
const double booz_joystick_neutral[JS_NB_AXIS] = { 113., 113., 112., 112., 112., 112., 30.};
|
|
const double booz_joystick_max[JS_NB_AXIS] = { 46., 178., 224., 224., 224., 179., 223.};
|
|
const double booz_joystick_min[JS_NB_AXIS] = { 179., 46., 1., 1., 1., 46., 30.};
|
|
|
|
|
|
|
|
static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
|
|
|
|
void booz_joystick_init(const char* device) {
|
|
int i;
|
|
for (i=0; i<JS_NB_AXIS; i++)
|
|
booz_joystick_value[i] = 0.;
|
|
booz_joystick_value[JS_MODE] = -0.7;
|
|
|
|
int fd = open(device, O_RDONLY | O_NONBLOCK);
|
|
if (fd == -1) {
|
|
printf("opening joystick serial device %s : %s\n", device, strerror(errno));
|
|
return;
|
|
}
|
|
GIOChannel* channel = g_io_channel_unix_new(fd);
|
|
g_io_channel_set_encoding(channel, NULL, NULL);
|
|
g_io_add_watch (channel, G_IO_IN , on_data_received, NULL);
|
|
|
|
}
|
|
|
|
|
|
static gboolean on_data_received(GIOChannel *source,
|
|
GIOCondition condition __attribute__ ((unused)),
|
|
gpointer data __attribute__ ((unused))) {
|
|
|
|
struct js_event js;
|
|
gsize len;
|
|
GError *err = NULL;
|
|
g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err);
|
|
|
|
if (js.type == JS_EVENT_AXIS) {
|
|
if (js.number < JS_NB_AXIS) {
|
|
// if (js.number == JS_THROTTLE) printf("joystick value %d\n",js.value);
|
|
booz_joystick_value[js.number] = (double)(js.value - booz_joystick_neutral[js.number]) /
|
|
(booz_joystick_max[js.number] - booz_joystick_min[js.number]);
|
|
}
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
#if 0
|
|
switch (js.number) {
|
|
case JS_THROTTLE:
|
|
ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30);
|
|
break;
|
|
case JS_PITCH:
|
|
ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1);
|
|
break;
|
|
case JS_ROLL:
|
|
ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224);
|
|
break;
|
|
case JS_YAW:
|
|
ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
|
|
break;
|
|
case JS_MODE:
|
|
ppm_pulses[RADIO_MODE] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
|
|
rc_values_contains_avg_channels = TRUE;
|
|
break;
|
|
}
|
|
#endif
|