PageRenderTime 6ms CodeModel.GetById 23ms app.highlight 73ms RepoModel.GetById 1ms app.codeStats 1ms

/Current_Code/Libraries/cppfiles/tests/Individual_Sensor_Tests/LIS331/LIS331.cpp

https://github.com/VT-ATEx/Core-2014
C++ | 912 lines | 618 code | 261 blank | 33 comment | 57 complexity | 572aecde5bff67fba0acf44e6d89ac52 MD5 | raw file
  1#ifndef LIS331_cpp
  2#define LIS331_cpp
  3#include "LIS331.h"
  4#include <iostream>
  5
  6using namespace std;
  7using std::cout;
  8
  9
 10void *update(void *ptr);
 11
 12LIS331::LIS331(const int bus, const int address)
 13{
 14
 15    char *device = new char[50];
 16    sprintf(device, "/dev/i2c-%d", bus);
 17
 18    if ((i2cfdacc = open(device, O_RDWR)) < 0)
 19    {
 20        sprintf(device, "Failed to open %s", device);
 21        perror(device);
 22    }
 23    delete[] device;
 24    char *addr = new char[50];
 25
 26    sprintf(addr, "0x%x", address);
 27
 28    if (ioctl(i2cfdacc, I2C_SLAVE, address) < 0)
 29    {
 30        sprintf(addr, "Start error with address %s", addr);
 31        perror(addr);
 32        delete[] addr;
 33
 34    }
 35    delete[] addr;
 36
 37
 38    //runs self test to ensure sensor functionality
 39
 40    char tx[2];
 41
 42    tx[0] = 0x20;
 43    tx[1] = 0x37;
 44
 45    clients = 42;
 46
 47
 48    write(i2cfdacc, tx, 2);
 49    usleep (100);
 50
 51    // tx[0] = 0x21;
 52    // tx[1] = 0x10;
 53
 54    // write(i2cfdacc, tx, 2);
 55    // usleep (100);
 56
 57    float valuex, valuey, valuez, valuext, valueyt, valuezt;
 58
 59    tx[0] = 0xA8;
 60    //tx[1]=0x27;
 61
 62
 63    write(i2cfdacc, tx, 1);
 64
 65    int tmp;
 66    char *data = new char[2];
 67    //gets x value
 68    tmp = read(i2cfdacc, data, 2);
 69
 70    if (tmp != 2)
 71        perror("Read did not return bytes specified");
 72
 73    valuex = ((data[0] | (data[1] << 8))) * .002929688;
 74
 75    tx[0] = 0xAA;
 76    //tx[1]=0x27;
 77
 78
 79    write(i2cfdacc, tx, 1);
 80
 81
 82    tmp = read(i2cfdacc, data, 2);
 83
 84    if (tmp != 2)
 85        perror("Read did not return bytes specified");
 86
 87    valuey = ((data[0] | (data[1] << 8))) * .002929688;
 88
 89    tx[0] = 0xAC;
 90    //tx[1]=0x27;
 91
 92
 93    write(i2cfdacc, tx, 1);
 94    ;
 95
 96    tmp = read(i2cfdacc, data, 2);
 97
 98    if (tmp != 2)
 99        perror("Read did not return bytes specified");
100
101    valuez = ((data[0] | (data[1] << 8))) * .002929688;
102
103
104
105
106    tx[0] = 0x23;
107    tx[1] = 0x02;
108
109
110    write(i2cfdacc, tx, 2);
111
112    usleep (100);
113
114
115    tmp = read(i2cfdacc, data, 2);
116
117    if (tmp != 2)
118        perror("Read did not return bytes specified");
119
120    valuext = ((data[0] | (data[1] << 8))) * .002929688;
121
122    tx[0] = 0xAA;
123    //tx[1]=0x27;
124
125
126    write(i2cfdacc, tx, 1);
127
128
129    tmp = read(i2cfdacc, data, 2);
130
131    if (tmp != 2)
132        perror("Read did not return bytes specified");
133
134    valueyt = ((data[0] | (data[1] << 8))) * .002929688;
135
136    tx[0] = 0xAC;
137    //tx[1]=0x27;
138
139
140    write(i2cfdacc, tx, 1);
141
142
143    tmp = read(i2cfdacc, data, 2);
144
145    if (tmp != 2)
146        perror("Read did not return bytes specified");
147
148    valuezt = ((data[0] | (data[1] << 8))) * .002929688;
149
150    tx[0] = 0x23;
151    tx[1] = 0x00;
152
153
154    write(i2cfdacc, tx, 2);
155
156    tx[0] = 0x20;
157    tx[1] = 0x07;
158
159
160    write(i2cfdacc, tx, 2);
161    usleep (100);
162
163
164    if ((valuext - valuex < 180) | (valuext - valuex > 50))
165    {
166
167    }
168    else
169    {
170        perror("X axis out of range!!!!");
171    }
172
173    if ((valueyt - valuey < -50) | (valueyt - valuey > -180))
174    {
175
176    }
177    else
178    {
179        perror("Y axis out of range!!!!");
180    }
181
182    if ((valuezt - valuez < 370) | (valuezt - valuez > 220))
183    {
184
185    }
186    else
187    {
188        perror("Z axis out of range!!!!");
189    }
190
191
192    pthread_mutex_init(&mutex, NULL);
193    status = INIT;
194    join = false;
195
196
197
198
199}
200
201
202
203
204void LIS331::Configure(int datarate, int g)
205{
206
207    char tx[2];
208    tx[0] = 0x20;
209
210    switch (datarate)
211    {
212    case 50:
213    {
214        tx[1] = 0x27;
215        break;
216    }
217
218    case 100:
219    {
220        tx[1] = 0x2F;
221        break;
222    }
223
224    case 400:
225    {
226        tx[1] = 0x37;
227        break;
228    }
229
230    case 1000:
231    {
232        tx[1] = 0x3F;
233        break;
234    }
235
236    default:
237    {
238        tx[1] = 0x2F;
239        break;
240    }
241    }
242
243
244
245    write(i2cfdacc, tx, 2);
246
247    tx[0] = 0x23;
248    switch (g) //0x23
249    {
250
251    case 6: //0x00
252    {
253        tx[1] = 0x00;
254        scale = .000183105;
255        break;
256    }
257
258    case 12: //0x10
259    {
260        tx[1] = 0x10;
261        scale = .000366211;
262        break;
263    }
264
265    case 24: //0x30
266    {
267        tx[1] = 0x30;
268        scale = .000732422;
269        break;
270    }
271
272
273    default:
274    {
275        tx[1] = 0x30;
276        scale = .000732422;
277        break;
278    }
279    }
280
281    write(i2cfdacc, tx, 2);
282
283    maxX = 0;
284    maxY = 0;
285    maxZ = 0;
286    minX = 0;
287    minY = 0;
288    minZ = 0;
289    X = 0;
290    Y = 0;
291    Z = 0;
292
293}
294
295
296
297float LIS331::getXvalue()
298{
299
300
301    char tx[2];
302    float value = 0;
303
304    tx[0] = 0xA8;
305    //tx[1]=0x27;
306
307
308    write(i2cfdacc, tx, 1);
309
310    int tmp;
311    char *data = new char[2];
312
313    tmp = read(i2cfdacc, data, 2);
314
315    if (tmp != 2)
316        perror("Read did not return bytes specified");
317
318    value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
319
320
321    delete[] data;
322
323    return value;
324
325
326
327}
328
329float LIS331::getYvalue()
330{
331    char tx[2];
332    float value = 0;
333
334    tx[0] = 0xAA;
335    //tx[1]=0x27;
336
337
338    write(i2cfdacc, &tx[0], 1);
339
340    int tmp;
341    char *data = new char[2];
342    //data[0]=0;
343    //data[1]=0;
344    tmp = read(i2cfdacc, data, 2);
345
346    if (tmp != 2)
347        perror("Read did not return bytes specified");
348
349    value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
350    //      printf ("%x, %x\n", data[0], data [1]);
351
352    delete[] data;
353
354    return value;
355
356
357
358}
359
360float LIS331::getZvalue()
361{
362    char tx[2];
363    float value = 0;
364
365    tx[0] = 0xAC;
366    //tx[1]=0x27;
367
368
369    write(i2cfdacc, &tx[0], 1);
370
371    int tmp;
372    char *data = new char[2];
373
374    tmp = read(i2cfdacc, data, 2);
375
376    if (tmp != 2)
377        perror("Read did not return bytes specified");
378
379    value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
380
381
382    delete[] data;
383
384    return value;
385
386
387
388}
389
390
391float LIS331::getX()
392{
393    float x = 0;
394    pthread_mutex_lock (&mutex);
395    x = X;
396    pthread_mutex_unlock (&mutex);
397    return x;
398
399}
400float LIS331::getY()
401{
402    float y = 0;
403    pthread_mutex_lock (&mutex);
404    y =  Y;
405    pthread_mutex_unlock (&mutex);
406    return y;
407
408}
409float LIS331::getZ()
410{
411    float z = 0;
412    pthread_mutex_lock (&mutex);
413    z = Z;
414    pthread_mutex_unlock (&mutex);
415    return z;
416}
417
418
419
420
421
422
423bool  LIS331::start_update()
424{
425
426
427    int status_local;
428    pthread_mutex_lock (&mutex);
429    status_local = status;
430    pthread_mutex_unlock (&mutex);
431    if (status_local == INIT)
432    {
433        pthread_mutex_lock (&mutex);
434        status_local = status;
435        runningaverageX = 0;
436        runningaverageY = 0;
437        runningaverageZ = 0;
438        X = 0;
439        Y = 0;
440        Z = 0;
441        pthread_mutex_unlock (&mutex);
442        pthread_create (&pt, NULL, update, (void *)this);
443        return true;
444    }
445    return false;
446}
447bool  LIS331::stop_update()
448{
449    pthread_mutex_lock (&mutex);
450    join = true;
451    pthread_mutex_unlock (&mutex);
452    //pthread_join (pt,NULL);
453    pthread_exit (&pt);
454    status = INIT;
455    join = false;
456    pt = NULL;
457    return connected;
458}
459
460
461float LIS331::getXaverage()
462{
463    float localx = 0;
464    pthread_mutex_lock (&mutex);
465    localx = runningaverageX;
466    pthread_mutex_unlock (&mutex);
467
468    return localx;
469
470}
471float LIS331::getYaverage()
472{
473    float localy = 0;
474    pthread_mutex_lock (&mutex);
475    localy = runningaverageY;
476    pthread_mutex_unlock (&mutex);
477
478    return localy;
479
480}
481float LIS331::getZaverage()
482{
483    float localz = 0;
484    pthread_mutex_lock (&mutex);
485    localz = runningaverageZ;
486    pthread_mutex_unlock (&mutex);
487
488    return localz;
489
490}
491
492void LIS331::setpollrate(int rate)
493{
494    pthread_mutex_lock (&mutex);
495    pollrate = rate;
496    pthread_mutex_unlock (&mutex);
497
498
499}
500void LIS331::setdatapoints(int points)
501{
502
503    pthread_mutex_lock (&mutex);
504    datapoints = points;
505    pthread_mutex_unlock (&mutex);
506}
507
508void LIS331::setclient (int c)
509{
510    pthread_mutex_lock (&mutex);
511    clients = c;
512    pthread_mutex_unlock (&mutex);
513
514
515}
516
517float LIS331::getXmax()
518{
519    float localx = 0;
520    pthread_mutex_lock (&mutex);
521    localx = maxX;
522    pthread_mutex_unlock (&mutex);
523
524    return localx;
525
526}
527float LIS331::getXmin()
528{
529    float localx = 0;
530    pthread_mutex_lock (&mutex);
531    localx = minX;
532    pthread_mutex_unlock (&mutex);
533
534    return localx;
535
536
537}
538float LIS331::getYmax()
539{
540    float localy = 0;
541    pthread_mutex_lock (&mutex);
542    localy = maxY;
543    pthread_mutex_unlock (&mutex);
544
545    return localy;
546
547
548}
549float LIS331::getYmin()
550{
551    float localy = 0;
552    pthread_mutex_lock (&mutex);
553    localy = minY;
554    pthread_mutex_unlock (&mutex);
555
556    return localy;
557
558}
559float LIS331::getZmax()
560{
561    float localz = 0;
562    pthread_mutex_lock (&mutex);
563    localz = maxZ;
564    pthread_mutex_unlock (&mutex);
565
566    return localz;
567
568
569}
570float LIS331::getZmin()
571{
572
573    float localz = 0;
574    pthread_mutex_lock (&mutex);
575    localz = minZ;
576    pthread_mutex_unlock (&mutex);
577
578    return localz;
579
580
581}
582
583
584void  LIS331::closer()
585{
586    close(i2cfdacc);
587}
588
589LIS331::~LIS331()
590{
591    closer();
592}
593
594
595
596void *update(void *ptr) //inside here need to poll and update sensors, compute running average
597{
598    LIS331 *thread = ( LIS331 *) ptr;
599    int status;
600    int join;
601    //create vectors for variables, add length
602
603    vector<float> valuesX;
604    vector<float> valuesY;
605    vector<float> valuesZ;
606
607    vector<float> maxx;
608    vector<float> maxy;
609    vector<float> maxz;
610
611    vector<float> minx;
612    vector<float> miny;
613    vector<float> minz;
614
615    float localx = 0;
616    float localy = 0;
617    float localz = 0;
618
619    float localaveragex = 0;
620    float localaveragey = 0;
621    float localaveragez = 0;
622
623    unsigned int i = 0;
624    unsigned int y = 0;
625
626    pthread_mutex_lock (&thread->mutex);
627    thread->X = thread->getXvalue();
628    thread->Y = thread->getYvalue();
629    thread->Z = thread->getZvalue();
630
631    localx = thread->X;
632    localy = thread->Y;
633    localz = thread->Z;
634
635    pthread_mutex_unlock (&thread->mutex);
636
637    usleep (400);
638
639    pthread_mutex_lock (&thread->mutex);
640    thread->X = thread->getXvalue();
641    thread->Y = thread->getYvalue();
642    thread->Z = thread->getZvalue();
643
644    localx = thread->X;
645    localy = thread->Y;
646    localz = thread->Z;
647
648    pthread_mutex_unlock (&thread->mutex);
649
650    // printf ("X(in thread): %f\n", localx);
651    // printf ("Y(in thread): %f\n", localy);
652    // printf ("Z(in thread): %f\n", localz);
653
654    while (i < 10)
655    {
656        maxx.push_back(localx);
657        i++;
658    }
659    i = 0;
660
661    while (i < 10)
662    {
663        minx.push_back(localx);
664        i++;
665    }
666    i = 0;
667
668    while (i < 10)
669    {
670        maxy.push_back(localx);
671        i++;
672    }
673    i = 0;
674
675    while (i < 10)
676    {
677        miny.push_back(localy);
678        i++;
679    }
680    i = 0;
681
682    while (i < 10)
683    {
684        maxz.push_back(localz);
685        i++;
686    }
687    i = 0;
688
689    while (i < 10)
690    {
691        minz.push_back(localz);
692        i++;
693    }
694    i = 0;
695
696
697
698
699
700    while (true)
701    {
702        //get the status variables
703        pthread_mutex_lock (&thread->mutex);
704        thread->status =  LIS331::PROCESSING;
705        status = thread->status;
706        join = thread->join;
707        pthread_mutex_unlock (&thread->mutex);
708        //EXIT if ASKED
709        if (join)
710            break;
711
712        //if the status is processing, then run
713        if (status ==  LIS331::PROCESSING)
714        {
715            pthread_mutex_lock (&thread->mutex);
716            thread->X = thread->getXvalue();
717            thread->Y = thread->getYvalue();
718            thread->Z = thread->getZvalue();
719
720            localx = thread->X;
721            localy = thread->Y;
722            localz = thread->Z;
723
724            pthread_mutex_unlock (&thread->mutex);
725
726            if (valuesX.size() < thread->datapoints && valuesY.size() < thread->datapoints && valuesZ.size() < thread->datapoints)
727            {
728                if (valuesX.size() < 1 && valuesY.size() < 1 && valuesZ.size() < 1)
729                {
730                    valuesX.push_back(localx);
731                    valuesY.push_back(localy);
732                    valuesZ.push_back(localz);
733                    localaveragex = valuesX[0];
734                    localaveragey = valuesY[0];
735                    localaveragez = valuesZ[0];
736                   // printf ("Inside of first if statement\n");
737
738                }
739                else
740                {
741                    localaveragex = ((localaveragex * valuesX.size()) + localx) / (valuesX.size() + 1);
742                    valuesX.push_back(localx);
743                    localaveragey = ((localaveragey * valuesY.size()) + localy) / (valuesY.size() + 1);
744                    valuesY.push_back(localy);
745                    localaveragez = ((localaveragez * valuesZ.size()) + localz) / (valuesZ.size() + 1);
746                    valuesZ.push_back(localz);
747                }
748            }
749
750            else
751            {
752                if (y < thread->datapoints)
753                {
754                    localaveragex = ((localaveragex * thread->datapoints) - valuesX[y] + localx) / thread->datapoints;
755                    valuesX[y] = localx;
756                    localaveragey = ((localaveragey * thread->datapoints) - valuesY[y] + localy) / thread->datapoints;
757                    valuesY[y] = localy;
758                    localaveragez = ((localaveragez * thread->datapoints) - valuesZ[y] + localz) / thread->datapoints;
759                    valuesZ[y] = localz;
760                    y++;
761                }
762
763                else
764                {
765                    y = 0;
766                    localaveragex = ((localaveragex * thread->datapoints) - valuesX[y] + localx) / thread->datapoints;
767                    valuesX[y] = localx;
768                    localaveragey = ((localaveragey * thread->datapoints) - valuesY[y] + localy) / thread->datapoints;
769                    valuesY[y] = localy;
770                    localaveragez = ((localaveragez * thread->datapoints) - valuesZ[y] + localz) / thread->datapoints;
771                    valuesZ[y] = localz;
772                    y++;
773                }
774
775            }
776
777            pthread_mutex_lock (&thread->mutex);
778            thread->runningaverageX = localaveragex;
779            thread->runningaverageY = localaveragey;
780            thread->runningaverageZ = localaveragez;
781            pthread_mutex_unlock (&thread->mutex);
782
783
784            // //min max stuff
785
786            while (i < 10)
787            {
788                if (localx > maxx[i])
789                {
790                    maxx[i] = localx;
791                    // printf ("X:%f\n", maxx[i]);
792                }
793                i++;
794            }
795            i = 0;
796
797            while (i < 10)
798            {
799                if (localx < minx[i])
800                {
801                    minx[i] = (localx);
802                }
803                i++;
804            }
805            i = 0;
806
807            while (i < 10)
808            {
809                if (localy > maxy[i])
810                {
811                    maxy[i] = localy;
812                    //printf ("Y:%f\n", maxy[i]);
813                }
814                i++;
815            }
816            i = 0;
817
818            while (i < 10)
819            {
820                if (localy < miny[i])
821                {
822                    miny [i] = localy;
823                }
824                i++;
825            }
826            i = 0;
827
828            while (i < 10)
829            {
830                if (localz > maxz[i])
831                {
832                    maxz[i] = localz;
833                    //printf ("Z:%f\n", maxz[i]);
834                }
835                i++;
836            }
837            i = 0;
838
839            while (i < 10)
840            {
841                if (localy < minz[i])
842                {
843                    minz[i] = localz;
844                }
845                i++;
846            }
847            i = 0;
848
849            pthread_mutex_lock (&thread->mutex);
850            int client = thread->clients;
851            pthread_mutex_unlock (&thread->mutex);
852
853            if (client < 42)
854            {
855                cout << maxx[client] << endl;
856                cout << maxy[client] << endl;
857                cout << maxz[client] << endl;
858                cout << minx[client] << endl;
859                cout << miny[client] << endl;
860                cout << minz[client] << endl;
861
862                pthread_mutex_lock (&thread->mutex);
863                thread->maxX =  maxx[client];
864                thread->maxY =  maxy[client];
865                thread->maxZ =  maxz[client];
866                thread->minX =  minx[client];
867                thread->minY =  miny[client];
868                thread->minZ =  minz[client];
869                pthread_mutex_unlock (&thread->mutex);
870
871                maxx[client] = thread->getX();
872                maxy[client] = thread->getY();
873                maxz[client] = thread->getZ();
874                minx[client] = thread->getX();
875                miny[client] = thread->getY();
876                minz[client] = thread->getZ();
877
878
879
880
881                pthread_mutex_lock (&thread->mutex);
882                thread->clients = 42;
883                pthread_mutex_unlock (&thread->mutex);
884                client = 42;
885
886            }
887
888            // printf ("X(value at end): %f\n", localx);
889            // printf ("Y(value at end): %f\n", localy);
890            // printf ("Z(value at end): %f\n", localz);
891
892
893
894
895
896
897
898
899
900
901            pthread_mutex_lock (&thread->mutex);
902            thread->status = LIS331::WAITING;
903            pthread_mutex_unlock (&thread->mutex);
904            usleep(thread->pollrate);
905            pthread_mutex_lock (&thread->mutex);
906            thread->status = LIS331::DONE;
907            pthread_mutex_unlock (&thread->mutex);
908        }
909    }
910    return (void *) NULL;
911}
912#endif