PageRenderTime 52ms CodeModel.GetById 24ms RepoModel.GetById 0ms 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
Possible License(s): GPL-2.0
  1. #include "LIS331.h"
  2. #include <iostream>
  3. using namespace std;
  4. using std::cout;
  5. void *update(void *ptr);
  6. LIS331::LIS331(const int bus, const int address)
  7. {
  8. char *device = new char[50];
  9. sprintf(device, "/dev/i2c-%d", bus);
  10. if ((i2cfdacc = open(device, O_RDWR)) < 0)
  11. {
  12. sprintf(device, "Failed to open %s", device);
  13. perror(device);
  14. }
  15. delete[] device;
  16. char *addr = new char[50];
  17. sprintf(addr, "0x%x", address);
  18. if (ioctl(i2cfdacc, I2C_SLAVE, address) < 0)
  19. {
  20. sprintf(addr, "Start error with address %s", addr);
  21. perror(addr);
  22. delete[] addr;
  23. }
  24. delete[] addr;
  25. //runs self test to ensure sensor functionality
  26. char tx[2];
  27. tx[0] = 0x20;
  28. tx[1] = 0x37;
  29. clients = 42;
  30. write(i2cfdacc, tx, 2);
  31. usleep (100);
  32. // tx[0] = 0x21;
  33. // tx[1] = 0x10;
  34. // write(i2cfdacc, tx, 2);
  35. // usleep (100);
  36. float valuex, valuey, valuez, valuext, valueyt, valuezt;
  37. tx[0] = 0xA8;
  38. //tx[1]=0x27;
  39. write(i2cfdacc, tx, 1);
  40. int tmp;
  41. char *data = new char[2];
  42. //gets x value
  43. tmp = read(i2cfdacc, data, 2);
  44. if (tmp != 2)
  45. perror("Read did not return bytes specified");
  46. valuex = ((data[0] | (data[1] << 8))) * .002929688;
  47. tx[0] = 0xAA;
  48. //tx[1]=0x27;
  49. write(i2cfdacc, tx, 1);
  50. tmp = read(i2cfdacc, data, 2);
  51. if (tmp != 2)
  52. perror("Read did not return bytes specified");
  53. valuey = ((data[0] | (data[1] << 8))) * .002929688;
  54. tx[0] = 0xAC;
  55. //tx[1]=0x27;
  56. write(i2cfdacc, tx, 1);
  57. ;
  58. tmp = read(i2cfdacc, data, 2);
  59. if (tmp != 2)
  60. perror("Read did not return bytes specified");
  61. valuez = ((data[0] | (data[1] << 8))) * .002929688;
  62. tx[0] = 0x23;
  63. tx[1] = 0x02;
  64. write(i2cfdacc, tx, 2);
  65. usleep (100);
  66. tmp = read(i2cfdacc, data, 2);
  67. if (tmp != 2)
  68. perror("Read did not return bytes specified");
  69. valuext = ((data[0] | (data[1] << 8))) * .002929688;
  70. tx[0] = 0xAA;
  71. //tx[1]=0x27;
  72. write(i2cfdacc, tx, 1);
  73. tmp = read(i2cfdacc, data, 2);
  74. if (tmp != 2)
  75. perror("Read did not return bytes specified");
  76. valueyt = ((data[0] | (data[1] << 8))) * .002929688;
  77. tx[0] = 0xAC;
  78. //tx[1]=0x27;
  79. write(i2cfdacc, tx, 1);
  80. tmp = read(i2cfdacc, data, 2);
  81. if (tmp != 2)
  82. perror("Read did not return bytes specified");
  83. valuezt = ((data[0] | (data[1] << 8))) * .002929688;
  84. tx[0] = 0x23;
  85. tx[1] = 0x00;
  86. write(i2cfdacc, tx, 2);
  87. tx[0] = 0x20;
  88. tx[1] = 0x07;
  89. write(i2cfdacc, tx, 2);
  90. usleep (100);
  91. if ((valuext - valuex < 180) | (valuext - valuex > 50))
  92. {
  93. }
  94. else
  95. {
  96. perror("X axis out of range!!!!");
  97. }
  98. if ((valueyt - valuey < -50) | (valueyt - valuey > -180))
  99. {
  100. }
  101. else
  102. {
  103. perror("Y axis out of range!!!!");
  104. }
  105. if ((valuezt - valuez < 370) | (valuezt - valuez > 220))
  106. {
  107. }
  108. else
  109. {
  110. perror("Z axis out of range!!!!");
  111. }
  112. pthread_mutex_init(&mutex, NULL);
  113. status = INIT;
  114. join = false;
  115. }
  116. void LIS331::Configure(int datarate, int g)
  117. {
  118. char tx[2];
  119. tx[0] = 0x20;
  120. switch (datarate)
  121. {
  122. case 50:
  123. {
  124. tx[1] = 0x27;
  125. break;
  126. }
  127. case 100:
  128. {
  129. tx[1] = 0x2F;
  130. break;
  131. }
  132. case 400:
  133. {
  134. tx[1] = 0x37;
  135. break;
  136. }
  137. case 1000:
  138. {
  139. tx[1] = 0x3F;
  140. break;
  141. }
  142. default:
  143. {
  144. tx[1] = 0x2F;
  145. break;
  146. }
  147. }
  148. write(i2cfdacc, tx, 2);
  149. tx[0] = 0x23;
  150. switch (g) //0x23
  151. {
  152. case 6: //0x00
  153. {
  154. tx[1] = 0x00;
  155. scale = .000183105;
  156. break;
  157. }
  158. case 12: //0x10
  159. {
  160. tx[1] = 0x10;
  161. scale = .000366211;
  162. break;
  163. }
  164. case 24: //0x30
  165. {
  166. tx[1] = 0x30;
  167. scale = .000732422;
  168. break;
  169. }
  170. default:
  171. {
  172. tx[1] = 0x30;
  173. scale = .000732422;
  174. break;
  175. }
  176. }
  177. write(i2cfdacc, tx, 2);
  178. maxX = 0;
  179. maxY = 0;
  180. maxZ = 0;
  181. minX = 0;
  182. minY = 0;
  183. minZ = 0;
  184. X = 0;
  185. Y = 0;
  186. Z = 0;
  187. }
  188. float LIS331::getXvalue()
  189. {
  190. char tx[2];
  191. float value = 0;
  192. tx[0] = 0xA8;
  193. //tx[1]=0x27;
  194. write(i2cfdacc, tx, 1);
  195. int tmp;
  196. char *data = new char[2];
  197. tmp = read(i2cfdacc, data, 2);
  198. if (tmp != 2)
  199. perror("Read did not return bytes specified");
  200. value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
  201. delete[] data;
  202. return value;
  203. }
  204. float LIS331::getYvalue()
  205. {
  206. char tx[2];
  207. float value = 0;
  208. tx[0] = 0xAA;
  209. //tx[1]=0x27;
  210. write(i2cfdacc, &tx[0], 1);
  211. int tmp;
  212. char *data = new char[2];
  213. //data[0]=0;
  214. //data[1]=0;
  215. tmp = read(i2cfdacc, data, 2);
  216. if (tmp != 2)
  217. perror("Read did not return bytes specified");
  218. value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
  219. // printf ("%x, %x\n", data[0], data [1]);
  220. delete[] data;
  221. return value;
  222. }
  223. float LIS331::getZvalue()
  224. {
  225. char tx[2];
  226. float value = 0;
  227. tx[0] = 0xAC;
  228. //tx[1]=0x27;
  229. write(i2cfdacc, &tx[0], 1);
  230. int tmp;
  231. char *data = new char[2];
  232. tmp = read(i2cfdacc, data, 2);
  233. if (tmp != 2)
  234. perror("Read did not return bytes specified");
  235. value = ((int16_t)(data[0] | (data[1] << 8))) * scale;
  236. delete[] data;
  237. return value;
  238. }
  239. float LIS331::getX()
  240. {
  241. float x = 0;
  242. pthread_mutex_lock (&mutex);
  243. x = X;
  244. pthread_mutex_unlock (&mutex);
  245. return x;
  246. }
  247. float LIS331::getY()
  248. {
  249. float y = 0;
  250. pthread_mutex_lock (&mutex);
  251. y = Y;
  252. pthread_mutex_unlock (&mutex);
  253. return y;
  254. }
  255. float LIS331::getZ()
  256. {
  257. float z = 0;
  258. pthread_mutex_lock (&mutex);
  259. z = Z;
  260. pthread_mutex_unlock (&mutex);
  261. return z;
  262. }
  263. bool LIS331::start_update()
  264. {
  265. int status_local;
  266. pthread_mutex_lock (&mutex);
  267. status_local = status;
  268. pthread_mutex_unlock (&mutex);
  269. if (status_local == INIT)
  270. {
  271. pthread_mutex_lock (&mutex);
  272. status_local = status;
  273. runningaverageX = 0;
  274. runningaverageY = 0;
  275. runningaverageZ = 0;
  276. X = 0;
  277. Y = 0;
  278. Z = 0;
  279. pthread_mutex_unlock (&mutex);
  280. pthread_create (&pt, NULL, update, (void *)this);
  281. return true;
  282. }
  283. return false;
  284. }
  285. bool LIS331::stop_update()
  286. {
  287. pthread_mutex_lock (&mutex);
  288. join = true;
  289. pthread_mutex_unlock (&mutex);
  290. //pthread_join (pt,NULL);
  291. pthread_exit (&pt);
  292. status = INIT;
  293. join = false;
  294. pt = NULL;
  295. return connected;
  296. }
  297. float LIS331::getXaverage()
  298. {
  299. float localx = 0;
  300. pthread_mutex_lock (&mutex);
  301. localx = runningaverageX;
  302. pthread_mutex_unlock (&mutex);
  303. return localx;
  304. }
  305. float LIS331::getYaverage()
  306. {
  307. float localy = 0;
  308. pthread_mutex_lock (&mutex);
  309. localy = runningaverageY;
  310. pthread_mutex_unlock (&mutex);
  311. return localy;
  312. }
  313. float LIS331::getZaverage()
  314. {
  315. float localz = 0;
  316. pthread_mutex_lock (&mutex);
  317. localz = runningaverageZ;
  318. pthread_mutex_unlock (&mutex);
  319. return localz;
  320. }
  321. void LIS331::setpollrate(int rate)
  322. {
  323. pthread_mutex_lock (&mutex);
  324. pollrate = rate;
  325. pthread_mutex_unlock (&mutex);
  326. }
  327. void LIS331::setdatapoints(int points)
  328. {
  329. pthread_mutex_lock (&mutex);
  330. datapoints = points;
  331. pthread_mutex_unlock (&mutex);
  332. }
  333. void LIS331::setclient (int c)
  334. {
  335. pthread_mutex_lock (&mutex);
  336. clients = c;
  337. pthread_mutex_unlock (&mutex);
  338. }
  339. float LIS331::getXmax()
  340. {
  341. float localx = 0;
  342. pthread_mutex_lock (&mutex);
  343. localx = maxX;
  344. pthread_mutex_unlock (&mutex);
  345. return localx;
  346. }
  347. float LIS331::getXmin()
  348. {
  349. float localx = 0;
  350. pthread_mutex_lock (&mutex);
  351. localx = minX;
  352. pthread_mutex_unlock (&mutex);
  353. return localx;
  354. }
  355. float LIS331::getYmax()
  356. {
  357. float localy = 0;
  358. pthread_mutex_lock (&mutex);
  359. localy = maxY;
  360. pthread_mutex_unlock (&mutex);
  361. return localy;
  362. }
  363. float LIS331::getYmin()
  364. {
  365. float localy = 0;
  366. pthread_mutex_lock (&mutex);
  367. localy = minY;
  368. pthread_mutex_unlock (&mutex);
  369. return localy;
  370. }
  371. float LIS331::getZmax()
  372. {
  373. float localz = 0;
  374. pthread_mutex_lock (&mutex);
  375. localz = maxZ;
  376. pthread_mutex_unlock (&mutex);
  377. return localz;
  378. }
  379. float LIS331::getZmin()
  380. {
  381. float localz = 0;
  382. pthread_mutex_lock (&mutex);
  383. localz = minZ;
  384. pthread_mutex_unlock (&mutex);
  385. return localz;
  386. }
  387. void LIS331::closer()
  388. {
  389. close(i2cfdacc);
  390. }
  391. LIS331::~LIS331()
  392. {
  393. closer();
  394. }
  395. void *update(void *ptr) //inside here need to poll and update sensors, compute running average
  396. {
  397. LIS331 *thread = ( LIS331 *) ptr;
  398. int status;
  399. int join;
  400. //create vectors for variables, add length
  401. vector<float> valuesX;
  402. vector<float> valuesY;
  403. vector<float> valuesZ;
  404. vector<float> maxx;
  405. vector<float> maxy;
  406. vector<float> maxz;
  407. vector<float> minx;
  408. vector<float> miny;
  409. vector<float> minz;
  410. float localx = 0;
  411. float localy = 0;
  412. float localz = 0;
  413. float localaveragex = 0;
  414. float localaveragey = 0;
  415. float localaveragez = 0;
  416. unsigned int i = 0;
  417. unsigned int y = 0;
  418. pthread_mutex_lock (&thread->mutex);
  419. thread->X = thread->getXvalue();
  420. thread->Y = thread->getYvalue();
  421. thread->Z = thread->getZvalue();
  422. localx = thread->X;
  423. localy = thread->Y;
  424. localz = thread->Z;
  425. pthread_mutex_unlock (&thread->mutex);
  426. usleep (400);
  427. pthread_mutex_lock (&thread->mutex);
  428. thread->X = thread->getXvalue();
  429. thread->Y = thread->getYvalue();
  430. thread->Z = thread->getZvalue();
  431. localx = thread->X;
  432. localy = thread->Y;
  433. localz = thread->Z;
  434. pthread_mutex_unlock (&thread->mutex);
  435. // printf ("X(in thread): %f\n", localx);
  436. // printf ("Y(in thread): %f\n", localy);
  437. // printf ("Z(in thread): %f\n", localz);
  438. while (i < 10)
  439. {
  440. maxx.push_back(localx);
  441. i++;
  442. }
  443. i = 0;
  444. while (i < 10)
  445. {
  446. minx.push_back(localx);
  447. i++;
  448. }
  449. i = 0;
  450. while (i < 10)
  451. {
  452. maxy.push_back(localx);
  453. i++;
  454. }
  455. i = 0;
  456. while (i < 10)
  457. {
  458. miny.push_back(localy);
  459. i++;
  460. }
  461. i = 0;
  462. while (i < 10)
  463. {
  464. maxz.push_back(localz);
  465. i++;
  466. }
  467. i = 0;
  468. while (i < 10)
  469. {
  470. minz.push_back(localz);
  471. i++;
  472. }
  473. i = 0;
  474. while (true)
  475. {
  476. //get the status variables
  477. pthread_mutex_lock (&thread->mutex);
  478. thread->status = LIS331::PROCESSING;
  479. status = thread->status;
  480. join = thread->join;
  481. pthread_mutex_unlock (&thread->mutex);
  482. //EXIT if ASKED
  483. if (join)
  484. break;
  485. //if the status is processing, then run
  486. if (status == LIS331::PROCESSING)
  487. {
  488. pthread_mutex_lock (&thread->mutex);
  489. thread->X = thread->getXvalue();
  490. thread->Y = thread->getYvalue();
  491. thread->Z = thread->getZvalue();
  492. localx = thread->X;
  493. localy = thread->Y;
  494. localz = thread->Z;
  495. pthread_mutex_unlock (&thread->mutex);
  496. if (valuesX.size() < thread->datapoints && valuesY.size() < thread->datapoints && valuesZ.size() < thread->datapoints)
  497. {
  498. if (valuesX.size() < 1 && valuesY.size() < 1 && valuesZ.size() < 1)
  499. {
  500. valuesX.push_back(localx);
  501. valuesY.push_back(localy);
  502. valuesZ.push_back(localz);
  503. localaveragex = valuesX[0];
  504. localaveragey = valuesY[0];
  505. localaveragez = valuesZ[0];
  506. // printf ("Inside of first if statement\n");
  507. }
  508. else
  509. {
  510. localaveragex = ((localaveragex * valuesX.size()) + localx) / (valuesX.size() + 1);
  511. valuesX.push_back(localx);
  512. localaveragey = ((localaveragey * valuesY.size()) + localy) / (valuesY.size() + 1);
  513. valuesY.push_back(localy);
  514. localaveragez = ((localaveragez * valuesZ.size()) + localz) / (valuesZ.size() + 1);
  515. valuesZ.push_back(localz);
  516. }
  517. }
  518. else
  519. {
  520. if (y < thread->datapoints)
  521. {
  522. localaveragex = ((localaveragex * thread->datapoints) - valuesX[y] + localx) / thread->datapoints;
  523. valuesX[y] = localx;
  524. localaveragey = ((localaveragey * thread->datapoints) - valuesY[y] + localy) / thread->datapoints;
  525. valuesY[y] = localy;
  526. localaveragez = ((localaveragez * thread->datapoints) - valuesZ[y] + localz) / thread->datapoints;
  527. valuesZ[y] = localz;
  528. y++;
  529. }
  530. else
  531. {
  532. y = 0;
  533. localaveragex = ((localaveragex * thread->datapoints) - valuesX[y] + localx) / thread->datapoints;
  534. valuesX[y] = localx;
  535. localaveragey = ((localaveragey * thread->datapoints) - valuesY[y] + localy) / thread->datapoints;
  536. valuesY[y] = localy;
  537. localaveragez = ((localaveragez * thread->datapoints) - valuesZ[y] + localz) / thread->datapoints;
  538. valuesZ[y] = localz;
  539. y++;
  540. }
  541. }
  542. pthread_mutex_lock (&thread->mutex);
  543. thread->runningaverageX = localaveragex;
  544. thread->runningaverageY = localaveragey;
  545. thread->runningaverageZ = localaveragez;
  546. pthread_mutex_unlock (&thread->mutex);
  547. // //min max stuff
  548. while (i < 10)
  549. {
  550. if (localx > maxx[i])
  551. {
  552. maxx[i] = localx;
  553. // printf ("X:%f\n", maxx[i]);
  554. }
  555. i++;
  556. }
  557. i = 0;
  558. while (i < 10)
  559. {
  560. if (localx < minx[i])
  561. {
  562. minx[i] = (localx);
  563. }
  564. i++;
  565. }
  566. i = 0;
  567. while (i < 10)
  568. {
  569. if (localy > maxy[i])
  570. {
  571. maxy[i] = localy;
  572. //printf ("Y:%f\n", maxy[i]);
  573. }
  574. i++;
  575. }
  576. i = 0;
  577. while (i < 10)
  578. {
  579. if (localy < miny[i])
  580. {
  581. miny [i] = localy;
  582. }
  583. i++;
  584. }
  585. i = 0;
  586. while (i < 10)
  587. {
  588. if (localz > maxz[i])
  589. {
  590. maxz[i] = localz;
  591. //printf ("Z:%f\n", maxz[i]);
  592. }
  593. i++;
  594. }
  595. i = 0;
  596. while (i < 10)
  597. {
  598. if (localy < minz[i])
  599. {
  600. minz[i] = localz;
  601. }
  602. i++;
  603. }
  604. i = 0;
  605. pthread_mutex_lock (&thread->mutex);
  606. int client = thread->clients;
  607. pthread_mutex_unlock (&thread->mutex);
  608. if (client < 42)
  609. {
  610. cout << maxx[client] << endl;
  611. cout << maxy[client] << endl;
  612. cout << maxz[client] << endl;
  613. cout << minx[client] << endl;
  614. cout << miny[client] << endl;
  615. cout << minz[client] << endl;
  616. pthread_mutex_lock (&thread->mutex);
  617. thread->maxX = maxx[client];
  618. thread->maxY = maxy[client];
  619. thread->maxZ = maxz[client];
  620. thread->minX = minx[client];
  621. thread->minY = miny[client];
  622. thread->minZ = minz[client];
  623. pthread_mutex_unlock (&thread->mutex);
  624. maxx[client] = thread->getX();
  625. maxy[client] = thread->getY();
  626. maxz[client] = thread->getZ();
  627. minx[client] = thread->getX();
  628. miny[client] = thread->getY();
  629. minz[client] = thread->getZ();
  630. pthread_mutex_lock (&thread->mutex);
  631. thread->clients = 42;
  632. pthread_mutex_unlock (&thread->mutex);
  633. client = 42;
  634. }
  635. // printf ("X(value at end): %f\n", localx);
  636. // printf ("Y(value at end): %f\n", localy);
  637. // printf ("Z(value at end): %f\n", localz);
  638. pthread_mutex_lock (&thread->mutex);
  639. thread->status = LIS331::WAITING;
  640. pthread_mutex_unlock (&thread->mutex);
  641. usleep(thread->pollrate);
  642. pthread_mutex_lock (&thread->mutex);
  643. thread->status = LIS331::DONE;
  644. pthread_mutex_unlock (&thread->mutex);
  645. }
  646. }
  647. return (void *) NULL;
  648. }