From aefee3f679114acd2d42e320101a5ad6c784f1e3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 13:29:54 +0100 Subject: [PATCH 1/7] initial debug commit for merge --- src/BLDCMotor.cpp | 50 ++++++++++---------- src/StepperMotor.cpp | 38 +++++++-------- src/communication/SimpleFOCDebug.cpp | 56 ++++++++++++++++++++++ src/communication/SimpleFOCDebug.h | 71 ++++++++++++++++++++++++++++ 4 files changed, 169 insertions(+), 46 deletions(-) create mode 100644 src/communication/SimpleFOCDebug.cpp create mode 100644 src/communication/SimpleFOCDebug.h diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d47ca51f..8f090fb1 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -24,7 +24,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) { // init hardware pins void BLDCMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); + SIMPLEFOC_DEBUG("MOT: Init"); // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit if( !current_sense && _isset(phase_resistance)) { @@ -54,7 +54,7 @@ void BLDCMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); } @@ -104,7 +104,8 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else + SIMPLEFOC_DEBUG("MOT: No sensor."); // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases @@ -112,13 +113,13 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction _delay(500); if(exit_flag){ if(current_sense) exit_flag *= alignCurrentSense(); - else if(monitor_port) monitor_port->println(F("MOT: No current sense.")); + else SIMPLEFOC_DEBUG("MOT: No current sense."); } if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); disable(); } @@ -129,18 +130,17 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction int BLDCMotor::alignCurrentSense() { int exit_flag = 1; // success - if(monitor_port) monitor_port->println(F("MOT: Align current sense.")); + SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver exit_flag = current_sense->driverAlign(driver, voltage_sensor_align); if(!exit_flag){ // error in current sense - phase either not measured or bad connection - if(monitor_port) monitor_port->println(F("MOT: Align error!")); + SIMPLEFOC_DEBUG("MOT: Align error!"); exit_flag = 0; }else{ // output the alignment status flag - if(monitor_port) monitor_port->print(F("MOT: Success: ")); - if(monitor_port) monitor_port->println(exit_flag); + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); } return exit_flag > 0; @@ -149,7 +149,7 @@ int BLDCMotor::alignCurrentSense() { // Encoder alignment to electrical 0 angle int BLDCMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction if(!_isset(sensor_direction)){ @@ -180,24 +180,23 @@ int BLDCMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); 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! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -213,13 +212,12 @@ int BLDCMotor::alignSensor() { //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); return exit_flag; } @@ -228,7 +226,7 @@ int BLDCMotor::alignSensor() { int BLDCMotor::absoluteZeroSearch() { // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision // of float is sufficient. - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -248,8 +246,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else SIMPLEFOC_DEBUG("MOT: Success!"); } return !sensor->needsSearch(); } @@ -298,7 +296,7 @@ void BLDCMotor::loopFOC() { break; default: // no torque control selected - if(monitor_port) monitor_port->println(F("MOT: no torque control selected!")); + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); break; } diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2acd6f11..2f30f373 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -25,7 +25,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { // init hardware pins void StepperMotor::init() { - if(monitor_port) monitor_port->println(F("MOT: Init")); + SIMPLEFOC_DEBUG("MOT: Init"); // if set the phase resistance of the motor use current limit to calculate the voltage limit if(_isset(phase_resistance)) { @@ -49,7 +49,7 @@ void StepperMotor::init() { _delay(500); // enable motor - if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); + SIMPLEFOC_DEBUG("MOT: Enable driver."); enable(); _delay(500); @@ -101,12 +101,12 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); + }else SIMPLEFOC_DEBUG("MOT: No sensor."); if(exit_flag){ - if(monitor_port) monitor_port->println(F("MOT: Ready.")); + SIMPLEFOC_DEBUG("MOT: Ready."); }else{ - if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); disable(); } @@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // Encoder alignment to electrical 0 angle int StepperMotor::alignSensor() { int exit_flag = 1; //success - if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); + SIMPLEFOC_DEBUG("MOT: Align sensor."); // if unknown natural direction if(!_isset(sensor_direction)){ @@ -147,24 +147,23 @@ int StepperMotor::alignSensor() { _delay(200); // determine the direction the sensor moved if (mid_angle == end_angle) { - if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); return 0; // failed calibration } else if (mid_angle < end_angle) { - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); sensor_direction = Direction::CCW; } else{ - if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); sensor_direction = Direction::CW; } // check pole pair number - if(monitor_port) monitor_port->print(F("MOT: PP check: ")); 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! - if(monitor_port) monitor_port->print(F("fail - estimated pp:")); - if(monitor_port) monitor_port->println(_2PI/moved,4); - }else if(monitor_port) monitor_port->println(F("OK!")); + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); + } else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); - }else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -179,13 +178,12 @@ int StepperMotor::alignSensor() { zero_electric_angle = electricalAngle(); _delay(20); if(monitor_port){ - monitor_port->print(F("MOT: Zero elec. angle: ")); - monitor_port->println(zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); } // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib.")); + }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); return exit_flag; } @@ -193,7 +191,7 @@ int StepperMotor::alignSensor() { // - to the index int StepperMotor::absoluteZeroSearch() { - if(monitor_port) monitor_port->println(F("MOT: Index search...")); + SIMPLEFOC_DEBUG("MOT: Index search..."); // search the absolute zero with small velocity float limit_vel = velocity_limit; float limit_volt = voltage_limit; @@ -213,8 +211,8 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!")); - else monitor_port->println(F("MOT: Success!")); + if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else SIMPLEFOC_DEBUG("MOT: Success!"); } return !sensor->needsSearch(); } diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp new file mode 100644 index 00000000..871fefc6 --- /dev/null +++ b/src/communication/SimpleFOCDebug.cpp @@ -0,0 +1,56 @@ + +#include "SimpleFOCDebug.h" + +#ifndef SIMPLEFOC_DISABLE_DEBUG + + +Print* SimpleFOCDebug::_debugPrint = NULL; + + +void SimpleFOCDebug::enable(Print* debugPrint) { + _debugPrint = debugPrint; +} + + + +void SimpleFOCDebug::println(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->println(str); + } +} + +void SimpleFOCDebug::println(const char* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const char* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} + +#endif \ No newline at end of file diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h new file mode 100644 index 00000000..bc8a61b2 --- /dev/null +++ b/src/communication/SimpleFOCDebug.h @@ -0,0 +1,71 @@ + +#ifndef __SIMPLEFOCDEBUG_H__ +#define __SIMPLEFOCDEBUG_H__ + +#include "Arduino.h" + + +/** + * SimpleFOCDebug class + * + * This class is used to print debug messages to a chosen output. + * Currently, Print instances are supported as targets, e.g. serial port. + * + * Activate debug output globally by calling enable(), optionally passing + * in a Print instance. If none is provided "Serial" is used by default. + * + * To produce debug output, use the macro SIMPLEFOC_DEBUG: + * SIMPLEFOC_DEBUG("Debug message!"); + * SIMPLEFOC_DEBUG("a float value:", 123.456); + * SIMPLEFOC_DEBUG("an integer value: ", 123); + * + * Keep debugging output short and simple. Some of our MCUs have limited + * RAM and limited serial output capabilities. + * + * By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to + * help preserve memory on Arduino boards. + * + * You can also disable debug output completely. In this case all debug output + * and the SimpleFOCDebug class is removed from the compiled code. + * Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in + * this way. + * + **/ + + +#ifndef SIMPLEFOC_DISABLE_DEBUG + +class SimpleFOCDebug { +public: + void enable(Print* debugPrint = Serial); + + void println(const __FlashStringHelper* msg); + void println(const char* msg); + void println(const __FlashStringHelper* msg, float val); + void println(const char* msg, float val); + void println(const __FlashStringHelper* msg, int val); + void println(const char* msg, int val); + +protected: + static Print* _debugPrint; +}; + + +#define SIMPLEFOC_DEBUG(msg, ...) \ + SimpleFOCDebug::println(F(msg) __VA_OPT__(,) __VA_ARGS__) + + + + + +#else //ifndef SIMPLEFOC_DISABLE_DEBUG + + + +#define SIMPLEFOC_DEBUG(msg, ...) + + + +#endif //ifndef SIMPLEFOC_DISABLE_DEBUG +#endif + From 0ec82e3e7f3f88addc29ef27da2ee83c3989d7d9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 23:38:43 +0100 Subject: [PATCH 2/7] New debug management independent of monitoring --- src/BLDCMotor.cpp | 1 + src/SimpleFOC.h | 1 + src/StepperMotor.cpp | 2 + src/communication/SimpleFOCDebug.cpp | 35 +++++ src/communication/SimpleFOCDebug.h | 24 +-- src/drivers/hardware_api.h | 2 + src/drivers/hardware_specific/samd21_mcu.cpp | 43 ++--- src/drivers/hardware_specific/samd51_mcu.cpp | 26 ++-- src/drivers/hardware_specific/samd_mcu.cpp | 156 +++++++++---------- src/drivers/hardware_specific/samd_mcu.h | 6 +- 10 files changed, 168 insertions(+), 128 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 74b62bd9..03cec466 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -1,4 +1,5 @@ #include "BLDCMotor.h" +#include "./communication/SimpleFOCDebug.h" // BLDCMotor( int pp , float R) // - pp - pole pair number diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 3c24221c..606046d0 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -114,5 +114,6 @@ void loop() { #include "current_sense/GenericCurrentSense.h" #include "communication/Commander.h" #include "communication/StepDirListener.h" +#include "communication/SimpleFOCDebug.h" #endif diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index e75f0743..d12f120c 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -1,4 +1,6 @@ #include "StepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + // StepperMotor(int pp) // - pp - pole pair number diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index 871fefc6..b89e5580 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -53,4 +53,39 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { } } + +void SimpleFOCDebug::print(const char* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + + +void SimpleFOCDebug::print(const __FlashStringHelper* str) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + } +} + + +void SimpleFOCDebug::print(int val) { + if (_debugPrint != NULL) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::print(float val) { + if (_debugPrint != NULL) { + _debugPrint->print(val); + } +} + + +void SimpleFOCDebug::println() { + if (_debugPrint != NULL) { + _debugPrint->println(); + } +} + #endif \ No newline at end of file diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index bc8a61b2..bccdf194 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -16,7 +16,7 @@ * * To produce debug output, use the macro SIMPLEFOC_DEBUG: * SIMPLEFOC_DEBUG("Debug message!"); - * SIMPLEFOC_DEBUG("a float value:", 123.456); + * SIMPLEFOC_DEBUG("a float value:", 123.456f); * SIMPLEFOC_DEBUG("an integer value: ", 123); * * Keep debugging output short and simple. Some of our MCUs have limited @@ -37,14 +37,20 @@ class SimpleFOCDebug { public: - void enable(Print* debugPrint = Serial); + static void enable(Print* debugPrint = &Serial); - void println(const __FlashStringHelper* msg); - void println(const char* msg); - void println(const __FlashStringHelper* msg, float val); - void println(const char* msg, float val); - void println(const __FlashStringHelper* msg, int val); - void println(const char* msg, int val); + static void println(const __FlashStringHelper* msg); + static void println(const char* msg); + static void println(const __FlashStringHelper* msg, float val); + static void println(const char* msg, float val); + static void println(const __FlashStringHelper* msg, int val); + static void println(const char* msg, int val); + static void println(); + + static void print(const char* msg); + static void print(const __FlashStringHelper* msg); + static void print(int val); + static void print(float val); protected: static Print* _debugPrint; @@ -52,7 +58,7 @@ class SimpleFOCDebug { #define SIMPLEFOC_DEBUG(msg, ...) \ - SimpleFOCDebug::println(F(msg) __VA_OPT__(,) __VA_ARGS__) + SimpleFOCDebug::println(F(msg), ##__VA_ARGS__) diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index 01f251b9..cb2385b7 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -3,6 +3,8 @@ #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../communication/SimpleFOCDebug.h" + // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index e4fb64b7..d9bb5b9a 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -164,7 +164,7 @@ void configureSAMDClock() { while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); + SIMPLEFOC_DEBUG("SAMD: Configured clock..."); #endif } } @@ -227,8 +227,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: Initialized TC ", tccConfig.tcc.tccn); #endif } else { @@ -264,15 +263,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } } @@ -298,15 +298,16 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->CTRLA.bit.ENABLE = 1; while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 19474d03..a9191702 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -190,7 +190,7 @@ void configureSAMDClock() { while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync -#ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("(Re-)Initialized TCC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); +#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG) + SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC "); + SimpleFOCDebug::print(tccConfig.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(tccConfig.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(tccConfig.wo); + SimpleFOCDebug::print("] pwm res "); + SimpleFOCDebug::print((int)pwm_resolution); + SimpleFOCDebug::println(); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { @@ -292,9 +293,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51"); + SIMPLEFOC_DEBUG("SAMD: Not initialized: TC ", tccConfig.tcc.tccn); + SIMPLEFOC_DEBUG("SAMD: TC units not supported on SAMD51"); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index b950fd09..4198cc9b 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -287,7 +287,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -297,9 +297,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 2)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 2)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); #endif @@ -308,7 +306,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... attachTCC(tccConfs[1]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -326,7 +324,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif SAMDHardwareDriverParams* params = new SAMDHardwareDriverParams { @@ -388,7 +386,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -399,9 +397,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 3)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 3)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -412,7 +408,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i attachTCC(tccConfs[1]); attachTCC(tccConfs[2]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -432,7 +428,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -469,7 +465,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -481,9 +477,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Found configuration: (score="); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(scorePermutation(tccConfs, 4)); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(")"); + SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 4)); printTCCConfiguration(tccConfs[0]); printTCCConfiguration(tccConfs[1]); printTCCConfiguration(tccConfs[2]); @@ -496,7 +490,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const attachTCC(tccConfs[2]); attachTCC(tccConfs[3]); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? @@ -518,7 +512,7 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res; getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -575,7 +569,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons if (compatibility<0) { // no result! #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Bad combination!"); + SIMPLEFOC_DEBUG("SAMD: Bad pin combination!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -589,7 +583,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5)); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Found configuration: "); + SIMPLEFOC_DEBUG("SAMD: Found configuration: "); printTCCConfiguration(pinAh); printTCCConfiguration(pinAl); printTCCConfiguration(pinBh); @@ -609,7 +603,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons if (!allAttached) return SIMPLEFOC_DRIVER_INIT_FAILED; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Attached pins..."); + SIMPLEFOC_DEBUG("SAMD: Attached pins..."); #endif // set up clock - if we did this right it should be possible to get all TCC units synchronized? // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API @@ -637,7 +631,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res; getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); + SIMPLEFOC_DEBUG("SAMD: Configured TCCs..."); #endif return new SAMDHardwareDriverParams { @@ -790,85 +784,85 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ * saves you hours of cross-referencing with the datasheet. */ void printAllPinInfos() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(); + SimpleFOCDebug::println(); for (uint8_t pin=0;pin=TCC_INST_NUM) - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TC Peripheral"); + SimpleFOCDebug::print(": TC Peripheral"); else - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(": TCC Peripheral"); + SimpleFOCDebug::print(": TCC Peripheral"); switch (info.peripheral) { case PIO_TIMER: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" E "); break; + SimpleFOCDebug::print(" E "); break; case PIO_TIMER_ALT: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" F "); break; + SimpleFOCDebug::print(" F "); break; #if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" G "); break; + SimpleFOCDebug::print(" G "); break; #endif default: - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" ? "); break; + SimpleFOCDebug::print(" ? "); break; } if (info.tcc.tccn>=0) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.tccn); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("-"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.tcc.chan); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(info.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SimpleFOCDebug::print(info.tcc.tccn); + SimpleFOCDebug::print("-"); + SimpleFOCDebug::print(info.tcc.chan); + SimpleFOCDebug::print("["); + SimpleFOCDebug::print(info.wo); + SimpleFOCDebug::println("]"); } else - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(" None"); + SimpleFOCDebug::println(" None"); } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 78f53664..181d8faf 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -3,11 +3,9 @@ #define SAMD_MCU_H -// uncomment to enable debug output to Serial port +// uncomment to enable debug output from SAMD driver +// can set this as build-flag in Arduino IDE or PlatformIO // #define SIMPLEFOC_SAMD_DEBUG -#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) -#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial -#endif #include "../hardware_api.h" From 00e550e2dbf8de59f6b350501989438f5115c713 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 12 Feb 2022 23:45:24 +0100 Subject: [PATCH 3/7] make the old behaviour the default --- src/common/base_classes/FOCMotor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 922076a6..2763b709 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -1,4 +1,5 @@ #include "FOCMotor.h" +#include "../../communication/SimpleFOCDebug.h" /** * Default constructor - setting all variabels to default values @@ -77,7 +78,8 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print - if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!")); + SimpleFOCDebug::enable(&print); + SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); } // utility function intended to be used with serial plotter to monitor motor variables From cac0484fb879a8fdc3ca2b7a7b56bee07b7c37fc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 27 Feb 2022 21:00:22 +0100 Subject: [PATCH 4/7] set driver initialized flag --- src/drivers/BLDCDriver3PWM.cpp | 1 + src/drivers/BLDCDriver6PWM.cpp | 1 + src/drivers/StepperDriver2PWM.cpp | 1 + src/drivers/StepperDriver4PWM.cpp | 1 + 4 files changed, 4 insertions(+) diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 00714767..5115cf37 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -57,6 +57,7 @@ int BLDCDriver3PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 2b039d79..22a50f69 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -59,6 +59,7 @@ int BLDCDriver6PWM::init() { // configure 6pwm // hardware specific function - depending on driver and mcu params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index bec6819b..dbbf5b8f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -80,6 +80,7 @@ int StepperDriver2PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure2PWM(pwm_frequency, pwm1, pwm2); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index a2fb4c77..836f5472 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -55,6 +55,7 @@ int StepperDriver4PWM::init() { // Set the pwm frequency to the pins // hardware specific function - depending on driver and mcu params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED); return params!=SIMPLEFOC_DRIVER_INIT_FAILED; } From 0066b5e06fd82c25e511fea07d3de594524e4d75 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 9 Mar 2022 02:16:33 +0100 Subject: [PATCH 5/7] STM32 driver refactoring --- src/drivers/hardware_specific/stm32_mcu.cpp | 623 +++++++++++++++----- 1 file changed, 481 insertions(+), 142 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index a20cbbc8..5c51c406 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -25,6 +25,13 @@ typedef struct STM32DriverParams { +#ifdef SIMPLEFOC_STM32_DEBUG +void printTimerCombination(int numPins, PinMap* timers[], int score); +int getTimerNumber(int timerIndex); +#endif + + + // setting pwm to hardware pin - instead analogWrite() @@ -40,27 +47,24 @@ void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution } + + + + // init pin pwm -HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) -{ - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - if (Instance == NP) { - Serial.print("No timer on pin "); - Serial.println(ulPin); - delay(1000); +HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { + // sanity check + if (timer==NP) return NP; - } - - uint32_t index = get_timer_index(Instance); + uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + uint32_t channel = STM_PIN_CHANNEL(timer->function); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin); HT->setOverflow(PWM_freq, HERTZ_FORMAT); HT->pause(); HT->refresh(); @@ -68,21 +72,24 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) } + + + + + // init high side pin -HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { - return _initPinPWM(PWM_freq, ulPin); + return _initPinPWM(PWM_freq, timer); } // init low side pin -HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer) { - PinName pin = digitalPinToPinName(ulPin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - uint32_t index = get_timer_index(Instance); + uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); @@ -97,12 +104,12 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; #endif - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + uint32_t channel = STM_PIN_CHANNEL(timer->function); HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + uint32_t channel = STM_PIN_CHANNEL(timer->function); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin); HT->setOverflow(PWM_freq, HERTZ_FORMAT); HT->pause(); HT->refresh(); @@ -110,8 +117,9 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) } + // align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3) { HT1->pause(); HT1->refresh(); @@ -124,8 +132,11 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) HT3->resume(); } + + + // align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4) { HT1->pause(); HT1->refresh(); @@ -141,30 +152,56 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha HT4->resume(); } -// configure hardware 6pwm interface only one timer with inverted channels -STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) -{ + + + +int getLLChannel(PinMap* timer) { +#if defined(TIM_CCER_CC1NE) + if (STM_PIN_INVERTED(timer->function)) { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1N; + case 2: return LL_TIM_CHANNEL_CH2N; + case 3: return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case 4: return LL_TIM_CHANNEL_CH4N; +#endif + default: return -1; + } + } else +#endif + { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1; + case 2: return LL_TIM_CHANNEL_CH2; + case 3: return LL_TIM_CHANNEL_CH3; + case 4: return LL_TIM_CHANNEL_CH4; + default: return -1; + } + } + return -1; +} + + + + +// configure hardware 6pwm for a complementary pair of channels +STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { + // sanity check + if (pinH==NP || pinL==NP) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; #if !defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface - PinName uhPinName = digitalPinToPinName(pinA_h); - PinName ulPinName = digitalPinToPinName(pinA_l); - PinName vhPinName = digitalPinToPinName(pinB_h); - PinName vlPinName = digitalPinToPinName(pinB_l); - PinName whPinName = digitalPinToPinName(pinC_h); - PinName wlPinName = digitalPinToPinName(pinC_l); - uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)); - uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)); - uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)); - uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)); - uint32_t channel5 = STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)); - uint32_t channel6 = STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)); - - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - - uint32_t index = get_timer_index(Instance); + uint32_t channel1 = STM_PIN_CHANNEL(pinH->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinL->function); + + // more sanity check + if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral); if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); @@ -172,18 +209,14 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, in } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, uhPinName); - HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, ulPinName); - HT->setMode(channel3, TIMER_OUTPUT_COMPARE_PWM1, vhPinName); - HT->setMode(channel4, TIMER_OUTPUT_COMPARE_PWM1, vlPinName); - HT->setMode(channel5, TIMER_OUTPUT_COMPARE_PWM1, whPinName); - HT->setMode(channel6, TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin); + HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin); // dead time is set in nanoseconds uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! - LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); // Set Trigger out for DMA transfer LL_TIM_SetTriggerOutput(HT->getHandle()->Instance, LL_TIM_TRGO_UPDATE); @@ -193,112 +226,294 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, in // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs. HT->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - HT->getHandle()->Instance->CNT = TIM1->ARR; + HT->getHandle()->Instance->CNT = HT->getHandle()->Instance->ARR; HT->resume(); + params->timers[paramsPos] = HT; + params->timers[paramsPos+1] = HT; + params->channels[paramsPos] = channel1; + params->channels[paramsPos+1] = channel2; + return params; +#else + return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing +#endif +} + + + + +STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { STM32DriverParams* params = new STM32DriverParams { - .timers = { HT }, - .channels = { channel1, channel3, channel5 }, + .timers = { NP, NP, NP, NP, NP, NP }, + .channels = { 0, 0, 0, 0, 0, 0 }, .pwm_frequency = PWM_freq, .dead_zone = dead_zone, .interface_type = _HARDWARE_6PWM }; - + + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if(_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + _alignPWMTimers(params->timers[0], params->timers[2], params->timers[4]); + return params; -#else - return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing -#endif } -// returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels -// returns -1 if neither - avoid configuring - error!!! -int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - PinName nameAH = digitalPinToPinName(pinA_h); - PinName nameAL = digitalPinToPinName(pinA_l); - PinName nameBH = digitalPinToPinName(pinB_h); - PinName nameBL = digitalPinToPinName(pinB_l); - PinName nameCH = digitalPinToPinName(pinC_h); - PinName nameCL = digitalPinToPinName(pinC_l); - int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); - int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); - int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); - int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); - int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); - int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); - -#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface - - if(tim1 == tim2 && tim3==tim4 && tim5==tim6) - return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer - else - return _ERROR_6PWM; // might be error avoid configuration -#else // the rest of stm32 boards - - if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer - else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) - return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer - else - return _ERROR_6PWM; // might be error avoid configuration + + + + +#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED +#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12 #endif +int numTimerPinsUsed; +PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; + + +/* + timer combination scoring function + assigns a score, and also checks the combination is valid + returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better + for 6 pwm, hardware 6-pwm is preferred over software 6-pwm + hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel + inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) +*/ +int scoreCombination(int numPins, PinMap* pinTimers[]) { + // check already used - TODO move this to outer loop also... + for (int i=0; iperipheral == timerPinsUsed[i]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function)) + return -2; // bad combination - timer channel already used + } + // check for inverted channels - TODO move this to outer loop also... + if (numPins < 6) { + for (int i=0; ifunction)) + return -3; // bad combination - inverted channel used in non-hardware 6pwm + } + } + // check for duplicated channels + for (int i=0; iperipheral == pinTimers[j]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) + && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) + return -4; // bad combination - duplicated channel + } + } + int score = 1; + for (int i=0; iperipheral == pinTimers[j]->peripheral) + break; + if (i==j) + score++; + } + } + if (numPins==6) { + // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels + // >1 timer, 3 channels, 3 matching inverted channels + // 1 timer, 6 channels (no inverted channels) + // >1 timer, 6 channels (no inverted channels) + // check for inverted high-side channels - TODO is this a configuration we should allow? what if all 3 high side channels are inverted and the low-side non-inverted? + if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + return -5; // bad combination - inverted channel used on high-side channel + if (pinTimers[0]->peripheral == pinTimers[1]->peripheral + && pinTimers[2]->peripheral == pinTimers[3]->peripheral + && pinTimers[4]->peripheral == pinTimers[5]->peripheral + && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) + && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) + && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) + && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { + // hardware 6pwm, score <10 + } + else { + + // check for inverted low-side channels + if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm + score += 10; // software 6pwm, score >10 + } + } + return score; +} + + + + +int findIndexOfFirstPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if (pinName == PinMap_TIM[i].pin) + return i; + i++; + } + return -1; +} + + +int findIndexOfLastPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) + && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; } + + + +#define NOT_FOUND 1000 + +int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { + PinMap* searchArray[numPins]; + for (int j=0;j=0 && score SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + int pins[2] = { pinA, pinB }; + PinMap* pinTimers[2] = { NP, NP }; + if (findBestTimerCombination(2, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); // allign the timers _alignPWMTimers(HT1, HT2, HT2); - uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); - uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); STM32DriverParams* params = new STM32DriverParams { .timers = { HT1, HT2 }, .channels = { channel1, channel2 }, .pwm_frequency = pwm_frequency }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; return params; } + + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if (numTimerPinsUsed+3 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + int pins[3] = { pinA, pinB, pinC }; + PinMap* pinTimers[3] = { NP, NP, NP }; + if (findBestTimerCombination(3, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); // allign the timers _alignPWMTimers(HT1, HT2, HT3); - uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); - uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); - uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC), PinMap_PWM)); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); + uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); STM32DriverParams* params = new STM32DriverParams { .timers = { HT1, HT2, HT3 }, .channels = { channel1, channel2, channel3 }, .pwm_frequency = pwm_frequency }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; return params; } @@ -308,28 +523,41 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in // - Stepper motor - 4PWM setting // - hardware speciffic void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + int pins[4] = { pinA, pinB, pinC, pinD }; + PinMap* pinTimers[4] = { NP, NP, NP, NP }; + if (findBestTimerCombination(4, pins, pinTimers)<0) + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); - uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA), PinMap_PWM)); - uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB), PinMap_PWM)); - uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC), PinMap_PWM)); - uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinD), PinMap_PWM)); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); + uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); + uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); STM32DriverParams* params = new STM32DriverParams { .timers = { HT1, HT2, HT3, HT4 }, .channels = { channel1, channel2, channel3, channel4 }, .pwm_frequency = pwm_frequency }; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; + timerPinsUsed[numTimerPinsUsed++] = pinTimers[3]; return params; } @@ -373,47 +601,57 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo // - BLDC driver - 6PWM setting // - hardware specific void* _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){ + if (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { + SIMPLEFOC_DEBUG("STM32: ERR: too many pins used"); + return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + } if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max // center-aligned frequency is uses two periods pwm_frequency *=2; // find configuration - int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }; + PinMap* pinTimers[6] = { NP, NP, NP, NP, NP, NP }; + int score = findBestTimerCombination(6, pins, pinTimers); + STM32DriverParams* params; // configure accordingly - switch(config){ - case _ERROR_6PWM: - return SIMPLEFOC_DRIVER_INIT_FAILED; - case _HARDWARE_6PWM: - params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - break; - case _SOFTWARE_6PWM: - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); - _initPinPWMLow(pwm_frequency, pinA_l); - HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency, pinB_h); - _initPinPWMLow(pwm_frequency, pinB_l); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinC_h); - _initPinPWMLow(pwm_frequency, pinC_l); - _alignPWMTimers(HT1, HT2, HT3); - uint32_t channel1 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA_h), PinMap_PWM)); - uint32_t channel2 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinA_l), PinMap_PWM)); - uint32_t channel3 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB_h), PinMap_PWM)); - uint32_t channel4 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinB_l), PinMap_PWM)); - uint32_t channel5 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC_h), PinMap_PWM)); - uint32_t channel6 = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(pinC_l), PinMap_PWM)); - params = new STM32DriverParams { - .timers = { HT1, HT2, HT3 }, - .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone, - .interface_type = _SOFTWARE_6PWM - }; - break; + if (score<0) + params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; + else if (score<10) // hardware pwm + params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); + else { // software pwm + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); + HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); + HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); + HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); + HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + _alignPWMTimers(HT1, HT2, HT3); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); + uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); + uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); + uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); + uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); + uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); + params = new STM32DriverParams { + .timers = { HT1, HT2, HT3, HT4, HT5, HT6 }, + .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone, + .interface_type = _SOFTWARE_6PWM + }; } + for (int i=0; i<6; i++) + timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; return params; // success } + + + + // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific @@ -421,8 +659,8 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: float dead_zone = ((STM32DriverParams*)params)->dead_zone; @@ -435,4 +673,105 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ break; } } -#endif \ No newline at end of file + + + + + +#ifdef SIMPLEFOC_STM32_DEBUG + +int getTimerNumber(int timerIndex) { + #if defined(TIM1_BASE) + if (timerIndex==TIMER1_INDEX) return 1; + #endif + #if defined(TIM2_BASE) + if (timerIndex==TIMER2_INDEX) return 2; + #endif + #if defined(TIM3_BASE) + if (timerIndex==TIMER3_INDEX) return 3; + #endif + #if defined(TIM4_BASE) + if (timerIndex==TIMER4_INDEX) return 4; + #endif + #if defined(TIM5_BASE) + if (timerIndex==TIMER5_INDEX) return 5; + #endif + #if defined(TIM6_BASE) + if (timerIndex==TIMER6_INDEX) return 6; + #endif + #if defined(TIM7_BASE) + if (timerIndex==TIMER7_INDEX) return 7; + #endif + #if defined(TIM8_BASE) + if (timerIndex==TIMER8_INDEX) return 8; + #endif + #if defined(TIM9_BASE) + if (timerIndex==TIMER9_INDEX) return 9; + #endif + #if defined(TIM10_BASE) + if (timerIndex==TIMER10_INDEX) return 10; + #endif + #if defined(TIM11_BASE) + if (timerIndex==TIMER11_INDEX) return 11; + #endif + #if defined(TIM12_BASE) + if (timerIndex==TIMER12_INDEX) return 12; + #endif + #if defined(TIM13_BASE) + if (timerIndex==TIMER13_INDEX) return 13; + #endif + #if defined(TIM14_BASE) + if (timerIndex==TIMER14_INDEX) return 14; + #endif + #if defined(TIM15_BASE) + if (timerIndex==TIMER15_INDEX) return 15; + #endif + #if defined(TIM16_BASE) + if (timerIndex==TIMER16_INDEX) return 16; + #endif + #if defined(TIM17_BASE) + if (timerIndex==TIMER17_INDEX) return 17; + #endif + #if defined(TIM18_BASE) + if (timerIndex==TIMER18_INDEX) return 18; + #endif + #if defined(TIM19_BASE) + if (timerIndex==TIMER19_INDEX) return 19; + #endif + #if defined(TIM20_BASE) + if (timerIndex==TIMER20_INDEX) return 20; + #endif + #if defined(TIM21_BASE) + if (timerIndex==TIMER21_INDEX) return 21; + #endif + #if defined(TIM22_BASE) + if (timerIndex==TIMER22_INDEX) return 22; + #endif + return -1; +} + + +void printTimerCombination(int numPins, PinMap* timers[], int score) { + for (int i=0; iperipheral))); + SimpleFOCDebug::print("-CH"); + SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); + if (STM_PIN_INVERTED(timers[i]->function)) + SimpleFOCDebug::print("N"); + } + SimpleFOCDebug::print(" "); + } + SimpleFOCDebug::println("score: ", score); +} + +#endif + + + + + +#endif From 0051af4f825b99391d10bd0b347923d6a799e64b Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 9 Mar 2022 02:33:47 +0100 Subject: [PATCH 6/7] stm32 driver: fix check for low-side channels --- src/drivers/hardware_specific/stm32_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 5c51c406..1feb58ad 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -337,7 +337,7 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) { else { // check for inverted low-side channels - if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm score += 10; // software 6pwm, score >10 } @@ -423,7 +423,7 @@ int findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]) { return -10; // no workable combination found } #ifdef SIMPLEFOC_STM32_DEBUG - SimpleFOCDebug::print("STM32: best score: "); + SimpleFOCDebug::print("STM32: best: "); printTimerCombination(numPins, pinTimers, bestScore); #endif return bestScore; From 3b7b261c85757c12fd1829324c8d4d967b6e0f9c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Thu, 10 Mar 2022 16:20:07 +0100 Subject: [PATCH 7/7] fix channel counting --- src/drivers/hardware_specific/stm32_mcu.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 1feb58ad..4b8185ad 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -307,15 +307,15 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) { return -4; // bad combination - duplicated channel } } - int score = 1; + int score = 0; for (int i=0; iperipheral == pinTimers[j]->peripheral) - break; - if (i==j) - score++; + found = true; } + if (!found) score++; } if (numPins==6) { // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels