Merge pull request #227 from runger1101001/dev

Fix compilation problems on teensy and nrf52

Author: runger1101001

src/BLDCMotor.cpp

@@ -113,8 +113,10 @@ int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
     // added the shaft_angle update
     sensor->update();
     shaft_angle = shaftAngle();
-  }else 
+  }else {
+    exit_flag = 0; // no FOC without sensor
     SIMPLEFOC_DEBUG("MOT: No sensor.");
+  }
 
   // aligning the current sensor - can be skipped
   // checks if driver phases are the same as current sense phases
@@ -201,7 +203,8 @@ int BLDCMotor::alignSensor() {
     setPhaseVoltage(0, 0, 0);
     _delay(200);
     // determine the direction the sensor moved
-    if (mid_angle == end_angle) {
+    float moved =  fabs(mid_angle - end_angle);
+    if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
       SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
       return 0; // failed calibration
     } else if (mid_angle < end_angle) {
@@ -212,7 +215,6 @@ int BLDCMotor::alignSensor() {
       sensor_direction = Direction::CW;
     }
     // check pole pair number
-    float moved =  fabs(mid_angle - end_angle);
     if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
       SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
     } else 

src/common/base_classes/Sensor.cpp

@@ -18,7 +18,8 @@ void Sensor::update() {
 float Sensor::getVelocity() {
     // calculate sample time
     float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
-    if (Ts < min_elapsed_time) return velocity; // don't update velocity if Ts is too small
+    // TODO handle overflow - we do need to reset vel_angle_prev_ts
+    if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
     velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
     vel_angle_prev = angle_prev;

src/common/foc_utils.h

@@ -34,6 +34,8 @@
 #define _ACTIVE 1
 #define _NC (NOT_SET)
 
+#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
+
 // dq current structure
 struct DQCurrent_s
 {

src/communication/StepDirListener.h

@@ -5,7 +5,7 @@
 #include "../common/foc_utils.h"
 
 
-#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE)
+#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE) || defined(CORE_TEENSY) || defined(NRF52_SERIES)
 #define PinStatus int
 #endif
 

src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp

@@ -0,0 +1,268 @@
+
+#if defined(TARGET_RP2040)
+
+
+#include "../../hardware_api.h"
+#include "./rp2040_mcu.h"
+#include "../../../drivers/hardware_specific/rp2040_mcu.h"
+#include "communication/SimpleFOCDebug.h"
+
+#include "hardware/dma.h"
+#include "hardware/irq.h"
+#include "hardware/pwm.h"
+
+
+/* Singleton instance of the ADC engine */
+RP2040ADCEngine engine;
+
+alignas(32) const uint32_t trigger_value = ADC_CS_START_ONCE_BITS; // start once
+
+/* Hardware API implementation */
+
+float _readADCVoltageInline(const int pinA, const void* cs_params) {
+    // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
+    // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
+    // like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
+    // new ADC conversions, which probably won't improve the accuracy.
+
+    if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
+        return engine.lastResults[pinA-26]*engine.adc_conv;
+    }
+
+    // otherwise return NaN
+    return NAN;
+};
+
+
+void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) {
+    if( _isset(pinA) )
+        engine.addPin(pinA);
+    if( _isset(pinB) )
+        engine.addPin(pinB);
+    if( _isset(pinC) )
+        engine.addPin(pinC);
+    engine.init(); // TODO this has to happen later if we want to support more than one motor...
+    engine.start();
+    return &engine;
+};
+
+
+void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC) {    
+    if( _isset(pinA) )
+        engine.addPin(pinA);
+    if( _isset(pinB) )
+        engine.addPin(pinB);
+    if( _isset(pinC) )
+        engine.addPin(pinC);
+    engine.setPWMTrigger(((RP2040DriverParams*)driver_params)->slice[0]);
+    engine.init();
+    engine.start();
+    return &engine;
+};
+
+
+void _startADC3PinConversionLowSide() {
+    // what is this for?
+};
+
+
+float _readADCVoltageLowSide(const int pinA, const void* cs_params) {
+    // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
+    // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
+    // like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
+    // new ADC conversions, which probably won't improve the accuracy.
+
+    if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
+        return engine.lastResults[pinA-26]*engine.adc_conv;
+    }
+
+    // otherwise return NaN
+    return NAN;
+};
+
+
+void _driverSyncLowSide(void* driver_params, void* cs_params) {
+    // nothing to do
+};
+
+
+
+volatile int rp2040_intcount = 0;
+
+void _adcConversionFinishedHandler() {
+    // conversion of all channels finished. copy results.
+    volatile uint8_t* from = engine.samples;
+    if (engine.channelsEnabled[0])
+        engine.lastResults[0] = (*from++);
+    if (engine.channelsEnabled[1])
+        engine.lastResults[1] = (*from++);
+    if (engine.channelsEnabled[2])
+        engine.lastResults[2] = (*from++);
+    if (engine.channelsEnabled[3])
+        engine.lastResults[3] = (*from++);
+    // TODO clear interrupt? dma_hw->ints0 = 1u << channel;
+    //irq_clear(DMA_IRQ_0);
+    //dma_channel_acknowledge_irq0(engine.copyDMAChannel);
+//    dma_start_channel_mask( (1u << engine.readDMAChannel) | (1u << engine.copyDMAChannel) );
+    dma_hw->ints0 = 1u << engine.readDMAChannel;
+    //dma_start_channel_mask( (1u << engine.readDMAChannel) );
+    dma_channel_set_write_addr(engine.readDMAChannel, engine.samples, true);
+    if (engine.triggerPWMSlice>=0)
+        dma_channel_set_trans_count(engine.triggerDMAChannel, 1, true);
+    rp2040_intcount++;
+};
+
+
+
+/* ADC engine implementation */
+
+
+RP2040ADCEngine::RP2040ADCEngine() {
+    channelsEnabled[0] = false;
+    channelsEnabled[1] = false;
+    channelsEnabled[2] = false;
+    channelsEnabled[3] = false;
+    initialized = false;
+};
+
+
+
+void RP2040ADCEngine::addPin(int pin){
+    if (pin>=26 && pin<=29)
+        channelsEnabled[pin-26] = true;
+    else
+        SIMPLEFOC_DEBUG("RP2040-CUR: ERR: Not an ADC pin: ", pin);
+};
+
+
+
+void RP2040ADCEngine::setPWMTrigger(uint slice){
+    triggerPWMSlice = slice;
+};
+
+
+
+
+bool RP2040ADCEngine::init(){
+    if (initialized)
+        return true;
+    
+    adc_init();
+    int enableMask = 0x00;
+    int channelCount = 0;
+    for (int i = 3; i>=0; i--) {
+        if (channelsEnabled[i]){
+            adc_gpio_init(i+26);
+            enableMask |= 0x01;
+            channelCount++;
+        }
+        enableMask = (enableMask<<1);
+    }
+    adc_set_round_robin(enableMask);
+    adc_fifo_setup(
+     true,              // Write each completed conversion to the sample FIFO
+     true,              // Enable DMA data request (DREQ)
+     channelCount,      // DREQ (and IRQ) asserted when all samples present
+     false,             // We won't see the ERR bit because of 8 bit reads; disable.
+     true               // Shift each sample to 8 bits when pushing to FIFO
+    );
+    samples_per_second = 20000;
+    if (samples_per_second<1 || samples_per_second>=500000) {
+        samples_per_second = 0;
+        adc_set_clkdiv(0);
+    }
+    else
+        adc_set_clkdiv(48000000/samples_per_second);
+    SIMPLEFOC_DEBUG("RP2040-CUR: ADC init");
+
+    readDMAChannel = dma_claim_unused_channel(true);
+    dma_channel_config cc1 = dma_channel_get_default_config(readDMAChannel);
+    channel_config_set_transfer_data_size(&cc1, DMA_SIZE_8);
+    channel_config_set_read_increment(&cc1, false);
+    channel_config_set_write_increment(&cc1, true);
+    channel_config_set_dreq(&cc1, DREQ_ADC);
+    channel_config_set_irq_quiet(&cc1, false);
+    dma_channel_configure(readDMAChannel,
+        &cc1,
+        samples,        // dest
+        &adc_hw->fifo,  // source
+        channelCount,   // count
+        false           // defer start
+    );
+    dma_channel_set_irq0_enabled(readDMAChannel, true);
+    irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+
+    // copyDMAChannel = dma_claim_unused_channel(true);
+    // dma_channel_config cc2 = dma_channel_get_default_config(copyDMAChannel);
+    // channel_config_set_transfer_data_size(&cc2, DMA_SIZE_32);
+    // channel_config_set_read_increment(&cc2, false);
+    // channel_config_set_write_increment(&cc2, false);
+    // channel_config_set_chain_to(&cc2, readDMAChannel);
+    // channel_config_set_irq_quiet(&cc2, false);
+    // dma_channel_configure(copyDMAChannel,
+    //     &cc2,
+    //     nextResults,    // dest
+    //     samples,        // source
+    //     1,              // count
+    //     false           // defer start
+    // );
+    // dma_channel_set_irq0_enabled(copyDMAChannel, true);
+    // irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+    SIMPLEFOC_DEBUG("RP2040-CUR: DMA init");
+
+    if (triggerPWMSlice>=0) { // if we have a trigger
+        triggerDMAChannel = dma_claim_unused_channel(true);
+        dma_channel_config cc3 = dma_channel_get_default_config(triggerDMAChannel);
+        channel_config_set_transfer_data_size(&cc3, DMA_SIZE_32);
+        channel_config_set_read_increment(&cc3, false);
+        channel_config_set_write_increment(&cc3, false);
+        channel_config_set_irq_quiet(&cc3, true);
+        channel_config_set_dreq(&cc3, DREQ_PWM_WRAP0+triggerPWMSlice); //pwm_get_dreq(triggerPWMSlice));
+        pwm_set_irq_enabled(triggerPWMSlice, true);
+        dma_channel_configure(triggerDMAChannel,
+            &cc3,
+            hw_set_alias_untyped(&adc_hw->cs),    // dest
+            &trigger_value, // source
+            1,              // count
+            true           // defer start
+        );
+        SIMPLEFOC_DEBUG("RP2040-CUR: PWM trigger init slice ", triggerPWMSlice);
+    }
+
+    initialized = true;
+    return initialized;
+};
+
+
+
+
+void RP2040ADCEngine::start(){
+    SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine starting");
+    irq_set_enabled(DMA_IRQ_0, true);
+    dma_start_channel_mask( (1u << readDMAChannel) ); // | (1u << copyDMAChannel));
+    for (int i=0;i<4;i++) {
+        if (channelsEnabled[i]) {
+            adc_select_input(i); // set input to first enabled channel
+            break;
+        }
+    }
+    if (triggerPWMSlice>=0) {
+        dma_start_channel_mask( (1u << triggerDMAChannel) );
+        //hw_set_bits(&adc_hw->cs, trigger_value);
+    }
+    else
+        adc_run(true);
+    SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine started");
+};
+
+void RP2040ADCEngine::stop(){
+    adc_run(false);
+    dma_channel_abort(readDMAChannel);
+    if (triggerPWMSlice>=0)
+        dma_channel_abort(triggerDMAChannel);
+    adc_fifo_drain();
+    SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine stopped");
+};
+
+
+#endif