diff --git a/Peripherals/Inc/peripherals_adc.hpp b/Peripherals/Inc/peripherals_adc.hpp
index 985e0b2..294c5f1 100644
--- a/Peripherals/Inc/peripherals_adc.hpp
+++ b/Peripherals/Inc/peripherals_adc.hpp
@@ -25,7 +25,6 @@ struct Adc
   static constexpr std::uint32_t readingsCount() { return N; }
 
   Adc() = delete;
-
   explicit Adc(ADC_HandleTypeDef* hadc) : hadc(hadc) {}
 
   Adc(const Adc&) = delete;
@@ -33,7 +32,7 @@ struct Adc
   Adc& operator=(const Adc&) = delete;
   Adc& operator=(Adc&&) = delete;
 
-  std::uint16_t read(const std::uint8_t channel, std::uint32_t sampling_time)
+  std::uint16_t read(const std::uint32_t channel, const std::uint32_t sampling_time, const std::uint32_t timeout = 100)
   {
     ADC_ChannelConfTypeDef conf;
 
@@ -44,7 +43,7 @@ struct Adc
     HAL_ADC_ConfigChannel(hadc, &conf);
 
     HAL_ADC_Start(hadc);
-    HAL_ADC_PollForConversion(hadc, 100);
+    HAL_ADC_PollForConversion(hadc, timeout);
 
     return HAL_ADC_GetValue(hadc);
   }
diff --git a/Peripherals/Inc/peripherals_hal_st.hpp b/Peripherals/Inc/peripherals_hal_st.hpp
index c7bb4b4..4599e6f 100644
--- a/Peripherals/Inc/peripherals_hal_st.hpp
+++ b/Peripherals/Inc/peripherals_hal_st.hpp
@@ -70,8 +70,8 @@ struct StaticHal
 
 struct Gpio
 {
-  GPIO_TypeDef* port;
-  std::uint16_t pin;
+  GPIO_TypeDef* port = nullptr;
+  std::uint16_t pin = 0;
 
   Gpio() = delete;
   explicit Gpio(GPIO_TypeDef* port, const std::uint16_t pin)
@@ -87,6 +87,9 @@ struct Gpio
   bool Read() const { return HAL_GPIO_ReadPin(port, pin); }
 };
 
+#define CREATE_GPIO(name) \
+  Peripherals::Hal::St::Gpio { name##_GPIO_Port, name##_Pin }
+
 #endif// HAL_GPIO_MODULE_ENABLED
 
 template<
diff --git a/Peripherals/Inc/peripherals_imu_icm.hpp b/Peripherals/Inc/peripherals_imu_icm.hpp
index 5ea19e4..ea57abe 100644
--- a/Peripherals/Inc/peripherals_imu_icm.hpp
+++ b/Peripherals/Inc/peripherals_imu_icm.hpp
@@ -222,6 +222,11 @@ public:
     return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
   }
 
+  std::uint8_t readProductId()
+  {
+    return registers.ReadRegister(_Registers::WHO_AM_I | _Registers::READ_MASK);
+  }
+
 private:
   GyroScale _scale_gyro = GyroScale::DPS_2000;
   AccelerometerScale _scale_accel = AccelerometerScale::G16;
diff --git a/Peripherals/Inc/peripherals_ir_sensors.hpp b/Peripherals/Inc/peripherals_ir_sensors.hpp
new file mode 100644
index 0000000..6b2f794
--- /dev/null
+++ b/Peripherals/Inc/peripherals_ir_sensors.hpp
@@ -0,0 +1,122 @@
+/*
+ * peripherals_ir_sensors.hpp
+ *
+ *  Created on: Apr 10, 2021
+ *      Author: erki
+ */
+
+#ifndef PERIPHERALS_IR_SENSORS_HPP_
+#define PERIPHERALS_IR_SENSORS_HPP_
+
+#include <cmath>
+#include <initializer_list>
+#include <tuple>
+
+#include <peripherals_adc.hpp>
+
+namespace Peripherals
+{
+
+template<typename G, std::size_t N, std::size_t Average>
+class IrSensors
+{
+public:
+  using gpio = G;
+
+  Adc<N> adc;
+
+  IrSensors() = delete;
+
+  template<typename... Args>
+  explicit IrSensors(ADC_HandleTypeDef* hadc,
+                     const std::array<std::uint32_t, N>& channels,
+                     Args&&... gpios)
+      : adc(hadc), _channels(channels), _gpios{std::forward<Args>(gpios)...}
+  {
+    static_assert(sizeof...(Args) == N, "Not enough GPIOs passed.");
+  }
+
+  void startReading()
+  {
+    for (auto& gpio : _gpios)
+      gpio.Set(true);
+
+    adc.startDma();
+  }
+
+  void stopReading()
+  {
+    for (auto& gpio : _gpios)
+      gpio.Set(false);
+
+    adc.stopDma();
+  }
+
+  void updateReadings()
+  {
+    for (std::size_t i = 0; i < N; i++)
+    {
+      _averages[i] -= _averages[i] / Average;
+      _averages[i] += adc.readings[i] / Average;
+    }
+  }
+
+  std::array<std::pair<float, bool>, N> getDistanceData()
+  {
+    std::array<std::pair<float, bool>, N> data;
+
+    for (std::size_t i = 0; i < N; i++)
+    {
+      const float interim = _averages[i] - _offsets[i];
+
+      data[i].first = _multipliers[i] * std::pow(interim, _exponents[i]);
+      data[i].second = data[i].first < _wall_thresholds[i];
+    }
+
+    return data;
+  }
+
+  void calibrate(const std::uint32_t samples, const std::uint32_t sample_time)
+  {
+    std::array<std::uint32_t, N> measurements;
+
+    for (std::uint32_t i = 0; i < samples; i++)
+    {
+      for (std::uint32_t ch = 0; ch < _channels.size(); ch++)
+        measurements[ch] += adc.read(_channels[ch], sample_time, 100);
+    }
+
+    for (std::uint32_t ch = 0; ch < _channels.size(); ch++)
+      _offsets[ch] = std::uint16_t(measurements[ch] / samples);
+  }
+
+  void setExponents(const std::array<float, N>& e)
+  {
+    _exponents = e;
+  }
+
+  void setMultipliers(const std::array<float, N>& m)
+  {
+    _multipliers = m;
+  }
+
+  void setWallThresholds(const std::array<float, N>& t)
+  {
+    _wall_thresholds = t;
+  }
+
+private:
+  std::array<std::uint32_t, N> _channels;
+  std::array<gpio, N> _gpios;
+  std::array<std::uint16_t, N> _averages;
+
+  std::array<std::uint16_t, N> _offsets;
+  std::array<float, N> _exponents;
+  std::array<float, N> _multipliers;
+  std::array<float, N> _wall_thresholds;
+};
+
+}// namespace Peripherals
+
+
+#endif /* PERIPHERALS_IR_SENSORS_HPP_ */