Nicolas Borla
/
Example_for_Joel
IMU measurement + Speed controller
IMU.cpp
- Committer:
- boro
- Date:
- 2019-05-30
- Revision:
- 1:17fdd812cb8d
- Parent:
- 0:5a93e4916fb1
File content as of revision 1:17fdd812cb8d:
/* * Main.cpp * Copyright (c) 2018, ZHAW * All rights reserved. */ #include "IMU.h" #include "mbed.h" using namespace std; const float IMU::M_PI = 3.14159265358979323846f; // the mathematical constant PI const float IMU::SAMPLE_TIME = 0.001f; const float IMU::STD_ALPHA = 0.02f; // standard deviation Measurement noise sensor gx - R const float IMU::STD_OMEGA = 0.034f; // standard deviation Measurement noise sensor gx - R /** * Creates an IMU object. * @param spi a reference to an spi controller to use. * @param csAG the chip select output for the accelerometer and the gyro sensor. * @param csM the chip select output for the magnetometer. */ IMU::IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM) : spi(spi), csAG(csAG), csM(csM) { // initialize SPI interface spi.format(8, 3); spi.frequency(1000000); // reset chip select lines to logical high csAG = 1; csM = 1; // initialize accelerometer and gyro writeRegister(csAG, CTRL_REG1_G, 0xC3); // ODR 952 Hz, full scale 245 deg/s writeRegister(csAG, CTRL_REG2_G, 0x00); // disable interrupt generation writeRegister(csAG, CTRL_REG3_G, 0x00); // disable low power mode, disable high pass filter, high pass cutoff frequency 57 Hz writeRegister(csAG, CTRL_REG4, 0x38); // enable gyro in all 3 axis writeRegister(csAG, CTRL_REG5_XL, 0x38); // no decimation, enable accelerometer in all 3 axis writeRegister(csAG, CTRL_REG6_XL, 0xC0); // ODR 952 Hz, full scale 2g writeRegister(csAG, CTRL_REG7_XL, 0x00); // high res mode disabled, filter bypassed writeRegister(csAG, CTRL_REG8, 0x00); // 4-wire SPI interface, LSB at lower address writeRegister(csAG, CTRL_REG9, 0x04); // disable gyro sleep mode, disable I2C interface, disable FIFO writeRegister(csAG, CTRL_REG10, 0x00); // self test disabled // initialize magnetometer writeRegister(csM, CTRL_REG1_M, 0x10); // temperature not compensated, low power mode for x & y axis, data rate 10 Hz writeRegister(csM, CTRL_REG2_M, 0x00); // full scale 4 gauss writeRegister(csM, CTRL_REG3_M, 0x80); // disable I2C interface, low power mode, SPI write only, continuous conversion mode writeRegister(csM, CTRL_REG4_M, 0x00); // low power mode for z axis, LSB at lower address writeRegister(csM, CTRL_REG5_M, 0x00); // fast read disabled } /** * Deletes the IMU object. */ IMU::~IMU() {} /** * This private method allows to write a register value. * @param cs the chip select output to use, either csAG or csM. * @param address the 7 bit address of the register. * @param value the value to write into the register. */ void IMU::writeRegister(DigitalOut& cs, uint8_t address, uint8_t value) { cs = 0; spi.write(0x7F & address); spi.write(value & 0xFF); cs = 1; } /** * This private method allows to read a register value. * @param cs the chip select output to use, either csAG or csM. * @param address the 7 bit address of the register. * @return the value read from the register. */ uint8_t IMU::readRegister(DigitalOut& cs, uint8_t address) { cs = 0; spi.write(0x80 | address); int32_t value = spi.write(0xFF); cs = 1; return static_cast<uint8_t>(value & 0xFF); } /** * Reads the gyroscope about the x-axis. * @return the rotational speed about the x-axis given in [rad/s]. */ float IMU::readGyroX() { uint8_t low = readRegister(csAG, OUT_X_L_G); uint8_t high = readRegister(csAG, OUT_X_H_G); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f; } /** * Reads the gyroscope about the y-axis. * @return the rotational speed about the y-axis given in [rad/s]. */ float IMU::readGyroY() { uint8_t low = readRegister(csAG, OUT_Y_L_G); uint8_t high = readRegister(csAG, OUT_Y_H_G); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f; } /** * Reads the gyroscope about the z-axis. * @return the rotational speed about the z-axis given in [rad/s]. */ float IMU::readGyroZ() { uint8_t low = readRegister(csAG, OUT_Z_L_G); uint8_t high = readRegister(csAG, OUT_Z_H_G); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*245.0f*M_PI/180.0f; } /** * Reads the acceleration in x-direction. * @return the acceleration in x-direction, given in [m/s2]. */ float IMU::readAccelerationX() { uint8_t low = readRegister(csAG, OUT_X_L_XL); uint8_t high = readRegister(csAG, OUT_X_H_XL); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*2.0f*9.81f; } /** * Reads the acceleration in y-direction. * @return the acceleration in y-direction, given in [m/s2]. */ float IMU::readAccelerationY() { uint8_t low = readRegister(csAG, OUT_Y_L_XL); uint8_t high = readRegister(csAG, OUT_Y_H_XL); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*2.0f*9.81f; } /** * Reads the acceleration in z-direction. * @return the acceleration in z-direction, given in [m/s2]. */ float IMU::readAccelerationZ() { uint8_t low = readRegister(csAG, OUT_Z_L_XL); uint8_t high = readRegister(csAG, OUT_Z_H_XL); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*2.0f*9.81f; } /** * Reads the magnetic field in x-direction. * @return the magnetic field in x-direction, given in [Gauss]. */ float IMU::readMagnetometerX() { uint8_t low = readRegister(csM, OUT_X_L_M); uint8_t high = readRegister(csM, OUT_X_H_M); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*4.0f; } /** * Reads the magnetic field in x-direction. * @return the magnetic field in x-direction, given in [Gauss]. */ float IMU::readMagnetometerY() { uint8_t low = readRegister(csM, OUT_Y_L_M); uint8_t high = readRegister(csM, OUT_Y_H_M); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*4.0f; } /** * Reads the magnetic field in x-direction. * @return the magnetic field in x-direction, given in [Gauss]. */ float IMU::readMagnetometerZ() { uint8_t low = readRegister(csM, OUT_Z_L_M); uint8_t high = readRegister(csM, OUT_Z_H_M); int16_t value = static_cast<int16_t>((static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low)); return static_cast<float>(value)/32768.0f*4.0f; }