nodding_lidar
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
pca9685.hpp
[詳解]
1
// PCA9685 (I2C 16ch PWM) の簡易ドライバ。Linuxの /dev/i2c-N を直接使う (追加ライブラリ不要)。
2
// 動作確認はしていません。実機で、I2Cのバス番号とアドレスを確認してください
3
// (確認: `sudo i2cdetect -y -r <バス番号>`、i2c-tools が必要)。
4
#pragma once
5
6
#include <fcntl.h>
7
#include <linux/i2c-dev.h>
8
#include <sys/ioctl.h>
9
#include <unistd.h>
10
11
#include <algorithm>
12
#include <chrono>
13
#include <cmath>
14
#include <cstdint>
15
#include <stdexcept>
16
#include <string>
17
#include <thread>
18
19
namespace
nodding_lidar
20
{
21
22
class
Pca9685
23
{
24
public
:
25
// bus: /dev/i2c-<bus>, address: 7bitアドレス (通常 0x40)
26
// osc_hz: 内部発振器の周波数 (25MHzが公称。実測でずれる場合は調整する)
27
Pca9685
(
int
bus,
int
address,
double
pwm_freq_hz,
double
osc_hz = 25.0e6)
28
: freq_hz_(pwm_freq_hz)
29
{
30
const
std::string dev =
"/dev/i2c-"
+ std::to_string(bus);
31
fd_ = ::open(dev.c_str(), O_RDWR);
32
if
(fd_ < 0) {
33
throw
std::runtime_error(
"cannot open "
+ dev +
" (権限とバス番号を確認してください)"
);
34
}
35
if
(::ioctl(fd_, I2C_SLAVE, address) < 0) {
36
::close(fd_);
37
fd_ = -1;
38
throw
std::runtime_error(
"cannot set I2C slave address"
);
39
}
40
41
// PWM周波数の設定は、スリープ中にしか書き込めない
42
write_reg(kMode1, 0x10);
// SLEEP
43
double
prescale = std::round(osc_hz / (4096.0 * pwm_freq_hz)) - 1.0;
44
prescale = std::clamp(prescale, 3.0, 255.0);
45
write_reg(kPrescale,
static_cast<
uint8_t
>
(prescale));
46
write_reg(kMode2, 0x04);
// OUTDRV (トーテムポール出力)
47
write_reg(kMode1, 0x00);
// スリープ解除
48
std::this_thread::sleep_for(std::chrono::milliseconds(5));
49
write_reg(kMode1, 0xA0);
// RESTART + AI (レジスタ自動インクリメント)
50
}
51
52
~Pca9685
()
53
{
54
if
(fd_ >= 0) {
55
::close(fd_);
56
}
57
}
58
59
Pca9685
(
const
Pca9685
&) =
delete
;
60
Pca9685
&
operator=
(
const
Pca9685
&) =
delete
;
61
62
// channel(0-15) のパルス幅を us で設定する
63
void
set_pulse_us
(
int
channel,
double
pulse_us)
64
{
65
if
(channel < 0 || channel > 15) {
66
throw
std::runtime_error(
"channel must be 0-15"
);
67
}
68
const
double
period_us = 1.0e6 / freq_hz_;
69
double
ticks = std::round(pulse_us / period_us * 4096.0);
70
ticks = std::clamp(ticks, 0.0, 4095.0);
71
const
auto
off =
static_cast<
uint16_t
>
(ticks);
72
73
const
uint8_t reg =
static_cast<
uint8_t
>
(kLed0OnL + 4 * channel);
74
const
uint8_t buf[5] = {
75
reg, 0x00, 0x00,
static_cast<
uint8_t
>
(off & 0xFF),
static_cast<
uint8_t
>
(off >> 8)};
76
if
(::write(fd_, buf,
sizeof
(buf)) !=
static_cast<
ssize_t
>
(
sizeof
(buf))) {
77
throw
std::runtime_error(
"I2C write failed"
);
78
}
79
}
80
81
private
:
82
static
constexpr
uint8_t kMode1 = 0x00;
83
static
constexpr
uint8_t kMode2 = 0x01;
84
static
constexpr
uint8_t kLed0OnL = 0x06;
85
static
constexpr
uint8_t kPrescale = 0xFE;
86
87
void
write_reg(uint8_t reg, uint8_t value)
88
{
89
const
uint8_t buf[2] = {reg, value};
90
if
(::write(fd_, buf,
sizeof
(buf)) !=
static_cast<
ssize_t
>
(
sizeof
(buf))) {
91
throw
std::runtime_error(
"I2C write failed"
);
92
}
93
}
94
95
int
fd_{-1};
96
double
freq_hz_;
97
};
98
99
}
// namespace nodding_lidar
nodding_lidar::Pca9685::Pca9685
Pca9685(int bus, int address, double pwm_freq_hz, double osc_hz=25.0e6)
定義
pca9685.hpp:27
nodding_lidar::Pca9685::~Pca9685
~Pca9685()
定義
pca9685.hpp:52
nodding_lidar::Pca9685::set_pulse_us
void set_pulse_us(int channel, double pulse_us)
定義
pca9685.hpp:63
nodding_lidar::Pca9685::operator=
Pca9685 & operator=(const Pca9685 &)=delete
nodding_lidar::Pca9685::Pca9685
Pca9685(const Pca9685 &)=delete
nodding_lidar
定義
geometry.hpp:17
include
nodding_lidar
pca9685.hpp
構築:
1.18.0