static void EnableAccelerometer(void)
{
  ENTER_OPERATING_MODE();
  ACCELEROMETER_INT_ENABLE();
}
void HandleAccelerometer(tMessage *pMsg)
{
  if (Control == INIT_MODE) InitAccelerometer();

  switch (pMsg->Options)
  {
  case MSG_OPT_ACCEL_DATA:

    if (Connected(CONN_TYPE_SPP) && QuerySniffState() == Active
#if SUPPORT_BLE
      || Connected(CONN_TYPE_BLE) && CurrentInterval(INTERVAL_STATE) == SHORT
#endif
      )
      AccelerometerSendDataHandler();
    break;

  case MSG_OPT_ACCEL_ENABLE:

    if (Connected(CONN_TYPE_BLE)) SendMessage(UpdConnParamMsg, SHORT);
    else if (Connected(CONN_TYPE_SPP)) SendMessage(SniffControlMsg, MSG_OPT_EXIT_SNIFF);

    if (pMsg->Length)
    {
      ENTER_STANDBY_MODE();
      Control &= ~ACCEL_RANGE_MASK;
      Control |= (*pMsg->pBuffer & 0x03) << ACCEL_RANGE_SHFT;
      AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
    }

    /* enable accelerometer */
    ENTER_OPERATING_MODE();
    ACCELEROMETER_INT_ENABLE();
    break;

  case MSG_OPT_ACCEL_DISABLE:
    /* put into low power mode */
    ACCELEROMETER_INT_DISABLE();
    ENTER_STANDBY_MODE();
    if (Connected(CONN_TYPE_BLE)) SendMessage(UpdConnParamMsg, LONG);
    break;

  case MSG_OPT_ACCEL_STREAMING:
  
    ENTER_STANDBY_MODE();
    Control &= ~WUF_ENABLE;
    Control |= DRDYE_DATA_AVAILABLE;
    AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
    ENTER_OPERATING_MODE();
    break;

  case MSG_OPT_ACCEL_WUF:
  
    ENTER_STANDBY_MODE();

    if (pMsg->Length) AccelerometerWrite(KIONIX_WUF_THRESH, pMsg->pBuffer, ONE_BYTE);

    Control &= ~DRDYE_DATA_AVAILABLE;
    Control |= WUF_ENABLE;
    AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
    ENTER_OPERATING_MODE();
    break;

  default:
    break;
  }
}