|
@@ -73,9 +73,16 @@ uint16_t MarlinHAL::adc_result;
|
73
|
73
|
esp_adc_cal_characteristics_t characteristics[ADC_ATTEN_MAX];
|
74
|
74
|
adc_atten_t attenuations[ADC1_CHANNEL_MAX] = {};
|
75
|
75
|
uint32_t thresholds[ADC_ATTEN_MAX];
|
76
|
|
-volatile int numPWMUsed = 0,
|
77
|
|
- pwmPins[MAX_PWM_PINS],
|
78
|
|
- pwmValues[MAX_PWM_PINS];
|
|
76
|
+
|
|
77
|
+volatile int numPWMUsed = 0;
|
|
78
|
+volatile struct { pin_t pin; int value; } pwmState[MAX_PWM_PINS];
|
|
79
|
+
|
|
80
|
+pin_t chan_pin[CHANNEL_MAX_NUM + 1] = { 0 }; // PWM capable IOpins - not 0 or >33 on ESP32
|
|
81
|
+
|
|
82
|
+struct {
|
|
83
|
+ uint32_t freq; // ledcReadFreq doesn't work if a duty hasn't been set yet!
|
|
84
|
+ uint16_t res;
|
|
85
|
+} pwmInfo[(CHANNEL_MAX_NUM + 1) / 2];
|
79
|
86
|
|
80
|
87
|
// ------------------------
|
81
|
88
|
// Public functions
|
|
@@ -254,25 +261,81 @@ void MarlinHAL::adc_start(const pin_t pin) {
|
254
|
261
|
adc1_set_attenuation(chan, atten);
|
255
|
262
|
}
|
256
|
263
|
|
257
|
|
-void analogWrite(pin_t pin, int value) {
|
258
|
|
- // Use ledc hardware for internal pins
|
259
|
|
- if (pin < 34) {
|
260
|
|
- static int cnt_channel = 1, pin_to_channel[40] = { 0 };
|
261
|
|
- if (pin_to_channel[pin] == 0) {
|
262
|
|
- ledcAttachPin(pin, cnt_channel);
|
263
|
|
- ledcSetup(cnt_channel, 490, 8);
|
264
|
|
- ledcWrite(cnt_channel, value);
|
265
|
|
- pin_to_channel[pin] = cnt_channel++;
|
|
264
|
+// ------------------------
|
|
265
|
+// PWM
|
|
266
|
+// ------------------------
|
|
267
|
+
|
|
268
|
+int8_t channel_for_pin(const uint8_t pin) {
|
|
269
|
+ for (int i = 0; i <= CHANNEL_MAX_NUM; i++)
|
|
270
|
+ if (chan_pin[i] == pin) return i;
|
|
271
|
+ return -1;
|
|
272
|
+}
|
|
273
|
+
|
|
274
|
+// get PWM channel for pin - if none then attach a new one
|
|
275
|
+// return -1 if fail or invalid pin#, channel # (0-15) if success
|
|
276
|
+int8_t get_pwm_channel(const pin_t pin, const uint32_t freq, const uint16_t res) {
|
|
277
|
+ if (!WITHIN(pin, 1, MAX_PWM_IOPIN)) return -1; // Not a hardware PWM pin!
|
|
278
|
+ int8_t cid = channel_for_pin(pin);
|
|
279
|
+ if (cid >= 0) return cid;
|
|
280
|
+
|
|
281
|
+ // Find an empty adjacent channel (same timer & freq/res)
|
|
282
|
+ for (int i = 0; i <= CHANNEL_MAX_NUM; i++) {
|
|
283
|
+ if (chan_pin[i] == 0) {
|
|
284
|
+ if (chan_pin[i ^ 0x1] != 0) {
|
|
285
|
+ if (pwmInfo[i / 2].freq == freq && pwmInfo[i / 2].res == res) {
|
|
286
|
+ chan_pin[i] = pin; // Allocate PWM to this channel
|
|
287
|
+ ledcAttachPin(pin, i);
|
|
288
|
+ return i;
|
|
289
|
+ }
|
|
290
|
+ }
|
|
291
|
+ else if (cid == -1) // Pair of empty channels?
|
|
292
|
+ cid = i & 0xFE; // Save lower channel number
|
266
|
293
|
}
|
267
|
|
- ledcWrite(pin_to_channel[pin], value);
|
|
294
|
+ }
|
|
295
|
+ // not attached, is an empty timer slot avail?
|
|
296
|
+ if (cid >= 0) {
|
|
297
|
+ chan_pin[cid] = pin;
|
|
298
|
+ pwmInfo[cid / 2].freq = freq;
|
|
299
|
+ pwmInfo[cid / 2].res = res;
|
|
300
|
+ ledcSetup(cid, freq, res);
|
|
301
|
+ ledcAttachPin(pin, cid);
|
|
302
|
+ }
|
|
303
|
+ return cid; // -1 if no channel avail
|
|
304
|
+}
|
|
305
|
+
|
|
306
|
+void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=_BV(PWM_RESOLUTION)-1*/, const bool invert/*=false*/) {
|
|
307
|
+ const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION);
|
|
308
|
+ if (cid >= 0) {
|
|
309
|
+ uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1);
|
|
310
|
+ ledcWrite(cid, duty);
|
|
311
|
+ }
|
|
312
|
+}
|
|
313
|
+
|
|
314
|
+int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) {
|
|
315
|
+ const int8_t cid = channel_for_pin(pin);
|
|
316
|
+ if (cid >= 0) {
|
|
317
|
+ if (f_desired == ledcReadFreq(cid)) return cid; // no freq change
|
|
318
|
+ ledcDetachPin(chan_pin[cid]);
|
|
319
|
+ chan_pin[cid] = 0; // remove old freq channel
|
|
320
|
+ }
|
|
321
|
+ return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one
|
|
322
|
+}
|
|
323
|
+
|
|
324
|
+// use hardware PWM if avail, if not then ISR
|
|
325
|
+void analogWrite(const pin_t pin, const uint16_t value, const uint32_t freq/*=PWM_FREQUENCY*/, const uint16_t res/*=8*/) { // always 8 bit resolution!
|
|
326
|
+ // Use ledc hardware for internal pins
|
|
327
|
+ const int8_t cid = get_pwm_channel(pin, freq, res);
|
|
328
|
+ if (cid >= 0) {
|
|
329
|
+ ledcWrite(cid, value); // set duty value
|
268
|
330
|
return;
|
269
|
331
|
}
|
270
|
332
|
|
|
333
|
+ // not a hardware PWM pin OR no PWM channels available
|
271
|
334
|
int idx = -1;
|
272
|
335
|
|
273
|
336
|
// Search Pin
|
274
|
337
|
for (int i = 0; i < numPWMUsed; ++i)
|
275
|
|
- if (pwmPins[i] == pin) { idx = i; break; }
|
|
338
|
+ if (pwmState[i].pin == pin) { idx = i; break; }
|
276
|
339
|
|
277
|
340
|
// not found ?
|
278
|
341
|
if (idx < 0) {
|
|
@@ -281,7 +344,7 @@ void analogWrite(pin_t pin, int value) {
|
281
|
344
|
|
282
|
345
|
// Take new slot for pin
|
283
|
346
|
idx = numPWMUsed;
|
284
|
|
- pwmPins[idx] = pin;
|
|
347
|
+ pwmState[idx].pin = pin;
|
285
|
348
|
// Start timer on first use
|
286
|
349
|
if (idx == 0) HAL_timer_start(MF_TIMER_PWM, PWM_TIMER_FREQUENCY);
|
287
|
350
|
|
|
@@ -289,7 +352,7 @@ void analogWrite(pin_t pin, int value) {
|
289
|
352
|
}
|
290
|
353
|
|
291
|
354
|
// Use 7bit internal value - add 1 to have 100% high at 255
|
292
|
|
- pwmValues[idx] = (value + 1) / 2;
|
|
355
|
+ pwmState[idx].value = (value + 1) / 2;
|
293
|
356
|
}
|
294
|
357
|
|
295
|
358
|
// Handle PWM timer interrupt
|
|
@@ -300,9 +363,9 @@ HAL_PWM_TIMER_ISR() {
|
300
|
363
|
|
301
|
364
|
for (int i = 0; i < numPWMUsed; ++i) {
|
302
|
365
|
if (count == 0) // Start of interval
|
303
|
|
- WRITE(pwmPins[i], pwmValues[i] ? HIGH : LOW);
|
304
|
|
- else if (pwmValues[i] == count) // End of duration
|
305
|
|
- WRITE(pwmPins[i], LOW);
|
|
366
|
+ digitalWrite(pwmState[i].pin, pwmState[i].value ? HIGH : LOW);
|
|
367
|
+ else if (pwmState[i].value == count) // End of duration
|
|
368
|
+ digitalWrite(pwmState[i].pin, LOW);
|
306
|
369
|
}
|
307
|
370
|
|
308
|
371
|
// 128 for 7 Bit resolution
|