Skip to content

Commit 0add5b0

Browse files
askuricaskuric
committed
Merge branch 'dev' of github.com:askuric/Arduino-FOC into dev
2 parents c32bdd1 + 54b5e6d commit 0add5b0

File tree

6 files changed

+116
-117
lines changed

6 files changed

+116
-117
lines changed

‎src/SimpleFOC.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@
4545
// BLDCMotor( pole_pairs )
4646
BLDCMotor motor = BLDCMotor(11);
4747
// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) )
48-
BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8);
48+
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
4949
// Encoder(pin_A, pin_B, CPR)
5050
Encoder encoder = Encoder(2, 3, 2048);
5151
// channel A and B callbacks

‎src/common/base_classes/FOCMotor.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ FOCMotor::FOCMotor()
3636
//monitor_port
3737
monitor_port = nullptr;
3838
//sensor
39+
sensor_offset = 0.0f;
3940
sensor = nullptr;
4041
//current sensor
4142
current_sense = nullptr;

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

Lines changed: 86 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,11 @@ alignas(32) const uint32_t trigger_value = ADC_CS_START_ONCE_BITS; // start once
2222
float _readADCVoltageInline(const int pinA, const void* cs_params) {
2323
// not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
2424
// 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
25+
// like this we either have to block interrupts, or of course have the chance of reading across
2626
// new ADC conversions, which probably won't improve the accuracy.
2727

2828
if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
29-
return engine.lastResults[pinA-26]*engine.adc_conv;
29+
return engine.lastResults.raw[pinA-26]*engine.adc_conv;
3030
}
3131

3232
// otherwise return NaN
@@ -47,43 +47,44 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p
4747
};
4848

4949

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-
};
50+
// not supported at the moment
51+
// void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC) {
52+
// if( _isset(pinA) )
53+
// engine.addPin(pinA);
54+
// if( _isset(pinB) )
55+
// engine.addPin(pinB);
56+
// if( _isset(pinC) )
57+
// engine.addPin(pinC);
58+
// engine.setPWMTrigger(((RP2040DriverParams*)driver_params)->slice[0]);
59+
// engine.init();
60+
// engine.start();
61+
// return &engine;
62+
// };
6263

6364

64-
void _startADC3PinConversionLowSide() {
65-
// what is this for?
66-
};
65+
// void _startADC3PinConversionLowSide() {
66+
// // what is this for?
67+
// };
6768

6869

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.
70+
// float _readADCVoltageLowSide(const int pinA, const void* cs_params) {
71+
// // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
72+
// // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
73+
// // like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
74+
// // new ADC conversions, which probably won't improve the accuracy.
7475

75-
if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
76-
return engine.lastResults[pinA-26]*engine.adc_conv;
77-
}
76+
// if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
77+
// return engine.lastResults[pinA-26]*engine.adc_conv;
78+
// }
7879

79-
// otherwise return NaN
80-
return NAN;
81-
};
80+
// // otherwise return NaN
81+
// return NAN;
82+
// };
8283

8384

84-
void _driverSyncLowSide(void* driver_params, void* cs_params) {
85-
// nothing to do
86-
};
85+
// void _driverSyncLowSide(void* driver_params, void* cs_params) {
86+
// // nothing to do
87+
// };
8788

8889

8990

@@ -93,22 +94,19 @@ void _adcConversionFinishedHandler() {
9394
// conversion of all channels finished. copy results.
9495
volatile uint8_t* from = engine.samples;
9596
if (engine.channelsEnabled[0])
96-
engine.lastResults[0] = (*from++);
97+
engine.lastResults.raw[0] = (*from++);
9798
if (engine.channelsEnabled[1])
98-
engine.lastResults[1] = (*from++);
99+
engine.lastResults.raw[1] = (*from++);
99100
if (engine.channelsEnabled[2])
100-
engine.lastResults[2] = (*from++);
101+
engine.lastResults.raw[2] = (*from++);
101102
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) );
103+
engine.lastResults.raw[3] = (*from++);
104+
//dma_channel_acknowledge_irq0(engine.readDMAChannel);
107105
dma_hw->ints0 = 1u << engine.readDMAChannel;
108106
//dma_start_channel_mask( (1u << engine.readDMAChannel) );
109107
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);
108+
// if (engine.triggerPWMSlice>=0)
109+
// dma_channel_set_trans_count(engine.triggerDMAChannel, 1, true);
112110
rp2040_intcount++;
113111
};
114112

@@ -127,7 +125,7 @@ RP2040ADCEngine::RP2040ADCEngine() {
127125

128126

129127

130-
void RP2040ADCEngine::addPin(int pin){
128+
void RP2040ADCEngine::addPin(int pin) {
131129
if (pin>=26 && pin<=29)
132130
channelsEnabled[pin-26] = true;
133131
else
@@ -136,14 +134,14 @@ void RP2040ADCEngine::addPin(int pin){
136134

137135

138136

139-
void RP2040ADCEngine::setPWMTrigger(uint slice){
140-
triggerPWMSlice = slice;
141-
};
137+
// void RP2040ADCEngine::setPWMTrigger(uint slice){
138+
// triggerPWMSlice = slice;
139+
// };
142140

143141

144142

145143

146-
bool RP2040ADCEngine::init(){
144+
bool RP2040ADCEngine::init() {
147145
if (initialized)
148146
return true;
149147

@@ -153,10 +151,9 @@ bool RP2040ADCEngine::init(){
153151
for (int i = 3; i>=0; i--) {
154152
if (channelsEnabled[i]){
155153
adc_gpio_init(i+26);
156-
enableMask |= 0x01;
154+
enableMask |= (0x01<<i);
157155
channelCount++;
158156
}
159-
enableMask = (enableMask<<1);
160157
}
161158
adc_set_round_robin(enableMask);
162159
adc_fifo_setup(
@@ -192,42 +189,26 @@ bool RP2040ADCEngine::init(){
192189
dma_channel_set_irq0_enabled(readDMAChannel, true);
193190
irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
194191

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);
211192
SIMPLEFOC_DEBUG("RP2040-CUR: DMA init");
212193

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-
}
194+
// if (triggerPWMSlice>=0) { // if we have a trigger
195+
// triggerDMAChannel = dma_claim_unused_channel(true);
196+
// dma_channel_config cc3 = dma_channel_get_default_config(triggerDMAChannel);
197+
// channel_config_set_transfer_data_size(&cc3, DMA_SIZE_32);
198+
// channel_config_set_read_increment(&cc3, false);
199+
// channel_config_set_write_increment(&cc3, false);
200+
// channel_config_set_irq_quiet(&cc3, true);
201+
// channel_config_set_dreq(&cc3, DREQ_PWM_WRAP0+triggerPWMSlice); //pwm_get_dreq(triggerPWMSlice));
202+
// pwm_set_irq_enabled(triggerPWMSlice, true);
203+
// dma_channel_configure(triggerDMAChannel,
204+
// &cc3,
205+
// hw_set_alias_untyped(&adc_hw->cs), // dest
206+
// &trigger_value, // source
207+
// 1, // count
208+
// true // defer start
209+
// );
210+
// SIMPLEFOC_DEBUG("RP2040-CUR: PWM trigger init slice ", triggerPWMSlice);
211+
// }
231212

232213
initialized = true;
233214
return initialized;
@@ -236,33 +217,45 @@ bool RP2040ADCEngine::init(){
236217

237218

238219

239-
void RP2040ADCEngine::start(){
220+
void RP2040ADCEngine::start() {
240221
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine starting");
241222
irq_set_enabled(DMA_IRQ_0, true);
242-
dma_start_channel_mask( (1u << readDMAChannel) ); // | (1u << copyDMAChannel));
223+
dma_start_channel_mask( (1u << readDMAChannel) );
243224
for (int i=0;i<4;i++) {
244225
if (channelsEnabled[i]) {
245226
adc_select_input(i); // set input to first enabled channel
246227
break;
247228
}
248229
}
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);
230+
// if (triggerPWMSlice>=0) {
231+
// dma_start_channel_mask( (1u << triggerDMAChannel) );
232+
// //hw_set_bits(&adc_hw->cs, trigger_value);
233+
// }
234+
// else
235+
adc_run(true);
255236
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine started");
256237
};
257238

258-
void RP2040ADCEngine::stop(){
239+
240+
241+
242+
void RP2040ADCEngine::stop() {
259243
adc_run(false);
260244
dma_channel_abort(readDMAChannel);
261-
if (triggerPWMSlice>=0)
262-
dma_channel_abort(triggerDMAChannel);
245+
// if (triggerPWMSlice>=0)
246+
// dma_channel_abort(triggerDMAChannel);
263247
adc_fifo_drain();
264248
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine stopped");
265249
};
266250

267251

252+
253+
ADCResults RP2040ADCEngine::getLastResults() {
254+
ADCResults r;
255+
r.value = lastResults.value;
256+
return r;
257+
};
258+
259+
260+
268261
#endif

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

Lines changed: 26 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -17,35 +17,29 @@
1717
* To use the other ADC channels, use them via this engine. Use addPin() to add them to the conversion, and getLastResult()
1818
* to retrieve their value at any time.
1919
*
20-
* For motor current sensing, the engine supports both inline sensing and low-side sensing.
20+
* For motor current sensing, the engine supports inline sensing only.
2121
*
2222
* Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz.
2323
* After starting the engine it will continuously sample and provide new values at the configured rate.
2424
*
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.
25+
* Low-side sensing is currently not supported.
2826
*
2927
* The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current
3028
* 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...
29+
* In theory, two motors could be sensed using 2 shunts on each motor.
3630
*
3731
* 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.
32+
* of inline sensing.
3933
*
4034
* Solution to trigger ADC conversion from PWM via DMA:
4135
* 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.
36+
* Unfortunately, I could not get this to work, so no low side sensing for the moment.
37+
*
4238
* Solution for ADC conversion:
4339
* 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.
40+
* where N is the number of ADC channels used. So this DMA copies all the values from one round-robin conversion.
41+
*
4742
*
48-
* TODO think about whether the second DMA is needed
4943
*/
5044

5145

@@ -54,32 +48,43 @@
5448
#define SIMPLEFOC_RP2040_ADC_VDDA 3.3f
5549
#endif
5650

51+
52+
union ADCResults {
53+
uint32_t value;
54+
uint8_t raw[4];
55+
struct {
56+
uint8_t ch0;
57+
uint8_t ch1;
58+
uint8_t ch2;
59+
uint8_t ch3;
60+
};
61+
};
62+
63+
5764
class RP2040ADCEngine {
5865

5966
public:
6067
RP2040ADCEngine();
6168
void addPin(int pin);
62-
void setPWMTrigger(uint slice);
69+
//void setPWMTrigger(uint slice);
6370

6471
bool init();
6572
void start();
6673
void stop();
6774

68-
void getLastResult();
69-
70-
void handleADCUpdate();
75+
ADCResults getLastResults(); // TODO find a better API and representation for this
7176

7277
int samples_per_second = 0; // leave at 0 to convert in tight loop
7378
float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float
7479

75-
int triggerPWMSlice = -1;
80+
//int triggerPWMSlice = -1;
7681
bool initialized;
7782
uint readDMAChannel;
7883
//uint copyDMAChannel;
79-
uint triggerDMAChannel;
84+
//uint triggerDMAChannel;
8085

8186
bool channelsEnabled[4];
8287
volatile uint8_t samples[4];
83-
volatile uint8_t lastResults[4];
88+
volatile ADCResults lastResults;
8489
//alignas(32) volatile uint8_t nextResults[4];
8590
};

‎src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "../../hardware_api.h"
22

3-
#if defined(__AVR_ATmega2560__)
3+
#if defined(__AVR_ATmega2560__) || defined(AVR_ATmega1280)
44

55
#define _PWM_FREQUENCY 32000
66
#define _PWM_FREQUENCY_MAX 32000

0 commit comments

Comments
 (0)