From 757a29d0137b974fff6ddcc945d76e69ae120ecb Mon Sep 17 00:00:00 2001 From: HampusM Date: Mon, 21 Mar 2022 13:00:36 +0100 Subject: refactor: use MPU6050_light & clean up bloat --- platformio.ini | 5 +- src/common/conversion.cpp | 42 ------ src/common/conversion.hpp | 36 ----- src/common/memory.hpp | 13 -- src/common/memory.tpp | 27 ---- src/common/memory/shared_ptr.hpp | 44 ------ src/common/memory/shared_ptr.tpp | 153 -------------------- src/common/memory/unique_ptr.hpp | 37 ----- src/common/memory/unique_ptr.tpp | 55 -------- src/common/string.cpp | 70 --------- src/common/string.hpp | 25 ---- src/common/time.cpp | 42 ------ src/common/time.hpp | 60 -------- src/gyronardo.cpp | 95 +++---------- src/sensor/calibration.cpp | 96 ------------- src/sensor/calibration.hpp | 76 ---------- src/sensor/registers.hpp | 136 ------------------ src/sensor/sensor.cpp | 298 --------------------------------------- src/sensor/sensor.hpp | 194 ------------------------- src/serial.cpp | 75 ---------- src/serial.hpp | 43 ------ src/utils.cpp | 6 - src/utils.hpp | 6 - 23 files changed, 25 insertions(+), 1609 deletions(-) delete mode 100644 src/common/conversion.cpp delete mode 100644 src/common/conversion.hpp delete mode 100644 src/common/memory.hpp delete mode 100644 src/common/memory.tpp delete mode 100644 src/common/memory/shared_ptr.hpp delete mode 100644 src/common/memory/shared_ptr.tpp delete mode 100644 src/common/memory/unique_ptr.hpp delete mode 100644 src/common/memory/unique_ptr.tpp delete mode 100644 src/common/string.cpp delete mode 100644 src/common/string.hpp delete mode 100644 src/common/time.cpp delete mode 100644 src/common/time.hpp delete mode 100644 src/sensor/calibration.cpp delete mode 100644 src/sensor/calibration.hpp delete mode 100644 src/sensor/registers.hpp delete mode 100644 src/sensor/sensor.cpp delete mode 100644 src/sensor/sensor.hpp delete mode 100644 src/serial.cpp delete mode 100644 src/serial.hpp delete mode 100644 src/utils.cpp delete mode 100644 src/utils.hpp diff --git a/platformio.ini b/platformio.ini index 718ffc5..54761bc 100644 --- a/platformio.ini +++ b/platformio.ini @@ -3,10 +3,11 @@ build_dir = build [env:leonardo] platform = atmelavr -board = leonardo -# board = leonardo-xinput +#board = leonardo +board = leonardo-xinput framework = arduino lib_deps = + https://github.com/rfetick/MPU6050_light https://github.com/dmadison/ArduinoXInput src_build_flags = -Wextra diff --git a/src/common/conversion.cpp b/src/common/conversion.cpp deleted file mode 100644 index b016537..0000000 --- a/src/common/conversion.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "conversion.hpp" - -#include -#include - -namespace common -{ - -common::UniquePtr doubleToStr(double num, unsigned int width, - unsigned int precision) -{ - auto str = common::make_unique(width + precision); - - dtostrf(num, static_cast(width), static_cast(precision), - str->c_str); - - return str; -} - -common::UniquePtr intToStr(int num) -{ - auto width = static_cast(log10(num)); - - auto str = common::make_unique(width + 1U); - - dtostrf(num, static_cast(width + 1U), 0U, str->c_str); - - return str; -} - -common::UniquePtr uintToStr(unsigned int num) -{ - auto width = static_cast(log10(num)); - - auto str = common::make_unique(width + 1U); - - dtostrf(num, static_cast(width + 1U), 0U, str->c_str); - - return str; -} - -} // namespace common diff --git a/src/common/conversion.hpp b/src/common/conversion.hpp deleted file mode 100644 index e87adc5..0000000 --- a/src/common/conversion.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "common/memory/unique_ptr.hpp" -#include "common/string.hpp" - -namespace common -{ - -/** - * Converts a double number to a string. - * - * @param num A double number - * @param width The desired double width - * @param precision The desired double precision - * @returns The double as a string. - */ -common::UniquePtr doubleToStr(double num, unsigned int width = 3, - unsigned int precision = 2); - -/** - * Converts a integer to a string. - * - * @param num A number - * @returns The number as a string. - */ -common::UniquePtr intToStr(int num); - -/** - * Converts a unsigned integer to a string. - * - * @param num A number - * @returns The number as a string. - */ -common::UniquePtr uintToStr(unsigned int num); - -} // namespace common diff --git a/src/common/memory.hpp b/src/common/memory.hpp deleted file mode 100644 index 888ede3..0000000 --- a/src/common/memory.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include - -namespace common -{ - -template -memType *malloc_s(unsigned int size); - -} // namespace common - -#include "memory.tpp" diff --git a/src/common/memory.tpp b/src/common/memory.tpp deleted file mode 100644 index af833bc..0000000 --- a/src/common/memory.tpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include "memory.hpp" - -#include "utils.hpp" - -#include -#include - -namespace common -{ - -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); -} - -} // namespace common diff --git a/src/common/memory/shared_ptr.hpp b/src/common/memory/shared_ptr.hpp deleted file mode 100644 index 7e8a910..0000000 --- a/src/common/memory/shared_ptr.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include - -namespace common -{ -template -class SharedPtr -{ -public: - SharedPtr() noexcept; - SharedPtr(nullptr_t) noexcept; // NOLINT(google-explicit-constructor) - - explicit SharedPtr(Target *target) noexcept; - - SharedPtr(const SharedPtr &shared_ptr) noexcept; - - SharedPtr(SharedPtr &&shared_ptr) noexcept; - - ~SharedPtr() noexcept; - - [[nodiscard]] unsigned int reference_cnt() const noexcept; - - [[nodiscard]] bool is_disposable() const noexcept; - - SharedPtr &operator=(const SharedPtr &rhs) noexcept; - - SharedPtr &operator=(SharedPtr &&rhs) noexcept; - - Target &operator*() const noexcept; - Target *operator->() const noexcept; - -private: - Target *_target = nullptr; - - unsigned int *_reference_cnt; -}; - -template -SharedPtr make_shared(Args &&...args) noexcept; - -} // namespace common - -#include "shared_ptr.tpp" diff --git a/src/common/memory/shared_ptr.tpp b/src/common/memory/shared_ptr.tpp deleted file mode 100644 index d9b57c3..0000000 --- a/src/common/memory/shared_ptr.tpp +++ /dev/null @@ -1,153 +0,0 @@ -#pragma once - -#include "shared_ptr.hpp" - -#include "common/memory.hpp" - -#include - -namespace common -{ - -template -SharedPtr::SharedPtr() noexcept - : _reference_cnt(malloc_s(sizeof(unsigned int))) -{ - (*_reference_cnt) = 0; -} - -template -SharedPtr::SharedPtr(nullptr_t) noexcept - : _reference_cnt(malloc_s(sizeof(unsigned int))) -{ - (*_reference_cnt) = 0; -} - -template -SharedPtr::SharedPtr(Target *target) noexcept - : _target(target), _reference_cnt(malloc_s(sizeof(unsigned int))) -{ - (*_reference_cnt) = 0; -} - -/** - * Copy constructor - */ -template -SharedPtr::SharedPtr(const SharedPtr &shared_ptr) noexcept - : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt) -{ - (*_reference_cnt)++; -} - -/** - * Move constructor - */ -template -SharedPtr::SharedPtr(SharedPtr &&shared_ptr) noexcept - : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt) -{ - shared_ptr._target = nullptr; -} - -template -SharedPtr::~SharedPtr() noexcept -{ - if ((*_reference_cnt) != 0U) - { - (*_reference_cnt)--; - } - - if ((*_reference_cnt) == 0U) - { - delete _target; - free(_reference_cnt); - _reference_cnt = nullptr; - } -} - -template -unsigned int SharedPtr::reference_cnt() const noexcept -{ - if (_reference_cnt == nullptr) - { - return 0; - } - - return *_reference_cnt; -} - -template -bool SharedPtr::is_disposable() const noexcept -{ - return _reference_cnt == nullptr || (*_reference_cnt) == 0U; -} - -/** - * Copy assignment operator - */ -template -SharedPtr &SharedPtr::operator=(const SharedPtr &rhs) noexcept -{ - if (&rhs != this) - { - if (is_disposable()) - { - delete _target; - free(_reference_cnt); - } - - _target = nullptr; - _target = rhs._target; - - _reference_cnt = nullptr; - _reference_cnt = rhs._reference_cnt; - (*_reference_cnt)++; - } - - return *this; -} - -/** - * Move assignment operator - */ -template -SharedPtr &SharedPtr::operator=(SharedPtr &&rhs) noexcept -{ - if (&rhs != this) - { - if (is_disposable()) - { - delete _target; - free(_reference_cnt); - } - - _target = rhs._target; - rhs._target = nullptr; - - _reference_cnt = rhs._reference_cnt; - rhs._reference_cnt = nullptr; - } - - return *this; -} - -template -Target &SharedPtr::operator*() const noexcept -{ - return *(_target); -} - -template -Target *SharedPtr::operator->() const noexcept -{ - return _target; -} - -template -SharedPtr make_shared(Args &&...args) noexcept -{ - return SharedPtr(new Target(args...)); -} - -} // namespace common diff --git a/src/common/memory/unique_ptr.hpp b/src/common/memory/unique_ptr.hpp deleted file mode 100644 index 94c02d0..0000000 --- a/src/common/memory/unique_ptr.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -namespace common -{ -template -class UniquePtr -{ -public: - explicit UniquePtr() = default; - explicit UniquePtr(Target *target); - - // Move constructor - UniquePtr(UniquePtr &&unique_ptr) noexcept; - - // Move assignment operator - UniquePtr &operator=(UniquePtr &&unique_ptr) noexcept; - - // Disable the copy constructor - UniquePtr(const UniquePtr &unique_ptr) = delete; - - // Disable the copy assignment operator - UniquePtr &operator=(const UniquePtr &unique_ptr) = delete; - - ~UniquePtr(); - - Target operator*() const; - Target *operator->() const; - -private: - Target *_target = nullptr; -}; - -template -UniquePtr make_unique(Args... args); -} // namespace common - -#include "unique_ptr.tpp" diff --git a/src/common/memory/unique_ptr.tpp b/src/common/memory/unique_ptr.tpp deleted file mode 100644 index 3cab9ec..0000000 --- a/src/common/memory/unique_ptr.tpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include "unique_ptr.hpp" - -namespace common -{ -template -UniquePtr::UniquePtr(Target *target) : _target(target) -{ -} - -template -UniquePtr::UniquePtr(UniquePtr &&unique_ptr) noexcept - : _target(unique_ptr._target) -{ - unique_ptr._target = nullptr; -} - -template -UniquePtr &UniquePtr::operator=(UniquePtr &&unique_ptr) noexcept -{ - if (&unique_ptr != this) - { - delete _target; - _target = unique_ptr._target; - unique_ptr._target = nullptr; - } - - return *this; -} - -template -UniquePtr::~UniquePtr() -{ - delete _target; -} - -template -Target UniquePtr::operator*() const -{ - return *(_target); -} - -template -Target *UniquePtr::operator->() const -{ - return _target; -} - -template -UniquePtr make_unique(Args... args) -{ - return UniquePtr(new Target(args...)); -} -} // namespace common diff --git a/src/common/string.cpp b/src/common/string.cpp deleted file mode 100644 index 27b65d4..0000000 --- a/src/common/string.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "string.hpp" - -#include "common/memory.hpp" - -#include - -namespace common -{ - -String::String(char *c_string) : c_str(c_string) -{ -} - -String::String(unsigned int size) : c_str(malloc_s(size + 1)) -{ -} - -String::String(const String &smart_str) - : c_str(malloc_s(strlen(smart_str.c_str) + 1)) -{ - memcpy(c_str, smart_str.c_str, strlen(smart_str.c_str) + 1); -} - -String::String(String &&smart_str) noexcept : c_str(smart_str.c_str) -{ - smart_str.c_str = nullptr; -} - -String &String::operator=(const String &smart_str) -{ - if (&smart_str != this) - { - free(c_str); - c_str = nullptr; - - auto str_size = strlen(smart_str.c_str) + 1; - - c_str = malloc_s(str_size); - memcpy(c_str, smart_str.c_str, str_size); - } - - return *this; -} - -String &String::operator=(String &&smart_str) noexcept -{ - if (&smart_str != this) - { - free(c_str); - c_str = smart_str.c_str; - smart_str.c_str = nullptr; - } - - return *this; -} - -String::~String() -{ - if (c_str != nullptr) - { - free(c_str); - } -} - -String::operator char *() const -{ - return c_str; -} - -} // namespace common diff --git a/src/common/string.hpp b/src/common/string.hpp deleted file mode 100644 index 2641457..0000000 --- a/src/common/string.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -namespace common -{ - -class String -{ -public: - explicit String(char *c_str); - explicit String(unsigned int size); - String(const String &smart_str); - String(String &&smart_str) noexcept; - - String &operator=(const String &smart_str); - - String &operator=(String &&smart_str) noexcept; - - ~String(); - - explicit operator char *() const; - - char *c_str = nullptr; -}; - -} // namespace common diff --git a/src/common/time.cpp b/src/common/time.cpp deleted file mode 100644 index ee297d7..0000000 --- a/src/common/time.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "time.hpp" - -#include - -namespace common -{ - - Time::Time(uint64_t time_micros) : _time_micros(time_micros) - { - } - - void Time::update() - { - _time_micros = micros(); - } - - Time Time::diff(Time prev_time) const - { - return Time(_time_micros - prev_time.microsecs()); - } - - double Time::secs() const - { - return static_cast(_time_micros) * MICROS_TO_SECS; - } - - double Time::millisecs() const - { - return static_cast(_time_micros) * MICROS_TO_MILLIS; - } - - uint64_t Time::microsecs() const - { - return _time_micros; - } - - Time time_now() - { - return Time(micros()); - } - -} // namespace common diff --git a/src/common/time.hpp b/src/common/time.hpp deleted file mode 100644 index 8efbd28..0000000 --- a/src/common/time.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once - -#include - -constexpr double MICROS_TO_SECS = 0.000001; -constexpr double MICROS_TO_MILLIS = 0.001; - -namespace common -{ - - /** - * A representation of time. - */ - class Time - { - public: - /** - * A representation of time. - * - * @param time_micros Time in microseconds - */ - explicit Time(uint64_t 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) const; - - /** - * Returns the time in seconds. - */ - double secs() const; - - /** - * Returns the time in milliseconds. - */ - double millisecs() const; - - /** - * Returns the time in microseconds. - */ - uint64_t microsecs() const; - - private: - uint64_t _time_micros; - }; - - /** - * Returns a time object for the time since the program started. - */ - Time time_now(); - -} // namespace common diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index 9b1aa24..b876193 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -1,107 +1,56 @@ -#include "common/memory/shared_ptr.hpp" -#include "sensor/calibration.hpp" -#include "sensor/sensor.hpp" -#include "serial.hpp" #include "status.hpp" -#include "utils.hpp" +#include #include #include -constexpr uint8_t SENSOR_ADDRESS = 0x68U; -constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds -constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds +constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds constexpr unsigned int BAUD_RATE = 9600U; +constexpr int32_t JOY_MAX = 180; +constexpr int32_t JOY_MIN = -180; + +#define LEONARDO_XINPUT_VID 0x045E // NOLINT(cppcoreguidelines-macro-usage) +#define LEONARDO_XINPUT_PID 0x028E // NOLINT(cppcoreguidelines-macro-usage) + void setup() { initialize_status_leds(); - auto sout = common::make_shared(Serial, BAUD_RATE); + auto sensor = MPU6050(Wire); - auto sensor = common::make_shared(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME); + Wire.begin(); - sout->waitReady(); + set_led_positive(HIGH); - while (!sensor->begin()) + while (sensor.begin() != 0) { - *sout << "Error: Could not connect to the _sensor-> Retrying after 2000 " - "milliseconds..." - << endl; + set_led_positive(LOW); delay(SENSOR_RETRY_TIME); } - if (!sensor->setAccelSensitivity(2)) // 8g - { - *sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: " - << static_cast(sensor->getStatus()) << endl; - - stop(); - } - - if (!sensor->setGyroSensitivity(1)) // 500 degrees/s - { - *sout << "Error: Failed to set the sensor's gyro sensitivity. Status: " - << static_cast(sensor->getStatus()) << endl; - - stop(); - } - - *sout << "Calibrating _sensor->.." << endl; - - SensorCalibrator sensor_calibrator(sensor, sout); - - if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME)) - { - *sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT - << " milliseconds" << endl; - - stop(); - } - set_led_positive(HIGH); - *sout << "Finished calibrating sensor\n"; - - *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"; - - *sout << "Starting..." << endl; + sensor.calcOffsets(); XInput.begin(); + XInput.setRange(JOY_LEFT, JOY_MIN, JOY_MAX); + while (true) { - delay(SENSOR_THROTTLE_TIME); - - if (!sensor->read()) - { - SensorStatus status = sensor->getStatus(); + sensor.update(); - if (status == SensorStatus::THROTTLED) - { - *sout << "Warning: The sensor was read too frequently and throttled" - << endl; - continue; - } + auto pitch = static_cast(sensor.getAngleX()); - *sout << "Error: Failed to read _sensor-> Status: " - << static_cast(status) << endl; - stop(); - } + auto roll = static_cast(sensor.getAngleY()); - *sout << "Pitch: " << sensor->getPitch() << " Roll: " << sensor->getRoll() - << endl; + XInput.setJoystick(JOY_LEFT, -roll, pitch); - XInput.setJoystick(JOY_LEFT, static_cast(sensor->getRoll()), - static_cast(sensor->getPitch())); +#if USB_VID != LEONARDO_XINPUT_VID && USB_PID != LEONARDO_XINPUT_PID XInput.printDebug(Serial); +#endif } } diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp deleted file mode 100644 index 5626388..0000000 --- a/src/sensor/calibration.cpp +++ /dev/null @@ -1,96 +0,0 @@ -#include "calibration.hpp" - -#include "common/time.hpp" -#include "utils.hpp" - -SensorCalibrator::SensorCalibrator(common::SharedPtr sensor, - common::SharedPtr sout) - : _sensor(sensor), _sout(sout) -{ -} - -bool SensorCalibrator::calibrate(unsigned int throttle_time) -{ - bool done = false; - auto start_time = common::time_now(); - - while (!done) - { - if (common::time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT) - { - return false; - } - - delay(throttle_time); - - auto values = SensorCalibratorValues(); - - for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++) - { - _updateValues(values); - } - - _adjustValues(values); - - *_sout << "Accel X: " << values.accel_x << " " - << "Accel Y: " << values.accel_y << " " - << "Accel Z: " << values.accel_z << " " - << "Gyro X: " << values.gyro_x << " " - << "Gyro Y: " << values.gyro_y << " " - << "Gyro Z: " << values.gyro_z << endl; - - if (_isValuesInRange(values)) - { - done = true; - } - - _adjustCalibrationWithValues(values); - } - - return true; -} - -void SensorCalibrator::_updateValues(SensorCalibratorValues &values) -{ - _sensor->read(); - - values.accel_x -= _sensor->getAccelX(); - values.accel_y -= _sensor->getAccelY(); - values.accel_z -= _sensor->getAccelZ(); - - values.gyro_x -= _sensor->getGyroX(); - values.gyro_y -= _sensor->getGyroY(); - values.gyro_z -= _sensor->getGyroZ(); -} - -void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values) -{ - _sensor->accel_cal_x += values.accel_x; - _sensor->accel_cal_y += values.accel_y; - _sensor->accel_cal_z += values.accel_z; - - _sensor->gyro_cal_x += values.gyro_x; - _sensor->gyro_cal_y += values.gyro_y; - _sensor->gyro_cal_z += values.gyro_z; -} - -void SensorCalibrator::_adjustValues(SensorCalibratorValues &values) -{ - values.accel_x *= SENSOR_VAL_ADJUST; - values.accel_y *= SENSOR_VAL_ADJUST; - values.accel_z *= SENSOR_VAL_ADJUST; - - values.gyro_x *= SENSOR_VAL_ADJUST; - values.gyro_y *= SENSOR_VAL_ADJUST; - values.gyro_z *= SENSOR_VAL_ADJUST; -} - -bool SensorCalibrator::_isValuesInRange(const SensorCalibratorValues &values) -{ - return (values.accel_x < ACCEL_CAL_X_MAX && values.accel_x > ACCEL_CAL_X_MIN && - values.accel_y < ACCEL_CAL_Y_MAX && values.accel_y > ACCEL_CAL_Y_MIN && - values.accel_z < ACCEL_CAL_Z_MAX && values.accel_z > ACCEL_CAL_Z_MIN && - values.gyro_x < GYRO_CAL_X_MAX && values.gyro_x > GYRO_CAL_X_MIN && - values.gyro_y < GYRO_CAL_Y_MAX && values.gyro_y > GYRO_CAL_Y_MIN && - values.gyro_z < GYRO_CAL_Z_MAX && values.gyro_z > GYRO_CAL_Z_MIN); -} diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp deleted file mode 100644 index c85d336..0000000 --- a/src/sensor/calibration.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include "common/memory/shared_ptr.hpp" -#include "sensor/sensor.hpp" -#include "serial.hpp" - -// Calibration precision -constexpr double ACCEL_CAL_X_MAX = 0.003; -constexpr double ACCEL_CAL_X_MIN = -0.003; - -constexpr double ACCEL_CAL_Y_MAX = 0.003; -constexpr double ACCEL_CAL_Y_MIN = -0.003; - -constexpr double ACCEL_CAL_Z_MAX = 0.003; -constexpr double ACCEL_CAL_Z_MIN = -0.003; - -constexpr double GYRO_CAL_X_MAX = 0.003; -constexpr double GYRO_CAL_X_MIN = -0.003; - -constexpr double GYRO_CAL_Y_MAX = 0.05; -constexpr double GYRO_CAL_Y_MIN = -0.05; - -constexpr double GYRO_CAL_Z_MAX = 0.04; -constexpr double GYRO_CAL_Z_MIN = -0.04; - -constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // milliseconds - -constexpr uint32_t SENSOR_READ_CNT = 20; -constexpr double SENSOR_VAL_ADJUST = 0.05; - -class SensorCalibratorValues -{ -public: - double accel_x = 0; - double accel_y = 0; - double accel_z = 0; - - double gyro_x = 0; - double gyro_y = 0; - double gyro_z = 0; -}; - -/** - * Sensor calibrator. - */ -class SensorCalibrator -{ -public: - /** - * Sensor calibrator. - * - * @param sensor A sensor to calibrate - * @param sout A Serial output stream - */ - SensorCalibrator(common::SharedPtr sensor, - common::SharedPtr sout); - - /** - * 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 _updateValues(SensorCalibratorValues &values); - void _adjustCalibrationWithValues(const SensorCalibratorValues &values); - - static void _adjustValues(SensorCalibratorValues &values); - static bool _isValuesInRange(const SensorCalibratorValues &values); - - common::SharedPtr _sensor; - common::SharedPtr _sout; -}; diff --git a/src/sensor/registers.hpp b/src/sensor/registers.hpp deleted file mode 100644 index fadb62b..0000000 --- a/src/sensor/registers.hpp +++ /dev/null @@ -1,136 +0,0 @@ -#pragma once - -#include - -namespace SensorRegisters -{ - const uint8_t XG_OFFS_TC = 0x00U; - const uint8_t YG_OFFS_TC = 0x01U; - const uint8_t ZG_OFFS_TC = 0x02U; - - const uint8_t X_FINE_GAIN = 0x03U; - const uint8_t Y_FINE_GAIN = 0x04U; - const uint8_t Z_FINE_GAIN = 0x05U; - - const uint8_t XA_OFFS_H = 0x06U; - const uint8_t XA_OFFS_L_TC = 0x07U; - const uint8_t YA_OFFS_H = 0x08U; - const uint8_t YA_OFFS_L_TC = 0x09U; - const uint8_t ZA_OFFS_H = 0x0AU; - const uint8_t ZA_OFFS_L_TC = 0x0BU; - - const uint8_t SELF_TEST_X = 0x0DU; - const uint8_t SELF_TEST_Y = 0x0EU; - const uint8_t SELF_TEST_Z = 0x0FU; - const uint8_t SELF_TEST_A = 0x10U; - - const uint8_t XG_OFFS_USRH = 0x13U; - const uint8_t XG_OFFS_USRL = 0x14U; - const uint8_t YG_OFFS_USRH = 0x15U; - const uint8_t YG_OFFS_USRL = 0x16U; - const uint8_t ZG_OFFS_USRH = 0x17U; - const uint8_t ZG_OFFS_USRL = 0x18U; - - const uint8_t SMPLRT_DIV = 0x19U; - const uint8_t CONFIG = 0x1AU; - const uint8_t GYRO_CONFIG = 0x1BU; - const uint8_t ACCEL_CONFIG = 0x1CU; - - const uint8_t FF_THR = 0x1DU; - const uint8_t FF_DUR = 0x1EU; - const uint8_t MOT_THR = 0x1FU; - const uint8_t MOT_DUR = 0x20U; - const uint8_t ZRMOT_THR = 0x21U; - const uint8_t ZRMOT_DUR = 0x22U; - const uint8_t FIFO_EN = 0x23U; - - const uint8_t I2C_MST_CTRL = 0x24U; - const uint8_t I2C_SLV0_ADDR = 0x25U; - const uint8_t I2C_SLV0_REG = 0x26U; - const uint8_t I2C_SLV0_CTRL = 0x27U; - const uint8_t I2C_SLV1_ADDR = 0x28U; - const uint8_t I2C_SLV1_REG = 0x29U; - const uint8_t I2C_SLV1_CTRL = 0x2AU; - const uint8_t I2C_SLV2_ADDR = 0x2BU; - const uint8_t I2C_SLV2_REG = 0x2CU; - const uint8_t I2C_SLV2_CTRL = 0x2DU; - const uint8_t I2C_SLV3_ADDR = 0x2EU; - const uint8_t I2C_SLV3_REG = 0x2FU; - const uint8_t I2C_SLV3_CTRL = 0x30U; - const uint8_t I2C_SLV4_ADDR = 0x31U; - const uint8_t I2C_SLV4_REG = 0x32U; - const uint8_t I2C_SLV4_DO = 0x33U; - const uint8_t I2C_SLV4_CTRL = 0x34U; - const uint8_t I2C_SLV4_DI = 0x35U; - const uint8_t I2C_MST_STATUS = 0x36U; - - const uint8_t INT_PIN_CFG = 0x37U; - const uint8_t INT_ENABLE = 0x38U; - const uint8_t DMP_INT_STATUS = 0x39U; - const uint8_t INT_STATUS = 0x3AU; - - const uint8_t ACCEL_XOUT_H = 0x3BU; - const uint8_t ACCEL_XOUT_L = 0x3CU; - const uint8_t ACCEL_YOUT_H = 0x3DU; - const uint8_t ACCEL_YOUT_L = 0x3EU; - const uint8_t ACCEL_ZOUT_H = 0x3FU; - const uint8_t ACCEL_ZOUT_L = 0x40U; - const uint8_t TEMP_OUT_H = 0x41U; - const uint8_t TEMP_OUT_L = 0x42U; - const uint8_t GYRO_XOUT_H = 0x43U; - const uint8_t GYRO_XOUT_L = 0x44U; - const uint8_t GYRO_YOUT_H = 0x45U; - const uint8_t GYRO_YOUT_L = 0x46U; - const uint8_t GYRO_ZOUT_H = 0x47U; - const uint8_t GYRO_ZOUT_L = 0x48U; - - const uint8_t EXT_SENS_DATA_00 = 0x49U; - const uint8_t EXT_SENS_DATA_01 = 0x4AU; - const uint8_t EXT_SENS_DATA_02 = 0x4BU; - const uint8_t EXT_SENS_DATA_03 = 0x4CU; - const uint8_t EXT_SENS_DATA_04 = 0x4DU; - const uint8_t EXT_SENS_DATA_05 = 0x4EU; - const uint8_t EXT_SENS_DATA_06 = 0x4FU; - const uint8_t EXT_SENS_DATA_07 = 0x50U; - const uint8_t EXT_SENS_DATA_08 = 0x51U; - const uint8_t EXT_SENS_DATA_09 = 0x52U; - const uint8_t EXT_SENS_DATA_10 = 0x53U; - const uint8_t EXT_SENS_DATA_11 = 0x54U; - const uint8_t EXT_SENS_DATA_12 = 0x55U; - const uint8_t EXT_SENS_DATA_13 = 0x56U; - const uint8_t EXT_SENS_DATA_14 = 0x57U; - const uint8_t EXT_SENS_DATA_15 = 0x58U; - const uint8_t EXT_SENS_DATA_16 = 0x59U; - const uint8_t EXT_SENS_DATA_17 = 0x5AU; - const uint8_t EXT_SENS_DATA_18 = 0x5BU; - const uint8_t EXT_SENS_DATA_19 = 0x5CU; - const uint8_t EXT_SENS_DATA_20 = 0x5DU; - const uint8_t EXT_SENS_DATA_21 = 0x5EU; - const uint8_t EXT_SENS_DATA_22 = 0x5FU; - const uint8_t EXT_SENS_DATA_23 = 0x60U; - - const uint8_t MOT_DETECT_STATUS = 0x61U; - - const uint8_t I2C_SLV0_DO = 0x63U; - const uint8_t I2C_SLV1_DO = 0x64U; - const uint8_t I2C_SLV2_DO = 0x65U; - const uint8_t I2C_SLV3_DO = 0x66U; - const uint8_t I2C_MST_DELAY_CTRL = 0x67U; - - const uint8_t SIGNAL_PATH_RESET = 0x68U; - const uint8_t MOT_DETECT_CTRL = 0x69U; - const uint8_t USER_CTRL = 0x6AU; - - const uint8_t PWR_MGMT_1 = 0x6BU; - const uint8_t PWR_MGMT_2 = 0x6CU; - const uint8_t BANK_SEL = 0x6DU; - const uint8_t MEM_START_ADDR = 0x6EU; - const uint8_t MEM_R_W = 0x6FU; - - const uint8_t DMP_CFG_1 = 0x70U; - const uint8_t DMP_CFG_2 = 0x71U; - const uint8_t FIFO_COUNTH = 0x72U; - const uint8_t FIFO_COUNTL = 0x73U; - const uint8_t FIFO_R_W = 0x74U; - const uint8_t WHO_AM_I = 0x75U; -} // namespace SensorRegisters diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp deleted file mode 100644 index a801270..0000000 --- a/src/sensor/sensor.cpp +++ /dev/null @@ -1,298 +0,0 @@ -#include "sensor.hpp" - -#include "sensor/registers.hpp" - -Sensor::Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept - : _wire(wire), - _address(address), - _throttle_enabled(true), - _throttle_time(throttle_time), - _last_time(0), - _status(SensorStatus::OK), - _accel_to_g_force(RAW_TO_G_FACTOR), - _ang_rate_to_dps(RAW_TO_DPS_FACTOR) -{ -} - -bool Sensor::begin() noexcept -{ - _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() noexcept -{ - _wire.beginTransmission(_address); - return (_wire.endTransmission() == 0); -} - -bool Sensor::read() noexcept -{ - auto now = common::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() != 0U) - { - _status = SensorStatus::ERR_WRITE; - return false; - } - - uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y); - - const uint8_t self_test_success = 14U; - - if (response_length != self_test_success) - { - _status = SensorStatus::ERR_READ; - return false; - } - - // Accelerometer - _accel_raw_x = _readHighLow(); - _accel_raw_y = _readHighLow(); - _accel_raw_z = _readHighLow(); - - // Gyroscope - _gyro_raw_x = _readHighLow(); - _gyro_raw_y = _readHighLow(); - _gyro_raw_z = _readHighLow(); - - // Duration interval - now.update(); - auto 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 - auto accel_y_pow_two = pow(_accel_raw_y, 2); - auto accel_z_pow_two = pow(_accel_raw_z, 2); - - _accel_x = atan2(_accel_raw_y, _accel_raw_z) * ONE_EIGHTY / PI; - - _accel_y = - atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI; - - // 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; - - _pitch = _gyro_y + _accel_y; - _roll = _gyro_x + _accel_x; - - _yaw = _gyro_z; - - return true; -} - -bool Sensor::setAccelSensitivity(uint8_t sensitivity) noexcept -{ - if (sensitivity > 3) - { - sensitivity = 3; - } - - _accel_sensitivity = sensitivity; - - auto accel_config = static_cast(getRegister(SensorRegisters::ACCEL_CONFIG)); - - if (_status != SensorStatus::OK) - { - return false; - } - - // No need to write same value - if (((accel_config >> 3) & 3) != _accel_sensitivity) - { - const uint8_t magic = 0xE7U; - - accel_config &= magic; - accel_config |= (_accel_sensitivity << 3U); - - if (!setRegister(SensorRegisters::ACCEL_CONFIG, - static_cast(accel_config))) - { - return false; - } - } - - // Calculate conversion factor. - _accel_to_g_force = static_cast(1 << _accel_sensitivity) * RAW_TO_G_FACTOR; - - return true; -} - -bool Sensor::setGyroSensitivity(uint8_t sensitivity) noexcept -{ - if (sensitivity > 3) - { - sensitivity = 3; - } - - _gyro_sensitivity = sensitivity; - - auto gyro_config = static_cast(getRegister(SensorRegisters::GYRO_CONFIG)); - - if (_status != SensorStatus::OK) - { - return false; - } - - // No need to write same value - if (((gyro_config >> 3) & 3) != _gyro_sensitivity) - { - const uint8_t magic = 0xE7U; - - gyro_config &= magic; - gyro_config |= (_gyro_sensitivity << 3U); - - if (!setRegister(SensorRegisters::GYRO_CONFIG, static_cast(gyro_config))) - { - return false; - } - } - - _ang_rate_to_dps = static_cast(1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; - - return true; -} - -double Sensor::getAccelX() const noexcept -{ - return _accel_raw_x; -} - -double Sensor::getAccelY() const noexcept -{ - return _accel_raw_y; -} - -double Sensor::getAccelZ() const noexcept -{ - return _accel_raw_z; -} - -double Sensor::getAngleX() const noexcept -{ - return _accel_x; -} - -double Sensor::getAngleY() const noexcept -{ - return _accel_y; -} - -double Sensor::getGyroX() const noexcept -{ - return _gyro_raw_x; -} - -double Sensor::getGyroY() const noexcept -{ - return _gyro_raw_y; -} - -double Sensor::getGyroZ() const noexcept -{ - return _gyro_raw_z; -} - -double Sensor::getPitch() const noexcept -{ - return _pitch; -} - -double Sensor::getRoll() const noexcept -{ - return _roll; -} - -bool Sensor::setRegister(uint8_t reg, uint8_t value) noexcept -{ - _wire.beginTransmission(_address); - _wire.write(reg); - _wire.write(value); - - if (_wire.endTransmission() != 0U) - { - _status = SensorStatus::ERR_WRITE; - return false; - } - - return true; -} - -uint8_t Sensor::getRegister(uint8_t reg) noexcept -{ - _wire.beginTransmission(_address); - _wire.write(reg); - - if (_wire.endTransmission() != 0U) - { - _status = SensorStatus::ERR_WRITE; - return 0U; - } - - uint8_t response_length = _wire.requestFrom(_address, 1U); - if (response_length != 1U) - { - _status = SensorStatus::ERR_READ; - return 0U; - } - - return static_cast(_wire.read()); -} - -SensorStatus Sensor::getStatus() noexcept -{ - SensorStatus status = _status; - _status = SensorStatus::OK; - - return status; -} - -int16_t Sensor::_readHighLow() noexcept -{ - const auto high = static_cast(_wire.read()); - const auto low = static_cast(_wire.read()); - - const int8_t bits_in_byte = 8; - - return static_cast(high << bits_in_byte | low); -} diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp deleted file mode 100644 index 1b99f6e..0000000 --- a/src/sensor/sensor.hpp +++ /dev/null @@ -1,194 +0,0 @@ -#pragma once - -#include "common/time.hpp" - -#include -#include - -constexpr uint8_t SENSOR_WAKEUP = 0x00U; - -constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0; -constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0; - -constexpr uint32_t ONE_EIGHTY = 180; - -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) noexcept; - - /** - * Initializes communication with the sensor. - * - * @returns Whether or not the sensor can be communicated with. - */ - bool begin() noexcept; - - /** - * Returns whether or not the sensor is connected. - */ - bool isConnected() noexcept; - - /** - * Reads from the sensor. - * - * @returns Whether or not it succeeded. - */ - bool read() noexcept; - - /** - * 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) noexcept; - - /** - * 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) noexcept; - - /** - * Returns the current X axis acceleration in g:s (g-force). - */ - double getAccelX() const noexcept; - - /** - * Returns the current Y axis acceleration in g:s (g-force). - */ - double getAccelY() const noexcept; - - /** - * Returns the current Z axis acceleration in g:s (g-force). - */ - double getAccelZ() const noexcept; - - /** - * Returns the current X angle. - */ - double getAngleX() const noexcept; - - /** - * Returns the current Y angle. - */ - double getAngleY() const noexcept; - - /** - * Returns the current X axis in degrees/s. - */ - double getGyroX() const noexcept; - - /** - * Returns the current Y axis in degrees/s. - */ - double getGyroY() const noexcept; - - /** - * Returns the current Z axis in degrees/s. - */ - double getGyroZ() const noexcept; - - /** - * Returns the current pitch angle. - */ - double getPitch() const noexcept; - - /** - * Returns the current roll angle. - */ - double getRoll() const noexcept; - - /** - * 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) noexcept; - - /** - * 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) noexcept; - - /** - * Returns the last sensor status. - */ - SensorStatus getStatus() noexcept; - - // Accelerometer calibration values - double accel_cal_x = 0; - double accel_cal_y = 0; - double accel_cal_z = 0; - - // Gyroscope calibration values - double gyro_cal_x = 0; - double gyro_cal_y = 0; - double gyro_cal_z = 0; - -private: - TwoWire _wire; - - uint8_t _address; - - bool _throttle_enabled; - unsigned int _throttle_time; - - common::Time _last_time; - - SensorStatus _status; - - uint8_t _accel_sensitivity = 0U; - float _accel_to_g_force; - - double _accel_raw_x = 0; - double _accel_raw_y = 0; - double _accel_raw_z = 0; - - double _accel_x = 0; - double _accel_y = 0; - - uint8_t _gyro_sensitivity = 0U; - float _ang_rate_to_dps; - - double _gyro_raw_x = 0; - double _gyro_raw_y = 0; - double _gyro_raw_z = 0; - - double _gyro_x = 0; - double _gyro_y = 0; - double _gyro_z = 0; - - double _pitch = 0; - double _roll = 0; - double _yaw = 0; - - int16_t _readHighLow() noexcept; -}; diff --git a/src/serial.cpp b/src/serial.cpp deleted file mode 100644 index 874d6d7..0000000 --- a/src/serial.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include "serial.hpp" - -#include "common/conversion.hpp" - -SerialStream::SerialStream(Serial_ serial, const unsigned long &baud_rate) noexcept - : _serial(serial) -{ - _serial.begin(baud_rate); -} - -void SerialStream::waitReady() -{ - while (!_serial) {} -} - -void SerialStream::write(const char *str) -{ - if (SerialStream::is_enabled()) - { - _serial.write(str); - } -} - -void SerialStream::flush() -{ - _serial.flush(); -} - -SerialStream &SerialStream::operator<<(const char *str) -{ - write(str); - return *this; -} - -SerialStream &SerialStream::operator<<(const common::String &str) -{ - write(str.c_str); - return *this; -} - -SerialStream &SerialStream::operator<<(const double &num) -{ - write(common::doubleToStr(num, 3U, 4U)->c_str); - return *this; -} - -SerialStream &SerialStream::operator<<(const int &num) -{ - write(common::intToStr(num)->c_str); - return *this; -} - -SerialStream &SerialStream::operator<<(const unsigned int &num) -{ - write(common::uintToStr(num)->c_str); - return *this; -} - -SerialStream &SerialStream::operator<<(const unsigned long &num) -{ - write(common::uintToStr(num)->c_str); - return *this; -} - -SerialStream &SerialStream::operator<<(void (*manipulator)(SerialStream *)) -{ - manipulator(this); - return *this; -} - -void endl(SerialStream *serial_stream) -{ - serial_stream->write("\n"); - serial_stream->flush(); -} diff --git a/src/serial.hpp b/src/serial.hpp deleted file mode 100644 index ae51c3f..0000000 --- a/src/serial.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include "common/string.hpp" - -#include - -#define LEONARDO_XINPUT_VID 0x045E // NOLINT(cppcoreguidelines-macro-usage) -#define LEONARDO_XINPUT_PID 0x028E // NOLINT(cppcoreguidelines-macro-usage) - -class SerialStream -{ -public: - SerialStream(Serial_ serial, const unsigned long &baud_rate) noexcept; - - void waitReady(); - - void write(const char *str); - - void flush(); - - constexpr static bool is_enabled() noexcept - { -#if USB_VID != LEONARDO_XINPUT_VID && USB_PID != LEONARDO_XINPUT_PID - return true; -#else - return false; -#endif - } - - SerialStream &operator<<(const char *str); - SerialStream &operator<<(const common::String &str); - SerialStream &operator<<(const double &num); - SerialStream &operator<<(const int &num); - SerialStream &operator<<(const unsigned int &num); - SerialStream &operator<<(const unsigned long &num); - - SerialStream &operator<<(void (*manipulator)(SerialStream *)); - -private: - Serial_ _serial; -}; - -void endl(SerialStream *serial_stream); diff --git a/src/utils.cpp b/src/utils.cpp deleted file mode 100644 index cb25471..0000000 --- a/src/utils.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "utils.hpp" - -void stop() -{ - while (true) {} -} diff --git a/src/utils.hpp b/src/utils.hpp deleted file mode 100644 index 33f3379..0000000 --- a/src/utils.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -/** - * Stops code execution. - */ -void stop(); -- cgit v1.2.3-18-g5258