294 lines
11 KiB
C
294 lines
11 KiB
C
/*
|
|
* gamepad_controller.c — Gamepad/joystick teleop over UDP or KCP
|
|
*
|
|
* Uses the Linux joystick API (/dev/input/js*).
|
|
* Zero external dependencies.
|
|
*
|
|
* Xbox controller mapping (xpad driver):
|
|
* Left stick Y (axis 1) → linear.x (forward/back, inverted)
|
|
* Left stick X (axis 0) → linear.y (strafe)
|
|
* Right stick X (axis 3) → angular.z (turn)
|
|
* Button A (0) → emergency stop
|
|
* Button B (1) → quit
|
|
*
|
|
* Build: gcc -Wall -O2 -I../common -o gamepad_controller gamepad_controller.c -lm
|
|
* Usage: ./gamepad_controller [-i IP] [-p PORT] [-d /dev/input/js0]
|
|
* [-l MAX_LIN] [-a MAX_ANG] [-z DEADZONE]
|
|
* [-t udp|kcp] [-s SERVER] [-r RELAY]
|
|
* [-I PEER_ID] [-T TARGET_PEER]
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <fcntl.h>
|
|
#include <signal.h>
|
|
#include <math.h>
|
|
#include <errno.h>
|
|
#include <sys/select.h>
|
|
#include <sys/time.h>
|
|
#include <sys/ioctl.h>
|
|
#include <getopt.h>
|
|
#include <linux/joystick.h>
|
|
|
|
#include "../common/protocol.h"
|
|
#include "../common/teleop_transport.h"
|
|
|
|
/* ── config ─────────────────────────────────────────────────────────── */
|
|
#define MAX_AXES 16
|
|
#define MAX_BUTTONS 16
|
|
#define JS_AXIS_MAX 32767.0f
|
|
|
|
/* Xbox mapping indices */
|
|
#define AXIS_LX 0 /* left stick X → strafe */
|
|
#define AXIS_LY 1 /* left stick Y → fwd/back (inverted) */
|
|
#define AXIS_RX 3 /* right stick X → turn */
|
|
|
|
#define BTN_STOP 0 /* A → emergency stop */
|
|
#define BTN_QUIT 1 /* B → quit */
|
|
|
|
static volatile sig_atomic_t g_running = 1;
|
|
|
|
static void sigint_handler(int sig) { (void)sig; g_running = 0; }
|
|
|
|
static int parse_port(const char *text, int *port_out)
|
|
{
|
|
char *end = NULL;
|
|
long value = strtol(text, &end, 10);
|
|
|
|
if (end == text || *end != '\0' || value < 1 || value > 65535)
|
|
return -1;
|
|
|
|
*port_out = (int)value;
|
|
return 0;
|
|
}
|
|
|
|
/* ── apply deadzone ─────────────────────────────────────────────────── */
|
|
static float apply_deadzone(float v, float dz)
|
|
{
|
|
if (fabsf(v) < dz) return 0.0f;
|
|
/* rescale so the output starts from 0 just outside the deadzone */
|
|
float sign = (v > 0) ? 1.0f : -1.0f;
|
|
return sign * (fabsf(v) - dz) / (1.0f - dz);
|
|
}
|
|
|
|
/* ── usage ──────────────────────────────────────────────────────────── */
|
|
static void usage(const char *prog)
|
|
{
|
|
fprintf(stderr,
|
|
"Usage: %s [options]\n"
|
|
" -i IP target IP (default %s)\n"
|
|
" -p PORT target port (default %d)\n"
|
|
" -d DEVICE joystick device (default /dev/input/js0)\n"
|
|
" -l SPEED max linear m/s (default 0.5)\n"
|
|
" -a SPEED max angular rad/s (default 0.5)\n"
|
|
" -z DZ deadzone 0<=DZ<1 (default 0.1)\n"
|
|
" -t MODE transport mode udp|kcp (default udp)\n"
|
|
" -s ADDR KCP server addr (default %s)\n"
|
|
" -r ADDR KCP relay addr (default none)\n"
|
|
" -I ID local KCP peer id (default %s)\n"
|
|
" -T ID target KCP peer id (default %s)\n"
|
|
" -h show help\n",
|
|
prog, DEFAULT_IP, DEFAULT_PORT,
|
|
DEFAULT_KCP_SERVER_ADDR,
|
|
DEFAULT_KCP_GAMEPAD_PEER_ID,
|
|
DEFAULT_KCP_TARGET_PEER_ID);
|
|
}
|
|
|
|
/* ──────────────────────────────────────────────────────────────────── */
|
|
int main(int argc, char *argv[])
|
|
{
|
|
char ip[64] = DEFAULT_IP;
|
|
int port = DEFAULT_PORT;
|
|
char device[128] = "/dev/input/js0";
|
|
char kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
|
|
char kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
|
|
char peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_GAMEPAD_PEER_ID;
|
|
char target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
|
|
float max_lin = 0.5f;
|
|
float max_ang = 0.5f;
|
|
float deadzone = 0.1f;
|
|
teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
|
|
teleop_transport_t transport;
|
|
teleop_transport_config_t transport_config;
|
|
|
|
int opt;
|
|
while ((opt = getopt(argc, argv, "i:p:d:l:a:z:t:s:r:I:T:h")) != -1) {
|
|
switch (opt) {
|
|
case 'i': strncpy(ip, optarg, sizeof(ip)-1); ip[sizeof(ip)-1] = '\0'; break;
|
|
case 'p':
|
|
if (parse_port(optarg, &port) != 0) {
|
|
fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
|
|
return 1;
|
|
}
|
|
break;
|
|
case 'd': strncpy(device, optarg, sizeof(device)-1); device[sizeof(device)-1] = '\0'; break;
|
|
case 'l': max_lin = strtof(optarg, NULL); break;
|
|
case 'a': max_ang = strtof(optarg, NULL); break;
|
|
case 'z': deadzone = strtof(optarg, NULL); break;
|
|
case 't':
|
|
if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
|
|
fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
|
|
return 1;
|
|
}
|
|
break;
|
|
case 's': strncpy(kcp_server, optarg, sizeof(kcp_server)-1); kcp_server[sizeof(kcp_server)-1] = '\0'; break;
|
|
case 'r': strncpy(kcp_relay, optarg, sizeof(kcp_relay)-1); kcp_relay[sizeof(kcp_relay)-1] = '\0'; break;
|
|
case 'I': strncpy(peer_id, optarg, sizeof(peer_id)-1); peer_id[sizeof(peer_id)-1] = '\0'; break;
|
|
case 'T': strncpy(target_peer, optarg, sizeof(target_peer)-1); target_peer[sizeof(target_peer)-1] = '\0'; break;
|
|
default: usage(argv[0]); return (opt == 'h') ? 0 : 1;
|
|
}
|
|
}
|
|
|
|
if (deadzone < 0.0f || deadzone >= 1.0f) {
|
|
fprintf(stderr, "Invalid deadzone %.3f: expected 0 <= dz < 1\n", deadzone);
|
|
return 1;
|
|
}
|
|
|
|
signal(SIGINT, sigint_handler);
|
|
|
|
/* ── open joystick ───────────────────────────────────────────── */
|
|
int jsfd = open(device, O_RDONLY | O_NONBLOCK);
|
|
if (jsfd < 0) {
|
|
fprintf(stderr, "Cannot open %s: %s\n"
|
|
" Hint: connect Xbox controller, check 'ls /dev/input/js*'\n",
|
|
device, strerror(errno));
|
|
return 1;
|
|
}
|
|
|
|
char js_name[128] = "Unknown";
|
|
ioctl(jsfd, JSIOCGNAME(sizeof(js_name)), js_name);
|
|
|
|
int num_axes = 0, num_buttons = 0;
|
|
ioctl(jsfd, JSIOCGAXES, &num_axes);
|
|
ioctl(jsfd, JSIOCGBUTTONS, &num_buttons);
|
|
|
|
printf("========================================\n");
|
|
printf(" Gamepad Teleop Controller\n");
|
|
printf("========================================\n");
|
|
printf(" Device : %s\n", device);
|
|
printf(" Name : %s\n", js_name);
|
|
printf(" Axes : %d Buttons: %d\n", num_axes, num_buttons);
|
|
printf(" Transport: %s\n", teleop_transport_mode_name(transport_mode));
|
|
if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
|
|
printf(" KCP server: %s\n", kcp_server);
|
|
if (kcp_relay[0] != '\0')
|
|
printf(" Relay via : %s\n", kcp_relay);
|
|
printf(" Peer ID : %s -> %s\n", peer_id, target_peer);
|
|
} else {
|
|
printf(" Target : %s:%d\n", ip, port);
|
|
}
|
|
printf(" Linear : %.2f m/s Angular: %.2f rad/s\n", max_lin, max_ang);
|
|
printf(" Deadzone: %.2f\n", deadzone);
|
|
printf("----------------------------------------\n");
|
|
printf(" Left stick → forward/back + strafe\n");
|
|
printf(" Right stick → turn\n");
|
|
printf(" A button → emergency stop\n");
|
|
printf(" B button → quit\n");
|
|
printf("========================================\n\n");
|
|
|
|
memset(&transport_config, 0, sizeof(transport_config));
|
|
transport_config.mode = transport_mode;
|
|
transport_config.udp_ip = ip;
|
|
transport_config.udp_port = port;
|
|
transport_config.server_addr = kcp_server;
|
|
transport_config.relay_via = kcp_relay;
|
|
transport_config.peer_id = peer_id;
|
|
transport_config.target_peer = target_peer;
|
|
|
|
if (teleop_transport_open(&transport, &transport_config) != 0) {
|
|
close(jsfd);
|
|
return 1;
|
|
}
|
|
|
|
/* ── state ───────────────────────────────────────────────────── */
|
|
float axes[MAX_AXES];
|
|
int buttons[MAX_BUTTONS];
|
|
memset(axes, 0, sizeof(axes));
|
|
memset(buttons, 0, sizeof(buttons));
|
|
|
|
twist_cmd_t cmd;
|
|
twist_cmd_zero(&cmd);
|
|
|
|
struct timeval last_send;
|
|
gettimeofday(&last_send, NULL);
|
|
|
|
int e_stop = 0;
|
|
|
|
/* ── main loop ───────────────────────────────────────────────── */
|
|
while (g_running) {
|
|
/* read all pending joystick events */
|
|
struct js_event ev;
|
|
while (read(jsfd, &ev, sizeof(ev)) == sizeof(ev)) {
|
|
ev.type &= ~JS_EVENT_INIT; /* strip init flag */
|
|
if (ev.type == JS_EVENT_AXIS && ev.number < MAX_AXES) {
|
|
axes[ev.number] = (float)ev.value / JS_AXIS_MAX;
|
|
} else if (ev.type == JS_EVENT_BUTTON && ev.number < MAX_BUTTONS) {
|
|
buttons[ev.number] = ev.value;
|
|
if (ev.number == BTN_QUIT && ev.value) {
|
|
g_running = 0;
|
|
break;
|
|
}
|
|
if (ev.number == BTN_STOP && ev.value) {
|
|
e_stop = !e_stop;
|
|
if (e_stop)
|
|
printf("\r ** EMERGENCY STOP ** ");
|
|
else
|
|
printf("\r ** E-STOP released ** ");
|
|
fflush(stdout);
|
|
}
|
|
}
|
|
}
|
|
/* EAGAIN is expected in non-blocking mode */
|
|
if (errno != EAGAIN && errno != 0) {
|
|
perror("read joystick");
|
|
break;
|
|
}
|
|
errno = 0;
|
|
|
|
/* map axes → twist (skip if e-stopped) */
|
|
if (e_stop) {
|
|
twist_cmd_zero(&cmd);
|
|
} else {
|
|
float lx_raw = apply_deadzone(-axes[AXIS_LY], deadzone); /* Y inverted */
|
|
float ly_raw = apply_deadzone(-axes[AXIS_LX], deadzone);
|
|
float az_raw = apply_deadzone(-axes[AXIS_RX], deadzone);
|
|
|
|
cmd.lx = lx_raw * max_lin;
|
|
cmd.ly = ly_raw * max_lin;
|
|
cmd.lz = 0.0f;
|
|
cmd.ax = 0.0f;
|
|
cmd.ay = 0.0f;
|
|
cmd.az = az_raw * max_ang;
|
|
}
|
|
|
|
/* rate-limit sending */
|
|
struct timeval now;
|
|
gettimeofday(&now, NULL);
|
|
long elapsed = (now.tv_sec - last_send.tv_sec) * 1000000
|
|
+ (now.tv_usec - last_send.tv_usec);
|
|
if (elapsed < SEND_INTERVAL_US) {
|
|
usleep(5000); /* 5 ms sleep to avoid busy-spin */
|
|
continue;
|
|
}
|
|
last_send = now;
|
|
|
|
teleop_transport_send_twist(&transport, &cmd);
|
|
|
|
printf("\r cmd: lx=%+.2f ly=%+.2f az=%+.2f | raw: LY=%+.2f LX=%+.2f RX=%+.2f ",
|
|
cmd.lx, cmd.ly, cmd.az,
|
|
axes[AXIS_LY], axes[AXIS_LX], axes[AXIS_RX]);
|
|
fflush(stdout);
|
|
}
|
|
|
|
/* send final stop */
|
|
twist_cmd_zero(&cmd);
|
|
teleop_transport_send_twist(&transport, &cmd);
|
|
|
|
close(jsfd);
|
|
teleop_transport_close(&transport);
|
|
printf("\nStopped.\n");
|
|
return 0;
|
|
}
|