Skip to content

Commit 3672051

Browse files
author
Richard Unger
committed
inline current sensing for rp2040, still testing
1 parent 1361c6e commit 3672051

File tree

4 files changed

+374
-9
lines changed

4 files changed

+374
-9
lines changed

‎src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp

Lines changed: 268 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,268 @@
1+
2+
#if defined(TARGET_RP2040)
3+
4+
5+
#include "../../hardware_api.h"
6+
#include "./rp2040_mcu.h"
7+
#include "../../../drivers/hardware_specific/rp2040_mcu.h"
8+
#include "communication/SimpleFOCDebug.h"
9+
10+
#include "hardware/dma.h"
11+
#include "hardware/irq.h"
12+
#include "hardware/pwm.h"
13+
14+
15+
/* Singleton instance of the ADC engine */
16+
RP2040ADCEngine engine;
17+
18+
alignas(32) const uint32_t trigger_value = ADC_CS_START_ONCE_BITS; // start once
19+
20+
/* Hardware API implementation */
21+
22+
float _readADCVoltageInline(const int pinA, const void* cs_params) {
23+
// not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
24+
// return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
25+
// like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
26+
// new ADC conversions, which probably won't improve the accuracy.
27+
28+
if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
29+
return engine.lastResults[pinA-26]*engine.adc_conv;
30+
}
31+
32+
// otherwise return NaN
33+
return NAN;
34+
};
35+
36+
37+
void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) {
38+
if( _isset(pinA) )
39+
engine.addPin(pinA);
40+
if( _isset(pinB) )
41+
engine.addPin(pinB);
42+
if( _isset(pinC) )
43+
engine.addPin(pinC);
44+
engine.init(); // TODO this has to happen later if we want to support more than one motor...
45+
engine.start();
46+
return &engine;
47+
};
48+
49+
50+
void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC) {
51+
if( _isset(pinA) )
52+
engine.addPin(pinA);
53+
if( _isset(pinB) )
54+
engine.addPin(pinB);
55+
if( _isset(pinC) )
56+
engine.addPin(pinC);
57+
engine.setPWMTrigger(((RP2040DriverParams*)driver_params)->slice[0]);
58+
engine.init();
59+
engine.start();
60+
return &engine;
61+
};
62+
63+
64+
void _startADC3PinConversionLowSide() {
65+
// what is this for?
66+
};
67+
68+
69+
float _readADCVoltageLowSide(const int pinA, const void* cs_params) {
70+
// not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
71+
// return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
72+
// like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
73+
// new ADC conversions, which probably won't improve the accuracy.
74+
75+
if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
76+
return engine.lastResults[pinA-26]*engine.adc_conv;
77+
}
78+
79+
// otherwise return NaN
80+
return NAN;
81+
};
82+
83+
84+
void _driverSyncLowSide(void* driver_params, void* cs_params) {
85+
// nothing to do
86+
};
87+
88+
89+
90+
volatile int rp2040_intcount = 0;
91+
92+
void _adcConversionFinishedHandler() {
93+
// conversion of all channels finished. copy results.
94+
volatile uint8_t* from = engine.samples;
95+
if (engine.channelsEnabled[0])
96+
engine.lastResults[0] = (*from++);
97+
if (engine.channelsEnabled[1])
98+
engine.lastResults[1] = (*from++);
99+
if (engine.channelsEnabled[2])
100+
engine.lastResults[2] = (*from++);
101+
if (engine.channelsEnabled[3])
102+
engine.lastResults[3] = (*from++);
103+
// TODO clear interrupt? dma_hw->ints0 = 1u << channel;
104+
//irq_clear(DMA_IRQ_0);
105+
//dma_channel_acknowledge_irq0(engine.copyDMAChannel);
106+
// dma_start_channel_mask( (1u << engine.readDMAChannel) | (1u << engine.copyDMAChannel) );
107+
dma_hw->ints0 = 1u << engine.readDMAChannel;
108+
//dma_start_channel_mask( (1u << engine.readDMAChannel) );
109+
dma_channel_set_write_addr(engine.readDMAChannel, engine.samples, true);
110+
if (engine.triggerPWMSlice>=0)
111+
dma_channel_set_trans_count(engine.triggerDMAChannel, 1, true);
112+
rp2040_intcount++;
113+
};
114+
115+
116+
117+
/* ADC engine implementation */
118+
119+
120+
RP2040ADCEngine::RP2040ADCEngine() {
121+
channelsEnabled[0] = false;
122+
channelsEnabled[1] = false;
123+
channelsEnabled[2] = false;
124+
channelsEnabled[3] = false;
125+
initialized = false;
126+
};
127+
128+
129+
130+
void RP2040ADCEngine::addPin(int pin){
131+
if (pin>=26 && pin<=29)
132+
channelsEnabled[pin-26] = true;
133+
else
134+
SIMPLEFOC_DEBUG("RP2040-CUR: ERR: Not an ADC pin: ", pin);
135+
};
136+
137+
138+
139+
void RP2040ADCEngine::setPWMTrigger(uint slice){
140+
triggerPWMSlice = slice;
141+
};
142+
143+
144+
145+
146+
bool RP2040ADCEngine::init(){
147+
if (initialized)
148+
return true;
149+
150+
adc_init();
151+
int enableMask = 0x00;
152+
int channelCount = 0;
153+
for (int i = 3; i>=0; i--) {
154+
if (channelsEnabled[i]){
155+
adc_gpio_init(i+26);
156+
enableMask |= 0x01;
157+
channelCount++;
158+
}
159+
enableMask = (enableMask<<1);
160+
}
161+
adc_set_round_robin(enableMask);
162+
adc_fifo_setup(
163+
true, // Write each completed conversion to the sample FIFO
164+
true, // Enable DMA data request (DREQ)
165+
channelCount, // DREQ (and IRQ) asserted when all samples present
166+
false, // We won't see the ERR bit because of 8 bit reads; disable.
167+
true // Shift each sample to 8 bits when pushing to FIFO
168+
);
169+
samples_per_second = 20000;
170+
if (samples_per_second<1 || samples_per_second>=500000) {
171+
samples_per_second = 0;
172+
adc_set_clkdiv(0);
173+
}
174+
else
175+
adc_set_clkdiv(48000000/samples_per_second);
176+
SIMPLEFOC_DEBUG("RP2040-CUR: ADC init");
177+
178+
readDMAChannel = dma_claim_unused_channel(true);
179+
dma_channel_config cc1 = dma_channel_get_default_config(readDMAChannel);
180+
channel_config_set_transfer_data_size(&cc1, DMA_SIZE_8);
181+
channel_config_set_read_increment(&cc1, false);
182+
channel_config_set_write_increment(&cc1, true);
183+
channel_config_set_dreq(&cc1, DREQ_ADC);
184+
channel_config_set_irq_quiet(&cc1, false);
185+
dma_channel_configure(readDMAChannel,
186+
&cc1,
187+
samples, // dest
188+
&adc_hw->fifo, // source
189+
channelCount, // count
190+
false // defer start
191+
);
192+
dma_channel_set_irq0_enabled(readDMAChannel, true);
193+
irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
194+
195+
// copyDMAChannel = dma_claim_unused_channel(true);
196+
// dma_channel_config cc2 = dma_channel_get_default_config(copyDMAChannel);
197+
// channel_config_set_transfer_data_size(&cc2, DMA_SIZE_32);
198+
// channel_config_set_read_increment(&cc2, false);
199+
// channel_config_set_write_increment(&cc2, false);
200+
// channel_config_set_chain_to(&cc2, readDMAChannel);
201+
// channel_config_set_irq_quiet(&cc2, false);
202+
// dma_channel_configure(copyDMAChannel,
203+
// &cc2,
204+
// nextResults, // dest
205+
// samples, // source
206+
// 1, // count
207+
// false // defer start
208+
// );
209+
// dma_channel_set_irq0_enabled(copyDMAChannel, true);
210+
// irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
211+
SIMPLEFOC_DEBUG("RP2040-CUR: DMA init");
212+
213+
if (triggerPWMSlice>=0) { // if we have a trigger
214+
triggerDMAChannel = dma_claim_unused_channel(true);
215+
dma_channel_config cc3 = dma_channel_get_default_config(triggerDMAChannel);
216+
channel_config_set_transfer_data_size(&cc3, DMA_SIZE_32);
217+
channel_config_set_read_increment(&cc3, false);
218+
channel_config_set_write_increment(&cc3, false);
219+
channel_config_set_irq_quiet(&cc3, true);
220+
channel_config_set_dreq(&cc3, DREQ_PWM_WRAP0+triggerPWMSlice); //pwm_get_dreq(triggerPWMSlice));
221+
pwm_set_irq_enabled(triggerPWMSlice, true);
222+
dma_channel_configure(triggerDMAChannel,
223+
&cc3,
224+
hw_set_alias_untyped(&adc_hw->cs), // dest
225+
&trigger_value, // source
226+
1, // count
227+
true // defer start
228+
);
229+
SIMPLEFOC_DEBUG("RP2040-CUR: PWM trigger init slice ", triggerPWMSlice);
230+
}
231+
232+
initialized = true;
233+
return initialized;
234+
};
235+
236+
237+
238+
239+
void RP2040ADCEngine::start(){
240+
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine starting");
241+
irq_set_enabled(DMA_IRQ_0, true);
242+
dma_start_channel_mask( (1u << readDMAChannel) ); // | (1u << copyDMAChannel));
243+
for (int i=0;i<4;i++) {
244+
if (channelsEnabled[i]) {
245+
adc_select_input(i); // set input to first enabled channel
246+
break;
247+
}
248+
}
249+
if (triggerPWMSlice>=0) {
250+
dma_start_channel_mask( (1u << triggerDMAChannel) );
251+
//hw_set_bits(&adc_hw->cs, trigger_value);
252+
}
253+
else
254+
adc_run(true);
255+
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine started");
256+
};
257+
258+
void RP2040ADCEngine::stop(){
259+
adc_run(false);
260+
dma_channel_abort(readDMAChannel);
261+
if (triggerPWMSlice>=0)
262+
dma_channel_abort(triggerDMAChannel);
263+
adc_fifo_drain();
264+
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine stopped");
265+
};
266+
267+
268+
#endif

‎src/current_sense/hardware_specific/rp2040/rp2040_mcu.h

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
2+
3+
#pragma once
4+
5+
/*
6+
* RP2040 ADC features are very weak :-(
7+
* - only 4 inputs
8+
* - only 9 bit effective resolution
9+
* - read only 1 input at a time
10+
* - 2 microseconds conversion time!
11+
* - no triggers from PWM / events, only DMA
12+
*
13+
* So to read 3 phases takes 6 microseconds. :-(
14+
*
15+
* The RP2040 ADC engine takes over the control of the MCU's ADC. Parallel ADC access is not permitted, as this would
16+
* cause conflicts with the engine's DMA based access and cause crashes.
17+
* To use the other ADC channels, use them via this engine. Use addPin() to add them to the conversion, and getLastResult()
18+
* to retrieve their value at any time.
19+
*
20+
* For motor current sensing, the engine supports both inline sensing and low-side sensing.
21+
*
22+
* Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz.
23+
* After starting the engine it will continuously sample and provide new values at the configured rate.
24+
*
25+
* Low-side sensing is supported by configuring a trigger from the PWM signal. The trigger happens at the middle-point of the
26+
* up/down counting PWM, which is the mid-point of the on-period.
27+
* So in the case of low-side sensing, all ADC channels are converted at the rate of the PWM frequency.
28+
*
29+
* The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current
30+
* sensing to work correctly, all PWM slices have to be set to the same PWM frequency.
31+
* In theory, two motors could be sensed using 2 shunts on each motor. In practice, due to the slow conversion rate of the RP2040's
32+
* ADC, this would mean 8us conversion time, which would have to fit in the low-side on-time even at low duty-cycles... It remains
33+
* to be seen how well this can work, or if it works at all, but presumably the PWM frequency would have to be quite low.
34+
*
35+
* TODO we need the mid-point of the low-side, which is actually the beginning/end of the PWM cycle - hmmmm...
36+
*
37+
* Note that if using other ADC channels along with the motor current sensing, those channels will be subject to the same conversion schedule as the motor's ADC channels, i.e. convert at the same fixed rate in case
38+
* of inline sensing, or based on the motor PWM in case of PWM-triggered low-side sensing.
39+
*
40+
* Solution to trigger ADC conversion from PWM via DMA:
41+
* use the PWM wrap as a DREQ to a DMA channel, and have the DMA channel write to the ADC's CS register to trigger an ADC sample.
42+
* Solution for ADC conversion:
43+
* ADC converts all channels in round-robin mode, and writes to FIFO. FIFO is emptied by a DMA which triggers after N conversions,
44+
* where N is the number of ADC channels used. So this DMA copies all the values from one round-robin conversion. This first DMA
45+
* triggers a second DMA which does a 32bit copy of all converted values (up to 4 channels x 8bit) at once, and triggers an interrupt.
46+
* The interrupt routine copies the values to the output buffer.
47+
*
48+
* TODO think about whether the second DMA is needed
49+
*/
50+
51+
52+
#define SIMPLEFOC_RP2040_ADC_RESOLUTION 256
53+
#ifndef SIMPLEFOC_RP2040_ADC_VDDA
54+
#define SIMPLEFOC_RP2040_ADC_VDDA 3.3f
55+
#endif
56+
57+
class RP2040ADCEngine {
58+
59+
public:
60+
RP2040ADCEngine();
61+
void addPin(int pin);
62+
void setPWMTrigger(uint slice);
63+
64+
bool init();
65+
void start();
66+
void stop();
67+
68+
void getLastResult();
69+
70+
void handleADCUpdate();
71+
72+
int samples_per_second = 0; // leave at 0 to convert in tight loop
73+
float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float
74+
75+
int triggerPWMSlice = -1;
76+
bool initialized;
77+
uint readDMAChannel;
78+
//uint copyDMAChannel;
79+
uint triggerDMAChannel;
80+
81+
bool channelsEnabled[4];
82+
volatile uint8_t samples[4];
83+
volatile uint8_t lastResults[4];
84+
//alignas(32) volatile uint8_t nextResults[4];
85+
};

‎src/drivers/hardware_specific/rp2040_mcu.cpp

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
#define SIMPLEFOC_DEBUG_RP2040
88

99
#include "../hardware_api.h"
10-
10+
#include "./rp2040_mcu.h"
1111

1212
// these defines determine the polarity of the PWM output. Normally, the polarity is active-high,
1313
// i.e. a high-level PWM output is expected to switch on the MOSFET. But should your driver design
@@ -33,14 +33,6 @@
3333
#define _PWM_FREQUENCY_MIN 5000
3434

3535

36-
typedef struct RP2040DriverParams {
37-
int pins[6];
38-
uint slice[6];
39-
uint chan[6];
40-
long pwm_frequency;
41-
float dead_zone;
42-
} RP2040DriverParams;
43-
4436

4537
// until I can figure out if this can be quickly read from some register, keep it here.
4638
// it also serves as a marker for what slices are already used.

0 commit comments

Comments
 (0)