git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Navigation/Sources/UI/wiimote.c

   1 /**
   2  *  \brief    Wiimote handling implementation
   3  *  \author   Aurelien Morelle <aurelien.morelle@parrot.com>
   4  *  \version  1.0
   5  *  \date     04/06/2007
   6  */
   7
   8 #include <errno.h>
   9 #include <fcntl.h>
  10 #include <string.h>
  11 #include <stddef.h>
  12 #include <stdio.h>
  13 #include <unistd.h>
  14
  15 #include <generated_custom.h>
  16
  17 #if defined(WIIMOTE_SUPPORT)
  18         //#warning Wiimote support enabled
  19         #include <bluetooth/bluetooth.h>
  20         #include <cwiid.h>
  21 #else
  22         //#warning Wiimote support disabled
  23 #endif
  24
  25 #include "wiimote.h"
  26
  27 input_device_t wiimote_device = {
  28   "Wiimote",
  29   open_wiimote,
  30   update_wiimote,
  31   close_wiimote
  32 };
  33
  34
  35 #if !defined(WIIMOTE_SUPPORT)
  36 //#warning Wiimote compilation disabled
  37 C_RESULT open_wiimote(void)
  38 {
  39   return C_FAIL;
  40 }
  41
  42 C_RESULT update_wiimote(void)
  43 {
  44   return C_FAIL;
  45 }
  46
  47 C_RESULT close_wiimote(void)
  48 {
  49   return C_OK;
  50 }
  51
  52 #else // ! WIIMOTE_SUPPORT
  53 #warning Wiimote compilation enabled
  54
  55 #include <VP_Os/vp_os_print.h>
  56
  57 static bdaddr_t bdaddr = {0};
  58 static cwiid_wiimote_t *wiimote = NULL;
  59
  60 static struct acc_cal wm_cal;
  61
  62 C_RESULT open_wiimote(void)
  63 {
  64   C_RESULT res = C_OK;
  65
  66   printf("\nSearching for Wiimote - press both '1' and '2' on your Wiimote !!\n");
  67
  68   if(cwiid_find_wiimote(&bdaddr, WIIMOTE_FIND_TIMEOUT) == 0)
  69   {
  70     if( ! (wiimote = cwiid_open(&bdaddr, 0)) )
  71       PRINT("Unable to connect to wiimote\n");
  72     else
  73       res = C_OK;
  74   }
  75
  76   if(SUCCEED(res))
  77     if(cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN|CWIID_RPT_ACC))
  78       res = C_FAIL;
  79
  80   if(SUCCEED(res))
  81     if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal))
  82       res = C_FAIL;
  83
  84   cwiid_set_led(wiimote,1);
  85
  86   printf(" ... Wiimote found.\n");
  87
  88   return res;
  89 }
  90
  91 void print_state(struct cwiid_state *state)
  92 {
  93   printf("Battery: %d%%\n", (int)(100.0 * state->battery / CWIID_BATTERY_MAX));
  94   printf("Buttons: %X\n", state->buttons);
  95   printf("Acc: x=%d y=%d z=%d\n", state->acc[CWIID_X], state->acc[CWIID_Y], state->acc[CWIID_Z]);
  96 }
  97
  98 //extern void api_set_iphone_acceleros(bool_t enable, float32_t fax, float32_t fay, float32_t faz);
  99
 100 #define NEW_AMOUNT 0.1
 101 #define OLD_AMOUNT (1.0-NEW_AMOUNT)
 102
 103 typedef struct{
 104         float x,y,z;
 105 }vec3;
 106
 107 typedef struct{
 108         float r,p,t;
 109 }vec3sph;
 110
 111
 112 static void rumble(int flag)
 113                 {
 114                 static int localflag=0;
 115                 if (flag!=localflag){
 116                         localflag=flag;
 117                         cwiid_set_rumble(wiimote,flag);
 118                 }
 119 }
 120
 121 static void leds(int flags)
 122 {
 123                 static int localflags=0;
 124                 if (flags!=localflags){
 125                         localflags=flags;
 126                         cwiid_set_led(wiimote,flags);
 127                 }
 128 }
 129
 130 C_RESULT update_wiimote(void)
 131 {
 132   C_RESULT res = C_OK;
 133   static struct cwiid_state state,previous_state;
 134
 135   static int control_mode = 1;
 136   static int valid_domain = 0;
 137
 138   int8_t x, y;
 139   static int start=0;
 140   static int select=0;
 141
 142   static vec3 a={0,0,0},s={0,0,0};
 143   static vec3sph r={0,0,0},pr={0,0,0},rref={0,0,0};
 144
 145   static float pitch=0.0f,roll=0.0f,gaz=0.0f,yaw=0.0f;
 146
 147   float tmp;
 148   static int fly=0;
 149
 150
 151
 152   if (cwiid_get_state(wiimote, &state))
 153     {
 154       fprintf(stderr, "Error getting state\n");
 155       res = C_FAIL;
 156     }
 157
 158
 159 #define SWITCHING(X) ((state.buttons&X) && !(previous_state.buttons&X))
 160 #define RELEASING(X) ((!state.buttons&X) && (previous_state.buttons&X))
 161 #define PRESSED(X) ((state.buttons&X))
 162   static int flag_rumble=0;
 163 #define RUMBLE_ON { if (!flag_rumble) {flag_rumble=1;  } }
 164   /* Sets how to use the wiimote */
 165 #define MAXMODE 4
 166   if (SWITCHING(CWIID_BTN_MINUS)) {
 167           control_mode--;
 168           if (control_mode<1) control_mode=MAXMODE;
 169           leds(1<<(control_mode-1));
 170   }
 171   if (SWITCHING(CWIID_BTN_PLUS))  {
 172           control_mode++;
 173           if (control_mode>MAXMODE) control_mode=1;
 174           leds(1<<(control_mode-1));
 175   }
 176
 177  /* Gets gravitation G projection on x,y,z axis */
 178           a.x = - (float32_t) ((((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
 179                                   (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X])));
 180           a.y = - (float32_t) ((((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
 181                                   (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y])));
 182           a.z = + (float32_t) ((((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
 183                           (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z])));
 184
 185           s.x = (a.x<0.0f)?(1.0f):(-1.0f);
 186           s.y = (a.y<0.0f)?(1.0f):(-1.0f);
 187           s.z = (a.z<0.0f)?(1.0f):(-1.0f);
 188
 189           float ax2 = a.x*a.x;
 190           float ay2 = a.y*a.y;
 191           float az2 = a.z*a.z;
 192
 193           r.r = sqrtf((ax2+ay2)+az2);
 194
 195           switch(control_mode)
 196           {
 197           case 1:
 198           case 2:
 199
 200                   if (r.r==0.0f) { r.p=r.t=0.0f; } else {
 201                           // Angle gauche/droite
 202                           r.p = asin(a.y);  if (isnan(r.p)) r.p=0.0f;
 203
 204                           // Sur plan vertical radial
 205                                   r.t = acos(a.z/(sqrtf(az2+ax2)));  if (isnan(r.t)) r.t=0.0f;
 206                                   r.t*=s.x;
 207                   }
 208                   break;
 209
 210           case 3:
 211           case 4:
 212
 213                           if (r.r==0.0f) { r.p=r.t=0.0f; } else {
 214                                   // Angle entre le projete de G sur le plan vertical longitudinal (yz) et l'axe z
 215                                           r.p = acos(a.z/(sqrtf(az2+ay2)));  if (isnan(r.p)) r.p=0.0f;
 216                                           /* If wiimote faces the ground */
 217                                             //if (a.z<0.0f) r.p= M_PI-r.p;
 218                                           r.p*=s.y;
 219                                   // Idem sur le plan vertical radial
 220                                           r.t = acos(a.z/(sqrtf(az2+ax2)));  if (isnan(r.t)) r.t=0.0f;
 221                                           r.t*=s.x;
 222                           }
 223                           break;
 224             }
 225
 226           r.r = (r.r+pr.r)/2.0f;
 227           r.t = (r.t+pr.t)/2.0f;
 228           r.p = (r.p+pr.p)/2.0f;
 229
 230
 231
 232   switch(control_mode)
 233   {
 234
 235                   case 1:
 236                   case 2:
 237                           /* Wiimote is handled horizontally.
 238                            * '2' button under left thumb
 239                            * directionnal cross under right thumb
 240                            */
 241
 242
 243                           /* 0 -> buttons facing sky */
 244
 245                           if ((SWITCHING(CWIID_BTN_1)||SWITCHING(CWIID_BTN_2)||SWITCHING(CWIID_BTN_B))){  rref=r;  }
 246
 247                           if (PRESSED(CWIID_BTN_1)||PRESSED(CWIID_BTN_2)||PRESSED(CWIID_BTN_B))
 248                           {
 249                                   /* If wiimote facing ground */
 250                                   if (a.z<0 && a.x>0)
 251                                   {
 252                                           rumble(1);
 253                                   }
 254                                   else
 255                                   {
 256                                           rumble(0);
 257                                           leds(1<<(control_mode-1));
 258                                           pitch = (r.t-rref.t)*1.0f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
 259                                           roll  = -(r.p-rref.p)*0.75f; if (roll<-1.0f)  roll=-1.0f; if (roll>1.0f) roll=1.0f;
 260                                           fly=1;
 261                                   }
 262                           }
 263                           else
 264                           {
 265                                   pitch=roll=0;
 266                                   rumble(0);
 267                                   leds(1<<(control_mode-1));
 268                                   fly=0;
 269                           }
 270
 271
 272                           gaz  = (PRESSED(CWIID_BTN_LEFT))? 1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? -1.0f : 0.0f;
 273                           yaw  = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP))   ? 1.0f : (control_mode==2) ? (-pitch*roll) : 0.0f;
 274
 275                           break;
 276
 277
 278                   case 3:
 279                   case 4:
 280
 281                           if (PRESSED(CWIID_BTN_B))
 282                           {
 283                                   if (a.z<-0.5f)
 284                                   {
 285                                           rumble(1);
 286                                   }
 287                                   else
 288                                   {
 289                                           rumble(0);
 290                                           leds(1<<(control_mode-1));
 291                                           pitch = -(r.p-rref.p)*1.5f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
 292                                           roll  = (r.t-rref.t)*1.5f; if (roll<-1.0f)  roll=-1.0f; if (roll>1.0f) roll=1.0f;
 293                                           fly=1;
 294                                   }
 295                           }
 296                           else
 297                                   {
 298                                          rumble(0);
 299                                      leds(1<<(control_mode-1));
 300                                          fly=0;
 301                                          pitch=roll=0;
 302
 303                                   }
 304
 305                           if (SWITCHING(CWIID_BTN_B))  { rref = r; }
 306
 307                           gaz  = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? +1.0f : 0.0f;
 308                           yaw  = (PRESSED(CWIID_BTN_LEFT))? -1.0f: (PRESSED(CWIID_BTN_RIGHT))   ? 1.0f : (control_mode==4) ? (-pitch*roll) : 0.0f;
 309
 310   }
 311   /* Buttons common to all modes */
 312           if (SWITCHING(CWIID_BTN_A)) { start^=1;  ardrone_tool_set_ui_pad_start(start); }
 313           if (SWITCHING(CWIID_BTN_HOME)) {
 314                                                                   select^=1;
 315                                                                   ardrone_tool_set_ui_pad_select(select);
 316                                                                   ardrone_tool_set_ui_pad_start(0);
 317                                                           }
 318
 319
 320           //
 321         //printf("Wiimote mode 2 [ax %f][ay %f][az %f]\n\033[1A", a.x,a.y,a.z);
 322         printf("Wiimode %i  [rr %3.2f][rp %3.2f][rt %3.2f]\n", control_mode,r.r,r.p,r.t);
 323         printf("[Fly %i][Ptc %3.2f][Rl %3.2f][Yw %3.2f][Gz %3.2f][Strt. %i][Sel. %i]\n\033[2A", fly,pitch,roll,yaw,gaz,start,select);
 324           ardrone_at_set_progress_cmd( fly,/*roll*/roll,/*pitch*/pitch,/*gaz*/gaz,/*yaw*/yaw);
 325
 326   /*api_set_iphone_acceleros(
 327                            (state.buttons&CWIID_BTN_2 ? 1 : 0)|(state.buttons&CWIID_BTN_A ? 2 : 0),
 328                            a_x, a_y, a_z);*/
 329
 330   previous_state = state;
 331   pr=r;
 332
 333   return C_OK;
 334 }
 335
 336 C_RESULT close_wiimote(void)
 337 {
 338   cwiid_close(wiimote);
 339
 340   return C_OK;
 341 }
 342
 343 #endif // ! WIIMOTE_SUPPORT
 344