make controllers more modular

7 files changed, 286 insertions, 241 deletions

diff --git a/Makefile b/Makefile
index c532dd3..327ac2d 100644
--- a/Makefile
+++ b/Makefile
@@ -6,9 +6,10 @@ CFLAGS=-std=gnu99 -Wall -O2
 LD=gcc
 LDFLAGS=-O2
 
-SOURCEDIRS=src
+SOURCEDIRS=src src/controllers
 INCLUDEDIRS=\
- src/include\
+ src \
+ src/controllers \
  ext/mavlink/include
 
 SOURCES=$(foreach dir,$(SOURCEDIRS),$(wildcard $(dir)/*.c))
@@ -31,4 +32,4 @@ clean:
 		rm -f $$dir/*.o; \
 	done
 	rm -f $(TARGET)
-	rm -f $(TARGET)-static
\ No newline at end of file
+	rm -f $(TARGET)-static
diff --git a/src/controller.h b/src/controller.h
new file mode 100644
index 0000000..4712dcc
--- /dev/null
+++ b/src/controller.h
@@ -0,0 +1,29 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#define CHANNEL_UNUSED 65535
+#define CHANNELS 8
+
+#define AXIS_MIN -32768
+#define AXIS_MAX 32767
+
+typedef uint16_t channel_t;
+
+struct controller {
+	void (*reset)(struct controller* cont);
+	void (*axis)(struct controller* cont, int axis_id, int value);
+	void (*button)(struct controller* cont, int button_id, bool down);
+	channel_t channels[CHANNELS];
+};
+
+inline channel_t channel_clamp(channel_t channel, channel_t min, channel_t max)
+{
+    channel_t upper = (channel < max) ? channel : max;
+    return (min < upper) ? upper : min;
+}
+
+#endif
diff --git a/src/controllers/generic.c b/src/controllers/generic.c
new file mode 100644
index 0000000..04044b0
--- /dev/null
+++ b/src/controllers/generic.c
@@ -0,0 +1,91 @@
+#include "generic.h"
+#include <stdio.h>
+
+#define CHANNEL_THROTTLE 0
+#define CHANNEL_ATTITUDE_OFFSET 1
+
+#define THROTTLE_MIN 1000
+#define THROTTLE_MAX 2000
+#define THROTTLE_STEP_SMALL 10
+#define THROTTLE_STEP_LARGE 100
+
+#define ATTITUDE_MIN 1000
+#define ATTITUDE_MAX 2000
+
+/* 
+ * convert the value from an axis to an appropriate ppm value
+ * between 1000 and 2000
+ */
+inline channel_t axis_to_attitude(int value)
+{
+	long centered = (long) value - AXIS_MIN;
+	long proportional = centered * ATTITUDE_MIN / (AXIS_MAX - AXIS_MIN);
+	return (channel_t) ATTITUDE_MAX - ATTITUDE_MIN + proportional;
+}
+
+inline void channel_step(channel_t* channels, size_t channel_index, char step, channel_t min, channel_t max)
+{
+    int current = (int) channels[channel_index];
+    int next = current + step;
+    if (next > (int) max) {
+    	next = max;
+    }
+    if (next < (int) min) {
+    	next = min;
+    }
+    channels[channel_index] = (channel_t) next;
+}
+
+static void reset(struct controller* controller)
+{
+	channel_t *channels = controller->channels;
+	
+	for (size_t i = 0; i < CHANNELS; ++i) {
+		channels[i] = CHANNEL_UNUSED;
+	}
+	
+	channels[0] = THROTTLE_MIN;
+	channels[1] = ATTITUDE_MIN;
+	channels[2] = ATTITUDE_MIN;
+	channels[3] = ATTITUDE_MIN;
+	channels[4] = ATTITUDE_MIN;
+}
+
+static void axis(struct controller* controller, int axis_id, int value)
+{
+	controller->channels[CHANNEL_ATTITUDE_OFFSET+axis_id] = axis_to_attitude(value);
+}
+
+static void button(struct controller* controller, int button_id, bool down)
+{
+	if (!down){
+		return;
+	}
+	
+	switch(button_id) {
+	case 0:
+		reset(controller);
+		break;
+	case 4:
+		channel_step(controller->channels, CHANNEL_THROTTLE, -THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
+		break;
+	case 5:
+		channel_step(controller->channels, CHANNEL_THROTTLE, THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
+		break;
+	case 6:
+		channel_step(controller->channels, CHANNEL_THROTTLE, -THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
+		break;
+	case 7:
+		channel_step(controller->channels, CHANNEL_THROTTLE, THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
+		break;
+	default:
+		fprintf(stderr, "Button number %d unsupported.\n", button_id);
+		break;
+	}
+}
+
+struct controller controller_generic = {
+	.reset = reset,
+	.axis = axis,
+	.button = button
+};
diff --git a/src/controllers/generic.h b/src/controllers/generic.h
new file mode 100644
index 0000000..f5566e1
--- /dev/null
+++ b/src/controllers/generic.h
@@ -0,0 +1,8 @@
+#ifndef CONTROLLERS_GENERIC_H
+#define CONTROLLERS_GENERIC_H
+
+#include <controller.h>
+
+struct controller controller_generic;
+
+#endif
diff --git a/src/generic.c b/src/generic.c
deleted file mode 100644
index 683598b..0000000
--- a/src/generic.c
+++ /dev/null
@@ -1,63 +0,0 @@
-#include "controller.h"
-#include <stdio.h>
-
-#define CHANNEL_THROTTLE 0
-#define CHANNEL_ATTITUDE_OFFSET 1
-
-#define THROTTLE_MIN 1000
-#define THROTTLE_MAX 2000
-#define THROTTLE_STEP_SMALL 10
-#define THROTTLE_STEP_LARGE 100
-
-#define ATTITUDE_MIN 1000
-#define ATTITUDE_MAX 2000
-
-/* 
- * convert the value from an axis to an appropriate ppm value
- * between 1000 and 2000
- */
-inline channel_t axis_to_attitude(int value) {
-	long centered = (long) value - AXIS_MIN;
-	long proportional = centered * ATTITUDE_MIN / (AXIS_MAX - AXIS_MIN);
-	return (channel_t) ATTITUDE_MAX - ATTITUDE_MIN + proportional;
-}
-
-void channel_reset(channel_t* channels) {
-	for (size_t i = 0; i < CHANNELS; ++i) {
-		channels[i] = CHANNEL_UNUSED;
-	}
-	channels[0] = THROTTLE_MIN;
-	channels[1] = ATTITUDE_MIN;
-	channels[2] = ATTITUDE_MIN;
-	channels[3] = ATTITUDE_MIN;
-	channels[4] = ATTITUDE_MIN;
-}
-
-void event_axis(channel_t* channels, int axis, int value) {
-	channels[CHANNEL_ATTITUDE_OFFSET+axis] = axis_to_attitude(value);
-}
-
-void event_button(channel_t* channels, int button, bool value) {
-	if (value) {
-		switch(button) {
-		case 0:
-			channel_reset(channels);
-			break;
-		case 4:
-			channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
-			break;
-		case 5:
-			channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
-			break;
-		case 6:
-			channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
-			break;
-		case 7:
-			channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
-			break;
-		default:
-			printf("button unsupported %d\n", button);
-			break;
-		}
-	}
-}
\ No newline at end of file
diff --git a/src/include/controller.h b/src/include/controller.h
deleted file mode 100644
index a74d035..0000000
--- a/src/include/controller.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef CONTROLLER_H
-#define CONTROLLER_H
-
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdint.h>
-
-#define AXIS_MIN -32768
-#define AXIS_MAX 32767
-
-#define CHANNEL_UNUSED 65535
-#define CHANNELS 8
-
-typedef uint16_t channel_t;
-
-inline channel_t channel_clamp(channel_t channel, channel_t min, channel_t max) {
-    channel_t upper = (channel < max) ? channel : max;
-    return (min < upper) ? upper : min;
-}
-
-inline void channel_step(channel_t* channels, size_t channel_index, char step, channel_t min, channel_t max) {
-    int current = (int) channels[channel_index];
-    int next = current + step;
-    if (next > (int) max) {
-    	next = max;
-    }
-    if (next < (int) min) {
-    	next = min;
-    }
-    channels[channel_index] = (channel_t) next;
-}
-
-void channel_reset(channel_t* channels);
-void event_axis(channel_t* channels, int axis, int value);
-void event_button(channel_t* channels, int button, bool value);
-
-
-#endif
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index adb62d1..29c1240 100644
--- a/src/main.c
+++ b/src/main.c
@@ -3,156 +3,173 @@
 #include <SDL.h>
 #include "mavlink/common/mavlink.h"
 #include "controller.h"
+#include "generic.h"
 
 struct conf {
 
-    //the joystick id to use by sdl
-    int joystick_id;
+	//the joystick id to use by sdl
+	int joystick_id;
 
-    //maximum delay before the controller's state is printed
-    int timeout;
+	//maximum delay before the controller's state is printed [ms]
+	int timeout;
 
-    //mav system id of local device
-    int mav_local_system;
+	//mav system id of local device
+	int mav_local_system;
 
-    //mav component id of local device
-    int mav_local_component;
+	//mav component id of local device
+	int mav_local_component;
 
-    //mav system id of target device
-    int mav_target_system;
-
-};
-
-static int run(const struct conf* conf);
-static void mav_out(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels);
+	//mav system id of target device
+	int mav_target_system;
 
+	//the controller to use
+	struct controller* controller;
 
-int main(int argc, char *argv[]) {
-    
-    //initialize joystick library
-    if (SDL_InitSubSystem(SDL_INIT_JOYSTICK) < 0) {
-        fprintf(stderr, "Couldn't initialize SDL joystick subsystem: %s\n", SDL_GetError());
-        exit(1);
-    }
-
-    struct conf conf = {
-        .joystick_id = 0,
-        .timeout = 500,
-        .mav_local_system = 42,
-        .mav_local_component = 22,
-        .mav_target_system = 0
-    };
+};
 
-    return run(&conf);
+static void print_mav(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels);
+
+static SDL_Joystick* joystick_open(int joystick_id)
+{
+	SDL_Joystick* joystick = SDL_JoystickOpen(joystick_id);
+	if(joystick) {
+		fprintf(stderr, "Opened joystick number %d\n", joystick_id);
+		fprintf(stderr, "Name: %s\n", SDL_JoystickName(joystick));
+		fprintf(stderr, "Number of axes: %d\n", SDL_JoystickNumAxes(joystick));
+		fprintf(stderr, "Number of buttons: %d\n", SDL_JoystickNumButtons(joystick));
+		fprintf(stderr, "Number of balls: %d\n", SDL_JoystickNumBalls(joystick));
+		return joystick;
+	} else {
+		fprintf(stderr, "Couldn't open joystick number %d.\n", joystick_id);
+		return NULL;
+	}
 }
 
-static int run(const struct conf* conf) {
-
-    channel_t channels[CHANNELS];
-    channel_reset(channels);
-
-    int joystick_id = conf->joystick_id;
-
-    //pointer to joystick in use
-    SDL_Joystick* joystick = NULL;
-
-    //event loop
-    SDL_Event e;
-    bool cont = true;
-    while (cont) {
-
-        if(SDL_WaitEventTimeout(&e, conf->timeout)) {
-            switch( e.type ) {
-
-            case SDL_JOYDEVICEADDED:
-                if (e.jdevice.which == joystick_id) {
-                    fprintf(stderr, "Joystick %d connected\n", e.jdevice.which);
-                    joystick=SDL_JoystickOpen(e.jdevice.which);
-                    if(joystick) {
-                        fprintf(stderr, "Opened joystick %d\n", e.jdevice.which);
-                        fprintf(stderr, "Name: %s\n", SDL_JoystickName(joystick));
-                        fprintf(stderr, "Number of axes: %d\n", SDL_JoystickNumAxes(joystick));
-                        fprintf(stderr, "Number of buttons: %d\n", SDL_JoystickNumButtons(joystick));
-                        fprintf(stderr, "Number of balls: %d\n", SDL_JoystickNumBalls(joystick));
-                    } else {
-                        fprintf(stderr, "Couldn't open joystick %d\n", e.jdevice.which);
-                    }
-                }
-                break;
-
-            case SDL_JOYDEVICEREMOVED:
-                if (e.jdevice.which == joystick_id) {
-                    if (SDL_JoystickGetAttached(joystick)) {
-                        SDL_JoystickClose(joystick);
-                    }
-                    fprintf(stderr, "Joystick %d disconnected\n", e.jdevice.which);
-                }
-                break;
-
-            case SDL_JOYAXISMOTION:
-                event_axis(channels, e.jaxis.axis, e.jaxis.value);
-                break;
-
-            case SDL_JOYBUTTONDOWN:
-                event_button(channels, e.jbutton.button, true);
-                break;
+static int run(const struct conf* conf)
+{
+	struct controller* cont = conf->controller;
+	cont->reset(cont);
+
+	int joystick_id = conf->joystick_id;
+	
+	//initialize joystick library
+	if (SDL_InitSubSystem(SDL_INIT_JOYSTICK) < 0) {
+		fprintf(stderr, "Couldn't initialize SDL joystick subsystem: %s\n", SDL_GetError());
+		return -1;
+	}
+
+	//pointer to joystick in use
+	SDL_Joystick* joystick = joystick_open(joystick_id);
+	if (joystick == NULL) {
+		return -1;
+	}
+
+	//event loop
+	SDL_Event e;
+	bool quit = false;
+	while (!quit) {
+
+		if(SDL_WaitEventTimeout(&e, conf->timeout)) {
+			switch (e.type) {
+			case SDL_JOYDEVICEADDED:
+				fprintf(stderr, "Joystick number %d connected\n", e.jdevice.which);
+				if (e.jdevice.which == joystick_id) {
+					joystick = joystick_open(joystick_id);
+				} else {
+					fprintf(stderr, "Wrong id. Found %d, expecting %d.\n", e.jdevice.which, joystick_id);
+				}
+				break;
+
+			case SDL_JOYDEVICEREMOVED:
+				if (e.jdevice.which == joystick_id) {
+					if (SDL_JoystickGetAttached(joystick)) {
+						SDL_JoystickClose(joystick);
+					}
+					fprintf(stderr, "Joystick %d disconnected\n", e.jdevice.which);
+				}
+				break;
+
+			case SDL_JOYAXISMOTION:
+				cont->axis(cont, e.jaxis.axis, e.jaxis.value);
+				break;
+
+			case SDL_JOYBUTTONDOWN:
+				cont->button(cont, e.jbutton.button, true);
+				break;
                 
-            case SDL_JOYBUTTONUP:
-                event_button(channels, e.jbutton.button, false);
-                break;
-
-            case SDL_QUIT:
-                cont = false;
-                break;
-
-            default:
-                fprintf(stderr,"Unsupported event type: %d\n", e.type);
-                break;
-            }
-            
-        }
-        mav_out(
-            conf->mav_local_system,
-            conf->mav_local_component,
-            conf->mav_target_system,
-            channels
-        );
-    }
-    return 0;
+			case SDL_JOYBUTTONUP:
+				cont->button(cont, e.jbutton.button, false);
+				break;
+
+			case SDL_QUIT:
+				quit = true;
+				break;
+
+			default:
+				fprintf(stderr,"Unsupported event type: %d\n", e.type);
+				break;
+			}
+		}
+	
+		print_mav(
+			conf->mav_local_system,
+			conf->mav_local_component,
+			conf->mav_target_system,
+			cont->channels
+			);
+	}
+	return 0;
 }
 
-static void mav_out(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels) {
-
-    mavlink_message_t msg;
-    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-
-    mavlink_msg_rc_channels_override_pack(
-        system_id,
-        component_id,
-        &msg,
-        rsystem_id,
-        MAV_COMP_ID_ALL,
-        channels[0],
-        channels[1],
-        channels[2],
-        channels[3],
-        channels[4],
-        channels[5],
-        channels[6],
-        channels[7]
-    );
+static void print_mav(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels)
+{
+
+	mavlink_message_t msg;
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	mavlink_msg_rc_channels_override_pack(
+		system_id,
+		component_id,
+		&msg,
+		rsystem_id,
+		MAV_COMP_ID_ALL,
+		channels[0],
+		channels[1],
+		channels[2],
+		channels[3],
+		channels[4],
+		channels[5],
+		channels[6],
+		channels[7]
+		);
  
-    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
-    fwrite(buf, len, 1, stdout);
-    fflush(stdout);
-    fsync(fileno(stdout));
-
-    fprintf(stderr, "local system id: \t%u\n", system_id);
-    fprintf(stderr, "local component id: \t%u\n", component_id);
-    fprintf(stderr, "remote system id: \t%u\n", rsystem_id);
-    fprintf(stderr, "remote component id: \t%u\n", MAV_COMP_ID_ALL);
-    for (size_t i = 0; i < CHANNELS; ++i) {
-        fprintf(stderr, "channel %zu: \t\t%u\n", i, channels[i]);
-    }
-    fprintf(stderr, "\n");
-}
\ No newline at end of file
+	uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+	fwrite(buf, len, 1, stdout);
+	fflush(stdout);
+	fsync(fileno(stdout));
+
+	fprintf(stderr, "local system id: \t%u\n", system_id);
+	fprintf(stderr, "local component id: \t%u\n", component_id);
+	fprintf(stderr, "remote system id: \t%u\n", rsystem_id);
+	fprintf(stderr, "remote component id: \t%u\n", MAV_COMP_ID_ALL);
+	for (size_t i = 0; i < CHANNELS; ++i) {
+		fprintf(stderr, "channel %zu: \t\t%u\n", i, channels[i]);
+	}
+	fprintf(stderr, "\n");
+}
+
+
+int main(int argc, char *argv[])
+{
+	struct conf conf = {
+		.joystick_id = 0,
+		.timeout = 500,
+		.mav_local_system = 42,
+		.mav_local_component = 22,
+		.mav_target_system = 0,
+		.controller = &controller_generic
+	};
+
+
+	return run(&conf);
+}