From ee076c66c99fbd7895459e80d679c374340a9ed2 Mon Sep 17 00:00:00 2001
From: HampusM <hampus@hampusmat.com>
Date: Mon, 7 Mar 2022 11:24:01 +0100
Subject: refactor: make misc improvements & update calibration precision
---
src/sensor/calibration.hpp | 30 +++++++++++++++---------------
src/sensor/sensor.cpp | 28 +++++++++++-----------------
src/sensor/sensor.hpp | 14 +++++++-------
3 files changed, 33 insertions(+), 39 deletions(-)
(limited to 'src/sensor')
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;
--
cgit v1.2.3-18-g5258