Nim leo niiiim
Diff: IMU.cpp
- Revision:
- 0:da791f233257
diff -r 000000000000 -r da791f233257 IMU.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.cpp Fri May 11 12:21:19 2018 +0000 @@ -0,0 +1,217 @@ +/* + * IMU.cpp + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#include <cmath> +#include "IMU.h" + +using namespace std; + +const float IMU::PI = 3.14159265f; // the constant PI + +/** + * Creates an IMU object. + * @param spi a reference to an spi controller to use. + * @param csG the chip select output for the gyro sensor. + * @param csXM the chip select output for the accelerometer and the magnetometer. + */ +IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) { + + // initialize SPI interface + + spi.format(8, 3); + spi.frequency(1000000); + + // reset chip select lines to logical high + + csG = 1; + csXM = 1; + + // initialize gyro + + writeRegister(csG, CTRL_REG1_G, 0x0F); // enable gyro in all 3 axis + + // initialize accelerometer + + writeRegister(csXM, CTRL_REG0_XM, 0x00); + writeRegister(csXM, CTRL_REG1_XM, 0x5F); + writeRegister(csXM, CTRL_REG2_XM, 0x00); + writeRegister(csXM, CTRL_REG3_XM, 0x04); + + // initialize magnetometer + + writeRegister(csXM, CTRL_REG5_XM, 0x94); + writeRegister(csXM, CTRL_REG6_XM, 0x00); + writeRegister(csXM, CTRL_REG7_XM, 0x00); + writeRegister(csXM, CTRL_REG4_XM, 0x04); + writeRegister(csXM, INT_CTRL_REG_M, 0x09); +} + +/** + * Deletes the IMU object. + */ +IMU::~IMU() {} + +/** + * This private method allows to write a register value. + * @param cs the chip select output to use, either csG or csXM. + * @param address the 7 bit address of the register. + * @param value the value to write into the register. + */ +void IMU::writeRegister(DigitalOut& cs, char address, char 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 csG or csXM. + * @param address the 7 bit address of the register. + * @return the value read from the register. + */ +char IMU::readRegister(DigitalOut& cs, char address) { + + cs = 0; + + spi.write(0x80 | address); + int value = spi.write(0xFF); + + cs = 1; + + return (char)(value & 0xFF); +} + +/** + * Reads the gyroscope about the x-axis. + * @return the rotational speed about the x-axis given in [rad/s]. + */ +float IMU::readGyroX() { + + char low = readRegister(csG, OUT_X_L_G); + char high = readRegister(csG, OUT_X_H_G); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (float)value/32768.0f*245.0f*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() { + + char low = readRegister(csG, OUT_Y_L_G); + char high = readRegister(csG, OUT_Y_H_G); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (float)value/32768.0f*245.0f*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() { + + char low = readRegister(csG, OUT_Z_L_G); + char high = readRegister(csG, OUT_Z_H_G); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (float)value/32768.0f*245.0f*PI/180.0f; +} + +/** + * Reads the acceleration in x-direction. + * @return the acceleration in x-direction, given in [m/s2]. + */ +float IMU::readAccelerationX() { + + char low = readRegister(csXM, OUT_X_L_A); + char high = readRegister(csXM, OUT_X_H_A); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (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() { + + char low = readRegister(csXM, OUT_Y_L_A); + char high = readRegister(csXM, OUT_Y_H_A); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (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() { + + char low = readRegister(csXM, OUT_Z_L_A); + char high = readRegister(csXM, OUT_Z_H_A); + + short value = (short)(((unsigned short)high << 8) | (unsigned short)low); + + return (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() { + + // bitte implementieren! + + return 0.0f; +} + +/** + * Reads the magnetic field in x-direction. + * @return the magnetic field in x-direction, given in [Gauss]. + */ +float IMU::readMagnetometerY() { + + // bitte implementieren! + + return 0.0f; +} + +/** + * Reads the magnetic field in x-direction. + * @return the magnetic field in x-direction, given in [Gauss]. + */ +float IMU::readMagnetometerZ() { + + // bitte implementieren! + + return 0.0f; +} + +/** + * Reads the compass heading about the z-axis. + * @return the compass heading in the range -PI to +PI, given in [rad]. + */ +float IMU::readHeading() { + + // bitte implementieren! + + return 0.0f; +}