/** *@brief 計測した加速度取得(オフセット有り) * @param ax 加速度(X) * @param ay 加速度(Y) * @param az 加速度(Z) */ void LSM303DLHC::getAcc(double &ax, double &ay, double &az) { getAccData(ax,ay,az); lastAX = _ar*ax + (1-_ar)*lastAX; lastAY = _ar*ay + (1-_ar)*lastAY; lastAZ = _ar*az + (1-_ar)*lastAZ; }
void logAccData(void){ z = getAccData(); LogToFile(x, y, z); if ((z>337) || (z< -337)){ /* greater than 1g? */ LED_G_On(); } else{ LED_G_Off(); } }
bool motion::crashDetected() // void --> debugging, bool --> correct/release mode { double pitch,roll = 0; /// if(digitalRead(_button_pin) == LOW) { ///for(int i = 0; i < 1000; i++) ///{ int X,Y,Z = 0; getAccData(); X = accX; Y = accY; Z = accZ; double temp = pow((pow(Y,2) + pow(Z,2)),0.5); roll = (atan2(-Y, Z)*180.0)/PI; pitch = (atan2(X, temp)*180.0)/PI; if(abs(pitch) <= 60) { pitch = 0; } if(abs(roll) <= 60) { roll = 0; } if(lock_out >= 1) { return true; } else { if((abs(pitch) + abs(roll)) >= 150) { lock_out++; return true; } else { return false; } } /* Serial.print(abs(pitch) + abs(roll)); Serial.print(","); delay(100); } Serial.println(" "); Serial.println("All Done!"); while(1) { } }*/}
/** *@brief 初期化 */ void LSM303DLHC::reset(void) { //_i2c->address(_Accaddr); writeByte(_Accaddr,CTRL_REG1_A,0x27); struct timespec ts; ts.tv_sec = 0; ts.tv_nsec = 200000000; nanosleep(&ts, NULL); setAccRange(_Ascale); writeByte(_Magnaddr,CRA_REG_M,0x14); nanosleep(&ts, NULL); //writeByte(_Magnaddr,CRB_REG_M,0x20); writeByte(_Magnaddr,MR_REG_M,0x00); nanosleep(&ts, NULL); setMagnRange(_Mscale); nanosleep(&ts, NULL); const double count = 10; double ax,ay,az; lastAX = 0; lastAY = 0; lastAZ = 0; for(int i=0;i < count;i++) { getAccData(ax,ay,az); lastAX += ax/count; lastAY += ay/count; lastAZ += az/count; usleep(10000); } double mx,my,mz; lastMX = 0; lastMY = 0; lastMZ = 0; for(int i=0;i < count;i++) { getMagnData(mx,my,mz); lastMX += mx/count; lastMY += my/count; lastMZ += mz/count; usleep(10000); } }