PageRenderTime 420ms CodeModel.GetById 111ms app.highlight 210ms RepoModel.GetById 92ms app.codeStats 0ms

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

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