src/lib/ecore_psl1ght/gemutil.c

   1 #include <stdio.h>
   2 #include <io/move.h>
   3 #include <sys/memory.h>
   4 #include <ppu-types.h>
   5 #include <io/camera.h>
   6 #include <ppu-types.h>
   7 #include <spurs/spurs.h>
   8 #include <sys/spu.h>
   9 #include <sys/thread.h>
  10 #include <sys/systime.h>
  11
  12 #define SPURS_DEFAULT_PREFIX_NAME "gem_spurs"
  13
  14 u16 oldGemPad = 0;
  15 u16 newGemPad = 0;
  16 u16 newGemAnalogT = 0;
  17 extern cameraReadInfo camread;
  18
  19 static inline float
  20 vec_array(vec_float4 vec, unsigned int idx)
  21 {
  22    union {
  23       vec_float4 vec;
  24       float      array[4];
  25    } v;
  26
  27    v.vec = vec;
  28
  29    if (idx > 3)
  30      return -1;
  31    return v.array[idx];
  32 }
  33
  34 int
  35 initMove()
  36 {
  37    Spurs *spurs;
  38    gemAttribute gem_attr;
  39    u8 gem_spu_priorities[8] = { 1, 0, 0, 0, 0, 0, 0, 0 };
  40    int ret;
  41    int i;
  42
  43    spurs = initSpurs (NULL);
  44
  45    if (spurs == NULL)
  46      goto error;
  47
  48    printf ("preparing GemAttribute structure with spurs\n");
  49
  50    initAttributeGem (&gem_attr, 1, NULL, spurs, gem_spu_priorities);
  51    gem_attr->version = 2;
  52    gem_attr->max = 1;
  53    gem_attr->spurs = spurs;
  54    gem_attr->memory = NULL;
  55    gem_attr->spu_priorities[0] = 1;
  56    for (i = 1; i < 8; ++i)
  57      gem_attr->spu_priorities[i] = 0;
  58
  59    printf ("calling GemInit with GemAttribute structure version=%d max_connect=%d spurs=%X\n",
  60            gem_attr.version, gem_attr.max, gem_attr.spurs);
  61    ret = gemInit (&gem_attr);
  62    printf ("return from GemInit %X \n", ret);
  63    if (ret)
  64      goto error;
  65
  66    ret = gemPrepareCamera (128, 0.5);
  67    printf ("GemPrepareCamera return %d exposure set to 128 and quality to 0.5\n",
  68            ret);
  69    if (ret)
  70      goto error;
  71    ret = gemReset (0);
  72    printf ("GemReset return %X \n", ret);
  73    if (ret)
  74      goto error;
  75    return ret;
  76
  77 error:
  78    if (spurs)
  79      endSpurs (spurs);
  80    return NULL;
  81 }
  82
  83 int
  84 endMove()
  85 {
  86    endSpurs (spurs);
  87    gemEnd ();
  88    return 0;
  89 }
  90
  91 int
  92 proccessGem(int t)
  93 {
  94    int ret;
  95
  96    switch (t) {
  97       case 0:
  98
  99         ret = gemUpdateStart (camread.buffer, camread.timestamp);
 100
 101         if (ret != 0)
 102           {
 103              printf ("Return from gemUpdateStart %X\n", ret);
 104           }
 105         break;
 106
 107       case 2:
 108
 109         ret = gemUpdateFinish ();
 110         if (ret != 0)
 111           {
 112              printf ("Return from gemUpdateFinish %X\n", ret);
 113           }
 114         break;
 115
 116       case 3:
 117         ret = gemConvertVideoFinish ();
 118         if (ret != 0)
 119           {
 120              printf ("Return from gemConvertVideoFinish %X\n", ret);
 121           }
 122         break;
 123
 124       default:
 125         ret = -1;
 126         break;
 127      }
 128    return ret;
 129 }
 130
 131 void
 132 readGemPad(int num_gem)
 133 {
 134    gemState gem_state;
 135    int ret;
 136    unsigned int hues[] = { 4 << 24, 4 << 24, 4 << 24, 4 << 24 };
 137    ret = gemGetState (0, 0, -22000, &gem_state);
 138
 139    newGemPad = gem_state.paddata.buttons & (~oldGemPad);
 140    newGemAnalogT = gem_state.paddata.ANA_T;
 141    oldGemPad = gem_state.paddata.buttons;
 142
 143    switch (ret) {
 144       case 2:
 145         gemForceRGB (num_gem, 0.5, 0.5, 0.5);
 146         break;
 147
 148       case 5:
 149
 150         gemTrackHues (hues, NULL);
 151         break;
 152
 153       default:
 154         break;
 155      }
 156 }
 157
 158 void
 159 readGemAccPosition(int num_gem)
 160 {
 161    vec_float4 position;
 162
 163    gemGetAccelerometerPositionInDevice (num_gem, &position);
 164
 165    printf (" accelerometer device coordinates [%f,%f,%f,%f]\n",
 166            vec_array (position, 0), vec_array (position, 1), vec_array (position, 2),
 167            vec_array (position, 3));
 168 }
 169
 170 void
 171 readGemInertial(int num_gem)
 172 {
 173    gemInertialState gem_inertial_state;
 174    int ret;
 175
 176    ret = gemGetInertialState (num_gem, 0, -22000, &gem_inertial_state);
 177    printf ("gemGetInertialState return %X\n", ret);
 178    printf ("counter %d temperature %f\n", gem_inertial_state.counter,
 179            gem_inertial_state.temperature);
 180
 181    printf (" accelerometer sensor   [%f,%f,%f,%f]\n",
 182            vec_array (gem_inertial_state.accelerometer, 0),
 183            vec_array (gem_inertial_state.accelerometer, 1),
 184            vec_array (gem_inertial_state.accelerometer, 2),
 185            vec_array (gem_inertial_state.accelerometer, 3));
 186
 187    printf (" accelerometer_bias sensor   [%f,%f,%f,%f]\n",
 188            vec_array (gem_inertial_state.accelerometer_bias, 0),
 189            vec_array (gem_inertial_state.accelerometer_bias, 1),
 190            vec_array (gem_inertial_state.accelerometer_bias, 2),
 191            vec_array (gem_inertial_state.accelerometer_bias, 3));
 192
 193    printf (" gyro sensor  [%f,%f,%f,%f]\n", vec_array (gem_inertial_state.gyro,
 194                                                        0), vec_array (gem_inertial_state.gyro, 1),
 195            vec_array (gem_inertial_state.gyro, 2),
 196            vec_array (gem_inertial_state.gyro, 3));
 197
 198    printf (" gyro_bias sensor  [%f,%f,%f,%f]\n",
 199            vec_array (gem_inertial_state.gyro_bias, 0),
 200            vec_array (gem_inertial_state.gyro_bias, 1),
 201            vec_array (gem_inertial_state.gyro_bias, 2),
 202            vec_array (gem_inertial_state.gyro_bias, 3));
 203 }
 204
 205 void
 206 readGem()
 207 {
 208    gemState gem_state;
 209    proccessGem (0);
 210
 211    proccessGem (2);
 212
 213    readGemPad (0); // This will read buttons from Move
 214    switch (newGemPad) {
 215       case 1:
 216         printf ("Select pressed \n");
 217         break;
 218
 219       case 2:
 220         printf ("T pressed value %d\n", newGemAnalogT);
 221         printf
 222           ("Frame %d center of the sphere in world coordinates %f %f %f %f \n",
 223           camread.frame, vec_array (gem_state.pos, 0), vec_array (gem_state.pos,
 224                                                                   1), vec_array (gem_state.pos, 2), vec_array (gem_state.pos, 3));
 225         break;
 226
 227       case 4:
 228         printf ("Move pressed \n");
 229         gemCalibrate (0);
 230         break;
 231
 232       case 8:
 233         printf ("Start pressed \n");
 234         pos_x = 0;
 235         pos_y = 0;
 236         break;
 237
 238       case 16:
 239         printf ("Triangle pressed \n");
 240         getImageState ();
 241         break;
 242
 243       case 32:
 244         printf ("Circle pressed \n");
 245         break;
 246
 247       case 64:
 248         printf ("Cross pressed \n");
 249         printf ("X,Y,Z position (mm) %f %f %f\n", vec_array (gem_state.pos, 0),
 250                 vec_array (gem_state.pos, 1), vec_array (gem_state.pos, 2));
 251         readGemAccPosition (0);
 252         break;
 253
 254       case 128:
 255         printf ("Square pressed \n");
 256         readGemInertial (0);
 257         break;
 258
 259       default:
 260         break;
 261      }
 262 }
 263
 264 void
 265 getImageState()
 266 {
 267    int ret;
 268
 269    gemImageState imgState;
 270
 271    gemGetImageState (0, &imgState);
 272    printf (" u  [%f]\n", imgState.u);
 273    printf (" v  [%f]\n", imgState.v);
 274    printf (" r  [%f]\n", imgState.r);
 275    printf (" projectionx  [%f]\n", imgState.projectionx);
 276    printf (" projectiony  [%f]\n", imgState.projectiony);
 277    printf (" distance  [%f]\n", imgState.distance);
 278    printf ("visible=%d r_valid=%d\n", imgState.visible, imgState.r_valid);
 279    printf ("tiemestamp=%Ld\n", imgState.frame_time);
 280 }
 281