/**
*@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);
}
}
