1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
|
// Communicating with a PCA9685 pwm device via linux i2c
//
// Copyright (C) 2017 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <fcntl.h> // open
#include <linux/i2c-dev.h> // I2C_SLAVE
#include <stdio.h> // snprintf
#include <sys/ioctl.h> // ioctl
#include <unistd.h> // write
#include "basecmd.h" // oid_alloc
#include "board/misc.h" // timer_from_us
#include "command.h" // DECL_COMMAND
#include "internal.h" // report_errno
#include "sched.h" // DECL_SHUTDOWN
/****************************************************************
* Low-level i2c
****************************************************************/
#define P9_MODE1 0x00
#define P9_PRESCALE 0xfe
#define CHANNEL_MAX 15
#define CHANNEL_ALL 61
#define VALUE_MAX 4096
#define OSC_MHZ 25
#define CLOCK_MHZ timer_from_us(1)
struct i2c_s {
uint8_t bus;
uint8_t addr;
uint32_t cycle_ticks;
int fd;
};
static struct i2c_s devices[16];
static int devices_count;
static void
pca9685_write(int fd, uint8_t channel, uint16_t value)
{
uint8_t full_on = 0x00;
if (value >= VALUE_MAX) {
full_on = 0x10;
value = 0x0000;
} else if (!value) {
value = 0x1000;
}
uint8_t msg[5] = { channel*4 + 0x06, 0x00, full_on, value, value >> 8 };
int ret = write(fd, msg, sizeof(msg));
if (ret < 0) {
report_errno("write value i2c", ret);
try_shutdown("Unable to update PCA9685 value");
}
}
static int
open_i2c(uint8_t bus, uint8_t addr, uint32_t cycle_ticks)
{
// Find existing device (if already opened)
int i;
for (i=0; i<devices_count; i++)
if (devices[i].bus == bus && devices[i].addr == addr) {
if (cycle_ticks != devices[i].cycle_ticks)
shutdown("All PCA9685 channels must have the same cycle_ticks");
return devices[i].fd;
}
// Setup new I2C device
if (devices_count >= ARRAY_SIZE(devices))
shutdown("Too many i2c devices");
char fname[256];
snprintf(fname, sizeof(fname), "/dev/i2c-%d", bus);
int fd = open(fname, O_RDWR|O_CLOEXEC);
if (fd < 0) {
report_errno("open i2c", fd);
shutdown("Unable to open i2c device");
}
int ret = ioctl(fd, I2C_SLAVE, addr);
if (ret < 0) {
report_errno("ioctl i2c", fd);
shutdown("Unable to set address on i2c device");
}
ret = set_non_blocking(fd);
if (ret < 0)
shutdown("Unable to set non-blocking on i2c device");
// Init PCA9685
const uint8_t sleep_msg[2] = { P9_MODE1, 0x31 };
ret = write(fd, sleep_msg, sizeof(sleep_msg));
if (ret < 0) {
report_errno("write sleep i2c", ret);
shutdown("Unable to sleep PCA9685");
}
uint32_t freq = DIV_ROUND_CLOSEST(OSC_MHZ*cycle_ticks, 4096*CLOCK_MHZ) - 1;
freq = freq > 0xff ? 0xff : (freq < 0x03 ? 0x03 : freq);
uint8_t freq_msg[2] = { P9_PRESCALE, freq };
ret = write(fd, freq_msg, sizeof(freq_msg));
if (ret < 0) {
report_errno("write freq i2c", ret);
shutdown("Unable to set freq on PCA9685");
}
const uint8_t wake_msg[2] = { P9_MODE1, 0x21 };
ret = write(fd, wake_msg, sizeof(wake_msg));
if (ret < 0) {
report_errno("write unsleep i2c", ret);
shutdown("Unable to wake PCA9685");
}
usleep(500);
pca9685_write(fd, CHANNEL_ALL, 0);
devices[devices_count].bus = bus;
devices[devices_count].addr = addr;
devices[devices_count].cycle_ticks = cycle_ticks;
devices[devices_count].fd = fd;
devices_count++;
return fd;
}
/****************************************************************
* Command interface
****************************************************************/
struct i2cpwm_s {
struct timer timer;
int fd;
uint8_t channel;
uint16_t value, default_value;
uint32_t max_duration;
};
DECL_CONSTANT(PCA9685_MAX, VALUE_MAX);
static uint_fast8_t
pca9685_end_event(struct timer *timer)
{
shutdown("Missed scheduling of next pca9685 event");
}
static uint_fast8_t
pca9685_event(struct timer *timer)
{
struct i2cpwm_s *p = container_of(timer, struct i2cpwm_s, timer);
pca9685_write(p->fd, p->channel, p->value);
if (p->value == p->default_value || !p->max_duration)
return SF_DONE;
p->timer.waketime += p->max_duration;
p->timer.func = pca9685_end_event;
return SF_RESCHEDULE;
}
void
command_config_pca9685(uint32_t *args)
{
uint8_t bus = args[1], addr = args[2], channel = args[3];
uint16_t value = args[5], default_value = args[6];
if (channel > CHANNEL_MAX || value > VALUE_MAX || default_value > VALUE_MAX)
shutdown("Invalid pca9685 channel or value");
int fd = open_i2c(bus, addr, args[4]);
if (value)
pca9685_write(fd, channel, value);
struct i2cpwm_s *p = oid_alloc(args[0], command_config_pca9685, sizeof(*p));
p->fd = fd;
p->channel = channel;
p->default_value = default_value;
p->max_duration = args[7];
}
DECL_COMMAND(command_config_pca9685, "config_pca9685 oid=%c bus=%c addr=%c"
" channel=%c cycle_ticks=%u value=%hu"
" default_value=%hu max_duration=%u");
void
command_schedule_pca9685_out(uint32_t *args)
{
struct i2cpwm_s *p = oid_lookup(args[0], command_config_pca9685);
sched_del_timer(&p->timer);
p->timer.func = pca9685_event;
p->timer.waketime = args[1];
p->value = args[2];
if (p->value > VALUE_MAX)
shutdown("Invalid pca9685 value");
sched_add_timer(&p->timer);
}
DECL_COMMAND(command_schedule_pca9685_out,
"schedule_pca9685_out oid=%c clock=%u value=%hu");
void
command_set_pca9685_out(uint32_t *args)
{
uint8_t bus = args[0], addr = args[1], channel = args[2];
uint16_t value = args[4];
if (channel > CHANNEL_MAX || value > VALUE_MAX)
shutdown("Invalid pca9685 channel or value");
int fd = open_i2c(bus, addr, args[3]);
pca9685_write(fd, channel, value);
}
DECL_COMMAND(command_set_pca9685_out, "set_pca9685_out bus=%c addr=%c"
" channel=%c cycle_ticks=%u value=%hu");
void
pca9685_shutdown(void)
{
int i;
for (i=0; i<devices_count; i++)
pca9685_write(devices[i].fd, CHANNEL_ALL, 0);
uint8_t j;
struct i2cpwm_s *p;
foreach_oid(j, p, command_config_pca9685) {
if (p->default_value)
pca9685_write(p->fd, p->channel, p->default_value);
}
}
DECL_SHUTDOWN(pca9685_shutdown);
|
