/*****************************************************************************/  
//  FreeFallDetect.ino
//  Hardware:      Grove - 6-Axis Accelerometer&Gyroscope
//  Arduino IDE:   Arduino-1.65
//  Author:        Lambor 
//  Date:          Nov,2015
//  Version:       v1.0
//
//  Modified by: 
//  Data:        
//  Description: 
//
//  by www.seeedstudio.com
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
//
/*******************************************************************************/

#include "SparkFunLSM6DS3.h"
#include "Wire.h"

#define CLEAR_STEP      true
#define NOT_CLEAR_STEP  false 

//Create a instance of class LSM6DS3
LSM6DS3 lsm6ds3( I2C_MODE, 0x6A );  //I2C device address 0x6A
uint16_t detectCount = 0;

void setup() {  
  Serial.begin(9600);    
  if( lsm6ds3.begin() != 0 ){
    Serial.println("Device error");
  }
  else{
      Serial.println("Device OK!");
  }
  
  if(0 != config_free_fall_detect())
    Serial.println("Fail to configure!");
  else
    Serial.println("Success to Configure!");
}

void loop()
{
  uint8_t readDataByte = 0;
	//Read the wake-up source register
	lsm6ds3.readRegister(&readDataByte, LSM6DS3_ACC_GYRO_WAKE_UP_SRC);
	//Mask off the FF_IA bit for free-fall detection
	readDataByte &= 0x20;	
  if( readDataByte )
	{   
    detectCount ++;
		Serial.print("Free fall detected!  ");        
    Serial.println(detectCount);
	}  
  delay(10);  
}

int config_free_fall_detect(void)
{  
	uint8_t error = 0;  
  uint8_t dataToWrite = 0;  
  
  dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz;
  dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g;
  dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz;
  
  error += lsm6ds3.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite);
	error += lsm6ds3.writeRegister( LSM6DS3_ACC_GYRO_WAKE_UP_DUR, 0x00 );
  error += lsm6ds3.writeRegister(LSM6DS3_ACC_GYRO_FREE_FALL, 0x33);
  error += lsm6ds3.writeRegister( LSM6DS3_ACC_GYRO_MD1_CFG, 0x10 );
	error += lsm6ds3.writeRegister( LSM6DS3_ACC_GYRO_MD2_CFG, 0x10 );
  error += lsm6ds3.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x01);
  
  return error;
}