diff options
Diffstat (limited to 'src/sensor/sensor.cpp')
| -rw-r--r-- | src/sensor/sensor.cpp | 28 |
1 files changed, 11 insertions, 17 deletions
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index 9f48e7f..4d7f256 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -121,13 +121,9 @@ bool Sensor::read() noexcept _accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / 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; - */ + _sout << "\nAccel x prepared: " << _accel_x << "\n" + << "Accel y prepared: " << _accel_y << "\n" + << endl; // Convert raw Gyro to degrees/s _gyro_raw_x *= _ang_rate_to_dps; @@ -153,16 +149,14 @@ bool Sensor::read() noexcept _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" + _sout << "\nGyro x w/o time: " << _gyro_x << "\n" + << "Gyro y w/o time: " << _gyro_y << "\n" + << "Gyro z w/o time: " << _gyro_z << "\n" << endl; - const float gyro_balance = 0.96F; - const float accel_balance = 0.04F; + _pitch = _gyro_y + _accel_y; + _roll = _gyro_x + _accel_x; - _pitch = gyro_balance * _gyro_y + accel_balance * _accel_y; - _roll = gyro_balance * _gyro_x + accel_balance * _accel_x; _yaw = _gyro_z; return true; @@ -296,7 +290,7 @@ bool Sensor::setRegister(uint8_t reg, uint8_t value) noexcept _wire.write(reg); _wire.write(value); - if (_wire.endTransmission() != 0) + if (_wire.endTransmission() != 0U) { _status = SensorStatus::ERR_WRITE; return false; @@ -336,8 +330,8 @@ SensorStatus Sensor::getStatus() noexcept int16_t Sensor::_readHighLow() noexcept { - const int16_t high = _wire.read(); - const int16_t low = _wire.read(); + const int16_t high = static_cast<int16_t>(_wire.read()); + const int16_t low = static_cast<int16_t>(_wire.read()); const int8_t bits_in_byte = 8; |