From 7892ef9d248c189be68ce7faf63230ec0a318b67 Mon Sep 17 00:00:00 2001 From: HampusM Date: Mon, 14 Feb 2022 10:11:32 +0100 Subject: refactor: reorganize & add debugging --- src/sensor/registers.hpp | 137 ++++++++++++++++++ src/sensor/sensor.cpp | 360 +++++++++++++++++++++++++++++++++++++++++++++++ src/sensor/sensor.hpp | 193 +++++++++++++++++++++++++ 3 files changed, 690 insertions(+) create mode 100644 src/sensor/registers.hpp create mode 100644 src/sensor/sensor.cpp create mode 100644 src/sensor/sensor.hpp (limited to 'src/sensor') diff --git a/src/sensor/registers.hpp b/src/sensor/registers.hpp new file mode 100644 index 0000000..c1b19b4 --- /dev/null +++ b/src/sensor/registers.hpp @@ -0,0 +1,137 @@ +#ifndef SENSOR_REGISTERS_HPP +#define SENSOR_REGISTERS_HPP + +namespace SensorRegisters +{ + const uint8_t XG_OFFS_TC = 0x00; + const uint8_t YG_OFFS_TC = 0x01; + const uint8_t ZG_OFFS_TC = 0x02; + + const uint8_t X_FINE_GAIN = 0x03; + const uint8_t Y_FINE_GAIN = 0x04; + const uint8_t Z_FINE_GAIN = 0x05; + + const uint8_t XA_OFFS_H = 0x06; + const uint8_t XA_OFFS_L_TC = 0x07; + const uint8_t YA_OFFS_H = 0x08; + const uint8_t YA_OFFS_L_TC = 0x09; + const uint8_t ZA_OFFS_H = 0x0A; + const uint8_t ZA_OFFS_L_TC = 0x0B; + + const uint8_t SELF_TEST_X = 0x0D; + const uint8_t SELF_TEST_Y = 0x0E; + const uint8_t SELF_TEST_Z = 0x0F; + const uint8_t SELF_TEST_A = 0x10; + + const uint8_t XG_OFFS_USRH = 0x13; + const uint8_t XG_OFFS_USRL = 0x14; + const uint8_t YG_OFFS_USRH = 0x15; + const uint8_t YG_OFFS_USRL = 0x16; + const uint8_t ZG_OFFS_USRH = 0x17; + const uint8_t ZG_OFFS_USRL = 0x18; + + const uint8_t SMPLRT_DIV = 0x19; + const uint8_t CONFIG = 0x1A; + const uint8_t GYRO_CONFIG = 0x1B; + const uint8_t ACCEL_CONFIG = 0x1C; + + const uint8_t FF_THR = 0x1D; + const uint8_t FF_DUR = 0x1E; + const uint8_t MOT_THR = 0x1F; + const uint8_t MOT_DUR = 0x20; + const uint8_t ZRMOT_THR = 0x21; + const uint8_t ZRMOT_DUR = 0x22; + const uint8_t FIFO_EN = 0x23; + + const uint8_t I2C_MST_CTRL = 0x24; + const uint8_t I2C_SLV0_ADDR = 0x25; + const uint8_t I2C_SLV0_REG = 0x26; + const uint8_t I2C_SLV0_CTRL = 0x27; + const uint8_t I2C_SLV1_ADDR = 0x28; + const uint8_t I2C_SLV1_REG = 0x29; + const uint8_t I2C_SLV1_CTRL = 0x2A; + const uint8_t I2C_SLV2_ADDR = 0x2B; + const uint8_t I2C_SLV2_REG = 0x2C; + const uint8_t I2C_SLV2_CTRL = 0x2D; + const uint8_t I2C_SLV3_ADDR = 0x2E; + const uint8_t I2C_SLV3_REG = 0x2F; + const uint8_t I2C_SLV3_CTRL = 0x30; + const uint8_t I2C_SLV4_ADDR = 0x31; + const uint8_t I2C_SLV4_REG = 0x32; + const uint8_t I2C_SLV4_DO = 0x33; + const uint8_t I2C_SLV4_CTRL = 0x34; + const uint8_t I2C_SLV4_DI = 0x35; + const uint8_t I2C_MST_STATUS = 0x36; + + const uint8_t INT_PIN_CFG = 0x37; + const uint8_t INT_ENABLE = 0x38; + const uint8_t DMP_INT_STATUS = 0x39; + const uint8_t INT_STATUS = 0x3A; + + const uint8_t ACCEL_XOUT_H = 0x3B; + const uint8_t ACCEL_XOUT_L = 0x3C; + const uint8_t ACCEL_YOUT_H = 0x3D; + const uint8_t ACCEL_YOUT_L = 0x3E; + const uint8_t ACCEL_ZOUT_H = 0x3F; + const uint8_t ACCEL_ZOUT_L = 0x40; + const uint8_t TEMP_OUT_H = 0x41; + const uint8_t TEMP_OUT_L = 0x42; + const uint8_t GYRO_XOUT_H = 0x43; + const uint8_t GYRO_XOUT_L = 0x44; + const uint8_t GYRO_YOUT_H = 0x45; + const uint8_t GYRO_YOUT_L = 0x46; + const uint8_t GYRO_ZOUT_H = 0x47; + const uint8_t GYRO_ZOUT_L = 0x48; + + const uint8_t EXT_SENS_DATA_00 = 0x49; + const uint8_t EXT_SENS_DATA_01 = 0x4A; + const uint8_t EXT_SENS_DATA_02 = 0x4B; + const uint8_t EXT_SENS_DATA_03 = 0x4C; + const uint8_t EXT_SENS_DATA_04 = 0x4D; + const uint8_t EXT_SENS_DATA_05 = 0x4E; + const uint8_t EXT_SENS_DATA_06 = 0x4F; + const uint8_t EXT_SENS_DATA_07 = 0x50; + const uint8_t EXT_SENS_DATA_08 = 0x51; + const uint8_t EXT_SENS_DATA_09 = 0x52; + const uint8_t EXT_SENS_DATA_10 = 0x53; + const uint8_t EXT_SENS_DATA_11 = 0x54; + const uint8_t EXT_SENS_DATA_12 = 0x55; + const uint8_t EXT_SENS_DATA_13 = 0x56; + const uint8_t EXT_SENS_DATA_14 = 0x57; + const uint8_t EXT_SENS_DATA_15 = 0x58; + const uint8_t EXT_SENS_DATA_16 = 0x59; + const uint8_t EXT_SENS_DATA_17 = 0x5A; + const uint8_t EXT_SENS_DATA_18 = 0x5B; + const uint8_t EXT_SENS_DATA_19 = 0x5C; + const uint8_t EXT_SENS_DATA_20 = 0x5D; + const uint8_t EXT_SENS_DATA_21 = 0x5E; + const uint8_t EXT_SENS_DATA_22 = 0x5F; + const uint8_t EXT_SENS_DATA_23 = 0x60; + + const uint8_t MOT_DETECT_STATUS = 0x61; + + const uint8_t I2C_SLV0_DO = 0x63; + const uint8_t I2C_SLV1_DO = 0x64; + const uint8_t I2C_SLV2_DO = 0x65; + const uint8_t I2C_SLV3_DO = 0x66; + const uint8_t I2C_MST_DELAY_CTRL = 0x67; + + const uint8_t SIGNAL_PATH_RESET = 0x68; + const uint8_t MOT_DETECT_CTRL = 0x69; + const uint8_t USER_CTRL = 0x6A; + + const uint8_t PWR_MGMT_1 = 0x6B; + const uint8_t PWR_MGMT_2 = 0x6C; + const uint8_t BANK_SEL = 0x6D; + const uint8_t MEM_START_ADDR = 0x6E; + const uint8_t MEM_R_W = 0x6F; + + const uint8_t DMP_CFG_1 = 0x70; + const uint8_t DMP_CFG_2 = 0x71; + const uint8_t FIFO_COUNTH = 0x72; + const uint8_t FIFO_COUNTL = 0x73; + const uint8_t FIFO_R_W = 0x74; + const uint8_t WHO_AM_I = 0x75; +} + +#endif diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp new file mode 100644 index 0000000..e11a74b --- /dev/null +++ b/src/sensor/sensor.cpp @@ -0,0 +1,360 @@ +#include "sensor.hpp" +#include "../utils/serial.hpp" +#include "../utils/time.hpp" +#include "registers.hpp" + +Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout, + unsigned int throttle_time) + : _last_time(0), _sout(sout) +{ + _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; + _throttle_time = throttle_time; + + _status = SensorStatus::OK; + + _accel_sensitivity = 0; + _accel_to_g_force = 1.0 / 16384.0; + + _gyro_sensitivity = 0; + _ang_rate_to_dps = 1.0 / 131.0; + + // Reset the sensor + reset(); +} + +bool Sensor::begin() +{ + _wire->begin(); + + if (isConnected()) + { + _wire->beginTransmission(_address); + _wire->write(SensorRegisters::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() +{ + _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; + + _pitch = 0; + _roll = 0; + _yaw = 0; +} + +bool Sensor::read() +{ + Time now = time_now(); + + if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time) + { + _status = SensorStatus::THROTTLED; + return false; + } + + _wire->beginTransmission(_address); + _wire->write(SensorRegisters::ACCEL_XOUT_H); + + if (_wire->endTransmission() != 0) + { + _status = SensorStatus::ERR_WRITE; + return false; + } + + int8_t response_length = _wire->requestFrom(_address, (uint8_t)14); + + if (response_length != 14) + { + _status = SensorStatus::ERR_READ; + return false; + } + + // Accelerometer + _accel_raw_x = _readHighLow(); + _accel_raw_y = _readHighLow(); + _accel_raw_z = _readHighLow(); + + _sout << "\nAccel raw x: " << _accel_raw_x << "\n" + << "Accel raw y: " << _accel_raw_y << "\n" + << "Accel raw z: " << _accel_raw_z << "\n" + << endl; + + // Gyroscope + _gyro_raw_x = _readHighLow(); + _gyro_raw_y = _readHighLow(); + _gyro_raw_z = _readHighLow(); + + _sout << "\nGyro raw x: " << _gyro_raw_x << "\n" + << "Gyro raw y: " << _gyro_raw_y << "\n" + << "Gyro raw z: " << _gyro_raw_z << "\n" + << endl; + + // Duration interval + now.update(); + float duration = now.diff(_last_time).secs(); + _last_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; + + _sout << "\nAccel raw x g:s: " << _accel_raw_x << "\n" + << "Accel raw y g:s: " << _accel_raw_y << "\n" + << "Accel raw z g:s: " << _accel_raw_z << "\n" + << endl; + + // Error correct raw acceleration + _accel_raw_x += accel_cal_x; + _accel_raw_y += accel_cal_y; + _accel_raw_z += accel_cal_z; + + _sout << "\nAccel raw x correct: " << _accel_raw_x << "\n" + << "Accel raw y correct: " << _accel_raw_y << "\n" + << "Accel raw z correct: " << _accel_raw_z << "\n" + << endl; + + // Prepare for Pitch Roll Yaw + float accel_y_pow_two = pow(_accel_raw_y, 2); + float accel_z_pow_two = pow(_accel_raw_z, 2); + + _accel_x = atan2(_accel_raw_y, _accel_raw_z) * 180 / PI; + + _accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * 180 / PI; + + /* + _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; + */ + + // 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; + + _sout << "\nGyro raw x dps: " << _gyro_raw_x << "\n" + << "Gyro raw y dps: " << _gyro_raw_y << "\n" + << "Gyro raw z dps: " << _gyro_raw_z << "\n" + << endl; + + // Error correct raw gyro measurements. + _gyro_raw_x += gyro_cal_x; + _gyro_raw_y += gyro_cal_y; + _gyro_raw_z += gyro_cal_z; + + _sout << "\nGyro raw x correct: " << _gyro_raw_x << "\n" + << "Gyro raw y correct: " << _gyro_raw_y << "\n" + << "Gyro raw z correct: " << _gyro_raw_z << "\n" + << endl; + + _gyro_x += _gyro_raw_x * duration; + _gyro_y += _gyro_raw_y * duration; + _gyro_z += _gyro_raw_z * duration; + + _sout << "\nGyro raw x w/o time: " << _gyro_raw_x << "\n" + << "Gyro raw y w/o time: " << _gyro_raw_y << "\n" + << "Gyro raw z w/o time: " << _gyro_raw_z << "\n" + << endl; + + _pitch = 0.96 * _gyro_y + 0.04 * _accel_y; + _roll = 0.96 * _gyro_x + 0.04 * _accel_x; + _yaw = _gyro_z; + + return true; +} + +bool Sensor::setAccelSensitivity(uint8_t sensitivity) +{ + if (sensitivity > 3) + sensitivity = 3; + + _accel_sensitivity = sensitivity; + + uint8_t accel_config = getRegister(SensorRegisters::ACCEL_CONFIG); + + if (_status != SensorStatus::OK) + return false; + + // No need to write same value + if (((accel_config >> 3) & 3) != _accel_sensitivity) + { + accel_config &= 0xE7; + accel_config |= (_accel_sensitivity << 3); + + if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config)) + return false; + } + + // Calculate conversion factor. + _accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR; + + return true; +} + +bool Sensor::setGyroSensitivity(uint8_t sensitivity) +{ + if (sensitivity > 3) + sensitivity = 3; + + _gyro_sensitivity = sensitivity; + + uint8_t gyro_config = getRegister(SensorRegisters::GYRO_CONFIG); + + if (_status != SensorStatus::OK) + { + return false; + } + + // No need to write same value + if (((gyro_config >> 3) & 3) != _gyro_sensitivity) + { + gyro_config &= 0xE7; + gyro_config |= (_gyro_sensitivity << 3); + + if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config)) + { + return false; + } + } + + _ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; + + return true; +} + +float Sensor::getAccelX() +{ + return _accel_raw_x; +} + +float Sensor::getAccelY() +{ + return _accel_raw_y; +} + +float Sensor::getAccelZ() +{ + return _accel_raw_z; +} + +float Sensor::getAngleX() +{ + return _accel_x; +} + +float Sensor::getAngleY() +{ + return _accel_y; +} + +float Sensor::getGyroX() +{ + return _gyro_raw_x; +} + +float Sensor::getGyroY() +{ + return _gyro_raw_y; +} + +float Sensor::getGyroZ() +{ + return _gyro_raw_z; +} + +float Sensor::getPitch() +{ + return _pitch; +} + +float Sensor::getRoll() +{ + return _roll; +} + +bool Sensor::setRegister(uint8_t reg, uint8_t value) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->write(value); + + if (_wire->endTransmission() != 0) + { + _status = SensorStatus::ERR_WRITE; + return false; + } + + return true; +} + +uint8_t Sensor::getRegister(uint8_t reg) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + + if (_wire->endTransmission() != 0) + { + _status = SensorStatus::ERR_WRITE; + return 0; + } + + uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1); + if (response_length != 1) + { + _status = SensorStatus::ERR_READ; + return 0; + } + + return _wire->read(); +} + +SensorStatus Sensor::getStatus() +{ + SensorStatus status = _status; + _status = SensorStatus::OK; + + return status; +} + +int16_t Sensor::_readHighLow() +{ + int16_t high = _wire->read(); + int16_t low = _wire->read(); + + return high << 8 | low; +} diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp new file mode 100644 index 0000000..c982fac --- /dev/null +++ b/src/sensor/sensor.hpp @@ -0,0 +1,193 @@ +#ifndef SENSOR_HPP +#define SENSOR_HPP + +#include "../utils/serial.hpp" +#include "../utils/time.hpp" + +#include "Wire.h" +#include + +#define SENSOR_WAKEUP 0x00 + +#define RAW_TO_DPS_FACTOR (1.0 / 131.0) +#define RAW_TO_G_FACTOR (1.0 / 16384.0) + +enum class SensorStatus +{ + OK = 0, + THROTTLED = 1, + ERR_READ = -1, + ERR_WRITE = -2, + ERR_NOT_CONNECTED = -3 +}; + +/** + * A GY521 gyro and accelerometer sensor. + */ +class Sensor +{ +public: + /** + * A GY521 gyro and accelerometer sensor. + * + * @param address The address of the sensor + * @param wire A Wire instance + * @param sout A serial output stream + * @param throttle_time A minumum time between sensor reads for the sensor to throttle + */ + Sensor(uint8_t address, TwoWire *wire, SerialStream sout, unsigned int throttle_time); + + /** + * 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(); + + /** + * Reads from the sensor. + * + * @returns Whether or not it succeeded. + */ + bool read(); + + /** + * 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); + + /** + * 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 Z axis acceleration in g:s (g-force). + */ + float getAccelZ(); + + /** + * Returns the current X angle. + */ + float getAngleX(); + + /** + * Returns the current Y angle. + */ + float getAngleY(); + + /** + * Returns the current X axis in degrees/s. + */ + float getGyroX(); + + /** + * Returns the current Y axis in degrees/s. + */ + float getGyroY(); + + /** + * Returns the current Z axis in degrees/s. + */ + float getGyroZ(); + + /** + * Returns the current pitch angle. + */ + float getPitch(); + + /** + * Returns the current roll angle. + */ + float getRoll(); + + /** + * 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 Whether or not it succeeded. + */ + bool 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. + */ + SensorStatus 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; + unsigned int _throttle_time; + + Time _last_time; + + SensorStatus _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; + + 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; + + int16_t _readHighLow(); + + TwoWire *_wire; + + SerialStream _sout; +}; + +#endif -- cgit v1.2.3-18-g5258