diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/calibration.cpp | 110 | ||||
| -rw-r--r-- | src/calibration.hpp | 71 | ||||
| -rw-r--r-- | src/gyronardo.cpp | 87 | ||||
| -rw-r--r-- | src/sensor.cpp | 273 | ||||
| -rw-r--r-- | src/sensor.hpp | 137 | ||||
| -rw-r--r-- | src/sensor_registers.hpp | 261 | ||||
| -rw-r--r-- | src/time_utils.cpp | 37 | ||||
| -rw-r--r-- | src/time_utils.hpp | 53 |
8 files changed, 590 insertions, 439 deletions
diff --git a/src/calibration.cpp b/src/calibration.cpp new file mode 100644 index 0000000..f76d8f8 --- /dev/null +++ b/src/calibration.cpp @@ -0,0 +1,110 @@ +#include "calibration.hpp" +#include "time_utils.hpp" + +SensorCalibrator::SensorCalibrator(Sensor *sensor) +{ + _sensor = sensor; + _resetValues(); +} + +bool SensorCalibrator::calibrate(unsigned int throttle_time) +{ + bool done = false; + Time start_time = time_now(); + + while (!done) + { + if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT) + return false; + + delay(throttle_time); + + _resetValues(); + + for (int i = 0; i < SENSOR_READ_CNT; i++) + { + _updateValues(); + } + + _adjustValues(); + + Serial.print("Accel X: "); + Serial.print(_accel_x); + Serial.print(" Accel Y: "); + Serial.print(_accel_y); + Serial.print(" Accel Z: "); + Serial.print(_accel_z); + + Serial.print(" Gyro X: "); + Serial.print(_gyro_x); + Serial.print(" Gyro Y: "); + Serial.print(_gyro_y); + Serial.print(" Gyro Z: "); + Serial.println(_gyro_z); + + if (_isValuesInRange()) + { + done = true; + } + + _adjustCalibration(); + } + + return true; +} + +void SensorCalibrator::_resetValues() +{ + _accel_x = 0; + _accel_y = 0; + _accel_z = 0; + + _gyro_x = 0; + _gyro_y = 0; + _gyro_z = 0; +} + +void SensorCalibrator::_updateValues() +{ + _sensor->read(); + + _accel_x -= _sensor->getAccelX(); + _accel_y -= _sensor->getAccelY(); + _accel_z -= _sensor->getAccelZ(); + + _gyro_x -= _sensor->getGyroX(); + _gyro_y -= _sensor->getGyroY(); + _gyro_z -= _sensor->getGyroZ(); +} + +void SensorCalibrator::_adjustValues() +{ + _accel_x *= SENSOR_VAL_ADJUST; + _accel_y *= SENSOR_VAL_ADJUST; + _accel_z *= SENSOR_VAL_ADJUST; + + _gyro_x *= SENSOR_VAL_ADJUST; + _gyro_y *= SENSOR_VAL_ADJUST; + _gyro_z *= SENSOR_VAL_ADJUST; +} + +bool SensorCalibrator::_isValuesInRange() +{ + return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN && + _accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN && + _accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN && + _gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN && + _gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN && + _gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN); +} + +void SensorCalibrator::_adjustCalibration() +{ + _sensor->accel_cal_x += _accel_x; + _sensor->accel_cal_y += _accel_y; + _sensor->accel_cal_z += _accel_z; + + _sensor->gyro_cal_x += _gyro_x; + _sensor->gyro_cal_y += _gyro_y; + _sensor->gyro_cal_z += _gyro_z; +} diff --git a/src/calibration.hpp b/src/calibration.hpp new file mode 100644 index 0000000..79b3758 --- /dev/null +++ b/src/calibration.hpp @@ -0,0 +1,71 @@ +#ifndef CALIBRATION_HPP +#define CALIBRATION_HPP + +#include "sensor.hpp" + +// Calibration precision +#define ACCEL_CAL_X_MAX 0.006 +#define ACCEL_CAL_X_MIN -0.006 + +#define ACCEL_CAL_Y_MAX 0.006 +#define ACCEL_CAL_Y_MIN -0.006 + +#define ACCEL_CAL_Z_MAX 0.006 +#define ACCEL_CAL_Z_MIN -0.006 + +#define GYRO_CAL_X_MAX 0.06 +#define GYRO_CAL_X_MIN -0.06 + +#define GYRO_CAL_Y_MAX 0.06 +#define GYRO_CAL_Y_MIN -0.06 + +#define GYRO_CAL_Z_MAX 0.06 +#define GYRO_CAL_Z_MIN -0.06 + +#define CALIBRATION_TIMEOUT 120000 // Milliseconds + +#define SENSOR_READ_CNT 20 +#define SENSOR_VAL_ADJUST 0.05 + +/** + * Sensor calibrator. + */ +class SensorCalibrator +{ +public: + /** + * Sensor calibrator. + * + * @param sensor A sensor to calibrate + */ + SensorCalibrator(Sensor *sensor); + + /** + * Calibrates the sensor. + * + * @param throttle_time The sensor's throttle time + * @returns Whether or not the calibration succeeded. Will return false on + * timeout. + */ + bool calibrate(unsigned int throttle_time); + +private: + void _resetValues(); + void _updateValues(); + void _adjustValues(); + bool _isValuesInRange(); + + void _adjustCalibration(); + + Sensor *_sensor; + + float _accel_x; + float _accel_y; + float _accel_z; + + float _gyro_x; + float _gyro_y; + float _gyro_z; +}; + +#endif diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index 025bb7d..f1e968e 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -1,8 +1,15 @@ #include "Arduino.h" #include "Wire.h" +#include "calibration.hpp" #include "sensor.hpp" -Sensor sensor(0x68, &Wire); +#define LINE_CLEAR_LENGTH 30 + +#define SENSOR_THROTTLE_TIME 50 // milliseconds + +Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME); + +char line_clear[LINE_CLEAR_LENGTH + 1]; /** * Stops code execution. @@ -21,47 +28,93 @@ void setup() while (!sensor.begin()) { - Serial.print(millis()); - Serial.println("Error: Could not connect to the 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.setThrottleEnabled(true); + if (!sensor.setAccelSensitivity(2)) // 8g + { + Serial.print( + "Error: Failed to set the sensor's accelerometer sensitivity. Status: "); + Serial.println(static_cast<int>(sensor.getStatus())); + stop(); + } - Serial.println("Automatically calibrating sensor..."); - bool cal_status = sensor.autoCalibrate(); + if (!sensor.setGyroSensitivity(1)) // 500 degrees/s + { + Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: "); + Serial.println(static_cast<int>(sensor.getStatus())); + stop(); + } - if (!cal_status) + Serial.println("Calibrating sensor..."); + SensorCalibrator sensor_calibrator(&sensor); + + if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME)) { - Serial.print("Error: Automatic calibration timed out after "); + Serial.print("Error: Sensor calibration timed out after "); Serial.print(CALIBRATION_TIMEOUT); Serial.println(" milliseconds"); stop(); } + Serial.println("Finished calibrating sensor"); + + Serial.println("Calibration values:"); - Serial.println("Finished calibrating"); + Serial.print("Accelerometer X: "); + Serial.println(sensor.accel_cal_x); - Serial.println("Starting..."); + Serial.print("Accelerometer Y: "); + Serial.println(sensor.accel_cal_y); + + Serial.print("Accelerometer Z: "); + Serial.println(sensor.accel_cal_z); + + Serial.print("Gyro X: "); + Serial.println(sensor.gyro_cal_x); + + Serial.print("Gyro Y: "); + Serial.println(sensor.gyro_cal_y); + + Serial.print("Gyro Z: "); + Serial.println(sensor.gyro_cal_z); + + Serial.println("\nStarting..."); + + size_t prev_len = strlen(line_clear); + memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len); } void loop() { - if (!sensor.is_calibrated) + delay(SENSOR_THROTTLE_TIME); + + if (!sensor.read()) { - return; - } + SensorStatus status = sensor.getStatus(); + + if (status == SensorStatus::THROTTLED) + { + Serial.println("Warning: The sensor was read too frequently and throttled"); + return; + } - sensor.read(); + Serial.print("Error: Failed to read sensor. Status: "); + Serial.println(static_cast<int>(status)); + stop(); + } float pitch = sensor.getPitch(); float roll = sensor.getRoll(); + Serial.print(line_clear); + Serial.print("\r"); Serial.print("Pitch: "); Serial.print(pitch, 3); Serial.print(" Roll: "); Serial.print(roll, 3); - Serial.println(); + Serial.print("\r"); + Serial.flush(); } diff --git a/src/sensor.cpp b/src/sensor.cpp index be57609..23edc17 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -1,8 +1,9 @@ #include "sensor.hpp" #include "math.h" #include "sensor_registers.hpp" +#include "time_utils.hpp" -Sensor::Sensor(uint8_t address, TwoWire *wire) +Sensor::Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time) : _last_time(0) { _address = address; _wire = wire; @@ -16,15 +17,10 @@ Sensor::Sensor(uint8_t address, TwoWire *wire) gyro_cal_y = 0; gyro_cal_z = 0; - is_calibrated = false; - _throttle_enabled = true; - _throttleTime = DEFAULT_THROTTLE_TIME; - - _lastTime = 0; - _lastMicros = 0; + _throttle_time = throttle_time; - _status = SENSOR_OK; + _status = SensorStatus::OK; _accel_sensitivity = 0; _accel_to_g_force = 1.0 / 16384.0; @@ -32,8 +28,6 @@ Sensor::Sensor(uint8_t address, TwoWire *wire) _gyro_sensitivity = 0; _ang_rate_to_dps = 1.0 / 131.0; - _temperature = 0; - // Reset the sensor reset(); } @@ -45,7 +39,7 @@ bool Sensor::begin() if (isConnected()) { _wire->beginTransmission(_address); - _wire->write(SENSOR_PWR_MGMT_1); + _wire->write(SensorRegisters::PWR_MGMT_1); _wire->write(SENSOR_WAKEUP); return (_wire->endTransmission() == 0); @@ -62,8 +56,6 @@ bool Sensor::isConnected() void Sensor::reset() { - setThrottleTime(DEFAULT_THROTTLE_TIME); - _accel_raw_x = 0; _accel_raw_y = 0; _accel_raw_z = 0; @@ -81,74 +73,47 @@ void Sensor::reset() _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() +bool Sensor::read() { - auto time_now = millis(); + Time now = time_now(); - if (_throttle_enabled) + if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time) { - if ((time_now - _lastTime) < _throttleTime) - { - return SENSOR_THROTTLED; - } + _status = SensorStatus::THROTTLED; + return false; } - _lastTime = time_now; - _wire->beginTransmission(_address); - _wire->write(SENSOR_ACCEL_XOUT_H); + _wire->write(SensorRegisters::ACCEL_XOUT_H); if (_wire->endTransmission() != 0) { - _status = SENSOR_ERR_WRITE; - return _status; + _status = SensorStatus::ERR_WRITE; + return false; } int8_t response_length = _wire->requestFrom(_address, (uint8_t)14); if (response_length != 14) { - _status = SENSOR_ERR_READ; - return _status; + _status = SensorStatus::ERR_READ; + return false; } - // ACCELEROMETER + // 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 + // 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; + 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; @@ -165,12 +130,13 @@ int16_t Sensor::read() 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; + _accel_x = + atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES; - // Convert temperature to celsius - _temperature = _temperature * 0.00294117647 + 36.53; + _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 raw Gyro to degrees/s _gyro_raw_x *= _ang_rate_to_dps; @@ -190,7 +156,7 @@ int16_t Sensor::read() _pitch = 0.96 * _gyro_y + 0.04 * _accel_y; _roll = 0.96 * _gyro_x + 0.04 * _accel_x; - return SENSOR_OK; + return true; } bool Sensor::setAccelSensitivity(uint8_t sensitivity) @@ -200,9 +166,9 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) if (_accel_sensitivity > 3) _accel_sensitivity = 3; - uint8_t val = getRegister(SENSOR_ACCEL_CONFIG); + uint8_t val = getRegister(SensorRegisters::ACCEL_CONFIG); - if (_status != SENSOR_OK) + if (_status != SensorStatus::OK) { return false; } @@ -213,7 +179,7 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) val &= 0xE7; val |= (_accel_sensitivity << 3); - if (setRegister(SENSOR_ACCEL_CONFIG, val) != SENSOR_OK) + if (!setRegister(SensorRegisters::ACCEL_CONFIG, val)) { return false; } @@ -225,20 +191,6 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) 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; @@ -246,9 +198,9 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) if (_gyro_sensitivity > 3) _gyro_sensitivity = 3; - uint8_t val = getRegister(SENSOR_GYRO_CONFIG); + uint8_t val = getRegister(SensorRegisters::GYRO_CONFIG); - if (_status != 0) + if (_status != SensorStatus::OK) { return false; } @@ -259,7 +211,7 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) val &= 0xE7; val |= (_gyro_sensitivity << 3); - if (setRegister(SENSOR_GYRO_CONFIG, val) != SENSOR_OK) + if (!setRegister(SensorRegisters::GYRO_CONFIG, val)) { return false; } @@ -270,39 +222,57 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) return true; } -uint8_t Sensor::getGyroSensitivity() +float Sensor::getAccelX() { - uint8_t val = getRegister(SENSOR_GYRO_CONFIG); + return _accel_raw_x; +}; - if (_status != SENSOR_OK) - { - return _status; - } +float Sensor::getAccelY() +{ + return _accel_raw_y; +}; - _gyro_sensitivity = (val >> 3) & 3; +float Sensor::getAccelZ() +{ + return _accel_raw_z; +}; - return _gyro_sensitivity; -} +float Sensor::getAngleX() +{ + return _accel_x; +}; -float Sensor::getAccelX() { return _accel_raw_x; }; -float Sensor::getAccelY() { return _accel_raw_y; }; -float Sensor::getAccelZ() { return _accel_raw_z; }; +float Sensor::getAngleY() +{ + return _accel_y; +}; -float Sensor::getAngleX() { return _accel_x; }; -float Sensor::getAngleY() { return _accel_y; }; +float Sensor::getGyroX() +{ + return _gyro_raw_x; +}; -float Sensor::getTemperature() { return _temperature; }; +float Sensor::getGyroY() +{ + return _gyro_raw_y; +}; -float Sensor::getGyroX() { return _gyro_raw_x; }; -float Sensor::getGyroY() { return _gyro_raw_y; }; -float Sensor::getGyroZ() { return _gyro_raw_z; }; +float Sensor::getGyroZ() +{ + return _gyro_raw_z; +}; -float Sensor::getPitch() { return _pitch; }; -float Sensor::getRoll() { return _roll; }; +float Sensor::getPitch() +{ + return _pitch; +}; -uint32_t Sensor::lastTime() { return _lastTime; }; +float Sensor::getRoll() +{ + return _roll; +}; -uint8_t Sensor::setRegister(uint8_t reg, uint8_t value) +bool Sensor::setRegister(uint8_t reg, uint8_t value) { _wire->beginTransmission(_address); _wire->write(reg); @@ -310,11 +280,11 @@ uint8_t Sensor::setRegister(uint8_t reg, uint8_t value) if (_wire->endTransmission() != 0) { - _status = SENSOR_ERR_WRITE; - return _status; + _status = SensorStatus::ERR_WRITE; + return false; } - return SENSOR_OK; + return true; } uint8_t Sensor::getRegister(uint8_t reg) @@ -324,15 +294,15 @@ uint8_t Sensor::getRegister(uint8_t reg) if (_wire->endTransmission() != 0) { - _status = SENSOR_ERR_WRITE; - return _status; + _status = SensorStatus::ERR_WRITE; + return 0; } uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1); if (response_length != 1) { - _status = SENSOR_ERR_READ; - return _status; + _status = SensorStatus::ERR_READ; + return 0; } uint8_t val = _wire->read(); @@ -340,12 +310,12 @@ uint8_t Sensor::getRegister(uint8_t reg) return val; } -int16_t Sensor::getStatus() +SensorStatus Sensor::getStatus() { - auto error = _status; - _status = SENSOR_OK; + SensorStatus status = _status; + _status = SensorStatus::OK; - return error; + return status; }; int16_t Sensor::_readTwoBytes() @@ -356,78 +326,3 @@ int16_t Sensor::_readTwoBytes() return response; } - -bool Sensor::autoCalibrate() -{ - bool done = false; - - float accel_x, accel_y, accel_z; - float gyro_x, gyro_y, gyro_z; - - auto cal_start_time = millis(); - - while (!done) - { - if (millis() - cal_start_time >= CALIBRATION_TIMEOUT) - { - return false; - } - - delay(100); - - accel_x = 0; - accel_y = 0; - accel_z = 0; - - gyro_x = 0; - gyro_y = 0; - gyro_z = 0; - - for (int i = 0; i < 20; i++) - { - read(); - - accel_x -= getAccelX(); - accel_y -= getAccelY(); - accel_z -= getAccelZ(); - - gyro_x -= getGyroX(); - gyro_y -= getGyroY(); - gyro_z -= getGyroZ(); - } - - const float adjust_val = 0.05; - - accel_x *= adjust_val; - accel_y *= adjust_val; - accel_z *= adjust_val; - - gyro_x *= adjust_val; - gyro_y *= adjust_val; - gyro_z *= adjust_val; - - if ( - accel_x < ACCEL_CAL_X_MAX && accel_x > ACCEL_CAL_X_MIN && - accel_y < ACCEL_CAL_Y_MAX && accel_y > ACCEL_CAL_Y_MIN && - accel_z < ACCEL_CAL_Z_MAX && accel_z > ACCEL_CAL_Z_MIN && - gyro_x < GYRO_CAL_X_MAX && gyro_x > GYRO_CAL_X_MIN && - gyro_y < GYRO_CAL_Y_MAX && gyro_y > GYRO_CAL_Y_MIN && - gyro_z < GYRO_CAL_Z_MAX && gyro_z > GYRO_CAL_Z_MIN) - { - done = true; - } - - // Adjust calibration values - accel_cal_x += accel_x; - accel_cal_y += accel_y; - accel_cal_z += accel_z; - - gyro_cal_x += gyro_x; - gyro_cal_y += gyro_y; - gyro_cal_z += gyro_z; - } - - is_calibrated = true; - - return true; -} diff --git a/src/sensor.hpp b/src/sensor.hpp index 3fbc544..07b8f95 100644 --- a/src/sensor.hpp +++ b/src/sensor.hpp @@ -3,15 +3,7 @@ #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 +#include "time_utils.hpp" #define SENSOR_WAKEUP 0x00 @@ -19,35 +11,33 @@ #define RAW_TO_DPS_FACTOR (1.0 / 131.0) #define RAW_TO_G_FACTOR (1.0 / 16384.0) -// Automatic calibration precision -#define ACCEL_CAL_X_MAX 0.002 -#define ACCEL_CAL_X_MIN -0.002 - -#define ACCEL_CAL_Y_MAX 0.002 -#define ACCEL_CAL_Y_MIN -0.002 - -#define ACCEL_CAL_Z_MAX 0.002 -#define ACCEL_CAL_Z_MIN -0.002 - -#define GYRO_CAL_X_MAX 0.2 -#define GYRO_CAL_X_MIN -0.2 - -#define GYRO_CAL_Y_MAX 0.02 -#define GYRO_CAL_Y_MIN -0.02 - -#define GYRO_CAL_Z_MAX 0.08 -#define GYRO_CAL_Z_MIN -0.08 - -#define CALIBRATION_TIMEOUT 120000 // Milliseconds +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: - Sensor(uint8_t address, TwoWire *wire); + /** + * A GY521 gyro and accelerometer sensor. + * + * @param address The address of the sensor + * @param wire A Wire instance + * @param throttle_time A minumum time between sensor reads for the sensor to throttle + */ + Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time); /** * Initializes communication with the sensor. - * + * * @returns Whether or not the sensor can be communicated with. */ bool begin(); @@ -63,59 +53,21 @@ public: 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 + * + * @returns Whether or not it succeeded. */ - uint8_t getAccelSensitivity(); + 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); /** - * 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. @@ -150,11 +102,6 @@ public: float getAngleY(); /** - * Returns the current temperature. - */ - float getTemperature(); - - /** * Returns the current X axis in degrees/s. */ float getGyroX(); @@ -180,22 +127,17 @@ public: 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 + * @returns Whether or not it succeeded. */ - uint8_t setRegister(uint8_t reg, uint8_t value); + 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); @@ -203,17 +145,7 @@ public: /** * Returns the last sensor status. */ - int16_t getStatus(); - - /** - * Automatically calibrates the sensor. - * - * @returns Whether or not the calibration succeeded. - * Will return false if it times out. - */ - bool autoCalibrate(); - - bool is_calibrated; + SensorStatus getStatus(); // Accelerometer calibration values float accel_cal_x; @@ -229,12 +161,11 @@ private: uint8_t _address; bool _throttle_enabled; - uint16_t _throttleTime; + unsigned int _throttle_time; - uint32_t _lastTime; - uint32_t _lastMicros; + Time _last_time; - int16_t _status; + SensorStatus _status; uint8_t _accel_sensitivity; float _accel_to_g_force; @@ -250,8 +181,6 @@ private: float _pitch, _roll, _yaw; - float _temperature; - int16_t _readTwoBytes(); TwoWire *_wire; diff --git a/src/sensor_registers.hpp b/src/sensor_registers.hpp index 34b47c6..c1b19b4 100644 --- a/src/sensor_registers.hpp +++ b/src/sensor_registers.hpp @@ -1,134 +1,137 @@ #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 +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/time_utils.cpp b/src/time_utils.cpp new file mode 100644 index 0000000..84c7d96 --- /dev/null +++ b/src/time_utils.cpp @@ -0,0 +1,37 @@ +#include "time_utils.hpp" +#include "Arduino.h" + +Time::Time(unsigned long time_micros) +{ + _time_micros = time_micros; +} + +void Time::update() +{ + _time_micros = micros(); +} + +Time Time::diff(Time prev_time) +{ + return Time(_time_micros - prev_time.microsecs()); +} + +unsigned long Time::microsecs() +{ + return _time_micros; +} + +unsigned long Time::millisecs() +{ + return _time_micros * 0.001; +} + +float Time::secs() +{ + return _time_micros * 0.000001; +} + +Time time_now() +{ + return Time(micros()); +} diff --git a/src/time_utils.hpp b/src/time_utils.hpp new file mode 100644 index 0000000..f8f2eb3 --- /dev/null +++ b/src/time_utils.hpp @@ -0,0 +1,53 @@ +#ifndef TIME_UTILS_HPP +#define TIME_UTILS_HPP + +/** + * A representation of time. + */ +class Time +{ +public: + /** + * A representation of time. + * + * @param time_micros Time in microseconds + */ + Time(unsigned long time_micros); + + /** + * Updates the time to the current time. + */ + void update(); + + /** + * Returns the difference between two points in time. + * + * @param prev_time A previous point in time + */ + Time diff(Time prev_time); + + /** + * Returns the time in seconds. + */ + float secs(); + + /** + * Returns the time in milliseconds. + */ + unsigned long millisecs(); + + /** + * Returns the time in microseconds. + */ + unsigned long microsecs(); + +private: + unsigned long _time_micros; +}; + +/** + * Returns a time object for the time since the program started. + */ +Time time_now(); + +#endif |