From 01ce0af940bd69c94a2fac8b65219262845cca98 Mon Sep 17 00:00:00 2001 From: HampusM Date: Mon, 14 Feb 2022 18:18:38 +0100 Subject: refactor: clean sewage --- src/sensor/sensor.cpp | 174 +++++++++++++++++++++++--------------------------- 1 file changed, 80 insertions(+), 94 deletions(-) (limited to 'src/sensor/sensor.cpp') diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index e11a74b..a3b9a5c 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -1,50 +1,34 @@ #include "sensor.hpp" -#include "../utils/serial.hpp" -#include "../utils/time.hpp" + #include "registers.hpp" +#include "utils/serial.hpp" +#include "utils/time.hpp" -Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout, +Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time) - : _last_time(0), _sout(sout) + : _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), + _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(); + _wire.begin(); if (isConnected()) { - _wire->beginTransmission(_address); - _wire->write(SensorRegisters::PWR_MGMT_1); - _wire->write(SENSOR_WAKEUP); + _wire.beginTransmission(_address); + _wire.write(SensorRegisters::PWR_MGMT_1); + _wire.write(SENSOR_WAKEUP); - return (_wire->endTransmission() == 0); + return (_wire.endTransmission() == 0); } return false; @@ -52,31 +36,13 @@ bool Sensor::begin() 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; + _wire.beginTransmission(_address); + return (_wire.endTransmission() == 0); } bool Sensor::read() { - Time now = time_now(); + auto now = time_now(); if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time) { @@ -84,18 +50,20 @@ bool Sensor::read() return false; } - _wire->beginTransmission(_address); - _wire->write(SensorRegisters::ACCEL_XOUT_H); + _wire.beginTransmission(_address); + _wire.write(SensorRegisters::ACCEL_XOUT_H); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0U) { _status = SensorStatus::ERR_WRITE; return false; } - int8_t response_length = _wire->requestFrom(_address, (uint8_t)14); + uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y); + + const uint8_t self_test_success = 14U; - if (response_length != 14) + if (response_length != self_test_success) { _status = SensorStatus::ERR_READ; return false; @@ -123,7 +91,7 @@ bool Sensor::read() // Duration interval now.update(); - float duration = now.diff(_last_time).secs(); + auto duration = now.diff(_last_time).secs(); _last_time = now; // Convert raw acceleration to g:s (g-force) @@ -147,12 +115,13 @@ bool Sensor::read() << 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); + 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) * 180 / PI; + _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)) * 180 / PI; + _accel_y = + atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI; /* _accel_x = @@ -191,8 +160,11 @@ bool Sensor::read() << "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; + const float gyro_balance = 0.96F; + const float accel_balance = 0.04F; + + _pitch = gyro_balance * _gyro_y + accel_balance * _accel_y; + _roll = gyro_balance * _gyro_x + accel_balance * _accel_x; _yaw = _gyro_z; return true; @@ -201,23 +173,31 @@ bool Sensor::read() 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; + const uint8_t magic = 0xE7U; + + accel_config &= magic; accel_config |= (_accel_sensitivity << 3); if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config)) + { return false; + } } // Calculate conversion factor. @@ -229,7 +209,9 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) bool Sensor::setGyroSensitivity(uint8_t sensitivity) { if (sensitivity > 3) + { sensitivity = 3; + } _gyro_sensitivity = sensitivity; @@ -243,7 +225,9 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) // No need to write same value if (((gyro_config >> 3) & 3) != _gyro_sensitivity) { - gyro_config &= 0xE7; + const uint8_t magic = 0xE7U; + + gyro_config &= magic; gyro_config |= (_gyro_sensitivity << 3); if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config)) @@ -257,63 +241,63 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) return true; } -float Sensor::getAccelX() +double Sensor::getAccelX() { return _accel_raw_x; } -float Sensor::getAccelY() +double Sensor::getAccelY() { return _accel_raw_y; } -float Sensor::getAccelZ() +double Sensor::getAccelZ() { return _accel_raw_z; } -float Sensor::getAngleX() +double Sensor::getAngleX() { return _accel_x; } -float Sensor::getAngleY() +double Sensor::getAngleY() { return _accel_y; } -float Sensor::getGyroX() +double Sensor::getGyroX() { return _gyro_raw_x; } -float Sensor::getGyroY() +double Sensor::getGyroY() { return _gyro_raw_y; } -float Sensor::getGyroZ() +double Sensor::getGyroZ() { return _gyro_raw_z; } -float Sensor::getPitch() +double Sensor::getPitch() { return _pitch; } -float Sensor::getRoll() +double Sensor::getRoll() { return _roll; } bool Sensor::setRegister(uint8_t reg, uint8_t value) { - _wire->beginTransmission(_address); - _wire->write(reg); - _wire->write(value); + _wire.beginTransmission(_address); + _wire.write(reg); + _wire.write(value); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0) { _status = SensorStatus::ERR_WRITE; return false; @@ -324,23 +308,23 @@ bool Sensor::setRegister(uint8_t reg, uint8_t value) uint8_t Sensor::getRegister(uint8_t reg) { - _wire->beginTransmission(_address); - _wire->write(reg); + _wire.beginTransmission(_address); + _wire.write(reg); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0U) { _status = SensorStatus::ERR_WRITE; - return 0; + return 0U; } - uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1); - if (response_length != 1) + uint8_t response_length = _wire.requestFrom(_address, 1U); + if (response_length != 1U) { _status = SensorStatus::ERR_READ; - return 0; + return 0U; } - return _wire->read(); + return _wire.read(); } SensorStatus Sensor::getStatus() @@ -353,8 +337,10 @@ SensorStatus Sensor::getStatus() int16_t Sensor::_readHighLow() { - int16_t high = _wire->read(); - int16_t low = _wire->read(); + const int16_t high = _wire.read(); + const int16_t low = _wire.read(); + + const int8_t bits_in_byte = 8; - return high << 8 | low; + return high << bits_in_byte | low; } -- cgit v1.2.3-18-g5258