diff options
| -rw-r--r-- | src/gyronardo.cpp | 11 | ||||
| -rw-r--r-- | src/sensor/calibration.hpp | 30 | ||||
| -rw-r--r-- | src/sensor/sensor.cpp | 28 | ||||
| -rw-r--r-- | src/sensor/sensor.hpp | 14 | ||||
| -rw-r--r-- | src/serial.cpp | 2 |
5 files changed, 38 insertions, 47 deletions
diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index d8aaf0e..1c75c80 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -6,16 +6,13 @@ #include <Arduino.h> #include <Wire.h> -// constexpr unsigned int LINE_CLEAR_LENGTH = 30U; - constexpr uint8_t SENSOR_ADDRESS = 0x68U; -constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds - -constexpr unsigned int BAUD_RATE = 9600U; +constexpr uint32_t SENSOR_THROTTLE_TIME = 50U; // milliseconds +constexpr uint32_t SENSOR_RETRY_TIME = 2000U; // milliseconds -constexpr unsigned int SENSOR_RETRY_TIME = 2000U; +constexpr uint32_t BAUD_RATE = 9600U; -SerialStream sout(Serial_(), BAUD_RATE); +auto sout = SerialStream(Serial_(), BAUD_RATE); auto sensor = Sensor(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME); diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp index 585d655..17f8fa9 100644 --- a/src/sensor/calibration.hpp +++ b/src/sensor/calibration.hpp @@ -4,28 +4,28 @@ #include "serial.hpp" // Calibration precision -constexpr float ACCEL_CAL_X_MAX = 0.006; -constexpr float ACCEL_CAL_X_MIN = -0.006; +constexpr double ACCEL_CAL_X_MAX = 0.003; +constexpr double ACCEL_CAL_X_MIN = -0.003; -constexpr float ACCEL_CAL_Y_MAX = 0.006; -constexpr float ACCEL_CAL_Y_MIN = -0.006; +constexpr double ACCEL_CAL_Y_MAX = 0.003; +constexpr double ACCEL_CAL_Y_MIN = -0.003; -constexpr float ACCEL_CAL_Z_MAX = 0.006; -constexpr float ACCEL_CAL_Z_MIN = -0.006; +constexpr double ACCEL_CAL_Z_MAX = 0.003; +constexpr double ACCEL_CAL_Z_MIN = -0.003; -constexpr float GYRO_CAL_X_MAX = 0.06; -constexpr float GYRO_CAL_X_MIN = -0.06; +constexpr double GYRO_CAL_X_MAX = 0.003; +constexpr double GYRO_CAL_X_MIN = -0.003; -constexpr float GYRO_CAL_Y_MAX = 0.06; -constexpr float GYRO_CAL_Y_MIN = -0.06; +constexpr double GYRO_CAL_Y_MAX = 0.05; +constexpr double GYRO_CAL_Y_MIN = -0.05; -constexpr float GYRO_CAL_Z_MAX = 0.06; -constexpr float GYRO_CAL_Z_MIN = -0.06; +constexpr double GYRO_CAL_Z_MAX = 0.04; +constexpr double GYRO_CAL_Z_MIN = -0.04; -constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds +constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // milliseconds -constexpr unsigned int SENSOR_READ_CNT = 20; -constexpr float SENSOR_VAL_ADJUST = 0.05; +constexpr uint32_t SENSOR_READ_CNT = 20; +constexpr double SENSOR_VAL_ADJUST = 0.05; class SensorCalibratorValues { 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; diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp index 5bdf6ca..b79b918 100644 --- a/src/sensor/sensor.hpp +++ b/src/sensor/sensor.hpp @@ -11,7 +11,7 @@ constexpr uint8_t SENSOR_WAKEUP = 0x00U; constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0; constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0; -constexpr unsigned int ONE_EIGHTY = 180; +constexpr uint32_t ONE_EIGHTY = 180; enum class SensorStatus { @@ -146,14 +146,14 @@ public: SensorStatus getStatus() noexcept; // Accelerometer calibration values - double accel_cal_x = 0.0F; - double accel_cal_y = 0.0F; - double accel_cal_z = 0.0F; + double accel_cal_x = 0; + double accel_cal_y = 0; + double accel_cal_z = 0; // Gyroscope calibration values - double gyro_cal_x = 0.0F; - double gyro_cal_y = 0.0F; - double gyro_cal_z = 0.0F; + double gyro_cal_x = 0; + double gyro_cal_y = 0; + double gyro_cal_z = 0; private: TwoWire _wire; diff --git a/src/serial.cpp b/src/serial.cpp index acf92fa..d3b3989 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -22,7 +22,7 @@ SerialStream &SerialStream::operator<<(const SmartString &str) SerialStream &SerialStream::operator<<(const double &num) { - write(doubleToStr(num)->c_str); + write(doubleToStr(num, 3U, 4U)->c_str); return *this; } |