forked from ArduPilot/ardupilot
/
RCOutput_PCA9685.cpp
274 lines (223 loc) · 7.26 KB
/
RCOutput_PCA9685.cpp
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
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
#include "RCOutput_PCA9685.h"
#include <cmath>
#include <dirent.h>
#include <fcntl.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include "GPIO.h"
#define PCA9685_RA_MODE1 0x00
#define PCA9685_RA_MODE2 0x01
#define PCA9685_RA_LED0_ON_L 0x06
#define PCA9685_RA_LED0_ON_H 0x07
#define PCA9685_RA_LED0_OFF_L 0x08
#define PCA9685_RA_LED0_OFF_H 0x09
#define PCA9685_RA_ALL_LED_ON_L 0xFA
#define PCA9685_RA_ALL_LED_ON_H 0xFB
#define PCA9685_RA_ALL_LED_OFF_L 0xFC
#define PCA9685_RA_ALL_LED_OFF_H 0xFD
#define PCA9685_RA_PRE_SCALE 0xFE
#define PCA9685_MODE1_RESTART_BIT (1 << 7)
#define PCA9685_MODE1_EXTCLK_BIT (1 << 6)
#define PCA9685_MODE1_AI_BIT (1 << 5)
#define PCA9685_MODE1_SLEEP_BIT (1 << 4)
#define PCA9685_MODE1_SUB1_BIT (1 << 3)
#define PCA9685_MODE1_SUB2_BIT (1 << 2)
#define PCA9685_MODE1_SUB3_BIT (1 << 1)
#define PCA9685_MODE1_ALLCALL_BIT (1 << 0)
#define PCA9685_ALL_LED_OFF_H_SHUT (1 << 4)
#define PCA9685_MODE2_INVRT_BIT (1 << 4)
#define PCA9685_MODE2_OCH_BIT (1 << 3)
#define PCA9685_MODE2_OUTDRV_BIT (1 << 2)
#define PCA9685_MODE2_OUTNE1_BIT (1 << 1)
#define PCA9685_MODE2_OUTNE0_BIT (1 << 0)
/*
* Drift for internal oscillator
* see: https://github.com/ArduPilot/ardupilot/commit/50459bdca0b5a1adf95
* and https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11
*/
#define PCA9685_INTERNAL_CLOCK (1.04f * 25000000.f)
using namespace Linux;
#define PWM_CHAN_COUNT 16
extern const AP_HAL::HAL& hal;
RCOutput_PCA9685::RCOutput_PCA9685(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
uint32_t external_clock,
uint8_t channel_offset,
int16_t oe_pin_number) :
_dev(std::move(dev)),
_enable_pin(nullptr),
_frequency(50),
_pulses_buffer(new uint16_t[PWM_CHAN_COUNT - channel_offset]),
_external_clock(external_clock),
_channel_offset(channel_offset),
_oe_pin_number(oe_pin_number)
{
if (_external_clock > 0) {
_osc_clock = _external_clock;
} else {
_osc_clock = PCA9685_INTERNAL_CLOCK;
}
}
RCOutput_PCA9685::~RCOutput_PCA9685()
{
delete [] _pulses_buffer;
}
void RCOutput_PCA9685::init()
{
reset_all_channels();
/* Set the initial frequency */
set_freq(0, 50);
/* Enable PCA9685 PWM */
if (_oe_pin_number != -1) {
_enable_pin = hal.gpio->channel(_oe_pin_number);
_enable_pin->mode(HAL_GPIO_OUTPUT);
_enable_pin->write(0);
}
}
void RCOutput_PCA9685::reset_all_channels()
{
if (!_dev || !_dev->get_semaphore()->take(10)) {
return;
}
uint8_t data[] = {PCA9685_RA_ALL_LED_ON_L, 0, 0, 0, 0};
_dev->transfer(data, sizeof(data), nullptr, 0);
/* Wait for the last pulse to end */
hal.scheduler->delay(2);
_dev->get_semaphore()->give();
}
void RCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
{
/* Correctly finish last pulses */
for (int i = 0; i < (PWM_CHAN_COUNT - _channel_offset); i++) {
write(i, _pulses_buffer[i]);
}
if (!_dev || !_dev->get_semaphore()->take(10)) {
return;
}
/* Shutdown before sleeping.
* see p.14 of PCA9685 product datasheet
*/
_dev->write_register(PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
/* Put PCA9685 to sleep (required to write prescaler) */
_dev->write_register(PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
/* Calculate prescale and save frequency using this value: it may be
* different from @freq_hz due to rounding/ceiling. We use ceil() rather
* than round() so the resulting frequency is never greater than @freq_hz
*/
uint8_t prescale = ceilf(_osc_clock / (4096 * freq_hz)) - 1;
_frequency = _osc_clock / (4096 * (prescale + 1));
/* Write prescale value to match frequency */
_dev->write_register(PCA9685_RA_PRE_SCALE, prescale);
if (_external_clock) {
/* Enable external clocking */
_dev->write_register(PCA9685_RA_MODE1,
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT);
}
/* Restart the device to apply new settings and enable auto-incremented write */
_dev->write_register(PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_dev->get_semaphore()->give();
}
uint16_t RCOutput_PCA9685::get_freq(uint8_t ch)
{
return _frequency;
}
void RCOutput_PCA9685::enable_ch(uint8_t ch)
{
}
void RCOutput_PCA9685::disable_ch(uint8_t ch)
{
write(ch, 0);
}
bool RCOutput_PCA9685::force_safety_on() {
if (!_dev || !_dev->get_semaphore()->take(10)) {
return false;
}
/* Shutdown before sleeping. */
_dev->write_register(PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
/* Put PCA9685 to sleep */
_dev->write_register(PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
_dev->get_semaphore()->give();
return true;
}
void RCOutput_PCA9685::force_safety_off() {
if (!_dev || !_dev->get_semaphore()->take(10)) {
return;
}
/* Restart the device and enable auto-incremented write */
_dev->write_register(PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_dev->get_semaphore()->give();
}
void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
{
if (ch >= (PWM_CHAN_COUNT - _channel_offset)) {
return;
}
_pulses_buffer[ch] = period_us;
_pending_write_mask |= (1U << ch);
if (!_corking) {
_corking = true;
push();
}
}
void RCOutput_PCA9685::cork()
{
_corking = true;
}
void RCOutput_PCA9685::push()
{
if (!_corking) {
return;
}
_corking = false;
if (_pending_write_mask == 0)
return;
// Calculate the number of channels for this transfer.
uint8_t max_ch = (sizeof(unsigned) * 8) - __builtin_clz(_pending_write_mask);
uint8_t min_ch = __builtin_ctz(_pending_write_mask);
/*
* scratch buffer size is always for all the channels, but we write only
* from min_ch to max_ch
*/
struct PACKED pwm_values {
uint8_t reg;
uint8_t data[PWM_CHAN_COUNT * 4];
} pwm_values;
for (unsigned ch = min_ch; ch < max_ch; ch++) {
uint16_t period_us = _pulses_buffer[ch];
uint16_t length = 0;
if (period_us) {
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
}
uint8_t *d = &pwm_values.data[(ch - min_ch) * 4];
*d++ = 0;
*d++ = 0;
*d++ = length & 0xFF;
*d++ = length >> 8;
}
if (!_dev || !_dev->get_semaphore()->take_nonblocking()) {
return;
}
pwm_values.reg = PCA9685_RA_LED0_ON_L + 4 * (_channel_offset + min_ch);
/* reg + all the channels we are going to write */
size_t payload_size = 1 + (max_ch - min_ch) * 4;
_dev->transfer((uint8_t *)&pwm_values, payload_size, nullptr, 0);
_dev->get_semaphore()->give();
_pending_write_mask = 0;
}
uint16_t RCOutput_PCA9685::read(uint8_t ch)
{
return _pulses_buffer[ch];
}
void RCOutput_PCA9685::read(uint16_t* period_us, uint8_t len)
{
for (int i = 0; i < len; i++) {
period_us[i] = read(0 + i);
}
}