NEHSROBOT / selfbalansingcar_V1

STILL DEBUGGING

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of X_NUCLEO_IKS01A1 by ST

x_nucleo_iks01a1.cpp@93:e28893a46ce7, 2018-08-02 (annotated)

Committer:
barry210110
Date:
Thu Aug 02 15:30:11 2018 +0000
Revision:
93:e28893a46ce7
Parent:
92:d1c67d482bad 
DEBUGGING_3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 3:088aa5839e0d 1 /**
Wolfgang Betz 4:566f2c41dc1d 2 ******************************************************************************
Wolfgang Betz 4:566f2c41dc1d 3 * @file x_nucleo_iks01a1.cpp
Wolfgang Betz 4:566f2c41dc1d 4 * @author AST / EST
Wolfgang Betz 4:566f2c41dc1d 5 * @version V0.0.1
Wolfgang Betz 4:566f2c41dc1d 6 * @date 08-October-2014
Wolfgang Betz 4:566f2c41dc1d 7 * @brief Implementation file for the X_NUCLEO_IKS01A1 singleton class
Wolfgang Betz 4:566f2c41dc1d 8 ******************************************************************************
Wolfgang Betz 4:566f2c41dc1d 9 * @attention
Wolfgang Betz 4:566f2c41dc1d 10 *
Wolfgang Betz 4:566f2c41dc1d 11 * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
Wolfgang Betz 4:566f2c41dc1d 12 *
Wolfgang Betz 4:566f2c41dc1d 13 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 4:566f2c41dc1d 14 * are permitted provided that the following conditions are met:
Wolfgang Betz 4:566f2c41dc1d 15 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 4:566f2c41dc1d 16 * this list of conditions and the following disclaimer.
Wolfgang Betz 4:566f2c41dc1d 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 4:566f2c41dc1d 18 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 4:566f2c41dc1d 19 * and/or other materials provided with the distribution.
Wolfgang Betz 4:566f2c41dc1d 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 4:566f2c41dc1d 21 * may be used to endorse or promote products derived from this software
Wolfgang Betz 4:566f2c41dc1d 22 * without specific prior written permission.
Wolfgang Betz 4:566f2c41dc1d 23 *
Wolfgang Betz 4:566f2c41dc1d 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 4:566f2c41dc1d 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 4:566f2c41dc1d 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 4:566f2c41dc1d 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 4:566f2c41dc1d 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 4:566f2c41dc1d 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 4:566f2c41dc1d 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 4:566f2c41dc1d 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 4:566f2c41dc1d 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 4:566f2c41dc1d 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 4:566f2c41dc1d 34 *
Wolfgang Betz 4:566f2c41dc1d 35 ******************************************************************************
Wolfgang Betz 3:088aa5839e0d 36 */
Wolfgang Betz 3:088aa5839e0d 37
Wolfgang Betz 3:088aa5839e0d 38 /* Includes ------------------------------------------------------------------*/
Wolfgang Betz 3:088aa5839e0d 39 #include "mbed.h"
Wolfgang Betz 3:088aa5839e0d 40 #include "x_nucleo_iks01a1.h"
Wolfgang Betz 3:088aa5839e0d 41
Wolfgang Betz 3:088aa5839e0d 42 /* Static variables ----------------------------------------------------------*/
Wolfgang Betz 3:088aa5839e0d 43 X_NUCLEO_IKS01A1* X_NUCLEO_IKS01A1::_instance = NULL;
Wolfgang Betz 3:088aa5839e0d 44
Wolfgang Betz 4:566f2c41dc1d 45
Wolfgang Betz 3:088aa5839e0d 46 /* Methods -------------------------------------------------------------------*/
Wolfgang Betz 3:088aa5839e0d 47 /**
Wolfgang Betz 3:088aa5839e0d 48 * @brief Constructor
Wolfgang Betz 3:088aa5839e0d 49 */
Wolfgang Betz 72:79a2a64c9b2f 50 X_NUCLEO_IKS01A1::X_NUCLEO_IKS01A1(DevI2C *ext_i2c, PinName ff_irq_pin) : dev_i2c(ext_i2c),
Wolfgang Betz 42:5490ac2d0a10 51 ht_sensor(new HTS221(*dev_i2c)),
Wolfgang Betz 42:5490ac2d0a10 52 magnetometer(new LIS3MDL(*dev_i2c)),
Wolfgang Betz 71:78164ec41ad2 53 pt_sensor(new LPS25H(*dev_i2c)),
Wolfgang Betz 72:79a2a64c9b2f 54 gyro_lsm6ds0(new LSM6DS0(*dev_i2c))
Wolfgang Betz 3:088aa5839e0d 55 {
Wolfgang Betz 72:79a2a64c9b2f 56 if(ff_irq_pin == NC) {
Wolfgang Betz 72:79a2a64c9b2f 57 gyro_lsm6ds3 = NULL;
Wolfgang Betz 72:79a2a64c9b2f 58 } else {
Wolfgang Betz 72:79a2a64c9b2f 59 gyro_lsm6ds3 = new LSM6DS3(*dev_i2c, ff_irq_pin);
Wolfgang Betz 72:79a2a64c9b2f 60 }
Wolfgang Betz 3:088aa5839e0d 61 }
Wolfgang Betz 3:088aa5839e0d 62
Wolfgang Betz 3:088aa5839e0d 63 /**
Wolfgang Betz 57:04563dd74269 64 * @brief Get singleton instance
Wolfgang Betz 73:e5b028bdd350 65 * @return a pointer to the initialized singleton instance of class X_NUCLEO_IKS01A1.
Wolfgang Betz 73:e5b028bdd350 66 * A return value of NULL indicates an out of memory situation.
Wolfgang Betz 57:04563dd74269 67 * @param[in] ext_i2c (optional) pointer to an instance of DevI2C to be used
Wolfgang Betz 57:04563dd74269 68 * for communication on the expansion board.
Wolfgang Betz 73:e5b028bdd350 69 * Defaults to NULL.
Wolfgang Betz 72:79a2a64c9b2f 70 * Taken into account only on the very first call of one of the 'Instance' functions.
Wolfgang Betz 57:04563dd74269 71 * If not provided a new DevI2C will be created with standard
Wolfgang Betz 57:04563dd74269 72 * configuration parameters.
Wolfgang Betz 57:04563dd74269 73 * The used DevI2C object gets saved in instance variable dev_i2c.
Wolfgang Betz 72:79a2a64c9b2f 74 * @param[in] ff_irq_pin (optional) PinName of the pin associated to asynchronous
Wolfgang Betz 72:79a2a64c9b2f 75 * (i.e. interrupt based) free fall detection in case a LSM6DS3 3D
Wolfgang Betz 72:79a2a64c9b2f 76 * Acceleromenter and 3D Gyroscope is mounted on top of the DIL 24-pin socket.
Wolfgang Betz 73:e5b028bdd350 77 * Defaults to IKS01A1_PIN_FF.
Wolfgang Betz 73:e5b028bdd350 78 * Taken into account only on the very first call of one of the 'Instance' functions.
Wolfgang Betz 72:79a2a64c9b2f 79 * A value of 'NC' will avoid instantiation of the LSM6DS3 even if present.
Wolfgang Betz 3:088aa5839e0d 80 */
Wolfgang Betz 72:79a2a64c9b2f 81 X_NUCLEO_IKS01A1* X_NUCLEO_IKS01A1::Instance(DevI2C *ext_i2c, PinName ff_irq_pin) {
Wolfgang Betz 3:088aa5839e0d 82 if(_instance == NULL) {
Wolfgang Betz 3:088aa5839e0d 83 if(ext_i2c == NULL)
Wolfgang Betz 3:088aa5839e0d 84 ext_i2c = new DevI2C(IKS01A1_PIN_I2C_SDA, IKS01A1_PIN_I2C_SCL);
Wolfgang Betz 10:cdf47a4b3f2f 85
Wolfgang Betz 10:cdf47a4b3f2f 86 if(ext_i2c != NULL)
Wolfgang Betz 72:79a2a64c9b2f 87 _instance = new X_NUCLEO_IKS01A1(ext_i2c, ff_irq_pin);
Wolfgang Betz 72:79a2a64c9b2f 88
Wolfgang Betz 72:79a2a64c9b2f 89 if(_instance != NULL) {
Wolfgang Betz 72:79a2a64c9b2f 90 bool ret = _instance->Init();
Wolfgang Betz 72:79a2a64c9b2f 91 if(!ret) {
Wolfgang Betz 72:79a2a64c9b2f 92 error("Failed to init X_NUCLEO_IKS01A1 expansion board!\n");
Wolfgang Betz 72:79a2a64c9b2f 93 }
Wolfgang Betz 72:79a2a64c9b2f 94 }
Wolfgang Betz 72:79a2a64c9b2f 95 }
Wolfgang Betz 72:79a2a64c9b2f 96
Wolfgang Betz 72:79a2a64c9b2f 97 return _instance;
Wolfgang Betz 72:79a2a64c9b2f 98 }
Wolfgang Betz 72:79a2a64c9b2f 99
Wolfgang Betz 72:79a2a64c9b2f 100 /**
Wolfgang Betz 72:79a2a64c9b2f 101 * @brief Get singleton instance
Wolfgang Betz 73:e5b028bdd350 102 * @return a pointer to the initialized singleton instance of class X_NUCLEO_IKS01A1.
Wolfgang Betz 73:e5b028bdd350 103 * A return value of NULL indicates an out of memory situation.
Wolfgang Betz 72:79a2a64c9b2f 104 * @param[in] sda I2C data line pin.
Wolfgang Betz 72:79a2a64c9b2f 105 * Taken into account only on the very first call of one of the 'Instance' functions.
Wolfgang Betz 72:79a2a64c9b2f 106 * A new DevI2C will be created based on parameters 'sda' and 'scl'.
Wolfgang Betz 72:79a2a64c9b2f 107 * The used DevI2C object gets saved in instance variable dev_i2c.
Wolfgang Betz 72:79a2a64c9b2f 108 * @param[in] scl I2C clock line pin.
Wolfgang Betz 72:79a2a64c9b2f 109 * Taken into account only on the very first call of one of the 'Instance' functions.
Wolfgang Betz 72:79a2a64c9b2f 110 * A new DevI2C will be created based on parameters 'sda' and 'scl'.
Wolfgang Betz 72:79a2a64c9b2f 111 * The used DevI2C object gets saved in instance variable dev_i2c.
Wolfgang Betz 72:79a2a64c9b2f 112 * @param[in] ff_irq_pin (optional) PinName of the pin associated to asynchronous
Wolfgang Betz 72:79a2a64c9b2f 113 * (i.e. interrupt based) free fall detection in case a LSM6DS3 3D
Wolfgang Betz 72:79a2a64c9b2f 114 * Acceleromenter and 3D Gyroscope is mounted on top of the DIL 24-pin socket.
Wolfgang Betz 73:e5b028bdd350 115 * Defaults to NC.
Wolfgang Betz 73:e5b028bdd350 116 * Taken into account only on the very first call of one of the 'Instance' functions.
Wolfgang Betz 72:79a2a64c9b2f 117 * A value of 'NC' will avoid instantiation of the LSM6DS3 even if present.
Wolfgang Betz 72:79a2a64c9b2f 118 */
Wolfgang Betz 72:79a2a64c9b2f 119 X_NUCLEO_IKS01A1* X_NUCLEO_IKS01A1::Instance(PinName sda, PinName scl, PinName ff_irq_pin) {
Wolfgang Betz 72:79a2a64c9b2f 120 if(_instance == NULL) {
Wolfgang Betz 72:79a2a64c9b2f 121 DevI2C *ext_i2c = new DevI2C(sda, scl);
Wolfgang Betz 72:79a2a64c9b2f 122
Wolfgang Betz 72:79a2a64c9b2f 123 if(ext_i2c != NULL)
Wolfgang Betz 72:79a2a64c9b2f 124 _instance = new X_NUCLEO_IKS01A1(ext_i2c, ff_irq_pin);
Wolfgang Betz 8:d49db2c9d9c1 125
Wolfgang Betz 8:d49db2c9d9c1 126 if(_instance != NULL) {
Wolfgang Betz 8:d49db2c9d9c1 127 bool ret = _instance->Init();
Wolfgang Betz 8:d49db2c9d9c1 128 if(!ret) {
Wolfgang Betz 8:d49db2c9d9c1 129 error("Failed to init X_NUCLEO_IKS01A1 expansion board!\n");
Wolfgang Betz 8:d49db2c9d9c1 130 }
Wolfgang Betz 8:d49db2c9d9c1 131 }
Wolfgang Betz 3:088aa5839e0d 132 }
Wolfgang Betz 8:d49db2c9d9c1 133
Wolfgang Betz 3:088aa5839e0d 134 return _instance;
Wolfgang Betz 3:088aa5839e0d 135 }
Wolfgang Betz 8:d49db2c9d9c1 136
Wolfgang Betz 8:d49db2c9d9c1 137 /**
Wolfgang Betz 44:d757094f6229 138 * @brief Initialize the singleton's HT sensor
Wolfgang Betz 42:5490ac2d0a10 139 * @retval true if initialization successful,
Wolfgang Betz 42:5490ac2d0a10 140 * @retval false otherwise
Wolfgang Betz 42:5490ac2d0a10 141 */
Wolfgang Betz 42:5490ac2d0a10 142 bool X_NUCLEO_IKS01A1::Init_HTS221(void) {
Wolfgang Betz 8:d49db2c9d9c1 143 uint8_t ht_id = 0;
Wolfgang Betz 8:d49db2c9d9c1 144 HUM_TEMP_InitTypeDef InitStructure;
Wolfgang Betz 8:d49db2c9d9c1 145
Wolfgang Betz 42:5490ac2d0a10 146 /* Check presence */
Wolfgang Betz 92:d1c67d482bad 147 if((ht_sensor->read_id(&ht_id) != HUM_TEMP_OK) ||
Wolfgang Betz 42:5490ac2d0a10 148 (ht_id != I_AM_HTS221))
Wolfgang Betz 42:5490ac2d0a10 149 {
Wolfgang Betz 42:5490ac2d0a10 150 delete ht_sensor;
Wolfgang Betz 42:5490ac2d0a10 151 ht_sensor = NULL;
Wolfgang Betz 42:5490ac2d0a10 152 return true;
Wolfgang Betz 42:5490ac2d0a10 153 }
Wolfgang Betz 42:5490ac2d0a10 154
Wolfgang Betz 8:d49db2c9d9c1 155 /* Configure sensor */
Wolfgang Betz 8:d49db2c9d9c1 156 InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;
Wolfgang Betz 8:d49db2c9d9c1 157
Wolfgang Betz 92:d1c67d482bad 158 if(ht_sensor->init(&InitStructure) != HUM_TEMP_OK)
Wolfgang Betz 8:d49db2c9d9c1 159 {
Wolfgang Betz 8:d49db2c9d9c1 160 return false;
Wolfgang Betz 8:d49db2c9d9c1 161 }
Wolfgang Betz 8:d49db2c9d9c1 162
Wolfgang Betz 8:d49db2c9d9c1 163 return true;
Wolfgang Betz 8:d49db2c9d9c1 164 }
Wolfgang Betz 8:d49db2c9d9c1 165
Wolfgang Betz 8:d49db2c9d9c1 166 /**
Wolfgang Betz 44:d757094f6229 167 * @brief Initialize the singleton's magnetometer
Wolfgang Betz 44:d757094f6229 168 * @retval true if initialization successful,
Wolfgang Betz 44:d757094f6229 169 * @retval false otherwise
Wolfgang Betz 8:d49db2c9d9c1 170 */
Wolfgang Betz 42:5490ac2d0a10 171 bool X_NUCLEO_IKS01A1::Init_LIS3MDL(void) {
Wolfgang Betz 8:d49db2c9d9c1 172 uint8_t m_id = 0;
Wolfgang Betz 8:d49db2c9d9c1 173 MAGNETO_InitTypeDef InitStructure;
Wolfgang Betz 8:d49db2c9d9c1 174
Wolfgang Betz 42:5490ac2d0a10 175 /* Check presence */
Wolfgang Betz 92:d1c67d482bad 176 if((magnetometer->read_id(&m_id) != MAGNETO_OK) ||
Wolfgang Betz 42:5490ac2d0a10 177 (m_id != I_AM_LIS3MDL_M))
Wolfgang Betz 42:5490ac2d0a10 178 {
Wolfgang Betz 42:5490ac2d0a10 179 delete magnetometer;
Wolfgang Betz 42:5490ac2d0a10 180 magnetometer = NULL;
Wolfgang Betz 42:5490ac2d0a10 181 return true;
Wolfgang Betz 42:5490ac2d0a10 182 }
Wolfgang Betz 42:5490ac2d0a10 183
Wolfgang Betz 8:d49db2c9d9c1 184 /* Configure sensor */
Wolfgang Betz 8:d49db2c9d9c1 185 InitStructure.M_FullScale = LIS3MDL_M_FS_4;
Wolfgang Betz 8:d49db2c9d9c1 186 InitStructure.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
Wolfgang Betz 8:d49db2c9d9c1 187 InitStructure.M_XYOperativeMode = LIS3MDL_M_OM_HP;
Wolfgang Betz 8:d49db2c9d9c1 188 InitStructure.M_OutputDataRate = LIS3MDL_M_DO_80;
Wolfgang Betz 8:d49db2c9d9c1 189
Wolfgang Betz 92:d1c67d482bad 190 if(magnetometer->init(&InitStructure) != MAGNETO_OK)
Wolfgang Betz 8:d49db2c9d9c1 191 {
Wolfgang Betz 8:d49db2c9d9c1 192 return false;
Wolfgang Betz 8:d49db2c9d9c1 193 }
Wolfgang Betz 8:d49db2c9d9c1 194
Wolfgang Betz 8:d49db2c9d9c1 195 return true;
Wolfgang Betz 8:d49db2c9d9c1 196 }
Wolfgang Betz 8:d49db2c9d9c1 197
Wolfgang Betz 8:d49db2c9d9c1 198 /**
Wolfgang Betz 44:d757094f6229 199 * @brief Initialize the singleton's pressure sensor
Wolfgang Betz 44:d757094f6229 200 * @retval true if initialization successful,
Wolfgang Betz 44:d757094f6229 201 * @retval false otherwise
Wolfgang Betz 8:d49db2c9d9c1 202 */
Wolfgang Betz 42:5490ac2d0a10 203 bool X_NUCLEO_IKS01A1::Init_LPS25H(void) {
Wolfgang Betz 8:d49db2c9d9c1 204 uint8_t p_id = 0;
Wolfgang Betz 8:d49db2c9d9c1 205 PRESSURE_InitTypeDef InitStructure;
Wolfgang Betz 8:d49db2c9d9c1 206
Wolfgang Betz 42:5490ac2d0a10 207 /* Check presence */
Wolfgang Betz 92:d1c67d482bad 208 if((pt_sensor->read_id(&p_id) != PRESSURE_OK) ||
Wolfgang Betz 42:5490ac2d0a10 209 (p_id != I_AM_LPS25H))
Wolfgang Betz 42:5490ac2d0a10 210 {
Wolfgang Betz 71:78164ec41ad2 211 delete pt_sensor;
Wolfgang Betz 71:78164ec41ad2 212 pt_sensor = NULL;
Wolfgang Betz 42:5490ac2d0a10 213 return true;
Wolfgang Betz 42:5490ac2d0a10 214 }
Wolfgang Betz 42:5490ac2d0a10 215
Wolfgang Betz 8:d49db2c9d9c1 216 /* Configure sensor */
Wolfgang Betz 8:d49db2c9d9c1 217 InitStructure.OutputDataRate = LPS25H_ODR_1Hz;
Wolfgang Betz 8:d49db2c9d9c1 218 InitStructure.BlockDataUpdate = LPS25H_BDU_CONT;
Wolfgang Betz 8:d49db2c9d9c1 219 InitStructure.DiffEnable = LPS25H_DIFF_ENABLE;
Wolfgang Betz 8:d49db2c9d9c1 220 InitStructure.SPIMode = LPS25H_SPI_SIM_3W;
Wolfgang Betz 8:d49db2c9d9c1 221 InitStructure.PressureResolution = LPS25H_P_RES_AVG_32;
Wolfgang Betz 8:d49db2c9d9c1 222 InitStructure.TemperatureResolution = LPS25H_T_RES_AVG_16;
Wolfgang Betz 8:d49db2c9d9c1 223
Wolfgang Betz 92:d1c67d482bad 224 if(pt_sensor->init(&InitStructure) != PRESSURE_OK)
Wolfgang Betz 8:d49db2c9d9c1 225 {
Wolfgang Betz 8:d49db2c9d9c1 226 return false;
Wolfgang Betz 8:d49db2c9d9c1 227 }
Wolfgang Betz 8:d49db2c9d9c1 228
Wolfgang Betz 8:d49db2c9d9c1 229 return true;
Wolfgang Betz 8:d49db2c9d9c1 230 }
Wolfgang Betz 8:d49db2c9d9c1 231
Wolfgang Betz 8:d49db2c9d9c1 232 /**
Wolfgang Betz 44:d757094f6229 233 * @brief Initialize the singleton's LSM6DS0 gyroscope
Wolfgang Betz 44:d757094f6229 234 * @retval true if initialization successful,
Wolfgang Betz 44:d757094f6229 235 * @retval false otherwise
Wolfgang Betz 8:d49db2c9d9c1 236 */
Wolfgang Betz 42:5490ac2d0a10 237 bool X_NUCLEO_IKS01A1::Init_LSM6DS0(void) {
Wolfgang Betz 8:d49db2c9d9c1 238 IMU_6AXES_InitTypeDef InitStructure;
Wolfgang Betz 8:d49db2c9d9c1 239 uint8_t xg_id = 0;
Wolfgang Betz 8:d49db2c9d9c1 240
Wolfgang Betz 42:5490ac2d0a10 241 /* Check presence */
Wolfgang Betz 46:badcff0675e8 242 if((gyro_lsm6ds3 != NULL) || // by default do not instantiate two gyroscopes
Wolfgang Betz 92:d1c67d482bad 243 (gyro_lsm6ds0->read_id(&xg_id) != IMU_6AXES_OK) ||
Wolfgang Betz 42:5490ac2d0a10 244 (xg_id != I_AM_LSM6DS0_XG))
Wolfgang Betz 42:5490ac2d0a10 245 {
Wolfgang Betz 44:d757094f6229 246 delete gyro_lsm6ds0;
Wolfgang Betz 44:d757094f6229 247 gyro_lsm6ds0 = NULL;
Wolfgang Betz 42:5490ac2d0a10 248 return true;
Wolfgang Betz 42:5490ac2d0a10 249 }
Wolfgang Betz 42:5490ac2d0a10 250
Wolfgang Betz 8:d49db2c9d9c1 251 /* Configure sensor */
Wolfgang Betz 24:92cc9c6e4b2b 252 InitStructure.G_FullScale = 2000.0f; /* 2000DPS */
Wolfgang Betz 24:92cc9c6e4b2b 253 InitStructure.G_OutputDataRate = 119.0f; /* 119HZ */
Wolfgang Betz 24:92cc9c6e4b2b 254 InitStructure.G_X_Axis = 1; /* Enable */
Wolfgang Betz 24:92cc9c6e4b2b 255 InitStructure.G_Y_Axis = 1; /* Enable */
Wolfgang Betz 24:92cc9c6e4b2b 256 InitStructure.G_Z_Axis = 1; /* Enable */
Wolfgang Betz 8:d49db2c9d9c1 257
Wolfgang Betz 24:92cc9c6e4b2b 258 InitStructure.X_FullScale = 2.0f; /* 2G */
Wolfgang Betz 24:92cc9c6e4b2b 259 InitStructure.X_OutputDataRate = 119.0f; /* 119HZ */
Wolfgang Betz 24:92cc9c6e4b2b 260 InitStructure.X_X_Axis = 1; /* Enable */
Wolfgang Betz 24:92cc9c6e4b2b 261 InitStructure.X_Y_Axis = 1; /* Enable */
Wolfgang Betz 24:92cc9c6e4b2b 262 InitStructure.X_Z_Axis = 1; /* Enable */
Wolfgang Betz 8:d49db2c9d9c1 263
Wolfgang Betz 92:d1c67d482bad 264 if(gyro_lsm6ds0->init(&InitStructure) != IMU_6AXES_OK)
Wolfgang Betz 8:d49db2c9d9c1 265 {
Wolfgang Betz 8:d49db2c9d9c1 266 return false;
Wolfgang Betz 8:d49db2c9d9c1 267 }
Wolfgang Betz 8:d49db2c9d9c1 268
Wolfgang Betz 8:d49db2c9d9c1 269 return true;
Wolfgang Betz 8:d49db2c9d9c1 270 }
Wolfgang Betz 44:d757094f6229 271
Wolfgang Betz 44:d757094f6229 272 /**
Wolfgang Betz 44:d757094f6229 273 * @brief Initialize the singleton's LSMDS3 gyroscope
Wolfgang Betz 44:d757094f6229 274 * @retval true if initialization successful,
Wolfgang Betz 44:d757094f6229 275 * @retval false otherwise
Wolfgang Betz 44:d757094f6229 276 */
Wolfgang Betz 44:d757094f6229 277 bool X_NUCLEO_IKS01A1::Init_LSM6DS3(void) {
Wolfgang Betz 44:d757094f6229 278 IMU_6AXES_InitTypeDef InitStructure;
Wolfgang Betz 44:d757094f6229 279 uint8_t xg_id = 0;
Wolfgang Betz 44:d757094f6229 280
Wolfgang Betz 44:d757094f6229 281 /* Check presence */
Wolfgang Betz 72:79a2a64c9b2f 282 if(gyro_lsm6ds3 == NULL) return true;
Wolfgang Betz 72:79a2a64c9b2f 283
Wolfgang Betz 92:d1c67d482bad 284 if((gyro_lsm6ds3->read_id(&xg_id) != IMU_6AXES_OK) ||
Wolfgang Betz 44:d757094f6229 285 (xg_id != I_AM_LSM6DS3_XG))
Wolfgang Betz 44:d757094f6229 286 {
Wolfgang Betz 44:d757094f6229 287 delete gyro_lsm6ds3;
Wolfgang Betz 44:d757094f6229 288 gyro_lsm6ds3 = NULL;
Wolfgang Betz 44:d757094f6229 289 return true;
Wolfgang Betz 44:d757094f6229 290 }
Wolfgang Betz 44:d757094f6229 291
Wolfgang Betz 44:d757094f6229 292 /* Configure sensor */
Wolfgang Betz 44:d757094f6229 293 InitStructure.G_FullScale = 2000.0f; /* 2000DPS */
Wolfgang Betz 44:d757094f6229 294 InitStructure.G_OutputDataRate = 104.0f; /* 104HZ */
Wolfgang Betz 44:d757094f6229 295 InitStructure.G_X_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 296 InitStructure.G_Y_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 297 InitStructure.G_Z_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 298
Wolfgang Betz 44:d757094f6229 299 InitStructure.X_FullScale = 2.0f; /* 2G */
Wolfgang Betz 44:d757094f6229 300 InitStructure.X_OutputDataRate = 104.0f; /* 104HZ */
Wolfgang Betz 44:d757094f6229 301 InitStructure.X_X_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 302 InitStructure.X_Y_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 303 InitStructure.X_Z_Axis = 1; /* Enable */
Wolfgang Betz 44:d757094f6229 304
Wolfgang Betz 92:d1c67d482bad 305 if(gyro_lsm6ds3->init(&InitStructure) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 306 {
Wolfgang Betz 44:d757094f6229 307 return false;
Wolfgang Betz 44:d757094f6229 308 }
Wolfgang Betz 44:d757094f6229 309
Wolfgang Betz 44:d757094f6229 310 return true;
Wolfgang Betz 44:d757094f6229 311 }