/**************************************************************************************************
* @fn          sensorMpu6500Sleep
*
* @brief       Place the MPU in low power mode
*
* @return
*/
static void sensorMpu6500Sleep(void)
{
  val = ALL_AXES;
  sensorWriteReg(PWR_MGMT_2, &val, 1);

  val = MPU_SLEEP;
  sensorWriteReg(PWR_MGMT_1, &val, 1);
}
/**************************************************************************************************
* @fn          sensorMpu6500WakeUp
*
* @brief       Exit low power mode
*
* @return      none
*/
static void sensorMpu6500WakeUp(void)
{
  val = MPU_WAKE_UP;
  sensorWriteReg(PWR_MGMT_1, &val, 1);
  
  val = ALL_AXES;
  sensorWriteReg(PWR_MGMT_2, &val, 1);
  mpuConfig = 0;
  
  if (accRangeReg != ACC_REG_CTRL_INVALID)
    sensorWriteReg(ACCEL_CONFIG, &accRangeReg, 1);
}
/*******************************************************************************
* @fn          sensorHdc1000Init
*
* @brief       Initialise the humidity sensor driver
*
* @return      True if I2C operation successful
*******************************************************************************/
bool sensorHdc1000Init(void)
{
  uint16_t val;

  if (!SENSOR_SELECT())
    return false;

  // Enable reading data in one operation
  val = SWAP(HDC1000_VAL_CONFIG);
  success = sensorWriteReg(HDC1000_REG_CONFIG,(uint8_t*)&val,2);

  SENSOR_DESELECT();

  return success;
}
/**************************************************************************************************
* @fn          sensorSht21Test
*
* @brief       Humidity sensor self test
*
* @return      none
**************************************************************************************************/
bool sensorSht21Test(void)
{
  uint8_t val;

  SENSOR_SELECT();

  // Verify write and read
  val = USR_REG_TEST_VAL;
  ST_ASSERT(sensorWriteReg(SHT21_CMD_WRITE_U_R,&val,1));
  ST_ASSERT(sensorReadReg(SHT21_CMD_READ_U_R,&val,1));
  ST_ASSERT(val == USR_REG_TEST_VAL);
  
  SENSOR_DESELECT();
  
  return true;
}
/**************************************************************************************************
* @fn          sensorSht21Init
*
* @brief       Initialize the humidity sensor driver
*
* @return      True if successful
**************************************************************************************************/
bool sensorSht21Init(void)
{
  SENSOR_SELECT();

  // Set 11 bit resolution
  success = sensorReadReg(SHT21_CMD_READ_U_R,&usr,1);
  if (success)
  {
    usr &= USR_REG_RES_MASK;
    usr |= USR_REG_11BITRES;
    success = sensorWriteReg(SHT21_CMD_WRITE_U_R,&usr,1);
  }
  
  SENSOR_DESELECT();
  
  return success;
}
/*********************************************************************
 * @fn      sensor_writeRegister
 *
 * @brief   Write registers from the data stored in 'registerData'
 *
 * @return  none
 */
static void sensor_writeRegister(void)
{
  uint8_t deviceAddress, interface, len;
  
  interface = registerDeviceID[0];
  deviceAddress = registerDeviceID[1];
  len = registerAddress[0];
  
  switch (interface)
  {
  // I2C interfaces
  case REGISTER_INTERFACE_I2C0:
  case REGISTER_INTERFACE_I2C1:
    {
      uint8_t regaddr;
      
      regaddr = registerAddress[1];
      bspI2cSelect(interface, deviceAddress);
      if (len>0)
      {
        sensorWriteReg(regaddr,registerData,len);
      }
      else
      {
        bspI2cWriteSingle(regaddr);
      }
      bspI2cDeselect();
    }
    break;
        
  case REGISTER_INTERFACE_MCU:
    {
      uint32_t addr;
      
      addr= *(uint32*)(&registerAddress[1]);
      memcpy((uint8_t*)addr,registerData,len);
    }

  default:
    break;
  }
}
/**************************************************************************************************
* @fn          sensorMpu6500SelectAxes
*
* @brief       MPU in sleep
*
* @return      none
*/
static void sensorMpu6500SelectAxes(void)
{
  val = ~mpuConfig;
  sensorWriteReg(PWR_MGMT_2, &val, 1);
}