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/calibration.cpp | 29 ++-- src/calibration.hpp | 11 +- src/gyronardo.cpp | 120 +++++++-------- src/sensor.cpp | 325 ---------------------------------------- src/sensor.hpp | 189 ------------------------ src/sensor/registers.hpp | 137 +++++++++++++++++ src/sensor/sensor.cpp | 360 +++++++++++++++++++++++++++++++++++++++++++++ src/sensor/sensor.hpp | 193 ++++++++++++++++++++++++ src/sensor_registers.hpp | 137 ----------------- src/time_utils.cpp | 37 ----- src/time_utils.hpp | 53 ------- src/utils/general.cpp | 14 ++ src/utils/general.hpp | 23 +++ src/utils/memory.hpp | 23 +++ src/utils/memory.tpp | 48 ++++++ src/utils/serial.cpp | 57 +++++++ src/utils/serial.hpp | 27 ++++ src/utils/smart_string.cpp | 26 ++++ src/utils/smart_string.hpp | 14 ++ src/utils/time.cpp | 37 +++++ src/utils/time.hpp | 53 +++++++ 21 files changed, 1090 insertions(+), 823 deletions(-) delete mode 100644 src/sensor.cpp delete mode 100644 src/sensor.hpp create mode 100644 src/sensor/registers.hpp create mode 100644 src/sensor/sensor.cpp create mode 100644 src/sensor/sensor.hpp delete mode 100644 src/sensor_registers.hpp delete mode 100644 src/time_utils.cpp delete mode 100644 src/time_utils.hpp create mode 100644 src/utils/general.cpp create mode 100644 src/utils/general.hpp create mode 100644 src/utils/memory.hpp create mode 100644 src/utils/memory.tpp create mode 100644 src/utils/serial.cpp create mode 100644 src/utils/serial.hpp create mode 100644 src/utils/smart_string.cpp create mode 100644 src/utils/smart_string.hpp create mode 100644 src/utils/time.cpp create mode 100644 src/utils/time.hpp diff --git a/src/calibration.cpp b/src/calibration.cpp index f76d8f8..a772ae9 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -1,9 +1,11 @@ #include "calibration.hpp" -#include "time_utils.hpp" -SensorCalibrator::SensorCalibrator(Sensor *sensor) +#include "utils/general.hpp" +#include "utils/time.hpp" + +SensorCalibrator::SensorCalibrator(unique_ptr &sensor, SerialStream sout) + : _sensor(sensor), _sout(sout) { - _sensor = sensor; _resetValues(); } @@ -15,7 +17,9 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) while (!done) { if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT) + { return false; + } delay(throttle_time); @@ -28,19 +32,12 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) _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); + _sout << "Accel X: " << _accel_x << " " + << "Accel Y: " << _accel_y << " " + << "Accel Z: " << _accel_z << " " + << "Gyro X: " << _gyro_x << " " + << "Gyro Y: " << _gyro_y << " " + << "Gyro Z: " << _gyro_z << endl; if (_isValuesInRange()) { diff --git a/src/calibration.hpp b/src/calibration.hpp index 79b3758..e66b6fc 100644 --- a/src/calibration.hpp +++ b/src/calibration.hpp @@ -1,7 +1,9 @@ #ifndef CALIBRATION_HPP #define CALIBRATION_HPP -#include "sensor.hpp" +#include "sensor/sensor.hpp" +#include "utils/memory.hpp" +#include "utils/serial.hpp" // Calibration precision #define ACCEL_CAL_X_MAX 0.006 @@ -37,8 +39,9 @@ public: * Sensor calibrator. * * @param sensor A sensor to calibrate + * @param sout A Serial output stream */ - SensorCalibrator(Sensor *sensor); + SensorCalibrator(unique_ptr &sensor, SerialStream sout); /** * Calibrates the sensor. @@ -57,7 +60,9 @@ private: void _adjustCalibration(); - Sensor *_sensor; + unique_ptr &_sensor; + + SerialStream _sout; float _accel_x; float _accel_y; diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index f1e968e..53a1912 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -1,94 +1,87 @@ #include "Arduino.h" + #include "Wire.h" #include "calibration.hpp" -#include "sensor.hpp" +#include "sensor/sensor.hpp" +#include "utils/memory.hpp" + +#include "utils/serial.hpp" #define LINE_CLEAR_LENGTH 30 +#define SENSOR_ADDRESS 0x68 #define SENSOR_THROTTLE_TIME 50 // milliseconds -Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME); - -char line_clear[LINE_CLEAR_LENGTH + 1]; +unique_ptr sout_ptr = nullptr; -/** - * Stops code execution. - */ -void stop() -{ - while (true) {} -} +unique_ptr sensor = nullptr; void setup() { - Serial.begin(9600); + sout_ptr = make_unique(Serial, 9600); - // Wait for Serial because the Arduino Leonardo is weird - while (!Serial) {} + auto sout = *sout_ptr; - while (!sensor.begin()) + sensor = make_unique(SENSOR_ADDRESS, &Wire, sout, SENSOR_THROTTLE_TIME); + + while (!sensor->begin()) { - Serial.println("Error: Could not connect to the sensor. Retrying after 2000 " - "milliseconds..."); + sout << "Error: Could not connect to the sensor. Retrying after 2000 " + "milliseconds..." + << endl; + delay(2000); } - if (!sensor.setAccelSensitivity(2)) // 8g + sout << "Skit!" << endl; + + if (!sensor->setAccelSensitivity(2)) // 8g { - Serial.print( - "Error: Failed to set the sensor's accelerometer sensitivity. Status: "); - Serial.println(static_cast(sensor.getStatus())); + sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: " + << static_cast(sensor->getStatus()) << endl; + stop(); } - if (!sensor.setGyroSensitivity(1)) // 500 degrees/s + if (!sensor->setGyroSensitivity(1)) // 500 degrees/s { - Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: "); - Serial.println(static_cast(sensor.getStatus())); + sout << "Error: Failed to set the sensor's gyro sensitivity. Status: " + << static_cast(sensor->getStatus()) << endl; + stop(); } - Serial.println("Calibrating sensor..."); - SensorCalibrator sensor_calibrator(&sensor); + sout << "Calibrating sensor..." << endl; + + SensorCalibrator sensor_calibrator(sensor, sout); if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME)) { - Serial.print("Error: Sensor calibration timed out after "); - Serial.print(CALIBRATION_TIMEOUT); - Serial.println(" milliseconds"); + sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT + << " milliseconds" << endl; stop(); } - Serial.println("Finished calibrating sensor"); - - Serial.println("Calibration values:"); - - Serial.print("Accelerometer X: "); - Serial.println(sensor.accel_cal_x); - - 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); + sout << "Finished calibrating sensor\n"; - Serial.print("Gyro Y: "); - Serial.println(sensor.gyro_cal_y); + sout << "Calibration values:\n" + << "Accelerometer X: " << sensor->accel_cal_x << "\n" + << "Accelerometer Y: " << sensor->accel_cal_y << "\n" + << "Accelerometer Z: " << sensor->accel_cal_z << "\n" + << "Gyro X: " << sensor->gyro_cal_x << "\n" + << "Gyro Y: " << sensor->gyro_cal_y << "\n" + << "Gyro Z: " << sensor->gyro_cal_z << "\n"; - Serial.print("Gyro Z: "); - Serial.println(sensor.gyro_cal_z); + sout << "Starting..." << endl; - Serial.println("\nStarting..."); - - size_t prev_len = strlen(line_clear); - memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len); + sout << "Pitch: " << sensor->getPitch() << " " + << "Roll: " << sensor->getRoll() << endl; } void loop() { + /* delay(SENSOR_THROTTLE_TIME); if (!sensor.read()) @@ -97,24 +90,25 @@ void loop() if (status == SensorStatus::THROTTLED) { - Serial.println("Warning: The sensor was read too frequently and throttled"); + printSerial("Warning: The sensor was read too frequently and throttled"); + endl(); + return; } - Serial.print("Error: Failed to read sensor. Status: "); - Serial.println(static_cast(status)); + printSerial("Error: Failed to read sensor. Status: %d", static_cast(status)); + endl(); + stop(); } - float pitch = sensor.getPitch(); - float roll = sensor.getRoll(); + char *pitch = floatToStr(sensor.getPitch(), FLOAT_WIDTH, FLOAT_PRECISION); + char *roll = floatToStr(sensor.getRoll(), FLOAT_WIDTH, FLOAT_PRECISION); - Serial.print(line_clear); - Serial.print("\r"); - Serial.print("Pitch: "); - Serial.print(pitch, 3); - Serial.print(" Roll: "); - Serial.print(roll, 3); - Serial.print("\r"); + printSerial("\rPitch: %s Roll: %s", pitch, roll); Serial.flush(); + + free(pitch); + free(roll); + */ } diff --git a/src/sensor.cpp b/src/sensor.cpp deleted file mode 100644 index 9d4157e..0000000 --- a/src/sensor.cpp +++ /dev/null @@ -1,325 +0,0 @@ -#include "sensor.hpp" -#include "math.h" -#include "sensor_registers.hpp" -#include "time_utils.hpp" - -Sensor::Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time) : _last_time(0) -{ - _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 = _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 - - // 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 - 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; - - // 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; - - // 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 true; -} - -bool Sensor::setAccelSensitivity(uint8_t sensitivity) -{ - _accel_sensitivity = sensitivity; - - if (_accel_sensitivity > 3) - _accel_sensitivity = 3; - - uint8_t val = getRegister(SensorRegisters::ACCEL_CONFIG); - - if (_status != SensorStatus::OK) - { - return false; - } - - // No need to write same value - if (((val >> 3) & 3) != _accel_sensitivity) - { - val &= 0xE7; - val |= (_accel_sensitivity << 3); - - if (!setRegister(SensorRegisters::ACCEL_CONFIG, val)) - { - return false; - } - } - - // Calculate conversion factor. - _accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR; - - return true; -} - -bool Sensor::setGyroSensitivity(uint8_t sensitivity) -{ - _gyro_sensitivity = sensitivity; - - if (_gyro_sensitivity > 3) - _gyro_sensitivity = 3; - - uint8_t val = getRegister(SensorRegisters::GYRO_CONFIG); - - if (_status != SensorStatus::OK) - { - return false; - } - - // No need to write same value - if (((val >> 3) & 3) != _gyro_sensitivity) - { - val &= 0xE7; - val |= (_gyro_sensitivity << 3); - - if (!setRegister(SensorRegisters::GYRO_CONFIG, val)) - { - 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; - } - - uint8_t val = _wire->read(); - - return val; -} - -SensorStatus Sensor::getStatus() -{ - SensorStatus status = _status; - _status = SensorStatus::OK; - - return status; -}; - -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 deleted file mode 100644 index 72ff396..0000000 --- a/src/sensor.hpp +++ /dev/null @@ -1,189 +0,0 @@ -#ifndef SENSOR_HPP -#define SENSOR_HPP - -#include "Arduino.h" -#include "Wire.h" -#include "time_utils.hpp" - -#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) - -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 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(); - - /** - * 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 _readTwoBytes(); - - TwoWire *_wire; -}; - -#endif 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 diff --git a/src/sensor_registers.hpp b/src/sensor_registers.hpp deleted file mode 100644 index c1b19b4..0000000 --- a/src/sensor_registers.hpp +++ /dev/null @@ -1,137 +0,0 @@ -#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/time_utils.cpp b/src/time_utils.cpp deleted file mode 100644 index 84c7d96..0000000 --- a/src/time_utils.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#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 deleted file mode 100644 index f8f2eb3..0000000 --- a/src/time_utils.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#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 diff --git a/src/utils/general.cpp b/src/utils/general.cpp new file mode 100644 index 0000000..bbcd4dd --- /dev/null +++ b/src/utils/general.cpp @@ -0,0 +1,14 @@ +#include "general.hpp" + +void stop() +{ + while (true) {} +} + +unique_ptr floatToStr(float num, unsigned int width, unsigned int precision) +{ + auto str = make_unique(width + precision); + dtostrf(num, width, precision, str->c_str); + + return str; +} diff --git a/src/utils/general.hpp b/src/utils/general.hpp new file mode 100644 index 0000000..9ad0655 --- /dev/null +++ b/src/utils/general.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "memory.hpp" +#include "smart_string.hpp" + +/** + * Stops code execution. + */ +void stop(); + +/** + * Converts a floating point number to a string. + * + * @param num A floating point number + * @param width The desired float width + * @param precision The desired float precision + * @returns The float as a string. + */ +unique_ptr floatToStr( + float num, + unsigned int width = 3, + unsigned int precision = 2 +); diff --git a/src/utils/memory.hpp b/src/utils/memory.hpp new file mode 100644 index 0000000..81e3757 --- /dev/null +++ b/src/utils/memory.hpp @@ -0,0 +1,23 @@ +#pragma once + +template +memType *malloc_s(unsigned int size); + +template +class unique_ptr +{ +public: + unique_ptr(Target *target); + ~unique_ptr(); + + Target operator *() const; + Target *operator ->() const; + +private: + Target *_target; +}; + +template +unique_ptr make_unique(Args&... args); + +#include "memory.tpp" diff --git a/src/utils/memory.tpp b/src/utils/memory.tpp new file mode 100644 index 0000000..276b0b4 --- /dev/null +++ b/src/utils/memory.tpp @@ -0,0 +1,48 @@ +#include "memory.hpp" + +#include "Arduino.h" +#include "general.hpp" + +template +memType *malloc_s(unsigned int size) +{ + auto *mem = malloc(size); + + if (mem == nullptr) + { + Serial.println("Error: Memory allocation failed"); + while (true) {} + } + + return static_cast(mem); +} + +template +unique_ptr::unique_ptr(Target *target) +{ + this->_target = target; +} + +template +unique_ptr::~unique_ptr() +{ + delete this->_target; +} + +template +Target unique_ptr::operator*() const +{ + return *(this->_target); +} + +template +Target *unique_ptr::operator->() const +{ + return this->_target; +} + +template +unique_ptr make_unique(Args... args) +{ + return unique_ptr(new Target(args...)); +} diff --git a/src/utils/serial.cpp b/src/utils/serial.cpp new file mode 100644 index 0000000..11be550 --- /dev/null +++ b/src/utils/serial.cpp @@ -0,0 +1,57 @@ +#include "serial.hpp" + +#include "general.hpp" + +SerialStream::SerialStream(Serial_ serial, unsigned long baud_rate) +{ + this->_serial = serial; + + this->_serial.begin(baud_rate); + + while (!this->_serial) {} +} + +SerialStream::~SerialStream() +{ + Serial.end(); +} + +SerialStream &SerialStream::operator<<(const char *str) +{ + this->write(str); + return *this; +} + +SerialStream &SerialStream::operator<<(const SmartString &str) +{ + this->write(str.c_str); + return *this; +} + +SerialStream &SerialStream::operator<<(const float num) +{ + this->write(floatToStr(num)->c_str); + return *this; +} + +SerialStream &SerialStream::operator<<(void (*manipulator)(SerialStream *)) +{ + manipulator(this); + return *this; +} + +void SerialStream::write(const char *str) +{ + this->_serial.write(str); +} + +void SerialStream::flush() +{ + this->_serial.flush(); +} + +void endl(SerialStream *serial_stream) +{ + serial_stream->write("\n"); + serial_stream->flush(); +} diff --git a/src/utils/serial.hpp b/src/utils/serial.hpp new file mode 100644 index 0000000..f97e1d8 --- /dev/null +++ b/src/utils/serial.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "smart_string.hpp" + +#include + +class SerialStream +{ +public: + SerialStream(Serial_ serial, unsigned long baud_rate); + ~SerialStream(); + + SerialStream &operator<<(const char *str); + SerialStream &operator<<(const SmartString &str); + SerialStream &operator<<(const float num); + + SerialStream &operator<<(void (*manipulator)(SerialStream *)); + + void write(const char *str); + + void flush(); + +private: + Serial_ _serial; +}; + +void endl(SerialStream *serial_stream); diff --git a/src/utils/smart_string.cpp b/src/utils/smart_string.cpp new file mode 100644 index 0000000..5258ffe --- /dev/null +++ b/src/utils/smart_string.cpp @@ -0,0 +1,26 @@ +#include "smart_string.hpp" + +#include "memory.hpp" + +#include + +SmartString::SmartString(char *c_string) +{ + this->c_str = c_string; +} + +SmartString::SmartString(unsigned int size) +{ + this->c_str = malloc_s(size + 1); +} + +SmartString::~SmartString() +{ + if (this->c_str != nullptr) + free(this->c_str); +} + +SmartString::operator char *() const +{ + return this->c_str; +} diff --git a/src/utils/smart_string.hpp b/src/utils/smart_string.hpp new file mode 100644 index 0000000..fcaff98 --- /dev/null +++ b/src/utils/smart_string.hpp @@ -0,0 +1,14 @@ +#pragma once + +class SmartString +{ +public: + SmartString(char *c_str); + SmartString(unsigned int size); + + ~SmartString(); + + operator char *() const; + + char *c_str = nullptr; +}; diff --git a/src/utils/time.cpp b/src/utils/time.cpp new file mode 100644 index 0000000..adc33db --- /dev/null +++ b/src/utils/time.cpp @@ -0,0 +1,37 @@ +#include "time.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/utils/time.hpp b/src/utils/time.hpp new file mode 100644 index 0000000..e0385ef --- /dev/null +++ b/src/utils/time.hpp @@ -0,0 +1,53 @@ +#ifndef TIME_HPP +#define TIME_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 -- cgit v1.2.3-18-g5258