diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/gyro.cpp | 22 | ||||
| -rw-r--r-- | src/sensor.cpp | 354 | ||||
| -rw-r--r-- | src/sensor.hpp | 219 | ||||
| -rw-r--r-- | src/sensor_registers.hpp | 134 |
4 files changed, 717 insertions, 12 deletions
diff --git a/src/gyro.cpp b/src/gyro.cpp index 668cef1..1a81850 100644 --- a/src/gyro.cpp +++ b/src/gyro.cpp @@ -1,7 +1,8 @@ #include "Arduino.h" -#include "GY521.h" +#include "Wire.h" +#include "sensor.hpp" -GY521 sensor(0x68); +Sensor sensor(0x68, &Wire); void setup() { @@ -10,26 +11,23 @@ void setup() // Wait for Serial because the Arduino Leonardo is weird while (!Serial) {} - Wire.begin(); - - delay(100); - while (!sensor.wakeup()) + while (!sensor.begin()) { Serial.print(millis()); - Serial.println("Error: Could not connect to the GY521 sensor. Retrying after 2000 milliseconds..."); + Serial.println("Error: Could not connect to the sensor. Retrying after 2000 milliseconds..."); delay(2000); } sensor.setAccelSensitivity(2); // 8g sensor.setGyroSensitivity(1); // 500 degrees/s - sensor.setThrottle(); + sensor.setThrottleEnabled(true); Serial.println("start..."); // Calibration values - sensor.axe = 0.198; - sensor.aye = -0.018; - sensor.gxe = 0.780; - sensor.gye = -1.495; + sensor.accel_cal_x = 0.198; + sensor.accel_cal_y = -0.018; + sensor.gyro_cal_x = 0.780; + sensor.gyro_cal_y = -1.495; } void loop() diff --git a/src/sensor.cpp b/src/sensor.cpp new file mode 100644 index 0000000..e3a2551 --- /dev/null +++ b/src/sensor.cpp @@ -0,0 +1,354 @@ +#include "sensor.hpp" +#include "math.h" +#include "sensor_registers.hpp" + +Sensor::Sensor(uint8_t address, TwoWire *wire) +{ + _address = address; + _wire = wire; + + // Set default values + accel_cal_x = 0; + accel_cal_y = 0; + accel_cal_z = 0; + + gyro_cal_x = 0; + gyro_cal_y = 0; + gyro_cal_z = 0; + + _throttle_enabled = true; + _throttleTime = DEFAULT_THROTTLE_TIME; + + _lastTime = 0; + _lastMicros = 0; + + _status = SENSOR_OK; + + _accel_sensitivity = 0; + _accel_to_g_force = 1.0 / 16384.0; + + _gyro_sensitivity = 0; + _ang_rate_to_dps = 1.0 / 131.0; + + _temperature = 0; + + // Reset the sensor + reset(); +} + +bool Sensor::begin() +{ + _wire->begin(); + + if (isConnected()) + { + _wire->beginTransmission(_address); + _wire->write(SENSOR_PWR_MGMT_1); + _wire->write(SENSOR_WAKEUP); + + return (_wire->endTransmission() == 0); + } + + return false; +} + +bool Sensor::isConnected() +{ + _wire->beginTransmission(_address); + return (_wire->endTransmission() == 0); +} + +void Sensor::reset() +{ + setThrottleTime(DEFAULT_THROTTLE_TIME); + + _accel_raw_x = 0; + _accel_raw_y = 0; + _accel_raw_z = 0; + + _gyro_raw_x = 0; + _gyro_raw_y = 0; + _gyro_raw_z = 0; + + _accel_x = 0; + _accel_y = 0; + _accel_z = 0; + + _pitch = 0; + _roll = 0; + _yaw = 0; +} + +void Sensor::setThrottleEnabled(bool throttle = true) +{ + _throttle_enabled = throttle; +}; + +bool Sensor::getThrottleEnabled() +{ + return _throttle_enabled; +}; + +void Sensor::setThrottleTime(uint16_t time_ms) +{ + _throttleTime = time_ms; +}; + +uint16_t Sensor::getThrottleTime() +{ + return _throttleTime; +}; + +int16_t Sensor::read() +{ + auto time_now = millis(); + + if (_throttle_enabled) + { + if ((time_now - _lastTime) < _throttleTime) + { + return SENSOR_THROTTLED; + } + } + + _lastTime = time_now; + + _wire->beginTransmission(_address); + _wire->write(SENSOR_ACCEL_XOUT_H); + + if (_wire->endTransmission() != 0) + { + _status = SENSOR_ERR_WRITE; + return _status; + } + + int8_t response_length = _wire->requestFrom(_address, (uint8_t)14); + + if (response_length != 14) + { + _status = SENSOR_ERR_READ; + return _status; + } + + // ACCELEROMETER + _accel_raw_x = _readTwoBytes(); // ACCEL_XOUT_H ACCEL_XOUT_L + _accel_raw_y = _readTwoBytes(); // ACCEL_YOUT_H ACCEL_YOUT_L + _accel_raw_z = _readTwoBytes(); // ACCEL_ZOUT_H ACCEL_ZOUT_L + + // TEMPERATURE + _temperature = _readTwoBytes(); // TEMP_OUT_H TEMP_OUT_L + + // GYROSCOPE + _gyro_raw_x = _readTwoBytes(); // GYRO_XOUT_H GYRO_XOUT_L + _gyro_raw_y = _readTwoBytes(); // GYRO_YOUT_H GYRO_YOUT_L + _gyro_raw_z = _readTwoBytes(); // GYRO_ZOUT_H GYRO_ZOUT_L + + // Duration interval + time_now = micros(); + float duration = (time_now - _lastMicros) * 1e-6; // Duration in seconds. + _lastMicros = time_now; + + // Convert raw acceleration to g:s (g-force) + _accel_raw_x *= _accel_to_g_force; + _accel_raw_y *= _accel_to_g_force; + _accel_raw_z *= _accel_to_g_force; + + // Error correct raw acceleration + _accel_raw_x += accel_cal_x; + _accel_raw_y += accel_cal_y; + _accel_raw_z += accel_cal_z; + + // Prepare for Pitch Roll Yaw + float accel_x_pow_two = pow(_accel_raw_x, 2); + float accel_y_pow_two = pow(_accel_raw_y, 2); + float accel_z_pow_two = pow(_accel_raw_z, 2); + + _accel_x = atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES; + _accel_y = atan(-1.0 * _accel_raw_x / sqrt(accel_y_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES; + _accel_z = atan(_accel_raw_z / sqrt(accel_x_pow_two + accel_y_pow_two)) * RAD_TO_DEGREES; + + // Convert temperature to celsius + _temperature = _temperature * 0.00294117647 + 36.53; + + // Convert raw Gyro to degrees/s + _gyro_raw_x *= _ang_rate_to_dps; + _gyro_raw_y *= _ang_rate_to_dps; + _gyro_raw_z *= _ang_rate_to_dps; + + // Error correct raw gyro measurements. + _gyro_raw_x += gyro_cal_x; + _gyro_raw_y += gyro_cal_y; + _gyro_raw_z += gyro_cal_z; + + _gyro_x += _gyro_raw_x * duration; + _gyro_y += _gyro_raw_y * duration; + _gyro_z += _gyro_raw_z * duration; + + _yaw = _gyro_z; + _pitch = 0.96 * _gyro_y + 0.04 * _accel_y; + _roll = 0.96 * _gyro_x + 0.04 * _accel_x; + + return SENSOR_OK; +} + +bool Sensor::setAccelSensitivity(uint8_t sensitivity) +{ + _accel_sensitivity = sensitivity; + + if (_accel_sensitivity > 3) + _accel_sensitivity = 3; + + uint8_t val = getRegister(SENSOR_ACCEL_CONFIG); + + if (_status != SENSOR_OK) + { + return false; + } + + // No need to write same value + if (((val >> 3) & 3) != _accel_sensitivity) + { + val &= 0xE7; + val |= (_accel_sensitivity << 3); + + if (setRegister(SENSOR_ACCEL_CONFIG, val) != SENSOR_OK) + { + return false; + } + } + + // Calculate conversion factor. + _accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR; + + return true; +} + +uint8_t Sensor::getAccelSensitivity() +{ + uint8_t val = getRegister(SENSOR_ACCEL_CONFIG); + + if (_status != SENSOR_OK) + { + return _status; // return and propagate error (best thing to do) + } + + _accel_sensitivity = (val >> 3) & 3; + + return _accel_sensitivity; +} + +bool Sensor::setGyroSensitivity(uint8_t sensitivity) +{ + _gyro_sensitivity = sensitivity; + + if (_gyro_sensitivity > 3) + _gyro_sensitivity = 3; + + uint8_t val = getRegister(SENSOR_GYRO_CONFIG); + + if (_status != 0) + { + return false; + } + + // No need to write same value + if (((val >> 3) & 3) != _gyro_sensitivity) + { + val &= 0xE7; + val |= (_gyro_sensitivity << 3); + + if (setRegister(SENSOR_GYRO_CONFIG, val) != SENSOR_OK) + { + return false; + } + } + + _ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; + + return true; +} + +uint8_t Sensor::getGyroSensitivity() +{ + uint8_t val = getRegister(SENSOR_GYRO_CONFIG); + + if (_status != SENSOR_OK) + { + return _status; + } + + _gyro_sensitivity = (val >> 3) & 3; + + return _gyro_sensitivity; +} + +float Sensor::getAccelX() { return _accel_raw_x; }; +float Sensor::getAccelY() { return _accel_raw_y; }; + +float Sensor::getAngleX() { return _accel_x; }; +float Sensor::getAngleY() { return _accel_y; }; + +float Sensor::getTemperature() { return _temperature; }; + +float Sensor::getGyroX() { return _gyro_raw_x; }; +float Sensor::getGyroY() { return _gyro_raw_y; }; + +float Sensor::getPitch() { return _pitch; }; +float Sensor::getRoll() { return _roll; }; + +uint32_t Sensor::lastTime() { return _lastTime; }; + +uint8_t Sensor::setRegister(uint8_t reg, uint8_t value) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->write(value); + + if (_wire->endTransmission() != 0) + { + _status = SENSOR_ERR_WRITE; + return _status; + } + + return SENSOR_OK; +} + +uint8_t Sensor::getRegister(uint8_t reg) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + + if (_wire->endTransmission() != 0) + { + _status = SENSOR_ERR_WRITE; + return _status; + } + + uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1); + if (response_length != 1) + { + _status = SENSOR_ERR_READ; + return _status; + } + + uint8_t val = _wire->read(); + + return val; +} + +int16_t Sensor::getStatus() +{ + auto error = _status; + _status = SENSOR_OK; + + return error; +}; + +int16_t Sensor::_readTwoBytes() +{ + int16_t response = _wire->read(); + response <<= 8; + response |= _wire->read(); + + return response; +} diff --git a/src/sensor.hpp b/src/sensor.hpp new file mode 100644 index 0000000..7ba01bf --- /dev/null +++ b/src/sensor.hpp @@ -0,0 +1,219 @@ +#ifndef SENSOR_HPP +#define SENSOR_HPP + +#include "Arduino.h" +#include "Wire.h" + +#define DEFAULT_THROTTLE_TIME 10 // milliseconds + +// Status codes +#define SENSOR_OK 0 +#define SENSOR_THROTTLED 1 +#define SENSOR_ERR_READ -1 +#define SENSOR_ERR_WRITE -2 +#define SENSOR_ERR_NOT_CONNECTED -3 + +#define SENSOR_WAKEUP 0x00 + +#define RAD_TO_DEGREES (180.0 / PI) +#define RAW_TO_DPS_FACTOR (1.0 / 131.0) +#define RAW_TO_G_FACTOR (1.0 / 16384.0) + +class Sensor +{ +public: + Sensor(uint8_t address, TwoWire *wire); + + /** + * Initializes communication with the sensor. + * + * @returns Whether or not the sensor can be communicated with. + */ + bool begin(); + + /** + * Returns whether or not the sensor is connected. + */ + bool isConnected(); + + /** + * Resets the sensor. + */ + void reset(); + + /** + * Returns whether or not throttling is enabled. + */ + bool getThrottleEnabled(); + + /** + * Sets whether or not throttling is enabled. + * + * @param throttle Throttling enable/disable + */ + void setThrottleEnabled(bool throttle); + + /** + * Returns the throttle time. + */ + uint16_t getThrottleTime(); + + /** + * Sets the throttle time. + * + * @param time_ms Time in milliseconds to throttle + */ + void setThrottleTime(uint16_t time_ms); + + /** + * Reads from the sensor. + * + * @returns The sensor status. + */ + int16_t read(); + + /** + * Returns the accelerometer sensitivity. + * + * 0,1,2,3 ==> 2g 4g 8g 16g + */ + uint8_t getAccelSensitivity(); + + /** + * Sets the accelerometer sensitivity. + * + * @param sensitivity Accelerometer sensitivity. 0, 1, 2, 3 => 2g 4g 8g 16g + * @returns Whether or not it succeeded. + */ + bool setAccelSensitivity(uint8_t sensitivity); + + /** + * Returns the gyro sensitivity. + * + * 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s + */ + uint8_t getGyroSensitivity(); + + /** + * Sets the gyro sensitivity. + * + * @param sensitivity Gyro sensitivity. + * 0, 1, 2, 3 => 250, 500, 1000, 2000 degrees/s + * @returns Whether or not it succeeded. + */ + bool setGyroSensitivity(uint8_t sensitivity); + + /** + * Returns the current X axis acceleration in g:s (g-force). + */ + float getAccelX(); + + /** + * Returns the current Y axis acceleration in g:s (g-force). + */ + float getAccelY(); + + /** + * Returns the current X angle. + */ + float getAngleX(); + + /** + * Returns the current Y angle. + */ + float getAngleY(); + + /** + * Returns the current temperature. + */ + float getTemperature(); + + /** + * Returns the current X axis in degrees/s. + */ + float getGyroX(); + + /** + * Returns the current Y axis in degrees/s. + */ + float getGyroY(); + + /** + * Returns the current pitch angle. + */ + float getPitch(); + + /** + * Returns the current roll angle. + */ + float getRoll(); + + /** + * Returns the last time the sensor was read. + */ + uint32_t lastTime(); + + /** + * Sets the value of a key in the sensor's register. + * + * @param reg The address of a registry key + * @param value A new value + * @returns The sensor status + */ + uint8_t setRegister(uint8_t reg, uint8_t value); + + /** + * Returns the value of a key in the sensor's registry. + * + * @param reg The address of a registry key + */ + uint8_t getRegister(uint8_t reg); + + /** + * Returns the last sensor status. + */ + int16_t getStatus(); + + // Accelerometer calibration values + float accel_cal_x; + float accel_cal_y; + float accel_cal_z; + + // Gyroscope calibration values + float gyro_cal_x; + float gyro_cal_y; + float gyro_cal_z; + +private: + uint8_t _address; + + bool _throttle_enabled; + uint16_t _throttleTime; + + uint32_t _lastTime; + uint32_t _lastMicros; + + int16_t _status; + + uint8_t _accel_sensitivity; + float _accel_to_g_force; + + float _accel_raw_x, _accel_raw_y, _accel_raw_z; + float _accel_x, _accel_y, _accel_z; + + uint8_t _gyro_sensitivity; + float _ang_rate_to_dps; + + float _gyro_raw_x, _gyro_raw_y, _gyro_raw_z; + float _gyro_x, _gyro_y, _gyro_z; + + float _pitch, _roll, _yaw; + + float _temperature; + + int16_t _readTwoBytes(); + + TwoWire *_wire; +}; + +#endif diff --git a/src/sensor_registers.hpp b/src/sensor_registers.hpp new file mode 100644 index 0000000..34b47c6 --- /dev/null +++ b/src/sensor_registers.hpp @@ -0,0 +1,134 @@ +#ifndef SENSOR_REGISTERS_HPP +#define SENSOR_REGISTERS_HPP + +#define SENSOR_XG_OFFS_TC 0x00 +#define SENSOR_YG_OFFS_TC 0x01 +#define SENSOR_ZG_OFFS_TC 0x02 + +#define SENSOR_X_FINE_GAIN 0x03 +#define SENSOR_Y_FINE_GAIN 0x04 +#define SENSOR_Z_FINE_GAIN 0x05 + +#define SENSOR_XA_OFFS_H 0x06 +#define SENSOR_XA_OFFS_L_TC 0x07 +#define SENSOR_YA_OFFS_H 0x08 +#define SENSOR_YA_OFFS_L_TC 0x09 +#define SENSOR_ZA_OFFS_H 0x0A +#define SENSOR_ZA_OFFS_L_TC 0x0B + +#define SENSOR_SELF_TEST_X 0x0D +#define SENSOR_SELF_TEST_Y 0x0E +#define SENSOR_SELF_TEST_Z 0x0F +#define SENSOR_SELF_TEST_A 0x10 + +#define SENSOR_XG_OFFS_USRH 0x13 +#define SENSOR_XG_OFFS_USRL 0x14 +#define SENSOR_YG_OFFS_USRH 0x15 +#define SENSOR_YG_OFFS_USRL 0x16 +#define SENSOR_ZG_OFFS_USRH 0x17 +#define SENSOR_ZG_OFFS_USRL 0x18 + +#define SENSOR_SMPLRT_DIV 0x19 +#define SENSOR_CONFIG 0x1A +#define SENSOR_GYRO_CONFIG 0x1B +#define SENSOR_ACCEL_CONFIG 0x1C + +#define SENSOR_FF_THR 0x1D +#define SENSOR_FF_DUR 0x1E +#define SENSOR_MOT_THR 0x1F +#define SENSOR_MOT_DUR 0x20 +#define SENSOR_ZRMOT_THR 0x21 +#define SENSOR_ZRMOT_DUR 0x22 +#define SENSOR_FIFO_EN 0x23 + +#define SENSOR_I2C_MST_CTRL 0x24 +#define SENSOR_I2C_SLV0_ADDR 0x25 +#define SENSOR_I2C_SLV0_REG 0x26 +#define SENSOR_I2C_SLV0_CTRL 0x27 +#define SENSOR_I2C_SLV1_ADDR 0x28 +#define SENSOR_I2C_SLV1_REG 0x29 +#define SENSOR_I2C_SLV1_CTRL 0x2A +#define SENSOR_I2C_SLV2_ADDR 0x2B +#define SENSOR_I2C_SLV2_REG 0x2C +#define SENSOR_I2C_SLV2_CTRL 0x2D +#define SENSOR_I2C_SLV3_ADDR 0x2E +#define SENSOR_I2C_SLV3_REG 0x2F +#define SENSOR_I2C_SLV3_CTRL 0x30 +#define SENSOR_I2C_SLV4_ADDR 0x31 +#define SENSOR_I2C_SLV4_REG 0x32 +#define SENSOR_I2C_SLV4_DO 0x33 +#define SENSOR_I2C_SLV4_CTRL 0x34 +#define SENSOR_I2C_SLV4_DI 0x35 +#define SENSOR_I2C_MST_STATUS 0x36 + +#define SENSOR_INT_PIN_CFG 0x37 +#define SENSOR_INT_ENABLE 0x38 +#define SENSOR_DMP_INT_STATUS 0x39 +#define SENSOR_INT_STATUS 0x3A + +#define SENSOR_ACCEL_XOUT_H 0x3B +#define SENSOR_ACCEL_XOUT_L 0x3C +#define SENSOR_ACCEL_YOUT_H 0x3D +#define SENSOR_ACCEL_YOUT_L 0x3E +#define SENSOR_ACCEL_ZOUT_H 0x3F +#define SENSOR_ACCEL_ZOUT_L 0x40 +#define SENSOR_TEMP_OUT_H 0x41 +#define SENSOR_TEMP_OUT_L 0x42 +#define SENSOR_GYRO_XOUT_H 0x43 +#define SENSOR_GYRO_XOUT_L 0x44 +#define SENSOR_GYRO_YOUT_H 0x45 +#define SENSOR_GYRO_YOUT_L 0x46 +#define SENSOR_GYRO_ZOUT_H 0x47 +#define SENSOR_GYRO_ZOUT_L 0x48 + +#define SENSOR_EXT_SENS_DATA_00 0x49 +#define SENSOR_EXT_SENS_DATA_01 0x4A +#define SENSOR_EXT_SENS_DATA_02 0x4B +#define SENSOR_EXT_SENS_DATA_03 0x4C +#define SENSOR_EXT_SENS_DATA_04 0x4D +#define SENSOR_EXT_SENS_DATA_05 0x4E +#define SENSOR_EXT_SENS_DATA_06 0x4F +#define SENSOR_EXT_SENS_DATA_07 0x50 +#define SENSOR_EXT_SENS_DATA_08 0x51 +#define SENSOR_EXT_SENS_DATA_09 0x52 +#define SENSOR_EXT_SENS_DATA_10 0x53 +#define SENSOR_EXT_SENS_DATA_11 0x54 +#define SENSOR_EXT_SENS_DATA_12 0x55 +#define SENSOR_EXT_SENS_DATA_13 0x56 +#define SENSOR_EXT_SENS_DATA_14 0x57 +#define SENSOR_EXT_SENS_DATA_15 0x58 +#define SENSOR_EXT_SENS_DATA_16 0x59 +#define SENSOR_EXT_SENS_DATA_17 0x5A +#define SENSOR_EXT_SENS_DATA_18 0x5B +#define SENSOR_EXT_SENS_DATA_19 0x5C +#define SENSOR_EXT_SENS_DATA_20 0x5D +#define SENSOR_EXT_SENS_DATA_21 0x5E +#define SENSOR_EXT_SENS_DATA_22 0x5F +#define SENSOR_EXT_SENS_DATA_23 0x60 + +#define SENSOR_MOT_DETECT_STATUS 0x61 + +#define SENSOR_I2C_SLV0_DO 0x63 +#define SENSOR_I2C_SLV1_DO 0x64 +#define SENSOR_I2C_SLV2_DO 0x65 +#define SENSOR_I2C_SLV3_DO 0x66 +#define SENSOR_I2C_MST_DELAY_CTRL 0x67 + +#define SENSOR_SIGNAL_PATH_RESET 0x68 +#define SENSOR_MOT_DETECT_CTRL 0x69 +#define SENSOR_USER_CTRL 0x6A + +#define SENSOR_PWR_MGMT_1 0x6B +#define SENSOR_PWR_MGMT_2 0x6C +#define SENSOR_BANK_SEL 0x6D +#define SENSOR_MEM_START_ADDR 0x6E +#define SENSOR_MEM_R_W 0x6F + +#define SENSOR_DMP_CFG_1 0x70 +#define SENSOR_DMP_CFG_2 0x71 +#define SENSOR_FIFO_COUNTH 0x72 +#define SENSOR_FIFO_COUNTL 0x73 +#define SENSOR_FIFO_R_W 0x74 +#define SENSOR_WHO_AM_I 0x75 + +#endif |