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