diff options
Diffstat (limited to 'src/sensor.cpp')
| -rw-r--r-- | src/sensor.cpp | 325 |
1 files changed, 0 insertions, 325 deletions
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; -} |