65 lines
1.5 KiB
C++
65 lines
1.5 KiB
C++
#include "encoder.hpp"
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <sys/mman.h>
|
|
#include <cmath>
|
|
|
|
Encoder::Encoder(int pwm_channel, int dir_gpio, int direction_polarity)
|
|
: _channel(pwm_channel)
|
|
, _dir_gpio(dir_gpio)
|
|
, _polarity(direction_polarity)
|
|
, _reg_base(nullptr)
|
|
, _mem_fd(-1)
|
|
, _rps(0.0f)
|
|
, _last_period(0)
|
|
{
|
|
_dir_gpio.setDirection("in");
|
|
_mmapRegs();
|
|
}
|
|
|
|
Encoder::~Encoder() { _munmapRegs(); }
|
|
|
|
void Encoder::_mmapRegs()
|
|
{
|
|
_mem_fd = open("/dev/mem", O_RDWR);
|
|
if (_mem_fd < 0) return;
|
|
|
|
_reg_base = (volatile uint32*)mmap(nullptr, 0x10000, PROT_READ | PROT_WRITE,
|
|
MAP_SHARED, _mem_fd, ENCODER_BASE);
|
|
}
|
|
|
|
void Encoder::_munmapRegs()
|
|
{
|
|
if (_reg_base && _reg_base != MAP_FAILED)
|
|
munmap((void*)_reg_base, 0x10000);
|
|
if (_mem_fd >= 0) { close(_mem_fd); _mem_fd = -1; }
|
|
}
|
|
|
|
uint32 Encoder::_readRegister(unsigned int offset) const
|
|
{
|
|
if (!_reg_base || _reg_base == MAP_FAILED) return 0;
|
|
return _reg_base[offset / sizeof(uint32)];
|
|
}
|
|
|
|
int Encoder::getDirection() const
|
|
{
|
|
return _dir_gpio.getValue() * _polarity;
|
|
}
|
|
|
|
void Encoder::update()
|
|
{
|
|
if (!_reg_base || _reg_base == MAP_FAILED) return;
|
|
|
|
uint32 period = _readRegister(REG_OFFSET + _channel * 0x100);
|
|
if (period > 0 && period < 0xFFFFFF)
|
|
{
|
|
float freq = 50000000.0f / period; // 50MHz 基频
|
|
_rps = (freq / 4.0f) * getDirection(); // 4倍频编码器
|
|
_last_period = period;
|
|
}
|
|
else
|
|
{
|
|
_rps *= 0.6f; // 无脉冲时速度衰减
|
|
}
|
|
}
|