From 588ff9cd2baa942a85cc938bd22410637b08e274 Mon Sep 17 00:00:00 2001 From: Daniel Friesel Date: Mon, 26 Dec 2011 22:51:14 +0100 Subject: Add acclog, reorganize plotting --- .gitignore | 1 + Makefile | 7 +++-- acclog.c | 83 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ mpplot/acc | 13 ++++++++++ mpplot/loop | 10 +++++--- mpplot/mplus | 12 +++++++++ mpplot/plot | 12 --------- 7 files changed, 121 insertions(+), 17 deletions(-) create mode 100644 acclog.c create mode 100644 mpplot/acc create mode 100644 mpplot/mplus delete mode 100644 mpplot/plot diff --git a/.gitignore b/.gitignore index 2465242..49ba942 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +/acclog /bal /mpcal /mplog diff --git a/Makefile b/Makefile index 2f9f4ac..76c8326 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,10 @@ -CFLAGS = -Wall -Wextra -pedantic -Wno-unused-parameter +CFLAGS = -Wall -Wextra -pedantic -Wno-unused-parameter -ggdb LDFLAGS = -lbluetooth -lcwiid -all: bal mpcal mplog wibble wiispkr +all: acclog bal mpcal mplog wibble wiispkr + +acclog: acclog.c + ${CC} ${CFLAGS} ${LDFLAGS} -lm -o $@ $< bal: bal.c ${CC} ${CFLAGS} ${LDFLAGS} -o $@ $< diff --git a/acclog.c b/acclog.c new file mode 100644 index 0000000..cf172b4 --- /dev/null +++ b/acclog.c @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2011 by Daniel Friesel + * License: WTFPL + * 0. You just DO WHAT THE FUCK YOU WANT TO. + */ + +#include +#include +#include +#include +#include +#include +#include + +cwiid_mesg_callback_t cwiid_callback; + +struct acc_cal wm_cal; + +int main() +{ + cwiid_wiimote_t *wiimote = NULL; + + if ((wiimote = cwiid_open(BDADDR_ANY, 0)) == NULL) { + fputs("Unable to connect\n", stderr); + return EXIT_FAILURE; + } + + sleep(2); + + cwiid_set_led(wiimote, (1 << 3) | (1)); + + if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal)) + fputs("unable to retrieve accelerometer calibration\n", stderr); + + if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) + fputs("cannot set callback\n", stderr); + + if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) + fputs("cannot enable callback\n", stderr); + + if (cwiid_set_rpt_mode(wiimote, + CWIID_RPT_ACC)) + fputs("cannot set report mode\n", stderr); + + while (1) { + /* nothing to do here */ + sleep(1); + } + + return EXIT_SUCCESS; +} + +void handle_acc(cwiid_wiimote_t *wiimote, struct cwiid_acc_mesg *am, + struct timespec *ts) +{ + double a_x = ((double)am->acc[CWIID_X] - wm_cal.zero[CWIID_X]) / + (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X]); + double a_y = ((double)am->acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) / + (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y]); + double a_z = ((double)am->acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) / + (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z]); + double accel = sqrt(pow(a_x,2)+pow(a_y,2)+pow(a_z,2)); + double roll = atan(a_x / a_z); + if (a_z <= 0.0) { + roll += 3.14159265358979323 * ((a_x > 0.0) ? 1 : -1); + } + double pitch = atan( a_y / a_z * cos(roll)); + + printf("%ld.%09ld %f %f %f %f %f %f\n", + ts->tv_sec, ts->tv_nsec, + a_x, a_y, a_z, + accel, roll, pitch + ); +} + +void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count, + union cwiid_mesg mesg[], struct timespec *ts) +{ + for (int i = 0; i < mesg_count; i++) + if (mesg[i].type == CWIID_MESG_ACC) + handle_acc(wiimote, &mesg[i].acc_mesg, ts); +} + diff --git a/mpplot/acc b/mpplot/acc new file mode 100644 index 0000000..d0ba7db --- /dev/null +++ b/mpplot/acc @@ -0,0 +1,13 @@ +set xlabel "Time" +set ylabel "Sensor Value" +#set yrange [0:16000] + +set size 1.0, 0.6 +set terminal postscript portrait enhanced mono dashed lw 1 "Helvetica" 14 + +set title "Sensors" +set output '/tmp/wii.ps' +plot '/tmp/wii.tail' using 1:2 with lines lt 1 lc rgb "green" title "X", \ + '/tmp/wii.tail' using 1:3 with lines lt 1 lc rgb "blue" title "Y", \ + '/tmp/wii.tail' using 1:4 with lines lt 1 lc rgb "red" title "Z", \ + '/tmp/wii.tail' using 1:5 with lines lt 2 lc rgb "black" title "A" diff --git a/mpplot/loop b/mpplot/loop index 51840db..cc1d47e 100755 --- a/mpplot/loop +++ b/mpplot/loop @@ -1,8 +1,12 @@ #!/bin/sh +set -e + +test -r "$1" + while true; do - tail -n 2000 /tmp/wibble.log | head -n 1999 > /tmp/wibble.tail - gnuplot plot - convert /tmp/mp.ps /tmp/mp.png + tail -n 2000 /tmp/wii.log | head -n 1999 > /tmp/wii.tail + gnuplot "$1" + convert /tmp/wii.ps /tmp/wii.png date done diff --git a/mpplot/mplus b/mpplot/mplus new file mode 100644 index 0000000..20c2bc1 --- /dev/null +++ b/mpplot/mplus @@ -0,0 +1,12 @@ +set xlabel "Time" +set ylabel "Sensor Value" +#set yrange [0:16000] + +set size 1.0, 0.6 +set terminal postscript portrait enhanced mono dashed lw 1 "Helvetica" 14 + +set title "Sensors" +set output '/tmp/wii.ps' +plot '/tmp/wii.tail' using 1:2 with lines lt 1 lc rgb "green" title "Pitch", \ + '/tmp/wii.tail' using 1:3 with lines lt 1 lc rgb "blue" title "Roll", \ + '/tmp/wii.tail' using 1:4 with lines lt 1 lc rgb "red" title "Yaw" diff --git a/mpplot/plot b/mpplot/plot deleted file mode 100644 index a5f1383..0000000 --- a/mpplot/plot +++ /dev/null @@ -1,12 +0,0 @@ -set xlabel "Time" -set ylabel "Sensor Value" -#set yrange [0:16000] - -set size 1.0, 0.6 -set terminal postscript portrait enhanced mono dashed lw 1 "Helvetica" 14 - -set title "Sensors" -set output '/tmp/mp.ps' -plot '/tmp/wibble.tail' using 1:2 with lines lt 1 lc rgb "green" title "Pitch", \ - '/tmp/wibble.tail' using 1:3 with lines lt 1 lc rgb "blue" title "Roll", \ - '/tmp/wibble.tail' using 1:4 with lines lt 1 lc rgb "red" title "Yaw" -- cgit v1.2.3