Index: cwiid-trunk/libcwiid/command.c =================================================================== --- cwiid-trunk/libcwiid/command.c (revision 90) +++ cwiid-trunk/libcwiid/command.c (working copy) @@ -186,7 +186,7 @@ } /* Send SET_REPORT */ - buf[0]=0; + buf[0]=0x0; buf[1]=rpt_mode; if (send_report(wiimote, 0, RPT_RPT_MODE, RPT_MODE_BUF_LEN, buf)) { cwiid_err(wiimote, "Error setting report state"); Index: cwiid-trunk/configure.ac =================================================================== --- cwiid-trunk/configure.ac (revision 90) +++ cwiid-trunk/configure.ac (working copy) @@ -102,6 +102,7 @@ [wminput/plugins/Makefile] [wminput/plugins/ir_ptr/Makefile] [wminput/plugins/acc/Makefile] + [wminput/plugins/acc_move/Makefile] [wminput/plugins/nunchuk_acc/Makefile] [lswm/Makefile] ) Index: cwiid-trunk/wminput/plugins/Makefile.in =================================================================== --- cwiid-trunk/wminput/plugins/Makefile.in (revision 90) +++ cwiid-trunk/wminput/plugins/Makefile.in (working copy) @@ -2,7 +2,7 @@ include @top_builddir@/defs.mak -PLUGINS = ir_ptr acc nunchuk_acc +PLUGINS = ir_ptr acc nunchuk_acc acc_move all install clean distclean uninstall: TARGET += $(MAKECMDGOALS) Index: cwiid-trunk/wminput/plugins/acc_move/Makefile.in =================================================================== --- cwiid-trunk/wminput/plugins/acc_move/Makefile.in (revision 0) +++ cwiid-trunk/wminput/plugins/acc_move/Makefile.in (revision 0) @@ -0,0 +1,16 @@ +#Copyright (C) 2007 L. Donnie Smith + +include @top_builddir@/defs.mak + +PLUGIN_NAME = acc_move +SOURCES = acc_move.c +CFLAGS += -I@top_builddir@/wminput -I@top_builddir@/libcwiid +LDLIBS += -lm +INST_DIR = $(CWIID_PLUGINS_DIR) + +include $(COMMON)/include/plugin.mak + +distclean: clean + rm Makefile + +.PHONY: distclean Index: cwiid-trunk/wminput/plugins/acc_move/acc_move.c =================================================================== --- cwiid-trunk/wminput/plugins/acc_move/acc_move.c (revision 0) +++ cwiid-trunk/wminput/plugins/acc_move/acc_move.c (revision 0) @@ -0,0 +1,277 @@ +/* Copyright (C) 2007 L. Donnie Smith + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + * ChangeLog: + * 2007-04-15 Martin Wickman + * * Uses timestep integration for position/velocity + * + * 2007-03-01 L. Donnie Smith + * * Initial ChangeLog + * * made global variables static + */ + +#include +#include + +#include "wmplugin.h" + + +struct vec3 { + double x; + double y; + double z; +}; + +#define INTEGRATE_RECT 1 +#define INTEGRATE_TRAP 2 + +static unsigned char info_init = 0; +static struct wmplugin_info info; +static struct wmplugin_data data; + +static struct vec3 acc_zero, acc_one; + +static int plugin_id; + +wmplugin_info_t wmplugin_info; +wmplugin_init_t wmplugin_init; +wmplugin_exec_t wmplugin_exec; +static void process_acc(struct cwiid_acc_mesg *mesg); + +struct wmplugin_info *wmplugin_info() { + if (!info_init) { + info.button_count = 0; + info.axis_count = 2; + info.axis_info[0].name = "X"; + info.axis_info[0].type = WMPLUGIN_REL; + info.axis_info[0].max = 0; + info.axis_info[0].min = 0; + info.axis_info[0].fuzz = 0; + info.axis_info[0].flat = 0; + info.axis_info[1].name = "Y"; + info.axis_info[1].type = WMPLUGIN_REL; + info.axis_info[1].max = 0; + info.axis_info[1].min = 0; + info.axis_info[1].fuzz = 0; + info.axis_info[1].flat = 0; + info.param_count = 5; + info.param_info[0].name = "XScale"; + info.param_info[0].type = WMPLUGIN_PARAM_FLOAT; + info.param_info[0].value.Float = 1.0; + info.param_info[1].name = "YScale"; + info.param_info[1].type = WMPLUGIN_PARAM_FLOAT; + info.param_info[1].value.Float = 1.0; + info.param_info[2].name = "IntegrationType"; + info.param_info[2].type = WMPLUGIN_PARAM_INT; + info.param_info[2].value.Int = INTEGRATE_RECT; + info.param_info[3].name = "ZeroCountLimit"; + info.param_info[3].type = WMPLUGIN_PARAM_INT; + info.param_info[3].value.Int = 25; + info.param_info[4].name = "ZeroWindow"; + info.param_info[4].type = WMPLUGIN_PARAM_INT; + info.param_info[4].value.Int = 5; + + info_init = 1; + } + return &info; +} + +static struct timeval time_prev; +static struct vec3 a0, a1, v0, v1, p0, p1, zero_count; +static int zero_count_limit, zero_window; + +int wmplugin_init(int id, cwiid_wiimote_t *wiimote) +{ + unsigned char buf[7]; + + plugin_id = id; + + data.buttons = 0; + data.axes[0].valid = 1; + data.axes[1].valid = 1; + if (wmplugin_set_report_mode(id, CWIID_RPT_ACC)) { + return -1; + } + + if (cwiid_read(wiimote, CWIID_RW_EEPROM, 0x16, 7, buf)) { + wmplugin_err(id, "unable to read wiimote info"); + return -1; + } + + acc_zero.x = buf[0]; + acc_zero.y = buf[1]; + acc_zero.z = buf[2]; + acc_one.x = buf[4]; + acc_one.y = buf[5]; + acc_one.z = buf[6]; + + /* + // Manually calibrated values for a specific wiimote + acc_zero.x = 125.75; + acc_zero.y = 126.2; + */ + + gettimeofday(&time_prev, NULL); + + return 0; +} + +struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg *mesg[]) +{ + int i; + struct wmplugin_data *ret = NULL; + + for (i=0; i < mesg_count; i++) { + switch (mesg[i]->type) { + case CWIID_MESG_ACC: + process_acc(&mesg[i]->acc_mesg); + ret = &data; + break; + default: + break; + } + } + + return ret; +} + +static void step_rectangle(float dt) +{ + /* Zero window */ + if (a1.x <= zero_window && a1.x >= -zero_window) + a1.x = 0.0f; + if (a1.y <= zero_window && a1.y >= -zero_window) + a1.y = 0.0f; + if (a1.z <= zero_window && a1.z >= -zero_window) + a1.z = 0.0f; + + /* Integrate */ + v1.x += a1.x * dt; + v1.y += a1.y * dt; + v1.z += a1.z * dt; + p1.x += v1.x * dt; + p1.y += v1.y * dt; + p1.z += v1.z * dt; + + /* Movement-end check: stop movement if acc=0 for some time */ + zero_count.x = fabs(a1.x) < 0.0001f ? zero_count.x + 1 : 0; + v1.x = zero_count.x >= zero_count_limit ? 0 : v1.x; + + zero_count.y = fabs(a1.y) < 0.0001f ? zero_count.y + 1 : 0; + v1.y = zero_count.y >= zero_count_limit ? 0 : v1.y; + + zero_count.z = fabs(a1.z) < 0.0001f ? zero_count.z + 1 : 0; + v1.z = zero_count.z >= zero_count_limit ? 0 : v1.z; +} + + +static void step_trapezium(float dt) +{ + /* Zero window */ + if (a1.x <= zero_window && a1.x >= -zero_window) + a1.x = 0.0f; + if (a1.y <= zero_window && a1.y >= -zero_window) + a1.y = 0.0f; + if (a1.z <= zero_window && a1.z >= -zero_window) + a1.z = 0.0f; + + /* Integrate */ + v1.x = v0.x + a0.x*dt + (a1.x - a0.x) * 0.5f*dt; + v1.y = v0.y + a0.y*dt + (a1.y - a0.y) * 0.5f*dt; + v1.z = v0.z + a0.z*dt + (a1.z - a0.z) * 0.5f*dt; + + p1.x = p0.x + v0.x*dt + (v1.x - v0.x) * 0.5f*dt; + p1.y = p0.y + v0.y*dt + (v1.y - v0.y) * 0.5f*dt; + p1.z = p0.z + v0.z*dt + (v1.z - v0.z) * 0.5f*dt; + + + a0.x = a1.x; + a0.y = a1.y; + a0.z = a1.z; + + v0.x = v1.x; + v0.y = v1.y; + v0.z = v1.z; + + /* Movement-end check: stop movement if acc=0 for some time */ + zero_count.x = fabs(a1.x) < 0.0001f ? zero_count.x + 1 : 0; + v1.x = zero_count.x >= zero_count_limit ? 0 : v1.x; + v0.x = zero_count.x >= zero_count_limit ? 0 : v0.x; + + zero_count.y = fabs(a1.y) < 0.0001f ? zero_count.y + 1 : 0; + v1.y = zero_count.y >= zero_count_limit ? 0 : v1.y; + v0.y = zero_count.y >= zero_count_limit ? 0 : v0.y; + + zero_count.z = fabs(a1.z) < 0.0001f ? zero_count.z + 1 : 0; + v1.z = zero_count.z >= zero_count_limit ? 0 : v1.z; + v0.z = zero_count.z >= zero_count_limit ? 0 : v0.z; +} + +static void step (float dt) +{ + int type = info.param_info[2].value.Int; + zero_count_limit = info.param_info[3].value.Int; + zero_window = info.param_info[4].value.Int; + + if (type == INTEGRATE_RECT) + step_rectangle(dt); + else if (type == INTEGRATE_TRAP) + step_trapezium(dt); + else + step_rectangle(dt); +} + +static void process_acc(struct cwiid_acc_mesg *mesg) +{ + unsigned int ticks; + struct timeval now; + gettimeofday(&now, NULL); + ticks = (now.tv_sec - time_prev.tv_sec) * 1000 + + (now.tv_usec - time_prev.tv_usec) / 1000; + float dt = ticks / 1000.0f; + gettimeofday(&time_prev, NULL); + + a1.x = -(((double)mesg->x - acc_zero.x)); + a1.y = (((double)mesg->y - acc_zero.y)); + a1.z = (((double)mesg->z - acc_zero.z)); + + a1.z = 0; + + const float timestep = 0.010; /* 10ms */ + + int i = 0; + + /* Some semi-clever timehandling */ + if (dt <= timestep) { + step(dt); + } else { + while (dt > timestep) { + i++; + dt -= timestep; + step(timestep); + } + } + /* + printf ("steps: %d dt: %.5f a: %.2f %.2f %.2f " + "v: %.2f %.2f %.2f p: %.2f %.2f %.2f old:%d %d \n", + i, dt, a1.x, a1.y, a1.z, v1.x, v1.y, v1.z, + p1.x, p1.y, p1.z, data.axes[0].value, data.axes[1].value); + */ + + data.axes[0].value = v1.x * info.param_info[0].value.Float; + data.axes[1].value = v1.y * info.param_info[1].value.Float; +} + Index: cwiid-trunk/wminput/configs/acc_move =================================================================== --- cwiid-trunk/wminput/configs/acc_move (revision 0) +++ cwiid-trunk/wminput/configs/acc_move (revision 0) @@ -0,0 +1,12 @@ +#acc_move + +include buttons + +Plugin.acc_move.X = REL_X +Plugin.acc_move.Y = REL_Y + +Plugin.acc_move.XScale = 10 +Plugin.acc_move.YScale = 10 +Plugin.acc_move.IntegrationType = 1 # 1 for rect, 2 for trapezium +Plugin.acc_move.ZeroCountLimit = 20 +Plugin.acc_move.ZeroWindow = 5