summaryrefslogtreecommitdiff
path: root/src/sensor
diff options
context:
space:
mode:
Diffstat (limited to 'src/sensor')
-rw-r--r--src/sensor/calibration.cpp41
-rw-r--r--src/sensor/calibration.hpp9
-rw-r--r--src/sensor/sensor.cpp53
-rw-r--r--src/sensor/sensor.hpp5
4 files changed, 33 insertions, 75 deletions
diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp
index 1a2b91e..5626388 100644
--- a/src/sensor/calibration.cpp
+++ b/src/sensor/calibration.cpp
@@ -3,7 +3,8 @@
#include "common/time.hpp"
#include "utils.hpp"
-SensorCalibrator::SensorCalibrator(Sensor &sensor, SerialStream sout)
+SensorCalibrator::SensorCalibrator(common::SharedPtr<Sensor> sensor,
+ common::SharedPtr<SerialStream> sout)
: _sensor(sensor), _sout(sout)
{
}
@@ -31,12 +32,12 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time)
_adjustValues(values);
- _sout << "Accel X: " << values.accel_x << " "
- << "Accel Y: " << values.accel_y << " "
- << "Accel Z: " << values.accel_z << " "
- << "Gyro X: " << values.gyro_x << " "
- << "Gyro Y: " << values.gyro_y << " "
- << "Gyro Z: " << values.gyro_z << endl;
+ *_sout << "Accel X: " << values.accel_x << " "
+ << "Accel Y: " << values.accel_y << " "
+ << "Accel Z: " << values.accel_z << " "
+ << "Gyro X: " << values.gyro_x << " "
+ << "Gyro Y: " << values.gyro_y << " "
+ << "Gyro Z: " << values.gyro_z << endl;
if (_isValuesInRange(values))
{
@@ -51,26 +52,26 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time)
void SensorCalibrator::_updateValues(SensorCalibratorValues &values)
{
- _sensor.read();
+ _sensor->read();
- values.accel_x -= _sensor.getAccelX();
- values.accel_y -= _sensor.getAccelY();
- values.accel_z -= _sensor.getAccelZ();
+ values.accel_x -= _sensor->getAccelX();
+ values.accel_y -= _sensor->getAccelY();
+ values.accel_z -= _sensor->getAccelZ();
- values.gyro_x -= _sensor.getGyroX();
- values.gyro_y -= _sensor.getGyroY();
- values.gyro_z -= _sensor.getGyroZ();
+ values.gyro_x -= _sensor->getGyroX();
+ values.gyro_y -= _sensor->getGyroY();
+ values.gyro_z -= _sensor->getGyroZ();
}
void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values)
{
- _sensor.accel_cal_x += values.accel_x;
- _sensor.accel_cal_y += values.accel_y;
- _sensor.accel_cal_z += values.accel_z;
+ _sensor->accel_cal_x += values.accel_x;
+ _sensor->accel_cal_y += values.accel_y;
+ _sensor->accel_cal_z += values.accel_z;
- _sensor.gyro_cal_x += values.gyro_x;
- _sensor.gyro_cal_y += values.gyro_y;
- _sensor.gyro_cal_z += values.gyro_z;
+ _sensor->gyro_cal_x += values.gyro_x;
+ _sensor->gyro_cal_y += values.gyro_y;
+ _sensor->gyro_cal_z += values.gyro_z;
}
void SensorCalibrator::_adjustValues(SensorCalibratorValues &values)
diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp
index 17f8fa9..c85d336 100644
--- a/src/sensor/calibration.hpp
+++ b/src/sensor/calibration.hpp
@@ -1,5 +1,6 @@
#pragma once
+#include "common/memory/shared_ptr.hpp"
#include "sensor/sensor.hpp"
#include "serial.hpp"
@@ -51,7 +52,8 @@ public:
* @param sensor A sensor to calibrate
* @param sout A Serial output stream
*/
- SensorCalibrator(Sensor &sensor, SerialStream sout);
+ SensorCalibrator(common::SharedPtr<Sensor> sensor,
+ common::SharedPtr<SerialStream> sout);
/**
* Calibrates the sensor.
@@ -69,7 +71,6 @@ private:
static void _adjustValues(SensorCalibratorValues &values);
static bool _isValuesInRange(const SensorCalibratorValues &values);
- Sensor &_sensor;
-
- SerialStream _sout;
+ common::SharedPtr<Sensor> _sensor;
+ common::SharedPtr<SerialStream> _sout;
};
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);
}
diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp
index 1e7e488..1b99f6e 100644
--- a/src/sensor/sensor.hpp
+++ b/src/sensor/sensor.hpp
@@ -1,7 +1,6 @@
#pragma once
#include "common/time.hpp"
-#include "serial.hpp"
#include <Arduino.h>
#include <Wire.h>
@@ -33,10 +32,9 @@ public:
*
* @param address The address of the sensor
* @param wire A Wire instance
- * @param sout A serial output stream
* @param throttle_time A minumum time between sensor reads for the sensor to throttle
*/
- Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time);
+ Sensor(uint8_t address, TwoWire wire, unsigned int throttle_time) noexcept;
/**
* Initializes communication with the sensor.
@@ -157,7 +155,6 @@ public:
private:
TwoWire _wire;
- SerialStream _sout;
uint8_t _address;
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-22 07:10:46 +0000