summaryrefslogtreecommitdiff
path: root/src/sensor
diff options
context:
space:
mode:
Diffstat (limited to 'src/sensor')
-rw-r--r--src/sensor/calibration.cpp107
-rw-r--r--src/sensor/calibration.hpp73
-rw-r--r--src/sensor/sensor.cpp28
-rw-r--r--src/sensor/sensor.hpp27
4 files changed, 206 insertions, 29 deletions
diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp
new file mode 100644
index 0000000..40bb52b
--- /dev/null
+++ b/src/sensor/calibration.cpp
@@ -0,0 +1,107 @@
+#include "calibration.hpp"
+
+#include "std/time.hpp"
+#include "utils.hpp"
+
+SensorCalibrator::SensorCalibrator(UniquePtr<Sensor> &sensor, SerialStream sout)
+ : _sensor(sensor), _sout(sout)
+{
+ _resetValues();
+}
+
+bool SensorCalibrator::calibrate(unsigned int throttle_time)
+{
+ bool done = false;
+ Time start_time = time_now();
+
+ while (!done)
+ {
+ if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
+ {
+ return false;
+ }
+
+ delay(throttle_time);
+
+ _resetValues();
+
+ for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++)
+ {
+ _updateValues();
+ }
+
+ _adjustValues();
+
+ _sout << "Accel X: " << _accel_x << " "
+ << "Accel Y: " << _accel_y << " "
+ << "Accel Z: " << _accel_z << " "
+ << "Gyro X: " << _gyro_x << " "
+ << "Gyro Y: " << _gyro_y << " "
+ << "Gyro Z: " << _gyro_z << endl;
+
+ if (_isValuesInRange())
+ {
+ done = true;
+ }
+
+ _adjustCalibration();
+ }
+
+ return true;
+}
+
+void SensorCalibrator::_resetValues()
+{
+ _accel_x = 0;
+ _accel_y = 0;
+ _accel_z = 0;
+
+ _gyro_x = 0;
+ _gyro_y = 0;
+ _gyro_z = 0;
+}
+
+void SensorCalibrator::_updateValues()
+{
+ _sensor->read();
+
+ _accel_x -= _sensor->getAccelX();
+ _accel_y -= _sensor->getAccelY();
+ _accel_z -= _sensor->getAccelZ();
+
+ _gyro_x -= _sensor->getGyroX();
+ _gyro_y -= _sensor->getGyroY();
+ _gyro_z -= _sensor->getGyroZ();
+}
+
+void SensorCalibrator::_adjustValues()
+{
+ _accel_x *= SENSOR_VAL_ADJUST;
+ _accel_y *= SENSOR_VAL_ADJUST;
+ _accel_z *= SENSOR_VAL_ADJUST;
+
+ _gyro_x *= SENSOR_VAL_ADJUST;
+ _gyro_y *= SENSOR_VAL_ADJUST;
+ _gyro_z *= SENSOR_VAL_ADJUST;
+}
+
+bool SensorCalibrator::_isValuesInRange() const
+{
+ return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN &&
+ _accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN &&
+ _accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN &&
+ _gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN &&
+ _gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN &&
+ _gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN);
+}
+
+void SensorCalibrator::_adjustCalibration()
+{
+ _sensor->accel_cal_x += _accel_x;
+ _sensor->accel_cal_y += _accel_y;
+ _sensor->accel_cal_z += _accel_z;
+
+ _sensor->gyro_cal_x += _gyro_x;
+ _sensor->gyro_cal_y += _gyro_y;
+ _sensor->gyro_cal_z += _gyro_z;
+}
diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp
new file mode 100644
index 0000000..03e2e9c
--- /dev/null
+++ b/src/sensor/calibration.hpp
@@ -0,0 +1,73 @@
+#pragma once
+
+#include "sensor/sensor.hpp"
+#include "serial.hpp"
+#include "std/memory.hpp"
+
+// Calibration precision
+constexpr float ACCEL_CAL_X_MAX = 0.006;
+constexpr float ACCEL_CAL_X_MIN = -0.006;
+
+constexpr float ACCEL_CAL_Y_MAX = 0.006;
+constexpr float ACCEL_CAL_Y_MIN = -0.006;
+
+constexpr float ACCEL_CAL_Z_MAX = 0.006;
+constexpr float ACCEL_CAL_Z_MIN = -0.006;
+
+constexpr float GYRO_CAL_X_MAX = 0.06;
+constexpr float GYRO_CAL_X_MIN = -0.06;
+
+constexpr float GYRO_CAL_Y_MAX = 0.06;
+constexpr float GYRO_CAL_Y_MIN = -0.06;
+
+constexpr float GYRO_CAL_Z_MAX = 0.06;
+constexpr float GYRO_CAL_Z_MIN = -0.06;
+
+constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds
+
+constexpr unsigned int SENSOR_READ_CNT = 20;
+constexpr float SENSOR_VAL_ADJUST = 0.05;
+
+/**
+ * Sensor calibrator.
+ */
+class SensorCalibrator
+{
+public:
+ /**
+ * Sensor calibrator.
+ *
+ * @param sensor A sensor to calibrate
+ * @param sout A Serial output stream
+ */
+ SensorCalibrator(UniquePtr<Sensor> &sensor, SerialStream sout);
+
+ /**
+ * Calibrates the sensor.
+ *
+ * @param throttle_time The sensor's throttle time
+ * @returns Whether or not the calibration succeeded. Will return false on
+ * timeout.
+ */
+ bool calibrate(unsigned int throttle_time);
+
+private:
+ void _resetValues();
+ void _updateValues();
+ void _adjustValues();
+ bool _isValuesInRange() const;
+
+ void _adjustCalibration();
+
+ UniquePtr<Sensor> &_sensor;
+
+ SerialStream _sout;
+
+ double _accel_x = 0;
+ double _accel_y = 0;
+ double _accel_z = 0;
+
+ double _gyro_x = 0;
+ double _gyro_y = 0;
+ double _gyro_z = 0;
+};
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
index a3b9a5c..5ca5dc3 100644
--- a/src/sensor/sensor.cpp
+++ b/src/sensor/sensor.cpp
@@ -1,20 +1,18 @@
#include "sensor.hpp"
-#include "registers.hpp"
-#include "utils/serial.hpp"
-#include "utils/time.hpp"
+#include "sensor/registers.hpp"
Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout,
unsigned int throttle_time)
: _wire(wire),
+ _sout(sout),
_address(address),
_throttle_enabled(true),
_throttle_time(throttle_time),
_last_time(0),
_status(SensorStatus::OK),
_accel_to_g_force(RAW_TO_G_FACTOR),
- _ang_rate_to_dps(RAW_TO_DPS_FACTOR),
- _sout(sout)
+ _ang_rate_to_dps(RAW_TO_DPS_FACTOR)
{
}
@@ -241,52 +239,52 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
return true;
}
-double Sensor::getAccelX()
+double Sensor::getAccelX() const
{
return _accel_raw_x;
}
-double Sensor::getAccelY()
+double Sensor::getAccelY() const
{
return _accel_raw_y;
}
-double Sensor::getAccelZ()
+double Sensor::getAccelZ() const
{
return _accel_raw_z;
}
-double Sensor::getAngleX()
+double Sensor::getAngleX() const
{
return _accel_x;
}
-double Sensor::getAngleY()
+double Sensor::getAngleY() const
{
return _accel_y;
}
-double Sensor::getGyroX()
+double Sensor::getGyroX() const
{
return _gyro_raw_x;
}
-double Sensor::getGyroY()
+double Sensor::getGyroY() const
{
return _gyro_raw_y;
}
-double Sensor::getGyroZ()
+double Sensor::getGyroZ() const
{
return _gyro_raw_z;
}
-double Sensor::getPitch()
+double Sensor::getPitch() const
{
return _pitch;
}
-double Sensor::getRoll()
+double Sensor::getRoll() const
{
return _roll;
}
diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp
index 3e56627..7b76ae2 100644
--- a/src/sensor/sensor.hpp
+++ b/src/sensor/sensor.hpp
@@ -1,7 +1,7 @@
#pragma once
-#include "utils/serial.hpp"
-#include "utils/time.hpp"
+#include "serial.hpp"
+#include "std/time.hpp"
#include <Arduino.h>
#include <Wire.h>
@@ -77,52 +77,52 @@ public:
/**
* Returns the current X axis acceleration in g:s (g-force).
*/
- double getAccelX();
+ double getAccelX() const;
/**
* Returns the current Y axis acceleration in g:s (g-force).
*/
- double getAccelY();
+ double getAccelY() const;
/**
* Returns the current Z axis acceleration in g:s (g-force).
*/
- double getAccelZ();
+ double getAccelZ() const;
/**
* Returns the current X angle.
*/
- double getAngleX();
+ double getAngleX() const;
/**
* Returns the current Y angle.
*/
- double getAngleY();
+ double getAngleY() const;
/**
* Returns the current X axis in degrees/s.
*/
- double getGyroX();
+ double getGyroX() const;
/**
* Returns the current Y axis in degrees/s.
*/
- double getGyroY();
+ double getGyroY() const;
/**
* Returns the current Z axis in degrees/s.
*/
- double getGyroZ();
+ double getGyroZ() const;
/**
* Returns the current pitch angle.
*/
- double getPitch();
+ double getPitch() const;
/**
* Returns the current roll angle.
*/
- double getRoll();
+ double getRoll() const;
/**
* Sets the value of a key in the sensor's register.
@@ -157,6 +157,7 @@ public:
private:
TwoWire _wire;
+ SerialStream _sout;
uint8_t _address;
@@ -193,6 +194,4 @@ private:
double _yaw = 0;
int16_t _readHighLow();
-
- SerialStream _sout;
};
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-22 05:42:31 +0000