summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/common/memory/shared_ptr.hpp44
-rw-r--r--src/common/memory/shared_ptr.tpp153
-rw-r--r--src/gyronardo.cpp102
-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
7 files changed, 282 insertions, 125 deletions
diff --git a/src/common/memory/shared_ptr.hpp b/src/common/memory/shared_ptr.hpp
new file mode 100644
index 0000000..7e8a910
--- /dev/null
+++ b/src/common/memory/shared_ptr.hpp
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <stddef.h>
+
+namespace common
+{
+template <class Target>
+class SharedPtr
+{
+public:
+ SharedPtr() noexcept;
+ SharedPtr(nullptr_t) noexcept; // NOLINT(google-explicit-constructor)
+
+ explicit SharedPtr(Target *target) noexcept;
+
+ SharedPtr(const SharedPtr &shared_ptr) noexcept;
+
+ SharedPtr(SharedPtr &&shared_ptr) noexcept;
+
+ ~SharedPtr() noexcept;
+
+ [[nodiscard]] unsigned int reference_cnt() const noexcept;
+
+ [[nodiscard]] bool is_disposable() const noexcept;
+
+ SharedPtr &operator=(const SharedPtr &rhs) noexcept;
+
+ SharedPtr &operator=(SharedPtr &&rhs) noexcept;
+
+ Target &operator*() const noexcept;
+ Target *operator->() const noexcept;
+
+private:
+ Target *_target = nullptr;
+
+ unsigned int *_reference_cnt;
+};
+
+template <typename Target, typename... Args>
+SharedPtr<Target> make_shared(Args &&...args) noexcept;
+
+} // namespace common
+
+#include "shared_ptr.tpp"
diff --git a/src/common/memory/shared_ptr.tpp b/src/common/memory/shared_ptr.tpp
new file mode 100644
index 0000000..d9b57c3
--- /dev/null
+++ b/src/common/memory/shared_ptr.tpp
@@ -0,0 +1,153 @@
+#pragma once
+
+#include "shared_ptr.hpp"
+
+#include "common/memory.hpp"
+
+#include <stdlib.h>
+
+namespace common
+{
+
+template <class Target>
+SharedPtr<Target>::SharedPtr() noexcept
+ : _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))
+{
+ (*_reference_cnt) = 0;
+}
+
+template <class Target>
+SharedPtr<Target>::SharedPtr(nullptr_t) noexcept
+ : _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))
+{
+ (*_reference_cnt) = 0;
+}
+
+template <class Target>
+SharedPtr<Target>::SharedPtr(Target *target) noexcept
+ : _target(target), _reference_cnt(malloc_s<unsigned int>(sizeof(unsigned int)))
+{
+ (*_reference_cnt) = 0;
+}
+
+/**
+ * Copy constructor
+ */
+template <class Target>
+SharedPtr<Target>::SharedPtr(const SharedPtr &shared_ptr) noexcept
+ : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt)
+{
+ (*_reference_cnt)++;
+}
+
+/**
+ * Move constructor
+ */
+template <class Target>
+SharedPtr<Target>::SharedPtr(SharedPtr &&shared_ptr) noexcept
+ : _target(shared_ptr._target), _reference_cnt(shared_ptr._reference_cnt)
+{
+ shared_ptr._target = nullptr;
+}
+
+template <class Target>
+SharedPtr<Target>::~SharedPtr() noexcept
+{
+ if ((*_reference_cnt) != 0U)
+ {
+ (*_reference_cnt)--;
+ }
+
+ if ((*_reference_cnt) == 0U)
+ {
+ delete _target;
+ free(_reference_cnt);
+ _reference_cnt = nullptr;
+ }
+}
+
+template <class Target>
+unsigned int SharedPtr<Target>::reference_cnt() const noexcept
+{
+ if (_reference_cnt == nullptr)
+ {
+ return 0;
+ }
+
+ return *_reference_cnt;
+}
+
+template <class Target>
+bool SharedPtr<Target>::is_disposable() const noexcept
+{
+ return _reference_cnt == nullptr || (*_reference_cnt) == 0U;
+}
+
+/**
+ * Copy assignment operator
+ */
+template <class Target>
+SharedPtr<Target> &SharedPtr<Target>::operator=(const SharedPtr &rhs) noexcept
+{
+ if (&rhs != this)
+ {
+ if (is_disposable())
+ {
+ delete _target;
+ free(_reference_cnt);
+ }
+
+ _target = nullptr;
+ _target = rhs._target;
+
+ _reference_cnt = nullptr;
+ _reference_cnt = rhs._reference_cnt;
+ (*_reference_cnt)++;
+ }
+
+ return *this;
+}
+
+/**
+ * Move assignment operator
+ */
+template <class Target>
+SharedPtr<Target> &SharedPtr<Target>::operator=(SharedPtr &&rhs) noexcept
+{
+ if (&rhs != this)
+ {
+ if (is_disposable())
+ {
+ delete _target;
+ free(_reference_cnt);
+ }
+
+ _target = rhs._target;
+ rhs._target = nullptr;
+
+ _reference_cnt = rhs._reference_cnt;
+ rhs._reference_cnt = nullptr;
+ }
+
+ return *this;
+}
+
+template <class Target>
+Target &SharedPtr<Target>::operator*() const noexcept
+{
+ return *(_target);
+}
+
+template <class Target>
+Target *SharedPtr<Target>::operator->() const noexcept
+{
+ return _target;
+}
+
+template <class Target, typename... Args>
+SharedPtr<Target> make_shared(Args &&...args) noexcept
+{
+ return SharedPtr<Target>(new Target(args...));
+}
+
+} // namespace common
diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index d05b3ce..bee13b6 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,100 +1,102 @@
+#include "common/memory/shared_ptr.hpp"
#include "sensor/calibration.hpp"
#include "sensor/sensor.hpp"
#include "serial.hpp"
#include "utils.hpp"
-#include <Arduino.h>
#include <Wire.h>
#include <XInput.h>
constexpr uint8_t SENSOR_ADDRESS = 0x68U;
-constexpr uint32_t SENSOR_THROTTLE_TIME = 50U; // milliseconds
-constexpr uint32_t SENSOR_RETRY_TIME = 2000U; // milliseconds
+constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds
+constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds
-constexpr uint32_t BAUD_RATE = 9600U;
-
-auto sout = SerialStream(Serial_(), BAUD_RATE);
-
-auto sensor = Sensor(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME);
+constexpr unsigned int BAUD_RATE = 9600U;
void setup()
{
- sout.waitReady();
+ auto sout = common::make_shared<SerialStream>(Serial, BAUD_RATE);
+
+ auto sensor = common::make_shared<Sensor>(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME);
- while (!sensor.begin())
+ sout->waitReady();
+
+ while (!sensor->begin())
{
- sout << "Error: Could not connect to the sensor. Retrying after 2000 "
- "milliseconds..."
- << endl;
+ *sout << "Error: Could not connect to the _sensor-> Retrying after 2000 "
+ "milliseconds..."
+ << endl;
delay(SENSOR_RETRY_TIME);
}
- if (!sensor.setAccelSensitivity(2)) // 8g
+ if (!sensor->setAccelSensitivity(2)) // 8g
{
- sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
- << static_cast<int>(sensor.getStatus()) << endl;
+ *sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
+ << static_cast<int>(sensor->getStatus()) << endl;
stop();
}
- if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
+ if (!sensor->setGyroSensitivity(1)) // 500 degrees/s
{
- sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
- << static_cast<int>(sensor.getStatus()) << endl;
+ *sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
+ << static_cast<int>(sensor->getStatus()) << endl;
stop();
}
- sout << "Calibrating sensor..." << endl;
+ *sout << "Calibrating _sensor->.." << endl;
SensorCalibrator sensor_calibrator(sensor, sout);
if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
{
- sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
- << " milliseconds" << endl;
+ *sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
+ << " milliseconds" << endl;
stop();
}
- sout << "Finished calibrating sensor\n";
+ *sout << "Finished calibrating sensor\n";
- sout << "Calibration values:\n"
- << "Accelerometer X: " << sensor.accel_cal_x << "\n"
- << "Accelerometer Y: " << sensor.accel_cal_y << "\n"
- << "Accelerometer Z: " << sensor.accel_cal_z << "\n"
- << "Gyro X: " << sensor.gyro_cal_x << "\n"
- << "Gyro Y: " << sensor.gyro_cal_y << "\n"
- << "Gyro Z: " << sensor.gyro_cal_z << "\n";
+ *sout << "Calibration values:\n"
+ << "Accelerometer X: " << sensor->accel_cal_x << "\n"
+ << "Accelerometer Y: " << sensor->accel_cal_y << "\n"
+ << "Accelerometer Z: " << sensor->accel_cal_z << "\n"
+ << "Gyro X: " << sensor->gyro_cal_x << "\n"
+ << "Gyro Y: " << sensor->gyro_cal_y << "\n"
+ << "Gyro Z: " << sensor->gyro_cal_z << "\n";
- sout << "Starting..." << endl;
+ *sout << "Starting..." << endl;
XInput.begin();
-}
-void loop()
-{
- delay(SENSOR_THROTTLE_TIME);
-
- if (!sensor.read())
+ while (true)
{
- SensorStatus status = sensor.getStatus();
+ delay(SENSOR_THROTTLE_TIME);
- if (status == SensorStatus::THROTTLED)
+ if (!sensor->read())
{
- sout << "Warning: The sensor was read too frequently and throttled" << endl;
- return;
+ SensorStatus status = sensor->getStatus();
+
+ if (status == SensorStatus::THROTTLED)
+ {
+ *sout << "Warning: The sensor was read too frequently and throttled"
+ << endl;
+ continue;
+ }
+
+ *sout << "Error: Failed to read _sensor-> Status: "
+ << static_cast<int>(status) << endl;
+ stop();
}
- sout << "Error: Failed to read sensor. Status: " << static_cast<int>(status)
- << endl;
- stop();
- }
+ *sout << "Pitch: " << sensor->getPitch() << " Roll: " << sensor->getRoll()
+ << endl;
- sout << "Pitch: " << sensor.getPitch() << " Roll: " << sensor.getRoll() << endl;
-
- XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor.getRoll()),
- static_cast<int32_t>(sensor.getPitch()));
- XInput.printDebug(Serial);
+ XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor->getRoll()),
+ static_cast<int32_t>(sensor->getPitch()));
+ XInput.printDebug(Serial);
+ }
}
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 01:16:05 +0000