Merge pull request #78 from runger1101001/rp2040

Rp2040

Author: askuric

src/common/base_classes/Sensor.h

@@ -15,8 +15,8 @@ enum Direction{
  *  Pullup configuration structure
  */
 enum Pullup{
-    INTERN, //!< Use internal pullups
-    EXTERN //!< Use external pullups
+    INTERNAL, //!< Use internal pullups
+    EXTERNAL //!< Use external pullups
 };
 
 /**
@@ -43,4 +43,4 @@ class Sensor{
         long velocity_calc_timestamp=0; //!< last velocity calculation timestamp
 };
 
-#endif
+#endif

src/drivers/hardware_specific/generic_mcu.cpp

@@ -18,6 +18,8 @@
 
 #elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__)  // samd51
 
+#elif defined(TARGET_RP2040)
+
 #else
 
 // function setting the high pwm frequency to the supplied pins

src/drivers/hardware_specific/rp2040_mcu.cpp

@@ -0,0 +1,163 @@
+
+/**
+ * Support for the RP2040 MCU, as found on the Raspberry Pi Pico.
+ */
+#if defined(TARGET_RP2040)
+
+#define SIMPLEFOC_DEBUG_RP2040
+
+
+#ifdef SIMPLEFOC_DEBUG_RP2040
+
+#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL
+#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial
+#endif
+
+#endif
+
+#include "Arduino.h"
+
+
+
+
+// until I can figure out if this can be quickly read from some register, keep it here.
+// it also serves as a marker for what slices are already used.
+uint16_t wrapvalues[NUM_PWM_SLICES];
+
+
+// TODO add checks which channels are already used...
+
+void setupPWM(int pin, long pwm_frequency, bool invert = false) {
+	gpio_set_function(pin, GPIO_FUNC_PWM);
+	uint slice = pwm_gpio_to_slice_num(pin);
+	uint chan = pwm_gpio_to_channel(pin);
+	pwm_set_clkdiv_int_frac(slice, 1, 0); // fastest pwm we can get
+	pwm_set_phase_correct(slice, true);
+	uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L;
+	if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000
+	if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300
+#ifdef SIMPLEFOC_DEBUG_RP2040
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency);
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value ");
+	SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue);
+#endif
+	pwm_set_wrap(slice, wrapvalue);
+	wrapvalues[slice] = wrapvalue;
+	if (invert) {
+		if (chan==0)
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS);
+		else
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS);
+	}
+	pwm_set_chan_level(slice, chan, 0); // switch off initially
+}
+
+
+void syncSlices() {
+	for (int i=0;i<NUM_PWM_SLICES;i++) {
+		pwm_set_enabled(i, false);
+		pwm_set_counter(i, 0);
+	}
+	// enable all slices
+	pwm_set_mask_enabled(0x7F);
+}
+
+
+void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+	setupPWM(pinA, pwm_frequency);
+	setupPWM(pinB, pwm_frequency);
+	syncSlices();
+}
+
+
+
+void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
+	setupPWM(pinA, pwm_frequency);
+	setupPWM(pinB, pwm_frequency);
+	setupPWM(pinC, pwm_frequency);
+	syncSlices();
+}
+
+
+
+
+void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+	setupPWM(pin1A, pwm_frequency);
+	setupPWM(pin1B, pwm_frequency);
+	setupPWM(pin2A, pwm_frequency);
+	setupPWM(pin2B, pwm_frequency);
+	syncSlices();
+}
+
+
+int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	// non-PIO solution...
+	setupPWM(pinA_h, pwm_frequency);
+	setupPWM(pinB_h, pwm_frequency);
+	setupPWM(pinC_h, pwm_frequency);
+	setupPWM(pinA_l, pwm_frequency, true);
+	setupPWM(pinB_l, pwm_frequency, true);
+	setupPWM(pinC_l, pwm_frequency, true);
+	syncSlices();
+	return 0;
+}
+
+
+
+
+
+void writeDutyCycle(float val, int pin) {
+	uint slice = pwm_gpio_to_slice_num(pin);
+	uint chan = pwm_gpio_to_channel(pin);
+	pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val);
+}
+
+
+
+
+
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, int pinA, int pinB) {
+	writeDutyCycle(dc_a, pinA);
+	writeDutyCycle(dc_b, pinB);
+}
+
+
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, int pinB, int pinC) {
+	writeDutyCycle(dc_a, pinA);
+	writeDutyCycle(dc_b, pinB);
+	writeDutyCycle(dc_c, pinC);
+}
+
+
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B) {
+	writeDutyCycle(dc_1a, pin1A);
+	writeDutyCycle(dc_1b, pin1B);
+	writeDutyCycle(dc_2a, pin2A);
+	writeDutyCycle(dc_2b, pin2B);
+}
+
+inline float swDti(float val, float dt) {
+	float ret = dt+val;
+	if (ret>1.0) ret = 1.0;
+	return ret;
+}
+
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) {
+	writeDutyCycle(dc_a, pinA_h);
+	writeDutyCycle(swDti(dc_a, dead_zone), pinA_l);
+	writeDutyCycle(dc_b, pinB_h);
+	writeDutyCycle(swDti(dc_b,dead_zone), pinB_l);
+	writeDutyCycle(dc_c, pinC_h);
+	writeDutyCycle(swDti(dc_c,dead_zone), pinC_l);
+}
+
+#endif

src/sensors/Encoder.cpp

@@ -32,7 +32,7 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
   prev_timestamp_us = _micros();
 
   // extern pullup as default
-  pullup = Pullup::EXTERN;
+  pullup = Pullup::EXTERNAL;
   // enable quadrature encoder by default
   quadrature = Quadrature::ON;
 }
@@ -160,7 +160,7 @@ int Encoder::hasIndex(){
 void Encoder::init(){
 
   // Encoder - check if pullup needed for your encoder
-  if(pullup == Pullup::INTERN){
+  if(pullup == Pullup::INTERNAL){
     pinMode(pinA, INPUT_PULLUP);
     pinMode(pinB, INPUT_PULLUP);
     if(hasIndex()) pinMode(index_pin,INPUT_PULLUP);

src/sensors/HallSensor.cpp

@@ -17,7 +17,7 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){
   cpr = _pp * 6; 
 
   // extern pullup as default
-  pullup = Pullup::EXTERN;
+  pullup = Pullup::EXTERNAL;
 }
 
 //  HallSensor interrupt callback functions
@@ -119,7 +119,7 @@ void HallSensor::init(){
   electric_rotations = 0;
 
   // HallSensor - check if pullup needed for your HallSensor
-  if(pullup == Pullup::INTERN){
+  if(pullup == Pullup::INTERNAL){
     pinMode(pinA, INPUT_PULLUP);
     pinMode(pinB, INPUT_PULLUP);
     pinMode(pinC, INPUT_PULLUP);

src/sensors/MagneticSensorAnalog.cpp

@@ -13,7 +13,7 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun
   min_raw_count = _min_raw_count;
   max_raw_count = _max_raw_count;
 
-  if(pullup == Pullup::INTERN){
+  if(pullup == Pullup::INTERNAL){
     pinMode(pinAnalog, INPUT_PULLUP);
   }else{
     pinMode(pinAnalog, INPUT);

src/sensors/MagneticSensorI2C.h

@@ -16,6 +16,12 @@ struct MagneticSensorI2CConfig_s  {
 // some predefined structures
 extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C;
 
+#if defined(TARGET_RP2040)
+#define SDA I2C_SDA
+#define SCL I2C_SCL
+#endif
+
+
 class MagneticSensorI2C: public Sensor{
  public:
     /**

src/sensors/MagneticSensorSPI.cpp

@@ -1,3 +1,5 @@
+#ifndef TARGET_RP2040
+
 #include "MagneticSensorSPI.h"
 
 /** Typical configuration for the 14bit AMS AS5147 magnetic sensor over SPI interface */
@@ -212,3 +214,6 @@ word MagneticSensorSPI::read(word angle_register){
 void MagneticSensorSPI::close(){
 	spi->end();
 }
+
+
+#endif

src/sensors/MagneticSensorSPI.h

@@ -1,6 +1,8 @@
 #ifndef MAGNETICSENSORSPI_LIB_H
 #define MAGNETICSENSORSPI_LIB_H
 
+#ifndef TARGET_RP2040
+
 #include "Arduino.h"
 #include <SPI.h>
 #include "../common/base_classes/Sensor.h"
@@ -91,3 +93,4 @@ class MagneticSensorSPI: public Sensor{
 
 
 #endif
+#endif