Côté Microcontrôleur

Environnement de Développement

IDE STM32CubeIDE


Projet Source

WORKSPACE_F411_BLEIOT_STM32CUBEIDE.zip


Capteurs

Capteur d’Humidité / Température HTS221

hts221.h

/*
 * hts221.h
 *
 *  Created on: Nov 17, 2017
 *      Author: kerhoas
 */

#ifndef INC_HTS221_H_
#define INC_HTS221_H_

#include "main.h"


uint8_t hts221_whoAmI();
void hts221_activate();
void hts221_storeCalibration();
int hts221_getTemperature(float*);
int hts221_getHumidity(float*);


#endif /* INC_HTS221_H_ */

hts221.c

/*
 * hts221.c
 *
 *  Created on: Nov 17, 2017
 *      Author: kerhoas
 */

// https://github.com/ameltech/sme-hts221-library

#include "hts221.h"
#include "drv_i2c.h"

//================================================================================
//              HUMIDITY AND TEMPERATURE SENSOR
//================================================================================

#define HTS221_I2C_ADDRESS      0xBE

#define TEMPERATURE_READY   0x1
#define HUMIDITY_READY      0x2
#define POWER_UP            0x80
#define BDU_SET             0x4
#define ODR0_SET            0x1 // setting sensor reading period 1Hz

//================================================================================
//              REGISTER ADDRESS MAP
//================================================================================

#define WHO_AM_I        0x0F
#define AV_CONF         0x10
#define CTRL_REG1       0x20
#define CTRL_REG2       0x21
#define CTRL_REG3       0x22
#define STATUS_REG      0x27
#define HUMIDITY_OUT_L  0x28
#define HUMIDITY_OUT_H  0x29
#define TEMP_OUT_L      0x2A
#define TEMP_OUT_H      0x2B

#define CALIB_0         0x30    //  H0_rH_x2
#define CALIB_1         0x31    //  H1_rH_x2
#define CALIB_2         0x32    //  T0_degC_x8
#define CALIB_3         0x33    //  T1_degC_x8
#define CALIB_4         0x34
#define CALIB_5         0x35    // T1/T0 msb
#define CALIB_6         0x36    // H0_T0_OUT
#define CALIB_7         0x37    // H0_T0_OUT
#define CALIB_8         0x38
#define CALIB_9         0x39
#define CALIB_A         0x3A    //  H1_T0_OUT
#define CALIB_B         0x3B    //  H1_T0_OUT
#define CALIB_C         0x3C    //  T0_OUT
#define CALIB_D         0x3D    //  T0_OUT
#define CALIB_E         0x3E    //  T1_OUT
#define CALIB_F         0x3F    //  T1_OUT

//================================================================================
//          CONF VARIABLES
//================================================================================

uint8_t _h0_rH, _h1_rH;
uint32_t  _T0_degC, _T1_degC;
uint32_t  _H0_T0, _H1_T0;
uint32_t  _T0_OUT, _T1_OUT;

//================================================================================
//          WHO AM I
//  defaut : 0xBC
//================================================================================
uint8_t hts221_whoAmI()
{
    uint8_t id=0;
    i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, WHO_AM_I,  &id, 1);
    return id;
}
//================================================================================
//          ACTIVATE
//================================================================================
void hts221_activate()
{
     uint8_t data;
    i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CTRL_REG1,  &data, 1);
    data |= POWER_UP;
    data |= ODR0_SET;
    i2c1_WriteRegBuffer(HTS221_I2C_ADDRESS, CTRL_REG1,  &data, 1);
}
//================================================================================
//          BDU ACTIVATE
//================================================================================
void hts221_bduActivate()
{
     uint8_t data;
    i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CTRL_REG1,  &data, 1);
    data |= BDU_SET;
    i2c1_WriteRegBuffer(HTS221_I2C_ADDRESS, CTRL_REG1,  &data, 1);
}



//================================================================================
//  Hum and Temp average Configuration
//  default : 0x1B  -   0b 00 - 011 - 011 // R-Temp-Hum
//================================================================================
void hts221_conf()
{
}
//================================================================================
//          STORE CALIBRATION
//================================================================================
void hts221_storeCalibration()
{
     uint8_t data;
     uint16_t tmp;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_0,  &data, 1);
     _h0_rH = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_1,  &data, 1);
     _h1_rH = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_2,  &data, 1);
     _T0_degC = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_3,  &data, 1);
     _T1_degC = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_5,  &data, 1);
     tmp = _T0_degC;
     _T0_degC = (data&0x3)<<8;
     _T0_degC |= tmp;
     tmp = _T1_degC;
     _T1_degC = ((data&0xC)>>2)<<8;
     _T1_degC |= tmp;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_6,  &data, 1);
     _H0_T0 = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_7,  &data, 1);
     _H0_T0 |= data<<8;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_A,  &data, 1);
     _H1_T0 = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_B,  &data, 1);
     _H1_T0 |= data<<8;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_C,  &data, 1);
     _T0_OUT = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_D,  &data, 1);
     _T0_OUT |= data << 8;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_E,  &data, 1);
     _T1_OUT = data;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, CALIB_F,  &data, 1);
     _T1_OUT |= data << 8;
}
//================================================================================
//          GET TEMPERATURE
//================================================================================
int hts221_getTemperature(float* tmp)
{
    uint8_t data = 0;
    int16_t t_out;
    uint8_t temp_lsb;
    uint8_t     temp_msb;
   static float _temperature = 0.0;

    float deg = 0.0 ;
    float t_temp = 0.0 ;

     i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, STATUS_REG,  &data, 1);
     if(data & TEMPERATURE_READY)
     {
         i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, TEMP_OUT_L,  &temp_lsb, 1);
         i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, TEMP_OUT_H,  &temp_msb , 1);

         t_out = ((int16_t)temp_msb << 8 ) | temp_lsb;

         // Decode Temperature
         deg    = (float)((int16_t)(_T1_degC) - (int16_t)(_T0_degC))/8.0; // remove x8 multiple

         // Calculate Temperature in decimal of grade centigrades i.e. 15.0 = 150.
         t_temp = (float)(((int16_t)t_out - (int16_t)_T0_OUT) * deg) /
                 (float)((int16_t)_T1_OUT - (int16_t)_T0_OUT);
         deg    = (float)((int16_t)_T0_degC) / 8.0;     // remove x8 multiple
         _temperature = deg + t_temp; // provide signed celsius measurement unit

         *tmp = _temperature;

         return 0;
     }


     else
     {
    	 return -1;
     }
}
//================================================================================
//          GET HUMIDITY
//================================================================================
int hts221_getHumidity(float *humid)
{
    uint8_t data   = 0;
    uint16_t h_out = 0;
    float h_temp  = 0.0;
    float hum     = 0.0;
    static float _humidity;

    i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS, STATUS_REG,  &data, 1);

    if (data & HUMIDITY_READY) {

        i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS,HUMIDITY_OUT_H,  &data, 1);
        h_out = data << 8;  // MSB
        i2c1_ReadRegBuffer(HTS221_I2C_ADDRESS,HUMIDITY_OUT_L,  &data, 1);
        h_out |= data;      // LSB

        // Decode Humidity
        hum = ((int16_t)(_h1_rH) - (int16_t)(_h0_rH))/2.0;  // remove x2 multiple

        // Calculate humidity in decimal of grade centigrades i.e. 15.0 = 150.
        h_temp = (float)(((int16_t)h_out - (int16_t)_H0_T0) * hum) /
             (float)((int16_t)_H1_T0 - (int16_t)_H0_T0);
        hum    = (float)((int16_t)_h0_rH) / 2.0; // remove x2 multiple
        _humidity = (hum + h_temp); // provide signed % measurement unit


        *humid = _humidity;
        return 0;

    }
    else
    {
    	return -1;
    }


}
//================================================================================
Capteur de Pression / Température LSP22HB

lps22hb.h

/*
 * lps22hb.h
 *
 *  Created on: Jan 29, 2021
 *      Author: kerhoas
 */

#ifndef LPS22HB_H_
#define LPS22HB_H_

#include "drv_i2c.h"

uint8_t lps22hb_whoAmI();
void lps22hb_setup();
int lps22hb_getPressure(float*);
int lps22hb_getTemperature(float*);

#endif /* LPS22HB_H_ */

lps22hb.c

#include "lps22hb.h"

//========================================================================
//			PRESSURE SENSOR
//========================================================================

#define lps22hb_I2C_ADDRESS		0xBA

//=======================================================
//			REGISTER ADDRESS MAP
//=======================================================

#define	REF_P_XL		0x08
#define	REF_P_L			0x09
#define	REF_P_H			0x0A
#define	WHO_AM_I		0x0F
#define	RES_CONF		0x10
#define	CTRL_REG1		0x20
#define	CTRL_REG2		0x21
#define	CTRL_REG3		0x22
#define	CTRL_REG4		0x23
#define	INTERRUPT_CFG	0x24
#define	INT_SOURCE		0x25
#define	STATUS_REG		0x27
#define	PRESS_OUT_XL	0x28
#define	PRESS_OUT_L		0x29
#define	PRESS_OUT_H		0x2A
#define	TEMP_OUT_L		0x2B
#define	TEMP_OUT_H		0x2C
#define	FIFO_CTRL		0x2E
#define	FIFO_STATUS		0x2F
#define	THS_P_L			0x30
#define	THS_P_H			0x31
#define	RPDS_L			0x39
#define	RPDS_H			0x3A

//========================================================================
//			WHO AM I
//========================================================================
// Default : 0xBD
uint8_t lps22hb_whoAmI()
{
	uint8_t id;
	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, WHO_AM_I,  &id, 1);
	return id;
}
//========================================================================
//			SETUP
//========================================================================
void lps22hb_setup()
{
	uint8_t data;
	data = 0x10;	// Active mode, continuous update
	i2c1_WriteRegBuffer(lps22hb_I2C_ADDRESS, CTRL_REG1,  &data, 1);

	data=0x1A;	// resolution: temp=32, pressure=128
	i2c1_WriteRegBuffer(lps22hb_I2C_ADDRESS, RES_CONF,  &data, 1);
}
//========================================================================
//			GET PRESSURE
//========================================================================
int lps22hb_getPressure(float* pressure)
{
	uint8_t data_xl[1];
	uint8_t data_l[1];
	uint8_t data_h[1];
	static float press = 0.0;

	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, (PRESS_OUT_XL | 0x80),  data_xl, 1);
	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, (PRESS_OUT_L | 0x80),  data_l, 1);
	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, (PRESS_OUT_H | 0x80),  data_h, 1);
	//data_h[0]=0x10|data_h[0];
	press = ((data_h[0] * 65536) + ((data_l[0] * 256) + data_xl[0])) / 4096.0;

	*pressure = press;

	return 0;
}
//========================================================================
//			GET TEMPERATURE
//========================================================================
int lps22hb_getTemperature(float* tmp)
{
	uint8_t tempH, tempL;
	static float temperature = 0.0;

	//uint8_t  data = 0x1;	// For one-shot mode
	//i2c1_WriteRegBuffer(lps22hb_I2C_ADDRESS, CTRL_REG2,  &data, 1);
	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, TEMP_OUT_L,  &tempL, 1);
	i2c1_ReadRegBuffer(lps22hb_I2C_ADDRESS, TEMP_OUT_H,  &tempH, 1);

	int16_t val = ((int16_t)tempH) << 8 | (tempL & 0xff);
	temperature = (float)val/100.0;

	*tmp=temperature;

	return 0;
}
//========================================================================








Accéléromètre / Gyroscope LSM6DS0

lsm6ds0.h

/*
 * lsm6d0.h
 *
 *  Created on: Nov 17, 2017
 *      Author: kerhoas
 */

#ifndef INC_LSM6D0_H_
#define INC_LSM6D0_H_


#include "main.h"


/***************************************** COMMON REGISTERS ********************************************/

/**
 * @brief Interrupt config register
 * \code
 * Read/write
 * Default value: 0x00
 * [7] INT_IG_G:    Gyroscope interrupt enable on INT pin
 * [6] INT_IG_XL:   Accelerometer interrupt generator on INT pin
 * [5] INT_FSS5:    FSS5 interrupt enable on INT pin
 * [4] INT_OVR:     Overrun interrupt on INT pin
 * [3] INT_FTH:     Gyroscope interrupt enable on INT pin
 * [2] INT_BOOT:    Accelerometer interrupt generator on INT pin
 * [1] INT_DRDY_G:  FSS5 interrupt enable on INT pin
 * [0] INT_DRDY_XL: Overrun interrupt on INT pin
 * \endcode
 */
#define LSM6DS0_XG_INT_CTRL                                 0x0C
#define LSM6DS0_XG_WHO_AM_I_ADDR                            0x0F
#define LSM6DS0_XG_CTRL_REG4                                0x1E
#define LSM6DS0_XG_CTRL_REG10                               0x24


/***************************************** GYROSCOPE REGISTERS ********************************************/

/**
 * @brief Angular rate sensor Control Register 1
 * \code
 * Read/write
 * Default value: 0x00
 * [7:5] ODR_G2-0: Gyroscope output data rate selection
 * [4:3] FS_G1-0: Gyroscope full-scale selection
 * [2] This bit must be set to ‘0’ for the correct operation of the device
 * [1:0] BW_G1-0: Gyroscope bandwidth selection
 * \endcode
 */
#define LSM6DS0_XG_CTRL_REG1_G                              0x10
#define LSM6DS0_XG_OUT_X_L_G                                0x18
#define LSM6DS0_XG_OUT_X_H_G                                0x19
#define LSM6DS0_XG_OUT_Y_L_G                                0x1A
#define LSM6DS0_XG_OUT_Y_H_G                                0x1B
#define LSM6DS0_XG_OUT_Z_L_G                                0x1C
#define LSM6DS0_XG_OUT_Z_H_G                                0x1D

/*************************************** ACCELEROMETER REGISTERS *******************************************/

/**
 * @brief Linear acceleration sensor Control Register 6
 * \code
 * Read/write
 * Default value: 0x00
 * [7:5] ODR_XL2-0: Accelerometer Output data rate and power mode selection
 * [4:3] FS1_XL-FS0_XL: Accelerometer full-scale selection
 * [2] BW_SCAL_ODR: Bandwidth selection
 * [1:0] BW_XL1-0: Anti-aliasing filter bandwidth selection
 * \endcode
 */
#define LSM6DS0_XG_CTRL_REG6_XL                              0x20
#define LSM6DS0_XG_CTRL_REG5_XL                              0x1F
#define LSM6DS0_XG_OUT_X_L_XL                                0x28
#define LSM6DS0_XG_OUT_X_H_XL                                0x29
#define LSM6DS0_XG_OUT_Y_L_XL                                0x2A
#define LSM6DS0_XG_OUT_Y_H_XL                                0x2B
#define LSM6DS0_XG_OUT_Z_L_XL                                0x2C
#define LSM6DS0_XG_OUT_Z_H_XL                                0x2D

/******************************************************************************/
/************* END ACCELEROMETER AND GYROSCOPE REGISTER MAPPING  **************/
/******************************************************************************/
/**
 * @brief Device Address
 */

#define LSM6DS0_XG_MEMS_ADDRESS                             0xD6    // SAD[0] = 1
#define I_AM_LSM6DS0_XG                                 ((uint8_t)0x68)

/************************************* GYROSCOPE REGISTERS VALUE *******************************************/


/** @defgroup LSM6DS0_XG Gyroscope Output Data Rate selection CTRL_REG1_G
 * @{
 */
#define LSM6DS0_G_ODR_PD                                ((uint8_t)0x00) /*!< Output Data Rate: Power-down*/
#define LSM6DS0_G_ODR_14_9HZ                            ((uint8_t)0x20) /*!< Output Data Rate: 14.9 Hz, cutoff 5Hz */
#define LSM6DS0_G_ODR_59_5HZ                            ((uint8_t)0x40) /*!< Output Data Rate: 59.5 Hz, cutoff 19Hz */
#define LSM6DS0_G_ODR_119HZ                             ((uint8_t)0x60) /*!< Output Data Rate: 119 Hz, cutoff 38Hz*/
#define LSM6DS0_G_ODR_238HZ                             ((uint8_t)0x80) /*!< Output Data Rate: 238 Hz, cutoff 76Hz*/
#define LSM6DS0_G_ODR_476HZ                             ((uint8_t)0xA0) /*!< Output Data Rate: 476 Hz, cutoff 100Hz*/
#define LSM6DS0_G_ODR_952HZ                             ((uint8_t)0xC0) /*!< Output Data Rate: 952 Hz, cutoff 100Hz*/

#define LSM6DS0_G_ODR_MASK                              ((uint8_t)0xE0)



/** @defgroup LSM6DS0_XG Gyroscope Bandwidth selection CTRL_REG1_G
 * @{
 */
#define LSM6DS0_G_BW_00                          ((uint8_t)0x00) /*!< Bandwidth selection:  - cutoff = n.a. when ODR = Power-down
                                                                                            - cutoff = n.a. when ODR = 14.9
                                                                                            - cutoff = 16 when ODR = 59.5
                                                                                            - cutoff = 14 when ODR = 119
                                                                                            - cutoff = 14 when ODR = 238
                                                                                            - cutoff = 21 when ODR = 476
                                                                                            - cutoff = 33 when ODR = 952  */
#define LSM6DS0_G_BW_01                          ((uint8_t)0x01) /*!< Bandwidth selection:  - cutoff = n.a. when ODR = Power-down
                                                                                            - cutoff = n.a. when ODR = 14.9
                                                                                            - cutoff = 16 when ODR = 59.5
                                                                                            - cutoff = 31 when ODR = 119
                                                                                            - cutoff = 29 when ODR = 238
                                                                                            - cutoff = 28 when ODR = 476
                                                                                            - cutoff = 40 when ODR = 952  */
#define LSM6DS0_G_BW_10                          ((uint8_t)0x02) /*!< Bandwidth selection:  - cutoff = n.a. when ODR = Power-down
                                                                                            - cutoff = n.a. when ODR = 14.9
                                                                                            - cutoff = 16 when ODR = 59.5
                                                                                            - cutoff = 31 when ODR = 119
                                                                                            - cutoff = 63 when ODR = 238
                                                                                            - cutoff = 57 when ODR = 476
                                                                                            - cutoff = 58 when ODR = 952  */
#define LSM6DS0_G_BW_11                          ((uint8_t)0x03) /*!< Bandwidth selection:  - cutoff = n.a. when ODR = Power-down
                                                                                            - cutoff = n.a. when ODR = 14.9
                                                                                            - cutoff = 16 when ODR = 59.5
                                                                                            - cutoff = 31 when ODR = 119
                                                                                            - cutoff = 78 when ODR = 238
                                                                                            - cutoff = 100 when ODR = 476
                                                                                            - cutoff = 100 when ODR = 952  */

#define LSM6DS0_G_BW_MASK                              ((uint8_t)0x03)


/** @defgroup LSM6DS0_XG Gyroscope Full scale selection CTRL_REG1_G
 * @{
 */
#define LSM6DS0_G_FS_245                               ((uint8_t)0x00) /*!< Full scale: 245 dps*/
#define LSM6DS0_G_FS_500                               ((uint8_t)0x08) /*!< Full scale: 500 dps */
#define LSM6DS0_G_FS_2000                              ((uint8_t)0x18) /*!< Full scale: 2000 dps */

#define LSM6DS0_G_FS_MASK                              ((uint8_t)0x18)


/** @defgroup LSM6DS0_XG Gyroscope’s Z-axis output enable selection CTRL_REG4
 * @{
 */
#define LSM6DS0_G_ZEN_DISABLE                          ((uint8_t)0x00) /*!< Gyroscope’s Z-axis output enable: disable */
#define LSM6DS0_G_ZEN_ENABLE                           ((uint8_t)0x20) /*!< Gyroscope’s Z-axis output enable: enable */

#define LSM6DS0_G_ZEN_MASK                             ((uint8_t)0x20)


/** @defgroup LSM6DS0_XG Gyroscope’s Y-axis output enable selection CTRL_REG4
 * @{
 */
#define LSM6DS0_G_YEN_DISABLE                          ((uint8_t)0x00) /*!< Gyroscope’s Y-axis output enable: disable */
#define LSM6DS0_G_YEN_ENABLE                           ((uint8_t)0x10) /*!< Gyroscope’s Y-axis output enable: enable */

#define LSM6DS0_G_YEN_MASK                             ((uint8_t)0x10)


/** @defgroup LSM6DS0_XG Gyroscope’s X-axis output enable selection CTRL_REG4
 * @{
 */
#define LSM6DS0_G_XEN_DISABLE                          ((uint8_t)0x00) /*!< Gyroscope’s X-axis output enable: disable */
#define LSM6DS0_G_XEN_ENABLE                           ((uint8_t)0x08) /*!< Gyroscope’s X-axis output enable: enable */

#define LSM6DS0_G_XEN_MASK                             ((uint8_t)0x08)


/** @defgroup LSM6DS0 Gyro selftest en/dis - LSM6DS0_XG_CTRL_REG10
 * @{
 */
#define LSM6DS0_G_ST_DISABLE                            ((uint8_t)0x00) /*!< Gyro selftest disable */
#define LSM6DS0_G_ST_ENABLE                             ((uint8_t)0x04) /*!< Gyro selftest enable */

#define LSM6DS0_G_ST_MASK                               ((uint8_t)0x04)

/**
 * @}
 */


/************************************ ACCELEROMETER REGISTERS VALUE *****************************************/

/** @defgroup LSM6DS0_XG Accelerometer Output Data Rate selection CTRL_REG6_XL
 * @{
 */
#define LSM6DS0_XL_ODR_PD                               ((uint8_t)0x00) /*!< Output Data Rate: Power-down*/
#define LSM6DS0_XL_ODR_10HZ                             ((uint8_t)0x20) /*!< Output Data Rate: 10 Hz*/
#define LSM6DS0_XL_ODR_50HZ                             ((uint8_t)0x40) /*!< Output Data Rate: 50 Hz */
#define LSM6DS0_XL_ODR_119HZ                            ((uint8_t)0x60) /*!< Output Data Rate: 119 Hz */
#define LSM6DS0_XL_ODR_238HZ                            ((uint8_t)0x80) /*!< Output Data Rate: 238 Hz */
#define LSM6DS0_XL_ODR_476HZ                            ((uint8_t)0xA0) /*!< Output Data Rate: 476 Hz */
#define LSM6DS0_XL_ODR_952HZ                            ((uint8_t)0xC0) /*!< Output Data Rate: 952 Hz */

#define LSM6DS0_XL_ODR_MASK                             ((uint8_t)0xE0)


/** @defgroup LSM6DS0_XG Accelerometer Full scale selection CTRL_REG6_XL
 * @{
 */
#define LSM6DS0_XL_FS_2G                                ((uint8_t)0x00) /*!< Full scale: +- 2g */
#define LSM6DS0_XL_FS_4G                                ((uint8_t)0x10) /*!< Full scale: +- 4g */
#define LSM6DS0_XL_FS_8G                                ((uint8_t)0x18) /*!< Full scale: +- 8g */

#define LSM6DS0_XL_FS_MASK                              ((uint8_t)0x18)


/** @defgroup LSM6DS0_XG Accelerometer Bandwidth selection CTRL_REG6_XL
 * @{
 */
#define LSM6DS0_XL_BW_SCAL_ODR                          ((uint8_t)0x00) /*!< Bandwidth selection: determined by ODR:
                                                                                                  - BW = 408Hz when ODR = 952Hz, 50Hz, 10Hz                                                                                                - BW = 211Hz when ODR = 476Hz
                                                                                                  - BW = 105Hz when ODR = 238Hz
                                                                                                  - BW = 50Hz when ODR = 119Hz */
#define LSM6DS0_XL_BW_SCAL_BW                           ((uint8_t)0x04) /*!< Bandwidth selection: selected according to Anti aliasing filter bandwidth */

#define LSM6DS0_XL_BW_SCAL_MASK                         ((uint8_t)0x04)


/** @defgroup LSM6DS0_XG Accelerometer Anti aliasing filter bandwidth selection CTRL_REG6_XL
 * @{
 */
#define LSM6DS0_XL_BW_408HZ                             ((uint8_t)0x00) /*!< Anti-aliasing filter bandwidht: 408 Hz */
#define LSM6DS0_XL_BW_211HZ                             ((uint8_t)0x01) /*!< Anti-aliasing filter bandwidht: 211 Hz */
#define LSM6DS0_XL_BW_105HZ                             ((uint8_t)0x02) /*!< Anti-aliasing filter bandwidht: 105 Hz */
#define LSM6DS0_XL_BW_50HZ                              ((uint8_t)0x03) /*!< Anti-aliasing filter bandwidht: 50 Hz */

#define LSM6DS0_XL_BW_MASK                              ((uint8_t)0x03)


/** @defgroup LSM6DS0_XG Accelerometer Decimation of acceleration data selection CTRL_REG5_XL
 * @{
 */
#define LSM6DS0_XL_DEC_NO                               ((uint8_t)0x00) /*!< Decimation of acceleration data: no decimation */
#define LSM6DS0_XL_DEC_EVERY_2S                         ((uint8_t)0x40) /*!< Decimation of acceleration data: update every 2 samples */
#define LSM6DS0_XL_DEC_EVERY_4S                         ((uint8_t)0x80) /*!< Decimation of acceleration data: update every 4 samples */
#define LSM6DS0_XL_DEC_EVERY_8S                         ((uint8_t)0xC0) /*!< Decimation of acceleration data: update every 8 samples */

#define LSM6DS0_XL_DEC_MASK                             ((uint8_t)0xC0)


/** @defgroup LSM6DS0_XG Accelerometer’s Z-axis output enable selection CTRL_REG5_XL
 * @{
 */
#define LSM6DS0_XL_ZEN_DISABLE                          ((uint8_t)0x00) /*!< Accelerometer’s Z-axis output enable: disable */
#define LSM6DS0_XL_ZEN_ENABLE                           ((uint8_t)0x20) /*!< Accelerometer’s Z-axis output enable: enable */

#define LSM6DS0_XL_ZEN_MASK                             ((uint8_t)0x20)


/** @defgroup LSM6DS0_XG Accelerometer’s Y-axis output enable selection CTRL_REG5_XL
 * @{
 */
#define LSM6DS0_XL_YEN_DISABLE                          ((uint8_t)0x00) /*!< Accelerometer’s Y-axis output enable: disable */
#define LSM6DS0_XL_YEN_ENABLE                           ((uint8_t)0x10) /*!< Accelerometer’s Y-axis output enable: enable */

#define LSM6DS0_XL_YEN_MASK                             ((uint8_t)0x10)

/** @defgroup LSM6DS0_XG Accelerometer’s X-axis output enable selection CTRL_REG5_XL
 * @{
 */
#define LSM6DS0_XL_XEN_DISABLE                          ((uint8_t)0x00) /*!< Accelerometer’s X-axis output enable: disable */
#define LSM6DS0_XL_XEN_ENABLE                           ((uint8_t)0x08) /*!< Accelerometer’s X-axis output enable: enable */

#define LSM6DS0_XL_XEN_MASK                             ((uint8_t)0x08)

/** @defgroup LSM6DS0 Accel selftest en/dis - LSM6DS0_XG_CTRL_REG10
 * @{
 */
#define LSM6DS0_XL_ST_DISABLE                           ((uint8_t)0x00) /*!< Accel selftest disable */
#define LSM6DS0_XL_ST_ENABLE                            ((uint8_t)0x01) /*!< Accel selftest enable */

#define LSM6DS0_XL_ST_MASK ((uint8_t)0x01)


//===================================================================================================

uint8_t lsm6ds0_whoAmI();
void lsm6ds0_setup(uint8_t gyro_data_rate, uint8_t gyro_full_scale,  uint8_t acc_data_rate, uint8_t acc_full_scale);
int16_t lsm6ds0_gyro_getAxesXRaw(void);
int16_t lsm6ds0_gyro_getAxesYRaw(void);
int16_t lsm6ds0_gyro_getAxesZRaw(void);
int16_t lsm6ds0_acc_getAxesXRaw(void);
int16_t lsm6ds0_acc_getAxesYRaw(void);
int16_t lsm6ds0_acc_getAxesZRaw(void);

int16_t lsm6ds0_acc_getAxesX(void);
int16_t lsm6ds0_acc_getAxesY(void);
int16_t lsm6ds0_acc_getAxesZ(void);




#endif /* INC_LSM6D0_H_ */

lsm6ds0.c

/*
 * LSM6DS0.c
 *
 *  Created on: Nov 17, 2017
 *      Author: kerhoas
 */

#include "lsm6ds0.h"
#include "drv_i2c.h"

//=========================================================================================
//					WHO AM I
//=========================================================================================
// Default : 0x68
uint8_t lsm6ds0_whoAmI()
{
	uint8_t id;
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR,  &id, 1);
	return id;
}
//=========================================================================================
//					INIT
//=========================================================================================
// lsm6ds0_setup(LSM6DS0_G_ODR_119HZ, LSM6DS0_G_FS_2000,  LSM6DS0_XL_ODR_119HZ, LSM6DS0_XL_FS_2G)
void lsm6ds0_setup(uint8_t gyro_data_rate, uint8_t gyro_full_scale,  uint8_t acc_data_rate, uint8_t acc_full_scale)
{
	/******* Gyroscope init *******/
	uint8_t tmp1;
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G,  &tmp1, 1);
	/* Output Data Rate selection */
    tmp1 &= ~(LSM6DS0_G_ODR_MASK);
    tmp1 |= gyro_data_rate; 	//tmp1 |= LSM6DS0_G_ODR_119HZ;
	/* Full scale selection */
	tmp1 &= ~(LSM6DS0_G_FS_MASK);
	tmp1 |= gyro_full_scale ;	//tmp1 |= LSM6DS0_G_FS_2000;
	i2c1_WriteRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G,  &tmp1, 1);

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4,  &tmp1, 1);
	/* Enable X axis selection */
    tmp1 &= ~(LSM6DS0_G_XEN_MASK);
    tmp1 |= LSM6DS0_G_XEN_ENABLE;
    /* Enable Y axis selection */
    tmp1 &= ~(LSM6DS0_G_YEN_MASK);
    tmp1 |= LSM6DS0_G_YEN_ENABLE;
    /* Enable Z axis selection */
    tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
    tmp1 |= LSM6DS0_G_ZEN_ENABLE;
	i2c1_WriteRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4,  &tmp1, 1);

	/***** Accelerometer init *****/

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL,  &tmp1, 1);
	/* Output Data Rate selection */
	tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
	tmp1 |= acc_data_rate;		// tmp1 |= LSM6DS0_XL_ODR_119HZ;

	/* Full scale selection */
	tmp1 &= ~(LSM6DS0_XL_FS_MASK);
	tmp1 |= acc_full_scale ;  	//tmp1 |= LSM6DS0_XL_FS_2G;
	i2c1_WriteRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL,  &tmp1, 1);

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL,  &tmp1, 1);
	/* Enable X axis selection */
	tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
	tmp1 |= LSM6DS0_XL_XEN_ENABLE;

	/* Enable Y axis selection */
	tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
	tmp1 |= LSM6DS0_XL_YEN_ENABLE;

	/* Enable Z axis selection */
	tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
	tmp1 |= LSM6DS0_XL_ZEN_ENABLE;
	i2c1_WriteRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL,  &tmp1, 1);
}
//=========================================================================================
//					GYRO
//=========================================================================================
int16_t lsm6ds0_gyro_getAxesXRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}

int16_t lsm6ds0_gyro_getAxesYRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}

int16_t lsm6ds0_gyro_getAxesZRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}
//=========================================================================================
float lsm6ds0_getGyroScale(void){

  	float sensitivity=0;
	uint8_t scale ;
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G,  &scale, 1);

	scale &= LSM6DS0_G_FS_MASK;
    switch(scale)
	{
		case LSM6DS0_G_FS_245:
			sensitivity = 8.75;
			break;
		case LSM6DS0_G_FS_500:
			sensitivity = 17.50;
			break;
		case LSM6DS0_G_FS_2000:
			sensitivity = 70;
			break;
    }
	return sensitivity;
}
//=========================================================================================
int16_t lsm6ds0_gyro_getAxesX(void)
{
	int16_t mx;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G | (1 << 7), data, 2);
	mx = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getGyroScale();
	return mx * sensitivity;
}

int16_t lsm6ds0_gyro_getAxesY(void)
{
	int16_t my;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G | (1 << 7), data, 2);
	my = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getGyroScale();
	return my * sensitivity;
}

int16_t lsm6ds0_gyro_getAxesZ(void)
{
	int16_t mz;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G | (1 << 7), data, 2);
	mz = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getGyroScale();
	return mz * sensitivity;
}


//=========================================================================================
//					ACCELEROMETER
//=========================================================================================
int16_t lsm6ds0_acc_getAxesXRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}

int16_t lsm6ds0_acc_getAxesYRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}

int16_t lsm6ds0_acc_getAxesZRaw(void)
{
	uint8_t data[2];
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL | (1 << 7), data, 2);
	return (int16_t)(data[1]<<8 | data[0]);
}
//=========================================================================================
float lsm6ds0_getAccScale(void){

  	float sensitivity=0;
	uint8_t acc_scale;
	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL,  &acc_scale, 1);
    acc_scale &= LSM6DS0_XL_FS_MASK;

    switch(acc_scale)
	{
		case LSM6DS0_XL_FS_2G:
			sensitivity = 0.061;
			break;
		case LSM6DS0_XL_FS_4G:
			sensitivity = 0.122;
			break;
		case LSM6DS0_XL_FS_8G:
			sensitivity = 0.244;
			break;
	}
	return sensitivity;
}
//=========================================================================================

int16_t lsm6ds0_acc_getAxesX(void)
{
	int16_t mx ;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL | (1 << 7), data, 2);
	mx = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getAccScale();
	return mx * sensitivity;
}

int16_t lsm6ds0_acc_getAxesY(void)
{
	int16_t my ;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL | (1 << 7), data, 2);
	my = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getAccScale();
	return my * sensitivity;
}

int16_t lsm6ds0_acc_getAxesZ(void)
{
	int16_t mz ;
	uint8_t data[2];

	i2c1_ReadRegBuffer(LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL | (1 << 7), data, 2);
	mz = (int16_t)(data[1]<<8 | data[0]);

	float sensitivity = lsm6ds0_getAccScale();
	return mz * sensitivity;
}

//=========================================================================================







Fichiers Principaux

main.c
#include "main.h"
#include "sensor_service.h"
#include "drv_i2c.h"
#include "hts221.h"
#include "lps25hb.h"
#include "lsm6ds0.h"
#include "hci_tl.h"
#include "sensor_service.h"
#include "role_type.h"
#include "bluenrg_utils.h"
#include "bluenrg_gatt_server.h"
#include "bluenrg_gap_aci.h"
#include "bluenrg_gatt_aci.h"
#include "bluenrg_hal_aci.h"
#include "util.h"

void SystemClock_Config(void);


extern volatile int     connected;
extern volatile uint8_t set_connectable;
extern volatile uint8_t notification_enabled;

volatile uint8_t user_button_init_state = 1;
volatile uint8_t user_button_pressed = 0;

#define SERVER_ROLE
#define BDADDR_SIZE 6

uint8_t bnrg_expansion_board = IDB04A1; /* at startup, suppose the X-NUCLEO-IDB04A1 is used */
BLE_RoleTypeDef BLE_Role = SERVER;

void MX_BlueNRG_MS_Init(void);

extern int pressure_subscription ;
extern int temperature_subscription ;

//===================================================================
//						>>> MAIN <<<
//===================================================================
int main(void)
{
	int i;

  HAL_Init();
  SystemClock_Config();
  i2c1_Init();
  BSP_PB_Init(BUTTON_KEY, BUTTON_MODE_EXTI);
  BSP_LED_Init(LED2);

  //BSP_LED_On(LED2);
  //BSP_LED_Off(LED2);



  BSP_COM_Init(COM1);

  user_button_init_state = BSP_PB_GetState(BUTTON_KEY);

  MX_BlueNRG_MS_Init();

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //				TEST CAPTEURS
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  i = lsm6ds0_whoAmI();

  lsm6ds0_setup(LSM6DS0_G_ODR_119HZ, LSM6DS0_G_FS_2000,  LSM6DS0_XL_ODR_119HZ, LSM6DS0_XL_FS_2G);

  /*
  int16_t X, Y, Z;

  while(1)
  {
	//X = lsm6ds0_gyro_getAxesX();
	//Y = lsm6ds0_gyro_getAxesY();
	//Z = lsm6ds0_gyro_getAxesZ();

	X = lsm6ds0_acc_getAxesX();
	Y = lsm6ds0_acc_getAxesY();
	Z = lsm6ds0_acc_getAxesZ();

	term_printf("%d %d %d \n\r", X, Y, Z);
	HAL_Delay(20);
  }

*/
  //-------------------------------------------------------------------

  i = lps22hb_whoAmI();
  lps22hb_setup();
  //--------------------------------------------------------------------

  hts221_activate();
  i = hts221_whoAmI();
  hts221_storeCalibration();

 /* while(1)
  {
	  pressure =   lps25hb_getPressure();
	  temp_lps25hb = lps25hb_getTemperature();

	  tp = hts221_getTemperature();
	  humidity = hts221_getHumidity(); //humidity = hts221_getHumidity();

	  HAL_Delay(500);
	  term_printf("humidity = %d temperature = %d pressure = %d temp_lps25hb= %d \n\r", (int) humidity, (int)tp, (int) pressure, (int) temp_lps25hb);
  }*/

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  while (1)
  {
	  if (set_connectable)
	  {
	    Make_Connection();
	    set_connectable = FALSE;
	    user_button_init_state = BSP_PB_GetState(BUTTON_KEY);
	  }

	  if(connected && pressure_subscription && temperature_subscription )
	  {

		  SendEnvironmentalData();
		  HAL_Delay(100);
	  }

	  hci_user_evt_proc();
  }
}
//=====================================================================================
//			 Send Environmetal Data (Temperature/Pressure/Humidity) to BLE
//=====================================================================================
void SendEnvironmentalData(void)
{
    static float pres_value, temp_value;
    int32_t PressToSend=0;
    int16_t TempToSend=0;
    int32_t decPart, intPart;

    lps22hb_getPressure(&pres_value);
    MCR_BLUEMS_F2I_2D(pres_value, intPart, decPart);
   	PressToSend=intPart*100+decPart;

   	hts221_getTemperature(&temp_value);
   	MCR_BLUEMS_F2I_1D(temp_value, intPart, decPart);
   	TempToSend = intPart*10+decPart;

   	Environmental_Update(PressToSend, TempToSend);
}
//===========================================================================
void MX_BlueNRG_MS_Init(void)
{

  uint8_t SERVER_BDADDR[] = {0xab, 0x00, 0x00, 0xE1, 0x80, 0x02};
  uint8_t bdaddr[BDADDR_SIZE];
  uint16_t service_handle, dev_name_char_handle, appearance_char_handle;

  uint8_t  hwVersion;
  uint16_t fwVersion;
  int ret;

  hci_init(user_notify, NULL);

  /* get the BlueNRG HW and FW versions */
  getBlueNRGVersion(&hwVersion, &fwVersion);

  /*
   * Reset BlueNRG again otherwise we won't
   * be able to change its MAC address.
   * aci_hal_write_config_data() must be the first
   * command after reset otherwise it will fail.
   */
  hci_reset();

  HAL_Delay(100);

  printf("HWver %d, FWver %d\n\r", hwVersion, fwVersion);

  if (hwVersion > 0x30) { /* X-NUCLEO-IDB05A1 expansion board is used */
    bnrg_expansion_board = IDB05A1;
  }

  BLUENRG_memcpy(bdaddr, SERVER_BDADDR, sizeof(SERVER_BDADDR));

  ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET,
                                  CONFIG_DATA_PUBADDR_LEN,
                                  bdaddr);
  	  	  	  	  	  if (ret) {    printf("Setting BD_ADDR failed 0x%02x.\n\r", ret);  }

  ret = aci_gatt_init();
  	  	  	  	  	  if (ret) {    printf("GATT_Init failed.\n\r");  }


  ret = aci_gap_init_IDB05A1(GAP_PERIPHERAL_ROLE_IDB05A1, 0, 0x07, &service_handle, &dev_name_char_handle, &appearance_char_handle);

  	  	  	  	  	  if (ret != BLE_STATUS_SUCCESS) {	printf("GAP_Init failed.\n\r");}

  ret = aci_gap_set_auth_requirement(MITM_PROTECTION_REQUIRED,
                                     OOB_AUTH_DATA_ABSENT,
                                     NULL,
                                     7,
                                     16,
                                     USE_FIXED_PIN_FOR_PAIRING,
                                     123456,
                                     BONDING);
  	  	  	  	  	  if (ret == BLE_STATUS_SUCCESS) { printf("BLE Stack Initialized.\n\r"); }


    printf("SERVER: BLE Stack Initialized\n\r");

    ret = Add_HWServW2ST_Service();		// HARDWARE SERVICE
						if (ret == BLE_STATUS_SUCCESS)
						  printf("Service added successfully.\n\r");
						else
						  printf("Error while adding service.\n\r");

  /* Set output power level */
  ret = aci_hal_set_tx_power_level(1,4);

}
//===================================================================
//				CALLBACK FUNCTIONS
//===================================================================
void BSP_PB_Callback(Button_TypeDef Button)
{
  user_button_pressed = 1;
}
//===================================================================
//				SYS CLOCK CONFIG
//===================================================================
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 16;
  RCC_OscInitStruct.PLL.PLLN = 336;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
  RCC_OscInitStruct.PLL.PLLQ = 7;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

//===================================================================
void Error_Handler(void)
{
}
//===================================================================
#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

//===================================================================
sensor_service.c
#include "sensor_service.h"
#include "bluenrg_gap_aci.h"
#include "bluenrg_gatt_aci.h"
#include "bluenrg_hal_aci.h"

#include "stm32f4xx_nucleo.h"
#include "util.h"

volatile int connected = FALSE;
volatile uint8_t set_connectable = 1;
volatile uint16_t connection_handle = 0;
volatile uint8_t notification_enabled = FALSE;
volatile uint8_t start_read_tx_char_handle = FALSE;
volatile uint8_t start_read_rx_char_handle = FALSE;
volatile uint8_t end_read_tx_char_handle = FALSE;
volatile uint8_t end_read_rx_char_handle = FALSE;

uint16_t tx_handle;
uint16_t rx_handle;

uint16_t sampleServHandle, TXCharHandle, RXCharHandle;

extern uint8_t bnrg_expansion_board;
extern BLE_RoleTypeDef BLE_Role;

static uint16_t HWServW2STHandle;

static uint16_t ledServiceHandle;
static uint16_t switchLedCharHandle;
static uint16_t stateLedCharHandle;
static uint16_t TemperatureCharHandle;
static uint16_t PressionCharHandle;

int pressure_subscription = 0;
int temperature_subscription = 0;
int LedState = 0;

//========================================================================
tBleStatus Add_HWServW2ST_Service(void)
{
	tBleStatus ret;
	uint8_t primary_short_uuid[2];

	HAL_Delay(1000); // !! Delay important, sinon les modifs de services/chars ne sont pas prises en compte.
	//-----------------------------------------------------------------------------------------
	// Light switch service 0xFF10
	primary_short_uuid[0] = 0x10;
	primary_short_uuid[1] = 0xFF;
	ret = aci_gatt_add_serv(UUID_TYPE_16,  primary_short_uuid, PRIMARY_SERVICE,
	                          13,&ledServiceHandle);
										if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);

	// SWITCH LED CHARACTERISTIC : 0xFF12
	primary_short_uuid[0] = 0x12;
	primary_short_uuid[1] = 0xFF;
	ret =  aci_gatt_add_char(ledServiceHandle, UUID_TYPE_16, primary_short_uuid, 1,
												CHAR_PROP_WRITE|CHAR_PROP_WRITE_WITHOUT_RESP,
					                           ATTR_PERMISSION_NONE,
											   GATT_NOTIFY_ATTRIBUTE_WRITE  ,
					                           16, 0, &switchLedCharHandle);
										if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);

	// STATE LED CHARACTERISTIC : 0xFF11
	primary_short_uuid[0] = 0x11;
	primary_short_uuid[1] = 0xFF;
	ret =  aci_gatt_add_char(ledServiceHandle, UUID_TYPE_16, primary_short_uuid, 2,
					                           CHAR_PROP_NOTIFY|CHAR_PROP_READ,
					                           ATTR_PERMISSION_NONE,
					                           GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP,
					                           16, 0, &stateLedCharHandle);

										if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);
	//-----------------------------------------------------------------------------------------
	//ENVIRONMENT SERVICE : 0x181A
	primary_short_uuid[0] = 0x1A;
	primary_short_uuid[1] = 0x18;
	ret = aci_gatt_add_serv(UUID_TYPE_16,  primary_short_uuid, PRIMARY_SERVICE,
	                          13,&HWServW2STHandle);

										if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);

	// TEMPERATURE CHAR : 0x2A6E
	primary_short_uuid[0] = 0x6E;
	primary_short_uuid[1] = 0x2A;
	ret =  aci_gatt_add_char(HWServW2STHandle, UUID_TYPE_16, primary_short_uuid, 2,
											    CHAR_PROP_NOTIFY|CHAR_PROP_READ,
											    ATTR_PERMISSION_NONE,
											    GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP,
											   16, 0, &TemperatureCharHandle);


										if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);

	// PRESSURE
	primary_short_uuid[0] = 0x6D;
	primary_short_uuid[1] = 0x2A;
	ret =  aci_gatt_add_char(HWServW2STHandle, UUID_TYPE_16, primary_short_uuid, 4,
											    CHAR_PROP_NOTIFY|CHAR_PROP_READ,
											    ATTR_PERMISSION_NONE,
											   GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP,
											   16, 0, &PressionCharHandle);

									if (ret != BLE_STATUS_SUCCESS) {	goto fail;}
	HAL_Delay(100);


	 return BLE_STATUS_SUCCESS;

	 fail:	return BLE_STATUS_ERROR;

}
//========================================================================
tBleStatus Environmental_Update(int32_t Press, int16_t Temp)
{
  tBleStatus ret;
  uint8_t buff_Temp[2];
  uint8_t buff_Press[2];

  STORE_LE_16(buff_Temp,Temp);
  ret = aci_gatt_update_char_value(HWServW2STHandle, TemperatureCharHandle, 0, 2,buff_Temp);

  STORE_LE_32(buff_Press,Press);
  ret = aci_gatt_update_char_value(HWServW2STHandle, PressionCharHandle, 0, 4,buff_Press);

  if (ret != BLE_STATUS_SUCCESS) {    return BLE_STATUS_ERROR;  }
  return BLE_STATUS_SUCCESS;
}

//========================================================================

void Make_Connection(void)
{  
  tBleStatus ret;
  const char local_name[] = {AD_TYPE_COMPLETE_LOCAL_NAME,'B','l','u','e','N','R','G','_','E','N','I','B'};
    
  hci_le_set_scan_resp_data(0,NULL);	  /* disable scan response */
    
  printf("General Discoverable Mode \n\r");
  ret = aci_gap_set_discoverable(ADV_DATA_TYPE, ADV_INTERV_MIN, ADV_INTERV_MAX, PUBLIC_ADDR,
                                   NO_WHITE_LIST_USE, 13, local_name, 0, NULL, 0, 0);
  printf("%d\n\r",ret);
}

//========================================================================
void receiveData(uint8_t* data_buffer, uint8_t Nb_bytes)
{
  BSP_LED_Toggle(LED2);

  for(int i = 0; i < Nb_bytes; i++) {
    printf("%c", data_buffer[i]);
  }
  fflush(stdout);
}
//========================================================================
void sendData(uint8_t* data_buffer, uint8_t Nb_bytes)
{
    aci_gatt_write_without_response(connection_handle, rx_handle+1, Nb_bytes, data_buffer);
}
//========================================================================
void enableNotification(void)
{
  uint8_t client_char_conf_data[] = {0x01, 0x00}; // Enable notifications
  
  uint32_t tickstart = HAL_GetTick();
  
  while(aci_gatt_write_charac_descriptor(connection_handle, tx_handle+2, 2, client_char_conf_data)==BLE_STATUS_NOT_ALLOWED){
    /* Radio is busy */
    if ((HAL_GetTick() - tickstart) > (10*HCI_DEFAULT_TIMEOUT_MS)) break;
  }
  notification_enabled = TRUE;
}

//========================================================================

tBleStatus LED_Update(uint8_t LedStatus, uint8_t* data_buffer, uint8_t Nb_bytes)
{
  tBleStatus ret;
  uint8_t buff[1];

  buff[0] = LedStatus;
  if (LedStatus == 1)
  	  	  	  	  	  {
	  	  	  	  	  BSP_LED_Off(LED2);
	  	  	  	  	  LedState=0;
	  	  	  	  	  buff[0]=0;
	  	  	  	  	  ret = aci_gatt_update_char_value(ledServiceHandle, stateLedCharHandle, 0, 1,buff);
  	  	  	  	  	  }
  else				  {
	  	  	  	  	  BSP_LED_On(LED2);
	  	  	  	  	  LedState=1;
	  	  	  	  	  buff[0]=1;
  	  	  	  	  	  ret = aci_gatt_update_char_value(ledServiceHandle, stateLedCharHandle, 0, 1,buff);
  	  	  	  	  	  }

  if (ret != BLE_STATUS_SUCCESS)
  {
    printf("Error Updating led Char\r\n");
    return BLE_STATUS_ERROR;
  }
  for(int i = 0; i < Nb_bytes; i++)
  {
    printf("data_buff=%d \n\r", data_buffer[i]);
  }
  return BLE_STATUS_SUCCESS;
}

//========================================================================
void Attribute_Modified_CB(uint16_t handle, uint8_t data_length, uint8_t *att_data)
{

	if(handle == PressionCharHandle + 2)
	{
		printf("Pression Char Handle \n\r");
		switch(att_data[0])
						{
						case 1 :	pressure_subscription = 1;
									break;
						case 0 :	pressure_subscription = 0;
									break;
						default : 	break;
						}
	}
	else
	if (handle == TemperatureCharHandle + 2)
	{
		printf("temperature char handle \n\r");
		switch(att_data[0])
						{
						case 1 :	temperature_subscription = 1;
									break;
						case 0 :	temperature_subscription = 0;
									break;
						default : 	break;
						}
	}
	else
	if (handle == stateLedCharHandle + 2)
	{
		printf("STATE LED CHANGE : attr_handle ==  %d + 1 \n\r", stateLedCharHandle);
	}
	else
	if (handle == switchLedCharHandle + 1)
	{
		  printf("SWITCH LED CHANGE : attr_handle ==  %d + 1 \n\r", switchLedCharHandle);
		  LED_Update(LedState,att_data, data_length);
	}
}

//========================================================================
void GAP_ConnectionComplete_CB(uint8_t addr[6], uint16_t handle)
{  
  connected = TRUE;
  connection_handle = handle;
  
  printf("Connected to device:\r\n");
  for(int i = 5; i > 0; i--){
    printf("%02X-", addr[i]);
  }
  printf("%02X\n\r", addr[0]);
}
//========================================================================
void GAP_DisconnectionComplete_CB(void)
{
  connected = FALSE;
  
  printf("Disconnected\n\r");
  /* Make the device connectable again. */
  set_connectable = TRUE;
  notification_enabled = FALSE;
  start_read_tx_char_handle = FALSE;
  start_read_rx_char_handle = FALSE;
  end_read_tx_char_handle = FALSE;
  end_read_rx_char_handle = FALSE;
}

//========================================================================
void GATT_Notification_CB(uint16_t attr_handle, uint8_t attr_len, uint8_t *attr_value)
{
  if (attr_handle == tx_handle+1) {
    receiveData(attr_value, attr_len);
  }
}
//========================================================================
void user_notify(void * pData)
{
  hci_uart_pckt *hci_pckt = pData;  
  /* obtain event packet */
  hci_event_pckt *event_pckt = (hci_event_pckt*)hci_pckt->data;
  
  if(hci_pckt->type != HCI_EVENT_PKT)
    return;
  
  switch(event_pckt->evt){
    
  case EVT_DISCONN_COMPLETE:
    {
      GAP_DisconnectionComplete_CB();
    }
    break;
    
  case EVT_LE_META_EVENT:
    {
      evt_le_meta_event *evt = (void *)event_pckt->data;
      
      switch(evt->subevent){
      case EVT_LE_CONN_COMPLETE:
        {
          evt_le_connection_complete *cc = (void *)evt->data;
          GAP_ConnectionComplete_CB(cc->peer_bdaddr, cc->handle);
        }
        break;
      }
    }
    break;
    
  case EVT_VENDOR:
    {
      evt_blue_aci *blue_evt = (void*)event_pckt->data;
      switch(blue_evt->ecode){
        
      case EVT_BLUE_GATT_ATTRIBUTE_MODIFIED:
        {
          if (bnrg_expansion_board == IDB05A1) {
            evt_gatt_attr_modified_IDB05A1 *evt = (evt_gatt_attr_modified_IDB05A1*)blue_evt->data;
            Attribute_Modified_CB(evt->attr_handle, evt->data_length, evt->att_data);
          }
          else {
            evt_gatt_attr_modified_IDB04A1 *evt = (evt_gatt_attr_modified_IDB04A1*)blue_evt->data;
            Attribute_Modified_CB(evt->attr_handle, evt->data_length, evt->att_data);
          }
          
        }
        break;
      case EVT_BLUE_GATT_NOTIFICATION:
        {
          evt_gatt_attr_notification *evt = (evt_gatt_attr_notification*)blue_evt->data;
          GATT_Notification_CB(evt->attr_handle, evt->event_data_length - 2, evt->attr_value);
        }
        break;
      case EVT_BLUE_GATT_DISC_READ_CHAR_BY_UUID_RESP:
        if(BLE_Role == CLIENT) {
          PRINTF("EVT_BLUE_GATT_DISC_READ_CHAR_BY_UUID_RESP\n\r");
          
          evt_gatt_disc_read_char_by_uuid_resp *resp = (void*)blue_evt->data;
          
          if (start_read_tx_char_handle && !end_read_tx_char_handle)
          {
            tx_handle = resp->attr_handle;
            printf("TX Char Handle %04X\n\r", tx_handle);
          }
          else if (start_read_rx_char_handle && !end_read_rx_char_handle)
          {
            rx_handle = resp->attr_handle;
            printf("RX Char Handle %04X\n\r", rx_handle);
          }
        }
        break;
        
      case EVT_BLUE_GATT_PROCEDURE_COMPLETE:
        if(BLE_Role == CLIENT) {
          /* Wait for gatt procedure complete event trigger related to Discovery Charac by UUID */
          //evt_gatt_procedure_complete *pr = (void*)blue_evt->data;
          
          if (start_read_tx_char_handle && !end_read_tx_char_handle)
          {
            end_read_tx_char_handle = TRUE;
          }
          else if (start_read_rx_char_handle && !end_read_rx_char_handle)
          {
            end_read_rx_char_handle = TRUE;
          }
        }
        break;
      }
    }
    break;
  }    
}
//========================================================================