Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed commands BLE_API nRF51822
Sensors.cpp
- Committer:
- dkester
- Date:
- 2015-06-01
- Revision:
- 3:a3e1a06c486d
- Parent:
- 2:871b5efb2043
- Child:
- 4:2a5a08b14539
File content as of revision 3:a3e1a06c486d:
#include "Sensors.h" I2C i2c(p29, p28); Sensors* Sensors::instance = new Sensors(); const int AS5600 = 0x36 << 1; const int MPU6500 = 0x68 << 1; Sensors::Sensors() { i2c.frequency(300000); this->angle_h = 0; this->angle_l = 0; this->accel_x_h = 0; this->accel_y_h = 0; this->accel_z_h = 0; this->accel_x_l = 0; this->accel_y_l = 0; this->accel_z_l = 0; this->gyro_x_h = 0; this->gyro_y_h = 0; this->gyro_z_h = 0; this->gyro_x_l = 0; this->gyro_y_l = 0; this->gyro_z_l = 0; setupIMU(); setupAngle(); } Sensors* Sensors::getInstance() { return instance; } void Sensors::updateAngle() { /* Read angle from AS5600 */ cmd[0] = 0x0E; i2c.write(AS5600,cmd,1); i2c.read(AS5600,out,2); this->angle_h = out[1]; //((out[0] << 8) + out[1]) * 0.087912087 ; this->angle_l = out[0]; } int8_t Sensors::getAngleH()[ return this->angle_h; } int8_t Sensors::getAngleL()[ return this->angle_l; } void Sensors::setupAngle() { //TODO } void Sensors::wakeIMU(){ writeRegister(0x6B, (readRegister(0x6B) & 0xbf)); } void Sensors::disableIMU(){ writeRegister(0x6B, (readRegister(0x6B) | 1 << 6)); } void Sensors::setupIMU() { //Sets sample rate to 8000/1+79 = 100Hz writeRegister(0x19,0x4f); //Disable FSync, 256 DLPF writeRegister(0x1A,0x00); // Disable gyroscope self tests, scale of 200 degrees/s // Disable accelerometer self tests, scale of +-2g, no DHPF cmd[0] = 0x1B; cmd[1] = 0x00; //bit4 and bit 3 to choose scale range cmd[2] = 0x00; i2c.write(MPU6500,cmd,3); //Freefall threshold of 0mg and duration limit of 0 cmd[0] = 0x1D; i2c.write(MPU6500,cmd,3); //Motion threshold of 0mg and duration limit of 0 cmd[0] = 0x21; i2c.write(MPU6500,cmd,3); //Disable sensor output FIFO buffer writeRegister(0x23,0x00); //Set aux I2C, from 0x24 to 0x36 = 0x00 //Setup INT pin and AUX I2C pass through cmd[0] = 0x37; cmd[1] = 0x02; cmd[2] = 0x01; i2c.write(MPU6500,cmd,3); //Reset sensor signal paths writeRegister(0x68,0x00); //Motion detection control writeRegister(0x69,0x00); //Disables FIFO, AUX I2C and I2C reset bits to 0 writeRegister(0x6A,0x00); //Sets clock source to gyro reference w/ PLL /* SLEEP = 0 */ writeRegister(0x6B,0x02); //Controls frequency of wakeups in accel low power mode plus the sensor standby modes writeRegister(0x6C,0x00); //Data transfer to and from the FIFO buffer writeRegister(0x74,0x00); } void Sensors::updateIMU() { // Read measurements from MPU6500 cmd[0] = 0x3B; i2c.write(MPU6500,cmd,1,true); i2c.read(MPU6500,out,14); /* this->accel_x = (out[0] << 8) | out[1]; this->accel_y = (out[2] << 8) | out[3]; this->accel_z = (out[4] << 8) | out[5]; this->temp = (out[6] << 8) | out[7]; this->gyro_x = (out[8] << 8) | out[9]; this->gyro_y = (out[10] << 8) | out[11]; this->gyro_z = (out[12] << 8) | out[13]; */ this->accel_x_h = out[0]; this->accel_x_l = out[1]; this->accel_y_h = out[2]; this->accel_y_l = out[3]; this->accel_z_h = out[4]; this->accel_z_l = out[5]; this->gyro_x_h = out[8]; this->gyro_x_l = out[9]; this->gyro_y_h = out[10]; this->gyro_y_l = out[11]; this->gyro_z_h = out[12]; this->gyro_z_l = out[13]; } int8_t Sensors::getAccelXH(){ return this->accel_x_h; } int8_t Sensors::getAccelXL(){ return this->accel_x_l; } int8_t Sensors::getAccelYH(){ return this->accel_y_h; } int8_t Sensors::getAccelYL(){ return this->accel_y_l; } int8_t Sensors::getAccelZH(){ return this->accel_z_h; } int8_t Sensors::getAccelZL(){ return this->accel_z_l; } int8_t Sensors::getGyroXH(){ return this->gyro_x_h; } int8_t Sensors::getGyroXL(){ return this->gyro_x_l; } int8_t Sensors::getGyroYH(){ return this->gyro_y_h; } int8_t Sensors::getGyroYL(){ return this->gyro_y_l; } int8_t Sensors::getGyroZH(){ return this->gyro_z_h; } int8_t Sensors::getGyroZL(){ return this->gyro_z_l; } int8_t Sensors::readRegister(int8_t addr) { cmd[0] = addr; i2c.write(MPU6500,cmd,1); i2c.read(MPU6500,out,1); return out[0]; } void Sensors::writeRegister(int8_t addr, int8_t value) { cmd[0] = addr; cmd[1] = value; i2c.write(MPU6500,cmd,2); }