123 lines
2.5 KiB
C++
123 lines
2.5 KiB
C++
/*
|
|
* 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_ */
|