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 --- src/sensor/sensor.cpp | 298 -------------------------------------------------- 1 file changed, 298 deletions(-) delete mode 100644 src/sensor/sensor.cpp (limited to 'src/sensor/sensor.cpp') 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); -} -- cgit v1.2.3-18-g5258