diff options
Diffstat (limited to 'src/sensor/sensor.cpp')
| -rw-r--r-- | src/sensor/sensor.cpp | 53 |
1 files changed, 6 insertions, 47 deletions
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index af25a96..a801270 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -2,10 +2,8 @@ #include "sensor/registers.hpp" -Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout, - unsigned int throttle_time) +Sensor::Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept : _wire(wire), - _sout(sout), _address(address), _throttle_enabled(true), _throttle_time(throttle_time), @@ -72,21 +70,11 @@ bool Sensor::read() noexcept _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(); auto duration = now.diff(_last_time).secs(); @@ -97,21 +85,11 @@ bool Sensor::read() noexcept _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 auto accel_y_pow_two = pow(_accel_raw_y, 2); auto accel_z_pow_two = pow(_accel_raw_z, 2); @@ -121,39 +99,20 @@ bool Sensor::read() noexcept _accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI; - _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; _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 x w/o time: " << _gyro_x << "\n" - << "Gyro y w/o time: " << _gyro_y << "\n" - << "Gyro z w/o time: " << _gyro_z << "\n" - << endl; - _pitch = _gyro_y + _accel_y; _roll = _gyro_x + _accel_x; @@ -194,7 +153,7 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) noexcept } // Calculate conversion factor. - _accel_to_g_force = (1 << _accel_sensitivity) * RAW_TO_G_FACTOR; + _accel_to_g_force = static_cast<float>(1 << _accel_sensitivity) * RAW_TO_G_FACTOR; return true; } @@ -229,7 +188,7 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) noexcept } } - _ang_rate_to_dps = (1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; + _ang_rate_to_dps = static_cast<float>(1 << _gyro_sensitivity) * RAW_TO_DPS_FACTOR; return true; } @@ -330,10 +289,10 @@ SensorStatus Sensor::getStatus() noexcept int16_t Sensor::_readHighLow() noexcept { - const int16_t high = static_cast<int16_t>(_wire.read()); - const int16_t low = static_cast<int16_t>(_wire.read()); + const auto high = static_cast<int16_t>(_wire.read()); + const auto low = static_cast<int16_t>(_wire.read()); const int8_t bits_in_byte = 8; - return high << bits_in_byte | low; + return static_cast<int16_t>(high << bits_in_byte | low); } |