Clément BENOIT / PYRN

This my big project for PYRN Board

Dependencies:   CAN HTTPClient MODSERIAL MyThings Pyrn3GModem Socket TinyGPS MyUSBHost lwip-sys lwip mbed-rtos mbed-src

IMUSensor.cpp@0:efe6085327fd, 2015-04-14 (annotated)

Committer:
clemounet
Date:
Tue Apr 14 13:30:02 2015 +0000
Revision:
0:efe6085327fd
All the projet PYRN 3G-CAN-ACC...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clemounet 0:efe6085327fd 1
clemounet 0:efe6085327fd 2 #define __DEBUG__ 0
clemounet 0:efe6085327fd 3 #ifndef __MODULE__
clemounet 0:efe6085327fd 4 #define __MODULE__ "IMUSensor.cpp"
clemounet 0:efe6085327fd 5 #endif
clemounet 0:efe6085327fd 6 #include "MyDebug.h"
clemounet 0:efe6085327fd 7
clemounet 0:efe6085327fd 8 #include "IMUSensor.h"
clemounet 0:efe6085327fd 9
clemounet 0:efe6085327fd 10 #define IMU_THREAD_STACK_SIZE 512
clemounet 0:efe6085327fd 11
clemounet 0:efe6085327fd 12 IMUSensor::IMUSensor(PinName sda, PinName scl, uint32_t idle):
clemounet 0:efe6085327fd 13 MySensor(SENSOR_NAME_IMU, SENSOR_TYPE_IMU, idle, IMU_THREAD_STACK_SIZE),
clemounet 0:efe6085327fd 14 Accelerometer(sda,scl){
clemounet 0:efe6085327fd 15 InitResults(SENSOR_RESSZ_DEFAULT);
clemounet 0:efe6085327fd 16 }
clemounet 0:efe6085327fd 17
clemounet 0:efe6085327fd 18 void IMUSensor::InitResultsStatic(void) {
clemounet 0:efe6085327fd 19 DBG("InitResultStatic have been defined");
clemounet 0:efe6085327fd 20 results.start = (uint8_t*)store;
clemounet 0:efe6085327fd 21 results.current = (uint8_t*)store;
clemounet 0:efe6085327fd 22 results.max = sizeof(store); //IMU_STORE_SIZE*sizeof(uint16_t);
clemounet 0:efe6085327fd 23 }
clemounet 0:efe6085327fd 24
clemounet 0:efe6085327fd 25 void IMUSensor::Loop(void) {
clemounet 0:efe6085327fd 26 // Do the reading
clemounet 0:efe6085327fd 27 float lax,lay,laz,lmx,lmy,lmz; // Locals
clemounet 0:efe6085327fd 28 Accelerometer.readAcc(&lax,&lay,&laz);
clemounet 0:efe6085327fd 29 Accelerometer.readMag(&lmx,&lmy,&lmz);
clemounet 0:efe6085327fd 30 //DBG("[%+.4f,%+.4f,%+.4f] - [%+.4f,%+.4f,%+.4f]", lax,lay,laz,lmx,lmy,lmz);
clemounet 0:efe6085327fd 31 // Do the conversion to the impact struct
clemounet 0:efe6085327fd 32 impact.ax = (int16_t) (lax * 1000);
clemounet 0:efe6085327fd 33 impact.ay = (int16_t) (lay * 1000);
clemounet 0:efe6085327fd 34 impact.az = (int16_t) (laz * 1000);
clemounet 0:efe6085327fd 35 impact.mx = (int16_t) (lmx * 1000);
clemounet 0:efe6085327fd 36 impact.my = (int16_t) (lmy * 1000);
clemounet 0:efe6085327fd 37 impact.mz = (int16_t) (lmz * 1000);
clemounet 0:efe6085327fd 38 // Print result if debug is activated
clemounet 0:efe6085327fd 39 INFO("[%+05d,%+05d,%+05d] - [%+05d,%+05d,%+05d]", impact.ax, impact.ay, impact.az, impact.mx, impact.my, impact.mz);
clemounet 0:efe6085327fd 40 }
clemounet 0:efe6085327fd 41
clemounet 0:efe6085327fd 42 void IMUSensor::StoreLastImpact(void) {
clemounet 0:efe6085327fd 43 if((results.max - results.num)>=sizeof(imuImpact)){
clemounet 0:efe6085327fd 44 if(resultsMutex.trylock()) {
clemounet 0:efe6085327fd 45 memcpy((char*)results.current,(const char*)&impact,sizeof(imuImpact));
clemounet 0:efe6085327fd 46 results.current += sizeof(imuImpact);
clemounet 0:efe6085327fd 47 results.num += sizeof(imuImpact);
clemounet 0:efe6085327fd 48 //DBG("%p %d %d",results.current,results.num,sizeof(imuImpact));
clemounet 0:efe6085327fd 49 resultsMutex.unlock();
clemounet 0:efe6085327fd 50 }
clemounet 0:efe6085327fd 51 } else
clemounet 0:efe6085327fd 52 WARN("No Space to Store results... Data will be overwrited at next Loop");
clemounet 0:efe6085327fd 53 }