1 /* 2 * Copyright (c) 2006-2018, RT-Thread Development Team 3 * 4 * SPDX-License-Identifier: Apache-2.0 5 * 6 * Change Logs: 7 * Date Author Notes 8 * 2014-12-20 Bernard the first version 9 * 2015-1-11 RT_learning modify the mpu6050 initialize 10 */ 11 12 #include <string.h> 13 #include <stdio.h> 14 #include <rtdevice.h> 15 16 #include "mpu6050_sensor.h" 17 18 19 const static sensor_t _MPU6050_sensor[] = 20 { 21 { 22 .name = "Accelerometer", 23 .vendor = "Invensense", 24 .version = sizeof(sensor_t), 25 .handle = 0, 26 .type = SENSOR_TYPE_ACCELEROMETER, 27 .maxRange = SENSOR_ACCEL_RANGE_16G, 28 .resolution = 1.0f, 29 .power = 0.5f, 30 .minDelay = 10000, 31 .fifoReservedEventCount = 0, 32 .fifoMaxEventCount = 64, 33 }, 34 { 35 .name = "Gyroscope", 36 .vendor = "Invensense", 37 .version = sizeof(sensor_t), 38 .handle = 0, 39 .type = SENSOR_TYPE_GYROSCOPE, 40 .maxRange = SENSOR_GYRO_RANGE_2000DPS, 41 .resolution = 1.0f, 42 .power = 0.5f, 43 .minDelay = 10000, 44 .fifoReservedEventCount = 0, 45 .fifoMaxEventCount = 64, 46 } 47 }; 48 49 MPU6050::MPU6050(int sensor_type, const char* iic_bus, int addr) 50 : SensorBase(sensor_type) 51 { 52 this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus); 53 if (this->i2c_bus == NULL) 54 { 55 printf("MPU6050: No IIC device:%s\n", iic_bus); 56 return; 57 } 58 59 this->i2c_addr = addr; 60 61 /* register to sensor manager */ 62 SensorManager::registerSensor(this); 63 } 64 65 int MPU6050::read_reg(rt_uint8_t reg, rt_uint8_t *value) 66 { 67 struct rt_i2c_msg msgs[2]; 68 69 msgs[0].addr = this->i2c_addr; 70 msgs[0].flags = RT_I2C_WR; 71 msgs[0].buf = ® 72 msgs[0].len = 1; 73 74 msgs[1].addr = this->i2c_addr; 75 msgs[1].flags = RT_I2C_RD; /* Read from slave */ 76 msgs[1].buf = (rt_uint8_t *)value; 77 msgs[1].len = 1; 78 79 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 80 return RT_EOK; 81 82 return -RT_ERROR; 83 } 84 85 int MPU6050::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size) 86 { 87 struct rt_i2c_msg msgs[2]; 88 89 msgs[0].addr = this->i2c_addr; 90 msgs[0].flags = RT_I2C_WR; 91 msgs[0].buf = ® 92 msgs[0].len = 1; 93 94 msgs[1].addr = this->i2c_addr; 95 msgs[1].flags = RT_I2C_RD; /* Read from slave */ 96 msgs[1].buf = (rt_uint8_t *)value; 97 msgs[1].len = size; 98 99 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 100 return RT_EOK; 101 102 return -RT_ERROR; 103 } 104 105 int MPU6050::write_reg(rt_uint8_t reg, rt_uint8_t value) 106 { 107 struct rt_i2c_msg msgs[2]; 108 109 msgs[0].addr = this->i2c_addr; 110 msgs[0].flags = RT_I2C_WR; 111 msgs[0].buf = ® 112 msgs[0].len = 1; 113 114 msgs[1].addr = this->i2c_addr; 115 msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; 116 msgs[1].buf = (rt_uint8_t *)&value; 117 msgs[1].len = 1; 118 119 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 120 return RT_EOK; 121 122 return -RT_ERROR; 123 } 124 125 126 MPU6050_Accelerometer::MPU6050_Accelerometer(const char* iic_name, int addr) 127 : MPU6050(SENSOR_TYPE_ACCELEROMETER, iic_name, addr) 128 { 129 int index; 130 uint8_t id; 131 rt_uint8_t value[6] = {0}; 132 rt_int32_t x, y, z; 133 SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G}; 134 135 /* initialize MPU6050 */ 136 write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */ 137 write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */ 138 write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */ 139 write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */ 140 write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */ 141 write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */ 142 143 144 x_offset = y_offset = z_offset = 0; 145 x = y = z = 0; 146 147 /* read MPU6050 id */ 148 read_buffer(MPU6050_WHOAMI, &id, 1); 149 if (id != MPU6050_ID) 150 { 151 printf("Warning: not found MPU6050 id: %02x\n", id); 152 } 153 154 /* get offset */ 155 for (index = 0; index < 200; index ++) 156 { 157 read_buffer(MPU6050_ACCEL_XOUT_H, value, 6); 158 159 x += (((rt_int16_t)value[0] << 8) | value[1]); 160 y += (((rt_int16_t)value[2] << 8) | value[3]); 161 z += (((rt_int16_t)value[4] << 8) | value[5]); 162 } 163 x_offset = x / 200; 164 y_offset = y / 200; 165 z_offset = z / 200; 166 167 this->enable = RT_FALSE; 168 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; 169 this->config = config; 170 } 171 172 int 173 MPU6050_Accelerometer::configure(SensorConfig *config) 174 { 175 int range; 176 uint8_t value; 177 178 if (config == RT_NULL) return -1; 179 180 /* TODO: set datarate */ 181 182 /* get range and calc the sensitivity */ 183 range = config->range.accel_range; 184 switch (range) 185 { 186 case SENSOR_ACCEL_RANGE_2G: 187 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; 188 range = 0; 189 break; 190 case SENSOR_ACCEL_RANGE_4G: 191 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G; 192 range = 0x01 << 2; 193 break; 194 case SENSOR_ACCEL_RANGE_8G: 195 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G; 196 range = 0x02 << 2; 197 break; 198 case SENSOR_ACCEL_RANGE_16G: 199 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G; 200 range = 0x03 << 2; 201 break; 202 default: 203 return -1; 204 } 205 206 /* set range to sensor */ 207 read_reg(MPU6050_ACCEL_CONFIG, &value); 208 value &= ~(0x3 << 2); 209 value |= range; 210 write_reg(MPU6050_ACCEL_CONFIG, value); 211 212 return 0; 213 } 214 215 int 216 MPU6050_Accelerometer::activate(int enable) 217 { 218 uint8_t value; 219 220 if (enable && this->enable == RT_FALSE) 221 { 222 /* enable accelerometer */ 223 read_reg(MPU6050_PWR_MGMT_2, &value); 224 value &= ~(0x07 << 2); 225 write_reg(MPU6050_PWR_MGMT_2, value); 226 } 227 228 if (!enable && this->enable == RT_TRUE) 229 { 230 /* disable accelerometer */ 231 read_reg(MPU6050_PWR_MGMT_2, &value); 232 value |= (0x07 << 2); 233 write_reg(MPU6050_PWR_MGMT_2, value); 234 } 235 236 if (enable) this->enable = RT_TRUE; 237 else this->enable = RT_FALSE; 238 239 return 0; 240 } 241 242 int 243 MPU6050_Accelerometer::poll(sensors_event_t *event) 244 { 245 rt_uint8_t value[6]; 246 rt_int16_t x, y, z; 247 248 /* parameters check */ 249 if (event == NULL) return -1; 250 251 /* get event data */ 252 event->version = sizeof(sensors_event_t); 253 event->sensor = (int32_t) this; 254 event->timestamp = rt_tick_get(); 255 event->type = SENSOR_TYPE_ACCELEROMETER; 256 257 read_buffer(MPU6050_ACCEL_XOUT_H, value, 6); 258 259 /* get raw data */ 260 x = (((rt_int16_t)value[0] << 8) | value[1]); 261 y = (((rt_int16_t)value[2] << 8) | value[3]); 262 z = (((rt_int16_t)value[4] << 8) | value[5]); 263 264 if (config.mode == SENSOR_MODE_RAW) 265 { 266 event->raw_acceleration.x = x; 267 event->raw_acceleration.y = y; 268 event->raw_acceleration.z = z; 269 } 270 else 271 { 272 273 x -= x_offset; y -= y_offset; z -= z_offset; 274 event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD; 275 event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD; 276 event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD; 277 } 278 279 return 0; 280 } 281 282 void 283 MPU6050_Accelerometer::getSensor(sensor_t *sensor) 284 { 285 /* get sensor description */ 286 if (sensor) 287 { 288 memcpy(sensor, &_MPU6050_sensor[0], sizeof(sensor_t)); 289 } 290 } 291 292 MPU6050_Gyroscope::MPU6050_Gyroscope(const char* iic_name, int addr) 293 : MPU6050(SENSOR_TYPE_GYROSCOPE, iic_name, addr) 294 { 295 int index; 296 uint8_t id; 297 rt_uint8_t value[6]; 298 rt_int32_t x, y, z; 299 300 /* initialize MPU6050 */ 301 write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */ 302 write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */ 303 write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */ 304 write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */ 305 write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */ 306 write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */ 307 308 x_offset = y_offset = z_offset = 0; 309 x = y = z = 0; 310 311 /* read MPU6050 id */ 312 read_reg(MPU6050_WHOAMI, &id); 313 if (id != MPU6050_ID) 314 { 315 printf("Warning: not found MPU6050 id: %02x\n", id); 316 } 317 318 /* get offset */ 319 for (index = 0; index < 200; index ++) 320 { 321 read_buffer(MPU6050_GYRO_XOUT_H, value, 6); 322 323 x += (((rt_int16_t)value[0] << 8) | value[1]); 324 y += (((rt_int16_t)value[2] << 8) | value[3]); 325 z += (((rt_int16_t)value[4] << 8) | value[5]); 326 } 327 x_offset = x / 200; 328 y_offset = y / 200; 329 z_offset = z / 200; 330 331 this->enable = RT_FALSE; 332 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; 333 } 334 335 int 336 MPU6050_Gyroscope::configure(SensorConfig *config) 337 { 338 int range; 339 uint8_t value; 340 341 if (config == RT_NULL) return -1; 342 343 /* TODO: set datarate */ 344 345 /* get range and calc the sensitivity */ 346 range = config->range.gyro_range; 347 switch (range) 348 { 349 case SENSOR_GYRO_RANGE_250DPS: 350 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; 351 range = 0; 352 break; 353 case SENSOR_GYRO_RANGE_500DPS: 354 this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS; 355 range = 0x01 << 2; 356 break; 357 case SENSOR_GYRO_RANGE_1000DPS: 358 this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS; 359 range = 0x02 << 2; 360 break; 361 case SENSOR_GYRO_RANGE_2000DPS: 362 this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS; 363 range = 0x03 << 2; 364 break; 365 default: 366 return -1; 367 } 368 369 /* set range to sensor */ 370 read_reg(MPU6050_GYRO_CONFIG, &value); 371 value &= ~(0x3 << 2); 372 value |= range; 373 write_reg(MPU6050_GYRO_CONFIG, value); 374 375 return 0; 376 } 377 378 int 379 MPU6050_Gyroscope::activate(int enable) 380 { 381 uint8_t value; 382 383 if (enable && this->enable == RT_FALSE) 384 { 385 /* enable gyroscope */ 386 read_reg(MPU6050_PWR_MGMT_1, &value); 387 value &= ~(0x01 << 4); 388 write_reg(MPU6050_PWR_MGMT_1, value); 389 390 read_reg(MPU6050_PWR_MGMT_2, &value); 391 value &= ~(0x07 << 0); 392 write_reg(MPU6050_PWR_MGMT_2, value); 393 } 394 395 if (!enable && this->enable == RT_TRUE) 396 { 397 /* disable gyroscope */ 398 read_reg(MPU6050_PWR_MGMT_2, &value); 399 value |= (0x07 << 0); 400 write_reg(MPU6050_PWR_MGMT_2, value); 401 } 402 403 if (enable) this->enable = RT_TRUE; 404 else this->enable = RT_FALSE; 405 406 return 0; 407 } 408 409 int 410 MPU6050_Gyroscope::poll(sensors_event_t *event) 411 { 412 rt_uint8_t value[6]; 413 rt_int16_t x, y, z; 414 415 /* parameters check */ 416 if (event == NULL) return -1; 417 418 /* get event data */ 419 event->version = sizeof(sensors_event_t); 420 event->sensor = (int32_t) this; 421 event->timestamp = rt_tick_get(); 422 event->type = SENSOR_TYPE_GYROSCOPE; 423 424 read_buffer(MPU6050_GYRO_XOUT_H, value, 6); 425 426 /* get raw data */ 427 x = (((rt_int16_t)value[0] << 8) | value[1]); 428 y = (((rt_int16_t)value[2] << 8) | value[3]); 429 z = (((rt_int16_t)value[4] << 8) | value[5]); 430 431 432 if (config.mode == SENSOR_MODE_RAW) 433 { 434 event->raw_gyro.x = x; 435 event->raw_gyro.y = y; 436 event->raw_gyro.z = z; 437 } 438 else 439 { 440 x -= x_offset; y -= y_offset; z -= z_offset; 441 event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS; 442 event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS; 443 event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS; 444 } 445 446 return 0; 447 } 448 449 void 450 MPU6050_Gyroscope::getSensor(sensor_t *sensor) 451 { 452 /* get sensor description */ 453 if (sensor) 454 { 455 memcpy(sensor, &_MPU6050_sensor[1], sizeof(sensor_t)); 456 } 457 } 458 459