132 lines
2.7 KiB
C++
132 lines
2.7 KiB
C++
/*
|
|
* peripherals_ir_sensors.hpp
|
|
*
|
|
* Created on: Apr 10, 2021
|
|
* Author: erki
|
|
*/
|
|
|
|
#ifndef SKULLC_PERIPHERALS_IR_SENSORS_HPP_
|
|
#define SKULLC_PERIPHERALS_IR_SENSORS_HPP_
|
|
|
|
#include <cmath>
|
|
#include <initializer_list>
|
|
#include <tuple>
|
|
|
|
#include <peripherals_adc.hpp>
|
|
|
|
namespace Peripherals
|
|
{
|
|
|
|
struct IrSensorsReading
|
|
{
|
|
/// Distance as read by the sensor.
|
|
float distance;
|
|
|
|
/// @c true if the @c distance is < the wall threshold.
|
|
bool sees_object;
|
|
};
|
|
|
|
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<IrSensorsReading, N> getDistanceData()
|
|
{
|
|
std::array<IrSensorsReading, N> data;
|
|
|
|
for (std::size_t i = 0; i < N; i++)
|
|
{
|
|
const float interim = averages_[i] - offsets_[i];
|
|
|
|
data[i].distance = multipliers_[i] * std::pow(interim, exponents_[i]);
|
|
data[i].sees_object = data[i].distance < 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 /* SKULLC_PERIPHERALS_IR_SENSORS_HPP_ */
|