sensor tag 1.4.0: gyroscope frequency updated

/**************************************************************************************************
  Filename:       hal_gyro.c
  Revised:        $Date: 2013-05-16 08:23:18 -0700 (Thu, 16 May 2013) $
  Revision:       $Revision: 34324 $

  Description:    Driver for the InvenSense IMU-3000 3-Axis Gyroscope


  Copyright 2012-2013  Texas Instruments Incorporated. All rights reserved.

  IMPORTANT: Your use of this Software is limited to those specific rights
  granted under the terms of a software license agreement between the user
  who downloaded the software, his/her employer (which must be your employer)
  and Texas Instruments Incorporated (the "License").  You may not use this
  Software unless you agree to abide by the terms of the License. The License
  limits your use, and you acknowledge, that the Software may not be modified,
  copied or distributed unless embedded on a Texas Instruments microcontroller
  or used solely and exclusively in conjunction with a Texas Instruments radio
  frequency transceiver, which is integrated into your product.  Other than for
  the foregoing purpose, you may not use, reproduce, copy, prepare derivative
  works of, modify, distribute, perform, display or sell this Software and/or
  its documentation for any purpose.

  YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
  PROVIDED �AS IS� WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
  INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
  NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
  TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
  NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
  LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
  INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
  OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
  OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
  (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.

  Should you have any questions regarding your right to use this Software,
  contact Texas Instruments Incorporated at www.TI.com.

**************************************************************************************************/

/* ------------------------------------------------------------------------------------------------
*                                          Includes
* ------------------------------------------------------------------------------------------------
*/
#include "hal_gyro.h"
#include "hal_i2c.h"
#include "hal_sensor.h"

/* ------------------------------------------------------------------------------------------------
*                                           Constants
* ------------------------------------------------------------------------------------------------
*/

/* Slave address */
#define HAL_GYRO_I2C_ADDRESS                    0x68
#define HAL_GYRO_DATA_SIZE                      6

/* GYRO register addresses */

#define HAL_GYRO_REG_WHOAMI                     0x00 // R/W

// Offset configuration registers
#define HAL_GYRO_REG_XOFFS_USRH                 0x0C // R/W
#define HAL_GYRO_REG_XOFFS_USRL                 0x0D // R/W
#define HAL_GYRO_REG_YOFFS_USRH                 0x0E // R/W
#define HAL_GYRO_REG_YOFFS_USRL                 0x0F // R/W
#define HAL_GYRO_REG_ZOFFS_USRH                 0x10 // R/W
#define HAL_GYRO_REG_ZOFFS_USRL                 0x11 // R/W

// Configuration registers
#define HAL_GYRO_REG_FIFO_EN                    0x12 // R/W
#define HAL_GYRO_REG_AUX_VDDIO                  0x13 // R/W
#define HAL_GYRO_REG_AUX_SLV_ADDR               0x14 // R/W
#define HAL_GYRO_REG_SMPLRT_DIV                 0x15 // R/W
#define HAL_GYRO_REG_DLPF_FS                    0x16 // R/W
#define HAL_GYRO_REG_INT_CFG                    0x17 // R/W
#define HAL_GYRO_REG_AUX_BURST_ADDR             0x18 // R/W
#define HAL_GYRO_REG_INT_STATUS                 0x1A // R

// Sensor data registers
#define HAL_GYRO_REG_TEMP_OUT_H                 0x1B // R
#define HAL_GYRO_REG_TEMP_OUT_L                 0x1C // R
#define HAL_GYRO_REG_GYRO_XOUT_H                0x1D // R
#define HAL_GYRO_REG_GYRO_XOUT_L                0x1E // R
#define HAL_GYRO_REG_GYRO_YOUT_H                0x1F // R
#define HAL_GYRO_REG_GYRO_YOUT_L                0x20 // R
#define HAL_GYRO_REG_GYRO_ZOUT_H                0x21 // R
#define HAL_GYRO_REG_GYRO_ZOUT_L                0x22 // R
#define HAL_GYRO_REG_AUX_XOUT_H                 0x23 // R
#define HAL_GYRO_REG_AUX_XOUT_L                 0x24 // R
#define HAL_GYRO_REG_AUX_YOUT_H                 0x25 // R
#define HAL_GYRO_REG_AUX_YOUT_L                 0x26 // R
#define HAL_GYRO_REG_AUX_ZOUT_H                 0x27 // R
#define HAL_GYRO_REG_AUX_ZOUT_L                 0x28 // R

// FIFO registers
#define HAL_GYRO_REG_FIFO_COUNTH                0x3A // R
#define HAL_GYRO_REG_FIFO_COUNTL                0x3B // R
#define HAL_GYRO_REG_FIFO_R                     0x3C // R

// User control and Power Management registers
#define HAL_GYRO_REG_USER_CTRL                  0x3D // R/W
#define HAL_GYRO_REG_PWR_MGM                    0x3E // R/W

/* GYRO Register Bit masks */
#define HAL_GYRO_REG_FIFO_EN_TEMP_OUT           0x80
#define HAL_GYRO_REG_FIFO_EN_GYRO_XOUT          0x40
#define HAL_GYRO_REG_FIFO_EN_GYRO_YOUT          0x20
#define HAL_GYRO_REG_FIFO_EN_GYRO_ZOUT          0x10
#define HAL_GYRO_REG_FIFO_EN_AUX_XOUT           0x08
#define HAL_GYRO_REG_FIFO_EN_AUX_YOUT           0x04
#define HAL_GYRO_REG_FIFO_EN_AUX_ZOUT           0x02
#define HAL_GYRO_REG_FIFO_EN_FIFO_FOOTER        0x01

#define HAL_GYRO_USER_CTRL_DMP_EN               0x80
#define HAL_GYRO_USER_CTRL_FIFO_EN              0x40
#define HAL_GYRO_USER_CTRL_AUX_IF_EN            0x20
#define HAL_GYRO_USER_CTRL_AUX_IF_RST_EN        0x08
#define HAL_GYRO_USER_CTRL_DMP_RST              0x04
#define HAL_GYRO_USER_CTRL_FIFO_RST             0x02
#define HAL_GYRO_USER_CTRL_GYRO_RST             0x01

#define HAL_GYRO_PWR_MGM_H_RESET                0x80
#define HAL_GYRO_PWR_MGM_SLEEP                  0x40
#define HAL_GYRO_PWR_MGM_STBY_XG                0x20
#define HAL_GYRO_PWR_MGM_STBY_YG                0x10
#define HAL_GYRO_PWR_MGM_STBY_ZG                0x08
#define HAL_GYRO_PWR_MGM_STBY_ALL               0x38  // All axes

// Clock select
#define HAL_GYRO_PWR_MGM_CLOCK_INT_OSC          0x00
#define HAL_GYRO_PWR_MGM_CLOCK_PLL_X            0x01
#define HAL_GYRO_PWR_MGM_CLOCK_PLL_Y            0x02
#define HAL_GYRO_PWR_MGM_CLOCK_PLL_Z            0x03
#define HAL_GYRO_PWR_MGM_CLOCK_PLL_32768KHZ     0x04
#define HAL_GYRO_PWR_MGM_CLOCK_PLL_19_2MHZ      0x05
#define HAL_GYRO_PWR_MGM_CLOCK_STOP             0x07

/* ------------------------------------------------------------------------------------------------
*                                           Local Functions
* ------------------------------------------------------------------------------------------------
*/
static void HalGyroSelect(void);

/* ------------------------------------------------------------------------------------------------
*                                           Local Variables
* ------------------------------------------------------------------------------------------------
*/
static uint8 mStatus;
// Selected axes, as in HAL_GYRO_PWR_MGM register:
// Bit 0-2: 0
// Bit 3: NOT X enabled
// Bit 4: NOT Y enabled
// Bit 5: NOT Z enabled
// Bit 6-7: 0
static uint8 mDisabledAxes;
static uint8 cfgOn;

/* ------------------------------------------------------------------------------------------------
*                                           Public functions
* -------------------------------------------------------------------------------------------------
*/


/**************************************************************************************************
 * @fn          HalGyroInit
 *
 * @brief       Initialise the gyro sensor driver
 *
 * @return      none
 **************************************************************************************************/
void HalGyroInit(void)
{
  mStatus = HAL_GYRO_STOPPED;
  mDisabledAxes = HAL_GYRO_PWR_MGM_STBY_ALL;

  HalGyroTurnOff();
}


/**************************************************************************************************
 * @fn          HalGyroTurnOn
 *
 * @brief       Turn the sensor on
 *
 * @return      none
 **************************************************************************************************/
void HalGyroTurnOn(void)
{
  bool success;
  uint8 clk_select;

  HalDcDcControl(ST_GYRO,true);

  HalGyroSelect();

  // Default to internal oscillator if no axis is enabled (should not happen)
  clk_select = HAL_GYRO_PWR_MGM_CLOCK_INT_OSC;
  if(!(mDisabledAxes & HAL_GYRO_PWR_MGM_STBY_XG))
  {
    clk_select = HAL_GYRO_PWR_MGM_CLOCK_PLL_X;
  }
  else if(!(mDisabledAxes & HAL_GYRO_PWR_MGM_STBY_YG))
  {
    clk_select = HAL_GYRO_PWR_MGM_CLOCK_PLL_Y;
  }
  else if(!(mDisabledAxes & HAL_GYRO_PWR_MGM_STBY_ZG))
  {
    clk_select = HAL_GYRO_PWR_MGM_CLOCK_PLL_Z;
  }

  // Wake up from sleep, disable standby for all gyros, select reference PLL according to selected axes
  cfgOn = mDisabledAxes & (~HAL_GYRO_PWR_MGM_SLEEP | clk_select);
  success = HalSensorWriteReg(HAL_GYRO_REG_PWR_MGM,&cfgOn,1);
  if(success)
  {
    mStatus = HAL_GYRO_DATA_READY;
  }
  else
  {
    mStatus = HAL_GYRO_STATE_ERROR;
  }
}

/**************************************************************************************************
 * @fn          HalGyroTurnOff
 *
 * @brief       Turn the sensor off
 *
 * @return      none
 **************************************************************************************************/
void HalGyroTurnOff(void)
{
  bool success;
  uint8 val;

  HalGyroSelect();

  // Standby, 5 uA current consumption
  val = HAL_GYRO_PWR_MGM_SLEEP;
  success = HalSensorWriteReg(HAL_GYRO_REG_PWR_MGM,&val,1);

  if(success)
  {
    uint8 rv;

    success = HalSensorReadReg(HAL_GYRO_REG_PWR_MGM,&rv,1);
    if (success && rv == val)
    {
      mStatus = HAL_GYRO_STOPPED;
      mDisabledAxes = HAL_GYRO_PWR_MGM_STBY_ALL;
    }
  }

  if (!success)
  {
    mStatus = HAL_GYRO_STATE_ERROR;
  }

  HalDcDcControl(ST_GYRO,false);
}


/**************************************************************************************************
 * @fn          HalGyroRead
 *
 * @brief       Read gyro data
 *
 * @param       Voltage and temperature in raw format [X_LOW, X_HIGH, Y_LOW, Y_HIGH, Z_LOW, Z_HIGH]
 *
 * @return      TRUE if valid data
 **************************************************************************************************/
bool HalGyroRead(uint8 *pBuf)
{
  bool success;
  uint8 buf[HAL_GYRO_DATA_SIZE];
  uint8 val;

  HalGyroSelect();

  // Read sensor
  success = HalSensorReadReg(HAL_GYRO_REG_GYRO_XOUT_H,buf,HAL_GYRO_DATA_SIZE);

  if (success)
  {
    // Result in LE
    pBuf[0] = buf[1];
    pBuf[1] = buf[0];
    pBuf[2] = buf[3];
    pBuf[3] = buf[2];
    pBuf[4] = buf[5];
    pBuf[5] = buf[4];
  }

  //RP disable gyro sleep
  // Put gyro back to sleep
  //val = HAL_GYRO_PWR_MGM_SLEEP;
  //success = HalSensorWriteReg(HAL_GYRO_REG_PWR_MGM,&val,1);
  //mStatus = HAL_GYRO_SLEEP;

  return success;
}


/**************************************************************************************************
 * @fn          HalGyroWakeUp
 *
 * @brief       Wake up the sensor
 *
 * @return      Gyro status
 **************************************************************************************************/
bool HalGyroWakeUp(void)
{
  HalGyroSelect();

  mStatus = HAL_GYRO_DATA_READY;

  // Wake up GYRO
  return HalSensorWriteReg(HAL_GYRO_REG_PWR_MGM,&cfgOn,1);
}


/**************************************************************************************************
 * @fn          HalGyroStatus
 *
 * @brief       Read the state of the sensor
 *
 * @return      Gyro status
 **************************************************************************************************/
uint8 HalGyroStatus(void)
{
  return mStatus;
}

/**************************************************************************************************
 * @fn          HalGyroSelectAxes
 *
 * @brief       Select axes for gyroscope
 *
 * @param       Bitmask representing enabled axes (1 enable, 0 disable)
 *              Bit 0: X axis
 *              Bit 1: Y axis
 *              Bit 2: Z axis
 *              Bit 3-7: reserved
 * @return      none
 **************************************************************************************************/
void HalGyroSelectAxes(uint8 axes)
{
  mDisabledAxes = HAL_GYRO_PWR_MGM_STBY_ALL;

  if(axes & 0x01)
  {
    mDisabledAxes ^= HAL_GYRO_PWR_MGM_STBY_XG;
  }

  if(axes & 0x02)
  {
    mDisabledAxes ^= HAL_GYRO_PWR_MGM_STBY_YG;
  }

  if(axes & 0x04)
  {
    mDisabledAxes ^= HAL_GYRO_PWR_MGM_STBY_ZG;
  }

  if(HalGyroStatus() != HAL_GYRO_STOPPED)
  {
    HalGyroTurnOn();
  }
}

/**************************************************************************************************
 * @fn          HalGyroTest
 *
 * @brief       Run a sensor self-test
 *
 * @return      TRUE if passed, FALSE if failed
 **************************************************************************************************/
bool HalGyroTest(void)
{
  uint8 val;

  HalGyroSelect();

  // Check the WHO AM I register
  ST_ASSERT(HalSensorReadReg(HAL_GYRO_REG_WHOAMI, &val, 1));
  ST_ASSERT((val&HAL_GYRO_I2C_ADDRESS) == HAL_GYRO_I2C_ADDRESS);

  return TRUE;
}

/* ------------------------------------------------------------------------------------------------
*                                           Private functions
* -------------------------------------------------------------------------------------------------
*/

/**************************************************************************************************
 * @fn          HalGyroSelect
 *
 * @brief       Select the Gyro slave and set the I2C bus speed
 *
 * @return      none
 **************************************************************************************************/
static void HalGyroSelect(void)
{
  // Select slave and set clock rate
  HalI2CInit(HAL_GYRO_I2C_ADDRESS,   i2cClock_533KHZ);
}

/*  Conversion algorithm for X, Y, Z
 *  ================================
 *
float calcGyro(int16 rawX)
{
    float v;

    //-- calculate rotation, unit deg/s, range -250, +250
    v = (rawX * 1.0) / (65536 / 500);

    return v;
}
*/

/*********************************************************************
*********************************************************************/

/**************************************************************************************************
  Filename:       gyroservice.c
  Revised:        $Date: 2013-08-23 11:45:31 -0700 (Fri, 23 Aug 2013) $
  Revision:       $Revision: 35100 $

  Description:    Gyroscope Service


  Copyright 2012 - 2013 Texas Instruments Incorporated. All rights reserved.

  IMPORTANT: Your use of this Software is limited to those specific rights
  granted under the terms of a software license agreement between the user
  who downloaded the software, his/her employer (which must be your employer)
  and Texas Instruments Incorporated (the "License").  You may not use this
  Software unless you agree to abide by the terms of the License. The License
  limits your use, and you acknowledge, that the Software may not be modified,
  copied or distributed unless embedded on a Texas Instruments microcontroller
  or used solely and exclusively in conjunction with a Texas Instruments radio
  frequency transceiver, which is integrated into your product.  Other than for
  the foregoing purpose, you may not use, reproduce, copy, prepare derivative
  works of, modify, distribute, perform, display or sell this Software and/or
  its documentation for any purpose.

  YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
  PROVIDED �AS IS� WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
  INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
  NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
  TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
  NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
  LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
  INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
  OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
  OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
  (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.

  Should you have any questions regarding your right to use this Software,
  contact Texas Instruments Incorporated at www.TI.com.
**************************************************************************************************/

/*********************************************************************
 * INCLUDES
 */
#include "bcomdef.h"
#include "linkdb.h"
#include "gatt.h"
#include "gatt_uuid.h"
#include "gattservapp.h"

#include "gyroservice.h"
#include "st_util.h"

/*********************************************************************
 * MACROS
 */

/*********************************************************************
 * CONSTANTS
 */

/* Service configuration values */
#define SENSOR_SERVICE_UUID     GYROSCOPE_SERV_UUID
#define SENSOR_DATA_UUID        GYROSCOPE_DATA_UUID
#define SENSOR_CONFIG_UUID      GYROSCOPE_CONF_UUID
#define SENSOR_PERIOD_UUID      GYROSCOPE_PERI_UUID

#define SENSOR_SERVICE          GYROSCOPE_SERVICE
#define SENSOR_DATA_LEN         GYROSCOPE_DATA_LEN

#define SENSOR_DATA_DESCR       "Gyro Data"
#define SENSOR_CONFIG_DESCR     "Gyro Conf."
#define SENSOR_PERIOD_DESCR     "Gyro Period"

//10ms
#define GYRO_SENSOR_MIN_UPDATE_PERIOD 10

/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */

// Service UUID
static CONST uint8 sensorServiceUUID[TI_UUID_SIZE] =
{
  TI_UUID(SENSOR_SERVICE_UUID),
};

// Characteristic UUID: data
static CONST uint8 sensorDataUUID[TI_UUID_SIZE] =
{
  TI_UUID(SENSOR_DATA_UUID),
};

// Characteristic UUID: config
static CONST uint8 sensorCfgUUID[TI_UUID_SIZE] =
{
  TI_UUID(SENSOR_CONFIG_UUID),
};

// Characteristic UUID: period
static CONST uint8 sensorPeriodUUID[TI_UUID_SIZE] =
{
  TI_UUID(SENSOR_PERIOD_UUID),
};


/*********************************************************************
 * EXTERNAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */

static sensorCBs_t *sensor_AppCBs = NULL;

/*********************************************************************
 * Profile Attributes - variables
 */

// Profile Service attribute
static CONST gattAttrType_t sensorService = { TI_UUID_SIZE, sensorServiceUUID };

// Characteristic Value: data
static uint8 sensorData[SENSOR_DATA_LEN] = { 0, 0, 0, 0, 0, 0};

// Characteristic Properties: data
static uint8 sensorDataProps = GATT_PROP_READ | GATT_PROP_NOTIFY;

// Characteristic Configuration: data
static gattCharCfg_t sensorDataConfig[GATT_MAX_NUM_CONN];

// Characteristic User Description: data
static uint8 sensorDataUserDescr[] = SENSOR_DATA_DESCR;

// Characteristic Properties: configuration
static uint8 sensorCfgProps = GATT_PROP_READ | GATT_PROP_WRITE;

// Characteristic Value: configuration
static uint8 sensorCfg = 0;

// Characteristic User Description: configuration
static uint8 sensorCfgUserDescr[] = SENSOR_CONFIG_DESCR; 

// Characteristic Properties: period
static uint8 sensorPeriodProps = GATT_PROP_READ | GATT_PROP_WRITE;

// Characteristic Value: period
static uint8 sensorPeriod = GYRO_SENSOR_MIN_UPDATE_PERIOD;

// Characteristic User Description: period
static uint8 sensorPeriodUserDescr[] = SENSOR_PERIOD_DESCR;

/*********************************************************************
 * Profile Attributes - Table
 */

static gattAttribute_t sensorAttrTable[] =
{
  {
    { ATT_BT_UUID_SIZE, primaryServiceUUID }, /* type */
    GATT_PERMIT_READ,                         /* permissions */
    0,                                        /* handle */
    (uint8 *)&sensorService                   /* pValue */
  },

    // Characteristic Declaration
    {
      { ATT_BT_UUID_SIZE, characterUUID },
      GATT_PERMIT_READ,
      0,
      &sensorDataProps
    },

      // Characteristic Value "Data"
      {
        { TI_UUID_SIZE, sensorDataUUID },
        GATT_PERMIT_READ,
        0,
        sensorData
      },

      // Characteristic configuration
      {
        { ATT_BT_UUID_SIZE, clientCharCfgUUID },
        GATT_PERMIT_READ | GATT_PERMIT_WRITE,
        0,
        (uint8 *)sensorDataConfig
      },

      // Characteristic User Description
      {
        { ATT_BT_UUID_SIZE, charUserDescUUID },
        GATT_PERMIT_READ,
        0,
        sensorDataUserDescr
      },

    // Characteristic Declaration
    {
      { ATT_BT_UUID_SIZE, characterUUID },
      GATT_PERMIT_READ,
      0,
      &sensorCfgProps
    },

      // Characteristic Value "Configuration"
      {
        { TI_UUID_SIZE, sensorCfgUUID },
        GATT_PERMIT_READ | GATT_PERMIT_WRITE,
        0,
        &sensorCfg
      },

      // Characteristic User Description
      {
        { ATT_BT_UUID_SIZE, charUserDescUUID },
        GATT_PERMIT_READ,
        0,
        sensorCfgUserDescr
      },
     // Characteristic Declaration "Period"
    {
      { ATT_BT_UUID_SIZE, characterUUID },
      GATT_PERMIT_READ,
      0,
      &sensorPeriodProps
    },

      // Characteristic Value "Period"
      {
        { TI_UUID_SIZE, sensorPeriodUUID },
        GATT_PERMIT_READ | GATT_PERMIT_WRITE,
        0,
        &sensorPeriod
      },

      // Characteristic User Description "Period"
      {
        { ATT_BT_UUID_SIZE, charUserDescUUID },
        GATT_PERMIT_READ,
        0,
        sensorPeriodUserDescr
      },
};


/*********************************************************************
 * LOCAL FUNCTIONS
 */
static uint8 sensor_ReadAttrCB( uint16 connHandle, gattAttribute_t *pAttr,
                            uint8 *pValue, uint8 *pLen, uint16 offset, uint8 maxLen );
static bStatus_t sensor_WriteAttrCB( uint16 connHandle, gattAttribute_t *pAttr,
                                 uint8 *pValue, uint8 len, uint16 offset );
static void sensor_HandleConnStatusCB( uint16 connHandle, uint8 changeType );


/*********************************************************************
 * PROFILE CALLBACKS
 */
// Simple Profile Service Callbacks
static CONST gattServiceCBs_t sensorCBs =
{
  sensor_ReadAttrCB,  // Read callback function pointer
  sensor_WriteAttrCB, // Write callback function pointer
  NULL                // Authorization callback function pointer
};

/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/*********************************************************************
 * @fn      Gyro_AddService
 *
 * @brief   Initializes the Sensor Profile service by registering
 *          GATT attributes with the GATT server.
 *
 * @param   services - services to add. This is a bit map and can
 *                     contain more than one service.
 *
 * @return  Success or Failure
 */
bStatus_t Gyro_AddService( uint32 services )
{
  uint8 status = SUCCESS;

  // Register with Link DB to receive link status change callback
  VOID linkDB_Register( sensor_HandleConnStatusCB );
  GATTServApp_InitCharCfg( INVALID_CONNHANDLE, sensorDataConfig );

  if (services & SENSOR_SERVICE )
  {
       // Register GATT attribute list and CBs with GATT Server App
    status = GATTServApp_RegisterService( sensorAttrTable,
                                          GATT_NUM_ATTRS( sensorAttrTable ),
                                          &sensorCBs );
  }

  return ( status );
}


/*********************************************************************
 * @fn      Gyro_RegisterAppCBs
 *
 * @brief   Registers the application callback function. Only call
 *          this function once.
 *
 * @param   callbacks - pointer to application callbacks.
 *
 * @return  SUCCESS or bleAlreadyInRequestedMode
 */
bStatus_t Gyro_RegisterAppCBs( sensorCBs_t *appCallbacks )
{
  if ( sensor_AppCBs == NULL )
  {
    if ( appCallbacks != NULL )
    {
      sensor_AppCBs = appCallbacks;
    }

    return ( SUCCESS );
  }

  return ( bleAlreadyInRequestedMode );
}

/*********************************************************************
 * @fn      Gyro_SetParameter
 *
 * @brief   Set a parameter.
 *
 * @param   param - Profile parameter ID
 * @param   len - length of data to right
 * @param   value - pointer to data to write.  This is dependent on
 *          the parameter ID and WILL be cast to the appropriate
 *          data type (example: data type of uint16 will be cast to
 *          uint16 pointer).
 *
 * @return  bStatus_t
 */
bStatus_t Gyro_SetParameter( uint8 param, uint8 len, void *value )
{
  bStatus_t ret = SUCCESS;

  switch ( param )
  {
    case SENSOR_DATA:
    if ( len == SENSOR_DATA_LEN )
    {
      VOID osal_memcpy( sensorData, value, SENSOR_DATA_LEN );
      // See if Notification has been enabled
      GATTServApp_ProcessCharCfg( sensorDataConfig, sensorData, FALSE,
                                 sensorAttrTable, GATT_NUM_ATTRS( sensorAttrTable ),
                                 INVALID_TASK_ID );
    }
    else
    {
      ret = bleInvalidRange;
    }
    break;

    case SENSOR_CONF:
      if ( len == sizeof ( uint8 ) )
      {
        sensorCfg = *((uint8*)value);
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    case SENSOR_PERI:
      if ( len == sizeof ( uint8 ) )
      {
        sensorPeriod = *((uint8*)value);
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    default:
      ret = INVALIDPARAMETER;
      break;
  }

  return ( ret );
}

/*********************************************************************
 * @fn      Gyro_GetParameter
 *
 * @brief   Get a Sensor Profile parameter.
 *
 * @param   param - Profile parameter ID
 * @param   value - pointer to data to put.  This is dependent on
 *          the parameter ID and WILL be cast to the appropriate
 *          data type (example: data type of uint16 will be cast to
 *          uint16 pointer).
 *
 * @return  bStatus_t
 */
bStatus_t Gyro_GetParameter( uint8 param, void *value )
{
  bStatus_t ret = SUCCESS;

  switch ( param )
  {
    case SENSOR_DATA:
      VOID osal_memcpy( value, sensorData, SENSOR_DATA_LEN );
      break;

    case SENSOR_CONF:
      *((uint8*)value) = sensorCfg;
      break;

    case SENSOR_PERI:
      *((uint8*)value) = sensorPeriod;
      break;

    default:
      ret = INVALIDPARAMETER;
      break;
  }

  return ( ret );
}

/*********************************************************************
 * @fn          sensor_ReadAttrCB
 *
 * @brief       Read an attribute.
 *
 * @param       connHandle - connection message was received on
 * @param       pAttr - pointer to attribute
 * @param       pValue - pointer to data to be read
 * @param       pLen - length of data to be read
 * @param       offset - offset of the first octet to be read
 * @param       maxLen - maximum length of data to be read
 *
 * @return      Success or Failure
 */
static uint8 sensor_ReadAttrCB( uint16 connHandle, gattAttribute_t *pAttr,
                            uint8 *pValue, uint8 *pLen, uint16 offset, uint8 maxLen )
{
  uint16 uuid;
  bStatus_t status = SUCCESS;

  // If attribute permissions require authorization to read, return error
  if ( gattPermitAuthorRead( pAttr->permissions ) )
  {
    // Insufficient authorization
    return ( ATT_ERR_INSUFFICIENT_AUTHOR );
  }

  // Make sure it's not a blob operation (no attributes in the profile are long)
  if ( offset > 0 )
  {
    return ( ATT_ERR_ATTR_NOT_LONG );
  }

  if (utilExtractUuid16(pAttr,&uuid) == FAILURE) {
    // Invalid handle
    *pLen = 0;
    return ATT_ERR_INVALID_HANDLE;
  }

  switch ( uuid )
  {
    // No need for "GATT_SERVICE_UUID" or "GATT_CLIENT_CHAR_CFG_UUID" cases;
    // gattserverapp handles those reads
    case SENSOR_DATA_UUID:
      *pLen = SENSOR_DATA_LEN;
      VOID osal_memcpy( pValue, pAttr->pValue, SENSOR_DATA_LEN );
      break;

    case SENSOR_CONFIG_UUID:
    case SENSOR_PERIOD_UUID:
      *pLen = 1;
      pValue[0] = *pAttr->pValue;
      break;

    default:
      *pLen = 0;
      status = ATT_ERR_ATTR_NOT_FOUND;
      break;
    }

  return ( status );
}

/*********************************************************************
* @fn      sensor_WriteAttrCB
*
* @brief   Validate attribute data prior to a write operation
*
* @param   connHandle - connection message was received on
* @param   pAttr - pointer to attribute
* @param   pValue - pointer to data to be written
* @param   len - length of data
* @param   offset - offset of the first octet to be written
*
* @return  Success or Failure
*/
static bStatus_t sensor_WriteAttrCB( uint16 connHandle, gattAttribute_t *pAttr,
                                           uint8 *pValue, uint8 len, uint16 offset )
{
  bStatus_t status = SUCCESS;
  uint8 notifyApp = 0xFF;
  uint16 uuid;

  // If attribute permissions require authorization to write, return error
  if ( gattPermitAuthorWrite( pAttr->permissions ) )
  {
    // Insufficient authorization
    return ( ATT_ERR_INSUFFICIENT_AUTHOR );
  }

  if (utilExtractUuid16(pAttr,&uuid) == FAILURE) {
    // Invalid handle
    return ATT_ERR_INVALID_HANDLE;
  }

  switch ( uuid )
  {
    case SENSOR_DATA_UUID:
      // Should not get here
      break;

    case SENSOR_CONFIG_UUID:
      // Validate the value
      // Make sure it's not a blob oper
      if ( offset == 0 )
      {
        if ( len != 1 )
        {
          status = ATT_ERR_INVALID_VALUE_SIZE;
        }
      }
      else
      {
        status = ATT_ERR_ATTR_NOT_LONG;
      }

      // Write the value
      if ( status == SUCCESS )
      {
        uint8 *pCurValue = (uint8 *)pAttr->pValue;

        *pCurValue = pValue[0];

        if( pAttr->pValue == &sensorCfg )
        {
          notifyApp = SENSOR_CONF;
        }
      }
      break;

    case SENSOR_PERIOD_UUID:
      // Validate the value
      // Make sure it's not a blob oper
      if ( offset == 0 )
      {
        if ( len != 1 )
        {
          status = ATT_ERR_INVALID_VALUE_SIZE;
        }
      }
      else
      {
        status = ATT_ERR_ATTR_NOT_LONG;
      }
      // Write the value
      if ( status == SUCCESS )
      {
        if (pValue[0]>=(GYRO_SENSOR_MIN_UPDATE_PERIOD))
        {

          uint8 *pCurValue = (uint8 *)pAttr->pValue;
          *pCurValue = pValue[0];

          if( pAttr->pValue == &sensorPeriod )
          {
            notifyApp = SENSOR_PERI;
          }
        }
        else
        {
           status = ATT_ERR_INVALID_VALUE;
        }
      }
      break;

    case GATT_CLIENT_CHAR_CFG_UUID:
      status = GATTServApp_ProcessCCCWriteReq( connHandle, pAttr, pValue, len,
                                              offset, GATT_CLIENT_CFG_NOTIFY );
      break;

    default:
      // Should never get here!
      status = ATT_ERR_ATTR_NOT_FOUND;
      break;
  }

  // If a charactersitic value changed then callback function to notify application of change
  if ( (notifyApp != 0xFF ) && sensor_AppCBs && sensor_AppCBs->pfnSensorChange )
  {
    sensor_AppCBs->pfnSensorChange( notifyApp );
  }

  return ( status );
}

/*********************************************************************
 * @fn          sensor_HandleConnStatusCB
 *
 * @brief       Sensor Profile link status change handler function.
 *
 * @param       connHandle - connection handle
 * @param       changeType - type of change
 *
 * @return      none
 */
static void sensor_HandleConnStatusCB( uint16 connHandle, uint8 changeType )
{
  // Make sure this is not loopback connection
  if ( connHandle != LOOPBACK_CONNHANDLE )
  {
    // Reset Client Char Config if connection has dropped
    if ( ( changeType == LINKDB_STATUS_UPDATE_REMOVED )      ||
         ( ( changeType == LINKDB_STATUS_UPDATE_STATEFLAGS ) &&
           ( !linkDB_Up( connHandle ) ) ) )
    {
      GATTServApp_InitCharCfg( connHandle, sensorDataConfig );
    }
  }
}


/*********************************************************************
*********************************************************************/

/**************************************************************************************************
  Filename:       sensorTag.c
  Revised:        $Date: 2013-08-23 11:45:31 -0700 (Fri, 23 Aug 2013) $
  Revision:       $Revision: 35100 $

  Description:    This file contains the Sensor Tag sample application
                  for use with the TI Bluetooth Low Energy Protocol Stack.

  Copyright 2012-2013  Texas Instruments Incorporated. All rights reserved.

  IMPORTANT: Your use of this Software is limited to those specific rights
  granted under the terms of a software license agreement between the user
  who downloaded the software, his/her employer (which must be your employer)
  and Texas Instruments Incorporated (the "License").  You may not use this
  Software unless you agree to abide by the terms of the License. The License
  limits your use, and you acknowledge, that the Software may not be modified,
  copied or distributed unless embedded on a Texas Instruments microcontroller
  or used solely and exclusively in conjunction with a Texas Instruments radio
  frequency transceiver, which is integrated into your product.  Other than for
  the foregoing purpose, you may not use, reproduce, copy, prepare derivative
  works of, modify, distribute, perform, display or sell this Software and/or
  its documentation for any purpose.

  YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
  PROVIDED �AS IS� WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
  INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
  NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
  TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
  NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
  LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
  INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
  OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
  OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
  (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.

  Should you have any questions regarding your right to use this Software,
  contact Texas Instruments Incorporated at www.TI.com.
**************************************************************************************************/

/*********************************************************************
 * INCLUDES
 */

#include "bcomdef.h"
#include "OSAL.h"
#include "OSAL_PwrMgr.h"

#include "OnBoard.h"
#include "hal_adc.h"
#include "hal_led.h"
#include "hal_keys.h"
#include "hal_i2c.h"

#include "gatt.h"
#include "hci.h"

#include "gapgattserver.h"
#include "gattservapp.h"
#include "gatt_profile_uuid.h"

#if defined ( PLUS_BROADCASTER )
  #include "peripheralBroadcaster.h"
#else
  #include "peripheral.h"
#endif

#include "gapbondmgr.h"

#if defined FEATURE_OAD
  #include "oad.h"
  #include "oad_target.h"
#endif

// Services
#include "st_util.h"
#include "devinfoservice-st.h"
#include "irtempservice.h"
#include "accelerometerservice.h"
#include "humidityservice.h"
#include "magnetometerservice.h"
#include "barometerservice.h"
#include "gyroservice.h"
#include "testservice.h"
#include "simplekeys.h"
#include "ccservice.h"
#include "battservice.h"

// Sensor drivers
#include "sensorTag.h"
#include "hal_sensor.h"

#include "hal_irtemp.h"
#include "hal_acc.h"
#include "hal_humi.h"
#include "hal_mag.h"
#include "hal_bar.h"
#include "hal_gyro.h"

/*********************************************************************
 * MACROS
 */

/*********************************************************************
 * CONSTANTS
 */

// How often to perform sensor reads (milliseconds)
#define TEMP_DEFAULT_PERIOD                   1000
#define HUM_DEFAULT_PERIOD                    1000
#define BAR_DEFAULT_PERIOD                    1000
#define MAG_DEFAULT_PERIOD                    2000
#define ACC_DEFAULT_PERIOD                    1000
#define GYRO_DEFAULT_PERIOD                   1000

// Constants for two-stage reading
#define TEMP_MEAS_DELAY                       275   // Conversion time 250 ms
#define BAR_FSM_PERIOD                        80
#define ACC_FSM_PERIOD                        20
#define HUM_FSM_PERIOD                        20
#define GYRO_STARTUP_TIME                     60    // Start-up time max. 50 ms

// What is the advertising interval when device is discoverable (units of 625us, 160=100ms)
#define DEFAULT_ADVERTISING_INTERVAL          160

// General discoverable mode advertises indefinitely
// RP advertises indefinitely
#define DEFAULT_DISCOVERABLE_MODE             GAP_ADTYPE_FLAGS_GENERAL

// Minimum connection interval (units of 1.25ms, 80=100ms) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_MIN_CONN_INTERVAL     6

// Maximum connection interval (units of 1.25ms, 800=1000ms) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_MAX_CONN_INTERVAL     6

// Slave latency to use if automatic parameter update request is enabled
#define DEFAULT_DESIRED_SLAVE_LATENCY         0

// Supervision timeout value (units of 10ms, 1000=10s) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_CONN_TIMEOUT          1000

// Whether to enable automatic parameter update request when a connection is formed
#define DEFAULT_ENABLE_UPDATE_REQUEST         TRUE

// Connection Pause Peripheral time value (in seconds)
#define DEFAULT_CONN_PAUSE_PERIPHERAL         8

// Company Identifier: Texas Instruments Inc. (13)
#define TI_COMPANY_ID                         0x000D

#define INVALID_CONNHANDLE                    0xFFFF

// Length of bd addr as a string
#define B_ADDR_STR_LEN                        15

#if defined ( PLUS_BROADCASTER )
  #define ADV_IN_CONN_WAIT                    500 // delay 500 ms
#endif

// Side key bit
#define SK_KEY_SIDE                           0x04

// Test mode bit
#define TEST_MODE_ENABLE                      0x80

// Common values for turning a sensor on and off + config/status
#define ST_CFG_SENSOR_DISABLE                 0x00
#define ST_CFG_SENSOR_ENABLE                  0x01
#define ST_CFG_CALIBRATE                      0x02
#define ST_CFG_ERROR                          0xFF

//Criterion on acceleration for led indicator
#define ACC_LED_THRESOLD                      0x30

// System reset
#define ST_SYS_RESET_DELAY                    3000

// Battery level is critical when it is less than this %
#define DEFAULT_BATT_CRITICAL_LEVEL 6

// Battery measurement period in ms
#define DEFAULT_BATT_PERIOD 15000
// Wait before first measure in ms
#define WAIT_BEFORE_FIRST_BATT 1000

/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */
static uint8 sensorTag_TaskID;   // Task ID for internal task/event processing

static gaprole_States_t gapProfileState = GAPROLE_INIT;

// GAP - SCAN RSP data (max size = 31 bytes)
static uint8 scanRspData[] =
{
  // complete name
  0x0A,   // length of this data
  GAP_ADTYPE_LOCAL_NAME_COMPLETE,
  0x53,   // 'S'
  0x65,   // 'e'
  0x6E,   // 'n'
  0x73,   // 's'
  0x6F,   // 'o'
  0x72,   // 'r'
  0x54,   // 'T'
  0x61,   // 'a'
  0x67,   // 'g'

  // connection interval range
  0x05,   // length of this data
  GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
  LO_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
  HI_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
  LO_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),
  HI_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),

  // Tx power level
  0x02,   // length of this data
  GAP_ADTYPE_POWER_LEVEL,
  0       // 0dBm
};

// GAP - Advertisement data (max size = 31 bytes, though this is
// best kept short to conserve power while advertisting)
static uint8 advertData[] =
{
  // Flags; this sets the device to use limited discoverable
  // mode (advertises for 30 seconds at a time) instead of general
  // discoverable mode (advertises indefinitely)
  0x02,   // length of this data
  GAP_ADTYPE_FLAGS,
  DEFAULT_DISCOVERABLE_MODE | GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,
};

// GAP GATT Attributes
static uint8 attDeviceName[] = "TI BLE Sensor Tag";

// Sensor State Variables
static bool   irTempEnabled = FALSE;
static bool   magEnabled = FALSE;
static uint8  accConfig = ST_CFG_SENSOR_DISABLE;
static bool   barEnabled = FALSE;
static bool   humiEnabled = FALSE;
static bool   gyroEnabled = FALSE;
static bool   battMeasureEnabled = FALSE;

static bool   barBusy = FALSE;
static uint8  humiState = 0;

static bool   sysResetRequest = FALSE;

static uint16 sensorMagPeriod = MAG_DEFAULT_PERIOD;
static uint16 sensorAccPeriod = ACC_DEFAULT_PERIOD;
static uint16 sensorTmpPeriod = TEMP_DEFAULT_PERIOD;
static uint16 sensorHumPeriod = HUM_DEFAULT_PERIOD;
static uint16 sensorBarPeriod = BAR_DEFAULT_PERIOD;
static uint16 sensorGyrPeriod = GYRO_DEFAULT_PERIOD;

static uint8  sensorGyroAxes = 0;
static bool   sensorGyroUpdateAxes = FALSE;
static uint16 selfTestResult = 0;
static bool   testMode = FALSE;

/*********************************************************************
 * LOCAL FUNCTIONS
 */
static void sensorTag_ProcessOSALMsg( osal_event_hdr_t *pMsg );
static void peripheralStateNotificationCB( gaprole_States_t newState );

static void readIrTempData( void );
static void readHumData( void );
static void readAccData( void );
static void readMagData( void );
static void readBarData( void );
static void readBarCalibration( void );
static void readGyroData( void );

static void barometerChangeCB( uint8 paramID );
static void irTempChangeCB( uint8 paramID );
static void accelChangeCB( uint8 paramID );
static void humidityChangeCB( uint8 paramID);
static void magnetometerChangeCB( uint8 paramID );
static void gyroChangeCB( uint8 paramID );
static void testChangeCB( uint8 paramID );
static void ccChangeCB( uint8 paramID );
static void gapRolesParamUpdateCB( uint16 connInterval, uint16 connSlaveLatency,uint16 connTimeout );
static void stBattCB(uint8 event);

static void resetSensorSetup( void );
static void sensorTag_HandleKeys( uint8 shift, uint8 keys );
static void resetCharacteristicValue( uint16 servID, uint8 paramID, uint8 value, uint8 paramLen );
static void resetCharacteristicValues( void );

static void stBattPeriodicTask( void );

/*********************************************************************
 * PROFILE CALLBACKS
 */

// GAP Role Callbacks
static gapRolesCBs_t sensorTag_PeripheralCBs =
{
  peripheralStateNotificationCB,  // Profile State Change Callbacks
  NULL                            // When a valid RSSI is read from controller (not used by application)
};

// GAP Bond Manager Callbacks
static gapBondCBs_t sensorTag_BondMgrCBs =
{
  NULL,                     // Passcode callback (not used by application)
  NULL                      // Pairing / Bonding state Callback (not used by application)
};

// Simple GATT Profile Callbacks
static sensorCBs_t sensorTag_BarometerCBs =
{
  barometerChangeCB,        // Characteristic value change callback
};

static sensorCBs_t sensorTag_IrTempCBs =
{
  irTempChangeCB,           // Characteristic value change callback
};

static sensorCBs_t sensorTag_AccelCBs =
{
  accelChangeCB,            // Characteristic value change callback
};

static sensorCBs_t sensorTag_HumidCBs =
{
  humidityChangeCB,         // Characteristic value change callback
};

static sensorCBs_t sensorTag_MagnetometerCBs =
{
  magnetometerChangeCB,     // Characteristic value change callback
};

static sensorCBs_t sensorTag_GyroCBs =
{
  gyroChangeCB,             // Characteristic value change callback
};

static testCBs_t sensorTag_TestCBs =
{
  testChangeCB,             // Charactersitic value change callback
};

static ccCBs_t sensorTag_ccCBs =
{
 ccChangeCB,               // Charactersitic value change callback
};

static gapRolesParamUpdateCB_t paramUpdateCB =
{
  gapRolesParamUpdateCB,
};

/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/*********************************************************************
 * @fn      SensorTag_Init
 *
 * @brief   Initialization function for the Simple BLE Peripheral App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ... ).
 *
 * @param   task_id - the ID assigned by OSAL.  This ID should be
 *                    used to send messages and set timers.
 *
 * @return  none
 */
void SensorTag_Init( uint8 task_id )
{
  sensorTag_TaskID = task_id;

  // Setup the GAP
  VOID GAP_SetParamValue( TGAP_CONN_PAUSE_PERIPHERAL, DEFAULT_CONN_PAUSE_PERIPHERAL );

  // Setup the GAP Peripheral Role Profile
  {
    // Device starts advertising upon initialization
    uint8 initial_advertising_enable = FALSE;

    // By setting this to zero, the device will go into the waiting state after
    // being discoverable for 30.72 second, and will not being advertising again
    // until the enabler is set back to TRUE
    uint16 gapRole_AdvertOffTime = 0;
    uint8 enable_update_request = DEFAULT_ENABLE_UPDATE_REQUEST;
    uint16 desired_min_interval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
    uint16 desired_max_interval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
    uint16 desired_slave_latency = DEFAULT_DESIRED_SLAVE_LATENCY;
    uint16 desired_conn_timeout = DEFAULT_DESIRED_CONN_TIMEOUT;

    // Set the GAP Role Parameters
    GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );
    GAPRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime );

    GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, sizeof ( scanRspData ), scanRspData );
    GAPRole_SetParameter( GAPROLE_ADVERT_DATA, sizeof( advertData ), advertData );

    GAPRole_SetParameter( GAPROLE_PARAM_UPDATE_ENABLE, sizeof( uint8 ), &enable_update_request );
    GAPRole_SetParameter( GAPROLE_MIN_CONN_INTERVAL, sizeof( uint16 ), &desired_min_interval );
    GAPRole_SetParameter( GAPROLE_MAX_CONN_INTERVAL, sizeof( uint16 ), &desired_max_interval );
    GAPRole_SetParameter( GAPROLE_SLAVE_LATENCY, sizeof( uint16 ), &desired_slave_latency );
    GAPRole_SetParameter( GAPROLE_TIMEOUT_MULTIPLIER, sizeof( uint16 ), &desired_conn_timeout );
  }

  // Set the GAP Characteristics
  GGS_SetParameter( GGS_DEVICE_NAME_ATT, sizeof(attDeviceName), attDeviceName );

  // Set advertising interval
  {
    uint16 advInt = DEFAULT_ADVERTISING_INTERVAL;

    GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MIN, advInt );
    GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MAX, advInt );
    GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MIN, advInt );
    GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MAX, advInt );
  }

  // Setup the GAP Bond Manager
  {
    uint32 passkey = 0; // passkey "000000"
    uint8 pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;
    uint8 mitm = TRUE;
    uint8 ioCap = GAPBOND_IO_CAP_DISPLAY_ONLY;
    uint8 bonding = TRUE;

    GAPBondMgr_SetParameter( GAPBOND_DEFAULT_PASSCODE, sizeof ( uint32 ), &passkey );
    GAPBondMgr_SetParameter( GAPBOND_PAIRING_MODE, sizeof ( uint8 ), &pairMode );
    GAPBondMgr_SetParameter( GAPBOND_MITM_PROTECTION, sizeof ( uint8 ), &mitm );
    GAPBondMgr_SetParameter( GAPBOND_IO_CAPABILITIES, sizeof ( uint8 ), &ioCap );
    GAPBondMgr_SetParameter( GAPBOND_BONDING_ENABLED, sizeof ( uint8 ), &bonding );
  }

  //Set critical level for battery service
  uint8 critical = DEFAULT_BATT_CRITICAL_LEVEL;
  Batt_SetParameter( BATT_PARAM_CRITICAL_LEVEL, sizeof (uint8 ), &critical );
  
  // Add services
  GGS_AddService( GATT_ALL_SERVICES );            // GAP
  GATTServApp_AddService( GATT_ALL_SERVICES );    // GATT attributes
  DevInfo_AddService();                           // Device Information Service
  IRTemp_AddService (GATT_ALL_SERVICES );         // IR Temperature Service
  Accel_AddService (GATT_ALL_SERVICES );          // Accelerometer Service
  Humidity_AddService (GATT_ALL_SERVICES );       // Humidity Service
  Magnetometer_AddService( GATT_ALL_SERVICES );   // Magnetometer Service
  Barometer_AddService( GATT_ALL_SERVICES );      // Barometer Service
  Gyro_AddService( GATT_ALL_SERVICES );           // Gyro Service
  SK_AddService( GATT_ALL_SERVICES );             // Simple Keys Profile
  Test_AddService( GATT_ALL_SERVICES );           // Test Profile
  CcService_AddService( GATT_ALL_SERVICES );      // Connection Control Service
  Batt_AddService( );                             // Battery service

  
  
#if defined FEATURE_OAD
  VOID OADTarget_AddService();                    // OAD Profile
#endif

  // Setup the Seensor Profile Characteristic Values
  resetCharacteristicValues();

  // Register for all key events - This app will handle all key events
  RegisterForKeys( sensorTag_TaskID );

  // makes sure LEDs are off
  HalLedSet( (HAL_LED_1 | HAL_LED_2), HAL_LED_MODE_OFF );

  // Initialise sensor drivers
  HALIRTempInit();
  HalHumiInit();
  HalMagInit();
  HalAccInit();
  HalBarInit();
  HalGyroInit();

  // Register callbacks with profile
  VOID IRTemp_RegisterAppCBs( &sensorTag_IrTempCBs );
  VOID Magnetometer_RegisterAppCBs( &sensorTag_MagnetometerCBs );
  VOID Accel_RegisterAppCBs( &sensorTag_AccelCBs );
  VOID Humidity_RegisterAppCBs( &sensorTag_HumidCBs );
  VOID Barometer_RegisterAppCBs( &sensorTag_BarometerCBs );
  VOID Gyro_RegisterAppCBs( &sensorTag_GyroCBs );
  VOID Test_RegisterAppCBs( &sensorTag_TestCBs );
  VOID CcService_RegisterAppCBs( &sensorTag_ccCBs );
  VOID GAPRole_RegisterAppCBs( &paramUpdateCB );
  Batt_Register ( stBattCB );

  // Enable clock divide on halt
  // This reduces active current while radio is active and CC254x MCU
  // is halted
  HCI_EXT_ClkDivOnHaltCmd( HCI_EXT_ENABLE_CLK_DIVIDE_ON_HALT );
  
  // ADDITION Adjust TX power level 
  //HCI_EXT_SetTxPowerCmd( HCI_EXT_TX_POWER_0_DBM );

  // Setup a delayed profile startup
  osal_set_event( sensorTag_TaskID, ST_START_DEVICE_EVT );
}

/*********************************************************************
 * @fn      SensorTag_ProcessEvent
 *
 * @brief   Simple BLE Peripheral Application Task event processor.  This function
 *          is called to process all events for the task.  Events
 *          include timers, messages and any other user defined events.
 *
 * @param   task_id  - The OSAL assigned task ID.
 * @param   events - events to process.  This is a bit map and can
 *                   contain more than one event.
 *
 * @return  events not processed
 */
uint16 SensorTag_ProcessEvent( uint8 task_id, uint16 events )
{
  VOID task_id; // OSAL required parameter that isn't used in this function

  if ( events & SYS_EVENT_MSG )
  {
    uint8 *pMsg;

    if ( (pMsg = osal_msg_receive( sensorTag_TaskID )) != NULL )
    {
      sensorTag_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );

      // Release the OSAL message
      VOID osal_msg_deallocate( pMsg );
    }

    // return unprocessed events
    return (events ^ SYS_EVENT_MSG);
  }

  // Handle system reset (long press on side key)
  if ( events & ST_SYS_RESET_EVT )
  {
    if (sysResetRequest)
    {
      HAL_SYSTEM_RESET();
    }
    return ( events ^ ST_SYS_RESET_EVT );
  }

  if ( events & ST_START_DEVICE_EVT )
  {
    // Start the Device
    VOID GAPRole_StartDevice( &sensorTag_PeripheralCBs );

    // Start Bond Manager
    VOID GAPBondMgr_Register( &sensorTag_BondMgrCBs );

    return ( events ^ ST_START_DEVICE_EVT );
  }

  //////////////////////////
  //    IR TEMPERATURE    //
  //////////////////////////
  if ( events & ST_IRTEMPERATURE_READ_EVT )
  {
    if ( irTempEnabled )
    {
      if (HalIRTempStatus() == TMP006_DATA_READY)
      {
        readIrTempData();
        osal_start_timerEx( sensorTag_TaskID, ST_IRTEMPERATURE_READ_EVT, sensorTmpPeriod-TEMP_MEAS_DELAY );
      }
      else if (HalIRTempStatus() == TMP006_OFF)
      {
        HalIRTempTurnOn();
        osal_start_timerEx( sensorTag_TaskID, ST_IRTEMPERATURE_READ_EVT, TEMP_MEAS_DELAY );
      }
    }
    else
    {
      //Turn off Temperatur sensor
      VOID HalIRTempTurnOff();
      VOID resetCharacteristicValue(IRTEMPERATURE_SERV_UUID,SENSOR_DATA,0,IRTEMPERATURE_DATA_LEN);
      VOID resetCharacteristicValue(IRTEMPERATURE_SERV_UUID,SENSOR_CONF,ST_CFG_SENSOR_DISABLE,sizeof ( uint8 ));
    }

    return (events ^ ST_IRTEMPERATURE_READ_EVT);
  }

  //////////////////////////
  //    Accelerometer     //
  //////////////////////////
  if ( events & ST_ACCELEROMETER_SENSOR_EVT )
  {
    if(accConfig != ST_CFG_SENSOR_DISABLE)
    {
      readAccData();
      osal_start_timerEx( sensorTag_TaskID, ST_ACCELEROMETER_SENSOR_EVT, sensorAccPeriod );
    }
    else
    {
      VOID resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_DATA, 0, ACCELEROMETER_DATA_LEN );
      VOID resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
      // Stops accelerometer led indicator
      HalLedSet(HAL_LED_2, HAL_LED_MODE_OFF );
    }

    return (events ^ ST_ACCELEROMETER_SENSOR_EVT);
  }

  //////////////////////////
  //      Humidity        //
  //////////////////////////
  if ( events & ST_HUMIDITY_SENSOR_EVT )
  {
    if (humiEnabled)
    {
      HalHumiExecMeasurementStep(humiState);
      if (humiState == 2)
      {
        readHumData();
        humiState = 0;
        osal_start_timerEx( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT, sensorHumPeriod );
      }
      else
      {
        humiState++;
        osal_start_timerEx( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT, HUM_FSM_PERIOD );
      }
    }
    else
    {
      resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_DATA, 0, HUMIDITY_DATA_LEN);
      resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
    }

    return (events ^ ST_HUMIDITY_SENSOR_EVT);
  }

  //////////////////////////
  //      Magnetometer    //
  //////////////////////////
  if ( events & ST_MAGNETOMETER_SENSOR_EVT )
  {
    if(magEnabled)
    {
      if (HalMagStatus() == MAG3110_DATA_READY)
      {
        readMagData();
      }
      else if (HalMagStatus() == MAG3110_OFF)
      {
        HalMagTurnOn();
      }

      osal_start_timerEx( sensorTag_TaskID, ST_MAGNETOMETER_SENSOR_EVT, sensorMagPeriod );
    }
    else
    {
      HalMagTurnOff();
      resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_DATA, 0, MAGNETOMETER_DATA_LEN);
      resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
    }

    return (events ^ ST_MAGNETOMETER_SENSOR_EVT);
  }

  //////////////////////////
  //        Barometer     //
  //////////////////////////
  if ( events & ST_BAROMETER_SENSOR_EVT )
  {
    if (barEnabled)
    {
      if (barBusy)
      {
        barBusy = FALSE;
        readBarData();
        osal_start_timerEx( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT, sensorBarPeriod );
      }
      else
      {
        barBusy = TRUE;
        HalBarStartMeasurement();
        osal_start_timerEx( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT, BAR_FSM_PERIOD );
      }
    }
    else
    {
      resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_DATA, 0, BAROMETER_DATA_LEN);
      resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
      resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_CALB, 0, BAROMETER_CALI_LEN);
    }

    return (events ^ ST_BAROMETER_SENSOR_EVT);
  }

  //////////////////////////
  //      Gyroscope       //
  //////////////////////////
  if ( events & ST_GYROSCOPE_SENSOR_EVT )
  {
    uint8 status;

    status = HalGyroStatus();

    if(gyroEnabled)
    {
      if (status == HAL_GYRO_STOPPED)
      {
        HalGyroSelectAxes(sensorGyroAxes);
        HalGyroTurnOn();
        osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, GYRO_STARTUP_TIME);
      }
      else
      {
        if(sensorGyroUpdateAxes)
        {
          HalGyroSelectAxes(sensorGyroAxes);
          sensorGyroUpdateAxes = FALSE;
        }

        if (status == HAL_GYRO_DATA_READY)
        {
          readGyroData();
          osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, sensorGyrPeriod);
        }
        else
        {
          //RP should never go here
          // Gyro needs to be activated;
          HalGyroWakeUp();
          osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, GYRO_STARTUP_TIME);
        }
      }
    }
    else
    {
      HalGyroTurnOff();
      resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_DATA, 0, GYROSCOPE_DATA_LEN);
      resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof( uint8 ));
    }
    return (events ^ ST_GYROSCOPE_SENSOR_EVT);
  }
  
  //////////////////////////
  //      Battery       //
  //////////////////////////
  if ( events & BATT_PERIODIC_EVT )
  {
    if(battMeasureEnabled)
    {
      // Perform periodic battery task
      stBattPeriodicTask();
    }
    else
    {
       resetCharacteristicValue(  BATT_SERV_UUID, BATT_PARAM_LEVEL, 100, BATT_DATA_LEN);
    }
    return (events ^ BATT_PERIODIC_EVT);
  }

#if defined ( PLUS_BROADCASTER )
  if ( events & ST_ADV_IN_CONNECTION_EVT )
  {
    uint8 turnOnAdv = TRUE;
    // Turn on advertising while in a connection
    GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &turnOnAdv );

    return (events ^ ST_ADV_IN_CONNECTION_EVT);
  }
#endif // PLUS_BROADCASTER

  // Discard unknown events
  return 0;
}

/*********************************************************************
 * @fn      sensorTag_test
 *
 * @brief   Run a self-test of the sensor TAG
 *
 * @param   none
 *
 * @return  bitmask of error flags
 */
uint16 sensorTag_test(void)
{
  selfTestResult = HalSensorTest();
  HalLedSet(HAL_LED_2,HAL_LED_MODE_OFF);

  // Write the self-test result to the test service
  Test_SetParameter( TEST_DATA_ATTR, TEST_DATA_LEN, &selfTestResult);

  return selfTestResult;
}

/*********************************************************************
* Private functions
*/


/*********************************************************************
 * @fn      sensorTag_ProcessOSALMsg
 *
 * @brief   Process an incoming task message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void sensorTag_ProcessOSALMsg( osal_event_hdr_t *pMsg )
{
  switch ( pMsg->event )
  {
    case KEY_CHANGE:
      sensorTag_HandleKeys( ((keyChange_t *)pMsg)->state, ((keyChange_t *)pMsg)->keys );
      break;

    default:
      // do nothing
      break;
  }
}

/*********************************************************************
 * @fn      sensorTag_HandleKeys
 *
 * @brief   Handles all key events for this device.
 *
 * @param   shift - true if in shift/alt.
 * @param   keys - bit field for key events. Valid entries:
 *                 HAL_KEY_SW_2
 *                 HAL_KEY_SW_1
 *
 * @return  none
 */
static void sensorTag_HandleKeys( uint8 shift, uint8 keys )
{
  uint8 SK_Keys = 0;
  VOID shift;  // Intentionally unreferenced parameter

  if (keys & HAL_KEY_SW_1)
  {
    // Reset the system if side key is pressed for more than 3 seconds
    sysResetRequest = TRUE;
    osal_start_timerEx( sensorTag_TaskID, ST_SYS_RESET_EVT, ST_SYS_RESET_DELAY );

    if (!testMode ) // Side key
    {
      // If device is not in a connection, pressing the side key should toggle
      //  advertising on and off
      if ( gapProfileState != GAPROLE_CONNECTED )
      {
        uint8 current_adv_enabled_status;
        uint8 new_adv_enabled_status;

        // Find the current GAP advertising status
        GAPRole_GetParameter( GAPROLE_ADVERT_ENABLED, &current_adv_enabled_status );

        if( current_adv_enabled_status == FALSE )
        {
          new_adv_enabled_status = TRUE;
        }
        else
        {
          new_adv_enabled_status = FALSE;
        }

        // Change the GAP advertisement status to opposite of current status
        GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &new_adv_enabled_status );
      }

      if ( gapProfileState == GAPROLE_CONNECTED )
      {
        uint8 adv_enabled = TRUE;

        // Disconnect
        GAPRole_TerminateConnection();
        // Start advertising
        GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &adv_enabled );
      }
    }
    else
    {
      // Test mode
      if ( keys & HAL_KEY_SW_1 ) // Side key
      {
        SK_Keys |= SK_KEY_SIDE;
      }
    }
  }

  if ( keys & HAL_KEY_SW_2 )   // Carbon S2
  {
    SK_Keys |= SK_KEY_LEFT;
  }

  if ( keys & HAL_KEY_SW_3 )   // Carbon S3
  {
    SK_Keys |= SK_KEY_RIGHT;
  }

  if (!(keys & HAL_KEY_SW_1))
  {
    // Cancel system reset request
    sysResetRequest = FALSE;
  }

  // Set the value of the keys state to the Simple Keys Profile;
  // This will send out a notification of the keys state if enabled
  SK_SetParameter( SK_KEY_ATTR, sizeof ( uint8 ), &SK_Keys );
}


/*********************************************************************
 * @fn      resetSensorSetup
 *
 * @brief   Turn off all sensors that are on
 *
 * @param   none
 *
 * @return  none
 */
static void resetSensorSetup (void)
{
  if (HalIRTempStatus()!=TMP006_OFF || irTempEnabled)
  {
    HalIRTempTurnOff();
    irTempEnabled = FALSE;
  }

  if (accConfig != ST_CFG_SENSOR_DISABLE)
  {
    accConfig = ST_CFG_SENSOR_DISABLE;
    // Stops accelerometer led indicator
    HalLedSet(HAL_LED_2, HAL_LED_MODE_OFF );
  }

  if (HalMagStatus()!=MAG3110_OFF || magEnabled)
  {
    HalMagTurnOff();
    magEnabled = FALSE;
  }

  if (gyroEnabled)
  {
    HalGyroTurnOff();
    gyroEnabled = FALSE;
  }

  if (barEnabled)
  {
    HalBarInit();
    barEnabled = FALSE;
  }

  if (humiEnabled)
  {
    HalHumiInit();
    humiEnabled = FALSE;
  }

  // Reset internal states
  sensorGyroAxes = 0;
  sensorGyroUpdateAxes = FALSE;
  testMode = FALSE;

  // Reset all characteristics values
  resetCharacteristicValues();
}


/*********************************************************************
 * @fn      peripheralStateNotificationCB
 *
 * @brief   Notification from the profile of a state change.
 *
 * @param   newState - new state
 *
 * @return  none
 */
static void peripheralStateNotificationCB( gaprole_States_t newState )
{
  switch ( newState )
  {
    case GAPROLE_STARTED:
    {
      uint8 ownAddress[B_ADDR_LEN];
      uint8 systemId[DEVINFO_SYSTEM_ID_LEN];

      GAPRole_GetParameter(GAPROLE_BD_ADDR, ownAddress);

      // use 6 bytes of device address for 8 bytes of system ID value
      systemId[0] = ownAddress[0];
      systemId[1] = ownAddress[1];
      systemId[2] = ownAddress[2];

      // set middle bytes to zero
      systemId[4] = 0x00;
      systemId[3] = 0x00;

      // shift three bytes up
      systemId[7] = ownAddress[5];
      systemId[6] = ownAddress[4];
      systemId[5] = ownAddress[3];

      DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);
    }
    break;

    case GAPROLE_ADVERTISING:
	    HalLedSet(HAL_LED_1, HAL_LED_MODE_ON );
	    break;

    case GAPROLE_CONNECTED:
      HalLedSet(HAL_LED_1, HAL_LED_MODE_OFF );
      break;

    case GAPROLE_WAITING:
      // Link terminated intentionally: reset all sensors
      resetSensorSetup();
      //Stop battery measure
      battMeasureEnabled = FALSE;
      break;

	  default:
	    break;
  }

  gapProfileState = newState;
}

/*********************************************************************
 * @fn      readAccData
 *
 * @brief   Read accelerometer data
 *
 * @param   none
 *
 * @return  none
 */
static void readAccData(void)
{
  uint8 aData[ACCELEROMETER_DATA_LEN];

  if (HalAccRead(aData))
  {
    Accel_SetParameter( SENSOR_DATA, ACCELEROMETER_DATA_LEN, aData);
    
    //LED indicator when accelerometer above a given threshold on an axis
    if(aData[0] >= ACC_LED_THRESOLD 
       || aData[1] >= ACC_LED_THRESOLD
       || aData[2] >= ACC_LED_THRESOLD)
    {
      //Blink led 2
      HalLedSet(HAL_LED_2, HAL_LED_MODE_BLINK );
    }
  }
}

/*********************************************************************
 * @fn      readMagData
 *
 * @brief   Read magnetometer data
 *
 * @param   none
 *
 * @return  none
 */
static void readMagData( void )
{
  uint8 mData[MAGNETOMETER_DATA_LEN];

  if (HalMagRead(mData))
  {
    Magnetometer_SetParameter(SENSOR_DATA, MAGNETOMETER_DATA_LEN, mData);
  }
}

/*********************************************************************
 * @fn      readHumData
 *
 * @brief   Read humidity data
 *
 * @param   none
 *
 * @return  none
 */
static void readHumData(void)
{
  uint8 hData[HUMIDITY_DATA_LEN];

  if (HalHumiReadMeasurement(hData))
  {
    Humidity_SetParameter( SENSOR_DATA, HUMIDITY_DATA_LEN, hData);
  }
}

/*********************************************************************
 * @fn      readBarData
 *
 * @brief   Read barometer data
 *
 * @param   none
 *
 * @return  none
 */
static void readBarData( void )
{
  uint8 bData[BAROMETER_DATA_LEN];

  if (HalBarReadMeasurement(bData))
  {
    Barometer_SetParameter( SENSOR_DATA, BAROMETER_DATA_LEN, bData);
  }
}

/*********************************************************************
 * @fn      readBarCalibration
 *
 * @brief   Read barometer calibration
 *
 * @param   none
 *
 * @return  none
 */
static void readBarCalibration( void )
{
  uint8* cData = osal_mem_alloc(BAROMETER_CALI_LEN);

  if (cData != NULL )
  {
    HalBarReadCalibration(cData);
    Barometer_SetParameter( SENSOR_CALB, BAROMETER_CALI_LEN, cData);
    osal_mem_free(cData);
  }
}

/*********************************************************************
 * @fn      readIrTempData
 *
 * @brief   Read IR temperature data
 *
 * @param   none
 *
 * @return  none
 */
static void readIrTempData( void )
{
  uint8 tData[IRTEMPERATURE_DATA_LEN];

  if (HalIRTempRead(tData))
  {
    IRTemp_SetParameter( SENSOR_DATA, IRTEMPERATURE_DATA_LEN, tData);
  }
}

/*********************************************************************
 * @fn      readGyroData
 *
 * @brief   Read gyroscope data
 *
 * @param   none
 *
 * @return  none
 */
static void readGyroData( void )
{
  uint8 gData[GYROSCOPE_DATA_LEN];

  if (HalGyroRead(gData))
  {
    Gyro_SetParameter( SENSOR_DATA, GYROSCOPE_DATA_LEN, gData);
  }
}

/*********************************************************************
 * @fn      barometerChangeCB
 *
 * @brief   Callback from Barometer Service indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void barometerChangeCB( uint8 paramID )
{
  uint8 newValue;

  switch( paramID )
  {
    case SENSOR_CONF:
      Barometer_GetParameter( SENSOR_CONF, &newValue );

      switch ( newValue)
      {
      case ST_CFG_SENSOR_DISABLE:
        if (barEnabled)
        {
          barEnabled = FALSE;
          osal_set_event( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT);
        }
        break;

      case ST_CFG_SENSOR_ENABLE:
        if(!barEnabled)
        {
          barEnabled = TRUE;
          osal_set_event( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT);
        }
        break;

      case ST_CFG_CALIBRATE:
        readBarCalibration();
        break;

      default:
        break;
      }
      break;

  case SENSOR_PERI:
      Barometer_GetParameter( SENSOR_PERI, &newValue );
      sensorBarPeriod = newValue*SENSOR_PERIOD_RESOLUTION;
      break;

    default:
      // should not get here!
      break;
  }
}

/*********************************************************************
 * @fn      irTempChangeCB
 *
 * @brief   Callback from IR Temperature Service indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void irTempChangeCB( uint8 paramID )
{
  uint8 newValue;
  
  switch (paramID) {
  case SENSOR_CONF:
    IRTemp_GetParameter( SENSOR_CONF, &newValue );
    
    if ( newValue == ST_CFG_SENSOR_DISABLE)
    {
      // Put sensor to sleep
      if (irTempEnabled)
      {
        irTempEnabled = FALSE;
        osal_set_event( sensorTag_TaskID, ST_IRTEMPERATURE_READ_EVT);
      }
    }
    else if (newValue == ST_CFG_SENSOR_ENABLE)
    {
      if (!irTempEnabled)
      {
        irTempEnabled = TRUE;
        osal_set_event( sensorTag_TaskID,ST_IRTEMPERATURE_READ_EVT);
      }
    }
    break;
    
  case SENSOR_PERI:
    IRTemp_GetParameter( SENSOR_PERI, &newValue );
    sensorTmpPeriod = newValue*SENSOR_PERIOD_RESOLUTION;
    break;
    
  default:
    // Should not get here
    break;
  }
}

/*********************************************************************
 * @fn      accelChangeCB
 *
 * @brief   Callback from Acceleromter Service indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void accelChangeCB( uint8 paramID )
{
  uint8 newValue;

  switch (paramID)
  {
    case SENSOR_CONF:
      Accel_GetParameter( SENSOR_CONF, &newValue );
      if ( newValue == ST_CFG_SENSOR_DISABLE)
      {
        // Put sensor to sleep
        if (accConfig != ST_CFG_SENSOR_DISABLE)
        {
          accConfig = ST_CFG_SENSOR_DISABLE;
          osal_set_event( sensorTag_TaskID, ST_ACCELEROMETER_SENSOR_EVT);
        }
      }
      else
      {
        if (accConfig == ST_CFG_SENSOR_DISABLE)
        {
          // Start scheduling only on change disabled -> enabled
          osal_set_event( sensorTag_TaskID, ST_ACCELEROMETER_SENSOR_EVT);
        }
        // Scheduled already, so just change range
        accConfig = newValue;
        HalAccSetRange(accConfig);
      }
      break;

    case SENSOR_PERI:
      Accel_GetParameter( SENSOR_PERI, &newValue );
      sensorAccPeriod = newValue;
      break;

    default:
      // Should not get here
      break;
  }
}

/*********************************************************************
 * @fn      magnetometerChangeCB
 *
 * @brief   Callback from Magnetometer Service indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void magnetometerChangeCB( uint8 paramID )
{
  uint8 newValue;

  switch (paramID)
  {
    case SENSOR_CONF:
      Magnetometer_GetParameter( SENSOR_CONF, &newValue );

      if ( newValue == ST_CFG_SENSOR_DISABLE )
      {
        if(magEnabled)
        {
          magEnabled = FALSE;
          osal_set_event( sensorTag_TaskID, ST_MAGNETOMETER_SENSOR_EVT);
        }
      }
      else if ( newValue == ST_CFG_SENSOR_ENABLE )
      {
        if(!magEnabled)
        {
          magEnabled = TRUE;
          osal_set_event( sensorTag_TaskID, ST_MAGNETOMETER_SENSOR_EVT);
        }
      }
      break;

    case SENSOR_PERI:
      Magnetometer_GetParameter( SENSOR_PERI, &newValue );
      sensorMagPeriod = newValue;
      break;

    default:
      // Should not get here
      break;
  }
}

/*********************************************************************
 * @fn      humidityChangeCB
 *
 * @brief   Callback from Humidity Service indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void humidityChangeCB( uint8 paramID )
{
  uint8 newValue;
  
  switch ( paramID)
  {
  case  SENSOR_CONF:
    Humidity_GetParameter( SENSOR_CONF, &newValue );
    
    if ( newValue == ST_CFG_SENSOR_DISABLE)
    {
      if (humiEnabled)
      {
        humiEnabled = FALSE;
        osal_set_event( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT);
      }
    }
    
    if ( newValue == ST_CFG_SENSOR_ENABLE )
    {
      if (!humiEnabled)
      {
        humiEnabled = TRUE;
        humiState = 0;
        osal_set_event( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT);
      }
    }
    break;
    
  case SENSOR_PERI:
    Humidity_GetParameter( SENSOR_PERI, &newValue );
    sensorHumPeriod = newValue*SENSOR_PERIOD_RESOLUTION;
    break;
    
  default:
    // Should not get here
    break;
  }
}

/*********************************************************************
 * @fn      gyroChangeCB
 *
 * @brief   Callback from GyroProfile indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void gyroChangeCB( uint8 paramID )
{
  uint8 newValue;
  
  switch (paramID) {
  case SENSOR_CONF:
    Gyro_GetParameter( SENSOR_CONF, &newValue );
    
    if (newValue == 0)
    {
      // All three axes off, put sensor to sleep
      if (gyroEnabled)
      {
        gyroEnabled = FALSE;
        osal_set_event( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT);
      }
    }
    else
    {
      // Bitmap tells which axis to enable (bit 0: X, but 1: Y, but 2: Z)
      gyroEnabled = TRUE;
      sensorGyroAxes = newValue & 0x07;
      sensorGyroUpdateAxes = TRUE;
      osal_set_event( sensorTag_TaskID,  ST_GYROSCOPE_SENSOR_EVT);
    }
    break;
    
  case SENSOR_PERI:
    Gyro_GetParameter( SENSOR_PERI, &newValue );
    sensorGyrPeriod = newValue;
    break;
    
  default:
    // Should not get here
    break;
  }
}

/*********************************************************************
 * @fn      testChangeCB
 *
 * @brief   Callback from Test indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void testChangeCB( uint8 paramID )
{
  if( paramID == TEST_CONF_ATTR )
  {
    uint8 newValue;

    Test_GetParameter( TEST_CONF_ATTR, &newValue );

    if (newValue & TEST_MODE_ENABLE)
    {
      testMode = TRUE;
    }
    else
    {
      testMode = FALSE;
    }

    if (testMode)
    {
      // Test mode: possible to operate LEDs. Key hits will cause notifications,
      // side key does not influence connection state
      if (newValue & 0x01)
      {
        HalLedSet(HAL_LED_1,HAL_LED_MODE_ON);
      }
      else
      {
        HalLedSet(HAL_LED_1,HAL_LED_MODE_OFF);
      }

      if (newValue & 0x02)
      {
        HalLedSet(HAL_LED_2,HAL_LED_MODE_ON);
      }
      else
      {
        HalLedSet(HAL_LED_2,HAL_LED_MODE_OFF);
      }
    }
    else
    {
      // Normal mode; make sure LEDs are reset and attribute cleared
      HalLedSet(HAL_LED_1,HAL_LED_MODE_OFF);
      HalLedSet(HAL_LED_2,HAL_LED_MODE_OFF);
      newValue = 0x00;
      Test_SetParameter( TEST_CONF_ATTR, 1, &newValue );
    }
  }
}

/*********************************************************************
 * @fn      ccChangeCB
 *
 * @brief   Callback from Connection Control indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void ccChangeCB( uint8 paramID )
{

  // CCSERVICE_CHAR1: read & notify only

  // CCSERVICE_CHAR: requested connection parameters
  if( paramID == CCSERVICE_CHAR2 )
  {
    uint8 buf[CCSERVICE_CHAR2_LEN];
    uint16 minConnInterval;
    uint16 maxConnInterval;
    uint16 slaveLatency;
    uint16 timeoutMultiplier;

    CcService_GetParameter( CCSERVICE_CHAR2, buf );

    minConnInterval = BUILD_UINT16(buf[0],buf[1]);
    maxConnInterval = BUILD_UINT16(buf[2],buf[3]);
    slaveLatency = BUILD_UINT16(buf[4],buf[5]);
    timeoutMultiplier = BUILD_UINT16(buf[6],buf[7]);

    // Update connection parameters
    GAPRole_SendUpdateParam( minConnInterval, maxConnInterval, slaveLatency, timeoutMultiplier, GAPROLE_TERMINATE_LINK);
  }

  // CCSERVICE_CHAR3: Disconnect request
  if( paramID == CCSERVICE_CHAR3 )
  {
    // Any change in the value will terminate the connection
    GAPRole_TerminateConnection();
  }
}


/*********************************************************************
 * @fn      gapRolesParamUpdateCB
 *
 * @brief   Called when connection parameters are updates
 *
 * @param   connInterval - new connection interval
 *
 * @param   connSlaveLatency - new slave latency
 *
 * @param   connTimeout - new connection timeout
 *
 * @return  none
*/
static void gapRolesParamUpdateCB( uint16 connInterval, uint16 connSlaveLatency,uint16 connTimeout )
{
  uint8 buf[CCSERVICE_CHAR1_LEN];

  buf[0] = LO_UINT16(connInterval);
  buf[1] = HI_UINT16(connInterval);
  buf[2] = LO_UINT16(connSlaveLatency);
  buf[3] = HI_UINT16(connSlaveLatency);
  buf[4] = LO_UINT16(connTimeout);
  buf[5] = HI_UINT16(connTimeout);
  CcService_SetParameter(CCSERVICE_CHAR1,sizeof(buf),buf);
}

/*********************************************************************
 * @fn      stBattCB
 *
 * @brief   Called on battery service events
 *
 * @param   event 
 *
 * @return  none
*/
static void stBattCB(uint8 event)
{
  if (event == BATT_LEVEL_NOTI_ENABLED)
  {
    // if connected start periodic measurement
    if (gapProfileState == GAPROLE_CONNECTED)
    {
      osal_start_timerEx( sensorTag_TaskID, BATT_PERIODIC_EVT, WAIT_BEFORE_FIRST_BATT );
      battMeasureEnabled = TRUE;
     } 
   }
   else if (event == BATT_LEVEL_NOTI_DISABLED)
   {
     // stop periodic measurement
     battMeasureEnabled = FALSE;
   }
}


/*********************************************************************
 * @fn      resetCharacteristicValue
 *
 * @brief   Initialize a characteristic value to zero
 *
 * @param   servID - service ID (UUID)
 *
 * @param   paramID - parameter ID of the value is to be cleared
 *
 * @param   vakue - value to initialise with
 *
 * @param   paramLen - length of the parameter
 *
 * @return  none
 */
static void resetCharacteristicValue(uint16 servUuid, uint8 paramID, uint8 value, uint8 paramLen)
{
  uint8* pData = osal_mem_alloc(paramLen);

  if (pData == NULL)
  {
    return;
  }

  osal_memset(pData,value,paramLen);

  switch(servUuid)
  {
    case IRTEMPERATURE_SERV_UUID:
      IRTemp_SetParameter( paramID, paramLen, pData);
      break;

    case ACCELEROMETER_SERV_UUID:
      Accel_SetParameter( paramID, paramLen, pData);
      break;

    case MAGNETOMETER_SERV_UUID:
      Magnetometer_SetParameter( paramID, paramLen, pData);
      break;

    case HUMIDITY_SERV_UUID:
      Humidity_SetParameter( paramID, paramLen, pData);
      break;

    case BAROMETER_SERV_UUID:
      Barometer_SetParameter( paramID, paramLen, pData);
      break;

    case GYROSCOPE_SERV_UUID:
      Gyro_SetParameter( paramID, paramLen, pData);
      break;
     
    case BATT_SERV_UUID:
      Batt_SetParameter( paramID, paramLen, pData);
      break;      

    default:
      // Should not get here
      break;
  }

  osal_mem_free(pData);
}

/*********************************************************************
 * @fn      resetCharacteristicValues
 *
 * @brief   Initialize all the characteristic values
 *
 * @return  none
 */
static void resetCharacteristicValues( void )
{
  resetCharacteristicValue( IRTEMPERATURE_SERV_UUID, SENSOR_DATA, 0, IRTEMPERATURE_DATA_LEN);
  resetCharacteristicValue( IRTEMPERATURE_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
  resetCharacteristicValue( IRTEMPERATURE_SERV_UUID, SENSOR_PERI, TEMP_DEFAULT_PERIOD / SENSOR_PERIOD_RESOLUTION, sizeof ( uint8 ));

  resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_DATA, 0, ACCELEROMETER_DATA_LEN );
  resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
  resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_PERI, ACC_DEFAULT_PERIOD, sizeof ( uint8 ));

  resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_DATA, 0, HUMIDITY_DATA_LEN);
  resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
  resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_PERI, HUM_DEFAULT_PERIOD / SENSOR_PERIOD_RESOLUTION, sizeof ( uint8 ));

  resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_DATA, 0, MAGNETOMETER_DATA_LEN);
  resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
  resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_PERI, MAG_DEFAULT_PERIOD, sizeof ( uint8 ));

  resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_DATA, 0, BAROMETER_DATA_LEN);
  resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
  resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_PERI, BAR_DEFAULT_PERIOD / SENSOR_PERIOD_RESOLUTION, sizeof ( uint8 ));

  resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_DATA, 0, GYROSCOPE_DATA_LEN);
  resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof( uint8 ));
  resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_PERI, GYRO_DEFAULT_PERIOD, sizeof ( uint8 ));
  
  resetCharacteristicValue( BATT_SERV_UUID, BATT_PARAM_LEVEL, 100, BATT_DATA_LEN);  
}

/*********************************************************************
 * @fn      stBattPeriodicTask
 *
 * @brief   Periodic battery measurement when battery service is enabled
 *
 * @return  none
 */
static void stBattPeriodicTask( void )
{
  if (gapProfileState == GAPROLE_CONNECTED)
  {
    // perform battery level check
    Batt_MeasLevel( );

    // Restart timer
    osal_start_timerEx( sensorTag_TaskID, BATT_PERIODIC_EVT, DEFAULT_BATT_PERIOD );
  }
}

/*********************************************************************
*********************************************************************/