root/libcwiid/state.c

Revision 9c5e97809d046e4ee8a5b76f80390ff22cdf1cfd, 8.6 kB (checked in by L. Donnie Smith <donnie.smith@…>, 2 years ago)

added low_speed data from-motionplus
  • Property mode set to 100644
Line 
1/* Copyright (C) 2007 L. Donnie Smith <donnie.smith@gatech.edu>
2 *
3 *  This program is free software; you can redistribute it and/or modify
4 *  it under the terms of the GNU General Public License as published by
5 *  the Free Software Foundation; either version 2 of the License, or
6 *  (at your option) any later version.
7 *
8 *  This program is distributed in the hope that it will be useful,
9 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
10 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 *  GNU General Public License for more details.
12 *
13 *  You should have received a copy of the GNU General Public License
14 *  along with this program; if not, write to the Free Software
15 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
16 *
17 */
18
19#include <string.h>
20#include <pthread.h>
21#include "cwiid_internal.h"
22
23int update_state(struct wiimote *wiimote, struct mesg_array *ma)
24{
25        int i;
26        union cwiid_mesg *mesg;
27
28        if (pthread_mutex_lock(&wiimote->state_mutex)) {
29                cwiid_err(wiimote, "Mutex lock error (state mutex)");
30                return -1;
31        }
32
33        for (i=0; i < ma->count; i++) {
34                mesg = &ma->array[i];
35
36                switch (mesg->type) {
37                case CWIID_MESG_STATUS:
38                        wiimote->state.battery = mesg->status_mesg.battery;
39                        if (wiimote->state.ext_type != mesg->status_mesg.ext_type) {
40                                memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
41                                wiimote->state.ext_type = mesg->status_mesg.ext_type;
42                        }
43                        break;
44                case CWIID_MESG_BTN:
45                        wiimote->state.buttons = mesg->btn_mesg.buttons;
46                        break;
47                case CWIID_MESG_ACC:
48                        memcpy(wiimote->state.acc, mesg->acc_mesg.acc,
49                               sizeof wiimote->state.acc);
50                        break;
51                case CWIID_MESG_IR:
52                        memcpy(wiimote->state.ir_src, mesg->ir_mesg.src,
53                               sizeof wiimote->state.ir_src);
54                        break;
55                case CWIID_MESG_NUNCHUK:
56                        memcpy(wiimote->state.ext.nunchuk.stick,
57                               mesg->nunchuk_mesg.stick,
58                               sizeof wiimote->state.ext.nunchuk.stick);
59                        memcpy(wiimote->state.ext.nunchuk.acc,
60                               mesg->nunchuk_mesg.acc,
61                               sizeof wiimote->state.ext.nunchuk.acc);
62                        wiimote->state.ext.nunchuk.buttons = mesg->nunchuk_mesg.buttons;
63                        break;
64                case CWIID_MESG_CLASSIC:
65                        memcpy(wiimote->state.ext.classic.l_stick,
66                               mesg->classic_mesg.l_stick,
67                               sizeof wiimote->state.ext.classic.l_stick);
68                        memcpy(wiimote->state.ext.classic.r_stick,
69                               mesg->classic_mesg.r_stick,
70                               sizeof wiimote->state.ext.classic.r_stick);
71                        wiimote->state.ext.classic.l = mesg->classic_mesg.l;
72                        wiimote->state.ext.classic.r = mesg->classic_mesg.r;
73                        wiimote->state.ext.classic.buttons = mesg->classic_mesg.buttons;
74                        break;
75                case CWIID_MESG_BALANCE:
76                        wiimote->state.ext.balance.right_top = mesg->balance_mesg.right_top;
77                        wiimote->state.ext.balance.right_bottom = mesg->balance_mesg.right_bottom;
78                        wiimote->state.ext.balance.left_top = mesg->balance_mesg.left_top;
79                        wiimote->state.ext.balance.left_bottom = mesg->balance_mesg.left_bottom;
80                        break;
81                case CWIID_MESG_MOTIONPLUS:
82                        memcpy(wiimote->state.ext.motionplus.angle_rate,
83                               mesg->motionplus_mesg.angle_rate,
84                               sizeof wiimote->state.ext.motionplus.angle_rate);
85                        memcpy(wiimote->state.ext.motionplus.low_speed,
86                               mesg->motionplus_mesg.low_speed,
87                               sizeof wiimote->state.ext.motionplus.low_speed);
88                        break;
89                case CWIID_MESG_ERROR:
90                        wiimote->state.error = mesg->error_mesg.error;
91                        break;
92                case CWIID_MESG_UNKNOWN:
93                        /* do nothing, error has already been printed */
94                        break;
95                }
96        }
97
98        if (pthread_mutex_unlock(&wiimote->state_mutex)) {
99                cwiid_err(wiimote, "Mutex unlock error (state mutex) - "
100                                   "deadlock warning");
101                return -1;
102        }
103
104        return 0;
105}
106
107/* IR Sensitivity Block */
108unsigned char ir_block1[] = MAX_SENSITIVITY_IR_BLOCK_1;
109unsigned char ir_block2[] = MAX_SENSITIVITY_IR_BLOCK_2;
110
111struct write_seq ir_enable10_seq[] = {
112        {WRITE_SEQ_RPT, RPT_IR_ENABLE1, (const void *)"\x04", 1, 0},
113        {WRITE_SEQ_RPT, RPT_IR_ENABLE2, (const void *)"\x04", 1, 0},
114        {WRITE_SEQ_MEM, 0xB00030, (const void *)"\x08", 1,     CWIID_RW_REG},
115        {WRITE_SEQ_MEM, 0xB00000, ir_block1, sizeof(ir_block1)-1, CWIID_RW_REG},
116        {WRITE_SEQ_MEM, 0xB0001A, ir_block2, sizeof(ir_block2)-1, CWIID_RW_REG},
117        {WRITE_SEQ_MEM, 0xB00033, (const void *)"\x01", 1,     CWIID_RW_REG}
118};
119
120struct write_seq ir_enable12_seq[] = {
121        {WRITE_SEQ_RPT, RPT_IR_ENABLE1, (const void *)"\x04", 1, 0},
122        {WRITE_SEQ_RPT, RPT_IR_ENABLE2, (const void *)"\x04", 1, 0},
123        {WRITE_SEQ_MEM, 0xB00030, (const void *)"\x08", 1,     CWIID_RW_REG},
124        {WRITE_SEQ_MEM, 0xB00000, ir_block1, sizeof(ir_block1)-1, CWIID_RW_REG},
125        {WRITE_SEQ_MEM, 0xB0001A, ir_block2, sizeof(ir_block2)-1, CWIID_RW_REG},
126        {WRITE_SEQ_MEM, 0xB00033, (const void *)"\x03", 1,     CWIID_RW_REG}
127};
128
129struct write_seq ir_disable_seq[] = {
130        {WRITE_SEQ_RPT, RPT_IR_ENABLE1, (const void *)"\x00", 1, 0},
131        {WRITE_SEQ_RPT, RPT_IR_ENABLE2, (const void *)"\x00", 1, 0}
132};
133
134#define RPT_MODE_BUF_LEN 2
135int update_rpt_mode(struct wiimote *wiimote, int8_t rpt_mode)
136{
137        unsigned char buf[RPT_MODE_BUF_LEN];
138        uint8_t rpt_type;
139        struct write_seq *ir_enable_seq;
140        int seq_len;
141
142        /* rpt_mode = bitmask of requested report types */
143        /* rpt_type = report id sent to the wiimote */
144        if (pthread_mutex_lock(&wiimote->rpt_mutex)) {
145                cwiid_err(wiimote, "Mutex lock error (rpt mutex)");
146                return -1;
147        }
148
149        /* -1 updates the reporting mode using old rpt_mode
150         * (reporting type may change if extensions are
151         * plugged in/unplugged */
152        if (rpt_mode == -1) {
153                rpt_mode = wiimote->state.rpt_mode;
154        }
155
156        /* Pick a report mode based on report flags */
157        if ((rpt_mode & CWIID_RPT_EXT) &&
158          ((wiimote->state.ext_type == CWIID_EXT_NUNCHUK) ||
159           (wiimote->state.ext_type == CWIID_EXT_CLASSIC) ||
160           (wiimote->state.ext_type == CWIID_EXT_MOTIONPLUS))) {
161                if ((rpt_mode & CWIID_RPT_IR) && (rpt_mode & CWIID_RPT_ACC)) {
162                        rpt_type = RPT_BTN_ACC_IR10_EXT6;
163                        ir_enable_seq = ir_enable10_seq;
164                        seq_len = SEQ_LEN(ir_enable10_seq);
165                }
166                else if (rpt_mode & CWIID_RPT_IR) {
167                        rpt_type = RPT_BTN_IR10_EXT9;
168                        ir_enable_seq = ir_enable10_seq;
169                        seq_len = SEQ_LEN(ir_enable10_seq);
170                }
171                else if (rpt_mode & CWIID_RPT_ACC) {
172                        rpt_type = RPT_BTN_ACC_EXT16;
173                }
174                else if (rpt_mode & CWIID_RPT_BTN) {
175                        rpt_type = RPT_BTN_EXT8;
176                }
177                else {
178                        rpt_type = RPT_EXT21;
179                }       
180        }
181        else if ((rpt_mode & CWIID_RPT_EXT) &&
182          wiimote->state.ext_type == CWIID_EXT_BALANCE) {
183                rpt_type = RPT_BTN_EXT8;
184        }
185        else {
186                if (rpt_mode & CWIID_RPT_IR) {
187                        rpt_type = RPT_BTN_ACC_IR12;
188                        ir_enable_seq = ir_enable12_seq;
189                        seq_len = SEQ_LEN(ir_enable12_seq);
190                }
191                else if (rpt_mode & CWIID_RPT_ACC) {
192                        rpt_type = RPT_BTN_ACC;
193                }
194                else {
195                        rpt_type = RPT_BTN;
196                }
197        }
198
199        /* Enable IR */
200        /* TODO: only do this when necessary (record old IR mode) */
201        if ((rpt_mode & CWIID_RPT_IR)) {
202                if (exec_write_seq(wiimote, seq_len, ir_enable_seq)) {
203                        cwiid_err(wiimote, "IR enable error");
204                        return -1;
205                }
206        }
207        /* Disable IR */
208        else if ((wiimote->state.rpt_mode & CWIID_RPT_IR) &&
209                 !(rpt_mode & CWIID_RPT_IR)) {
210                if (exec_write_seq(wiimote, SEQ_LEN(ir_disable_seq), ir_disable_seq)) {
211                        cwiid_err(wiimote, "IR disable error");
212                        return -1;
213                }
214        }
215
216        /* Send SET_REPORT */
217        buf[0] = (wiimote->flags & CWIID_FLAG_CONTINUOUS) ? 0x04 : 0;
218        buf[1] = rpt_type;
219        if (cwiid_send_rpt(wiimote, 0, RPT_RPT_MODE, RPT_MODE_BUF_LEN, buf)) {
220                cwiid_err(wiimote, "Send report error (report mode)");
221                return -1;
222        }
223
224        /* clear state for unreported data */
225        if (CWIID_RPT_BTN & ~rpt_mode & wiimote->state.rpt_mode) {
226                wiimote->state.buttons = 0;
227        }
228        if (CWIID_RPT_ACC & ~rpt_mode & wiimote->state.rpt_mode) {
229                memset(wiimote->state.acc, 0, sizeof wiimote->state.acc);
230        }
231        if (CWIID_RPT_IR & ~rpt_mode & wiimote->state.rpt_mode) {
232                memset(wiimote->state.ir_src, 0, sizeof wiimote->state.ir_src);
233        }
234        if ((wiimote->state.ext_type == CWIID_EXT_NUNCHUK) &&
235          (CWIID_RPT_NUNCHUK & ~rpt_mode & wiimote->state.rpt_mode)) {
236                memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
237        }
238        else if ((wiimote->state.ext_type == CWIID_EXT_CLASSIC) &&
239          (CWIID_RPT_CLASSIC & ~rpt_mode & wiimote->state.rpt_mode)) {
240                memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
241        }
242        else if ((wiimote->state.ext_type == CWIID_EXT_BALANCE) &&
243          (CWIID_RPT_BALANCE & ~rpt_mode & wiimote->state.rpt_mode)) {
244                memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
245        }
246        else if ((wiimote->state.ext_type == CWIID_EXT_MOTIONPLUS) &&
247          (CWIID_RPT_MOTIONPLUS & ~rpt_mode & wiimote->state.rpt_mode)) {
248                memset(&wiimote->state.ext, 0, sizeof wiimote->state.ext);
249        }
250
251        wiimote->state.rpt_mode = rpt_mode;
252
253        if (pthread_mutex_unlock(&wiimote->rpt_mutex)) {
254                cwiid_err(wiimote, "Mutex unlock error (rpt mutex) - "
255                          "deadlock warning");
256                return -1;
257        }
258
259        return 0;
260}
Note: See TracBrowser for help on using the browser.