diff --git a/README.md b/README.md index 424f3cd7..747b0f34 100644 --- a/README.md +++ b/README.md @@ -24,34 +24,20 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.0 -> - Arduino Mega 6pwm more timers supported -> - Arduino boards - frequency change support either 32kHz or 4kHz -> - Arduino Uno - synched timers in 3pwm and 6pwm mode [#71](https://github.com/simplefoc/Arduino-FOC/issues/71) -> - Teensy 3.x initial support for 6pwm -> - Teensy 4.x initial support for 6pwm -> - Example for v3.1 SimpleFOCShield -> - RP2040 compatibility for earlehillpower core [#234](https://github.com/simplefoc/Arduino-FOC/pull/234) [#236](https://github.com/simplefoc/Arduino-FOC/pull/236) -> - More flexible monitoring API -> - start, end and separator characters -> - decimal places (settable through commander) -> - Added machine readable verbose mode in `Commander` [#233](https://github.com/simplefoc/Arduino-FOC/pull/233) -> - *Simple**FOC**WebController* - Web based user interface for SimpleFOC by [@geekuillaume](https://github.com/geekuillaume) - [webcontroller.simplefoc.com](webcontroller.simplefoc.com) -> - bugfix - `MagneticSensorPWM` multiple occasions - [#258](https://github.com/simplefoc/Arduino-FOC/pull/258) -> - bugfix - current sense align - added offset exchange when exchanging pins -> - bugfix - trapezoid 150 fixed -> - bugfix - 4pwm on ESP8266 [#224](https://github.com/simplefoc/Arduino-FOC/pull/224) -> - Additional `InlineCurrentSense` and `LowsideCurrentSense` constructor using milliVolts per Amp [#253](https://github.com/simplefoc/Arduino-FOC/pull/253) -> - STM32L4xx current sense support by [@Triple6](https://github.com/Triple6) (discord) [#257](https://github.com/simplefoc/Arduino-FOC/pull/257) -> - phase disable in 6pwm mode -> - stm32 - software and hardware 6pwm -> - atmega328 -> - atmega2560 -> - Lag compensation using motor inductance [#246](https://github.com/simplefoc/Arduino-FOC/issues/246) -> - current control through voltage torque mode enhancement -> - extended `BLDCMotor` and `StepperMotor` constructors to receive the inductance paramerer -> - can also be set using `motor.phase_inductance` or through `Commander` -## Arduino *SimpleFOClibrary* v2.3 +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.1 +> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported) +> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells)) +> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok)) +> - Improved default trig functions (sine, cosine) - faster, smaller +> - Overridable trig functions - plug in your own optimized versions +> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287) +> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627)) +> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release) +> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28)) +> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688)) +> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040 + +## Arduino *SimpleFOClibrary* v2.3.1

@@ -78,7 +64,7 @@ This video demonstrates the *Simple**FOC**library* basic usage, electronic conne - **Cross-platform**: - Seamless code transfer from one microcontroller family to another - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers): - - Arduino: UNO, MEGA, DUE, Leonardo .... + - Arduino: UNO R4, UNO, MEGA, DUE, Leonardo, Nano, Nano33 .... - STM32 - ESP32 - Teensy @@ -132,7 +118,9 @@ Please do not hesitate to leave an issue if you have problems/advices/suggestion Pull requests are welcome, but let's first discuss them in [community forum](https://community.simplefoc.com)! -If you'd like to contribute to this porject but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. +If you'd like to contribute to this project but you are not very familiar with github, don't worry, let us know either by posting at the community forum , by posting a github issue or at our discord server. + +If you are familiar, we accept pull requests to the dev branch! ## Arduino code example This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder. diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index 83b1fb32..5a915dd8 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -62,6 +62,7 @@ void setup() { void loop() { // open loop angle movements // using motor.voltage_limit and motor.velocity_limit + // angles can be positive or negative, negative angles correspond to opposite motor direction motor.move(target_position); // user communication diff --git a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino index 84f87ab9..43fc6f73 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_velocity_example/open_loop_velocity_example.ino @@ -60,6 +60,7 @@ void loop() { // open loop velocity movement // using motor.voltage_limit and motor.velocity_limit + // to turn the motor "backwards", just set a negative target_velocity motor.move(target_velocity); // user communication diff --git a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino index 3ad87cca..27163dfb 100644 --- a/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino +++ b/examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino @@ -2,8 +2,9 @@ * Simple example intended to help users find the zero offset and natural direction of the sensor. * * These values can further be used to avoid motor and sensor alignment procedure. - * - * motor.initFOC(zero_offset, sensor_direction); + * To use these values add them to the code:"); + * motor.sensor_direction=Direction::CW; // or Direction::CCW + * motor.zero_electric_angle=1.2345; // use the real value! * * This will only work for abosolute value sensors - magnetic sensors. * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors. @@ -44,6 +45,9 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::torque; + // force direction search - because default is CW + motor.sensor_direction = Direction::UNKNOWN; + // initialize motor motor.init(); // align sensor and start FOC @@ -54,9 +58,16 @@ void setup() { Serial.println("Sensor zero offset is:"); Serial.println(motor.zero_electric_angle, 4); Serial.println("Sensor natural direction is: "); - Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW"); + Serial.println(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + + Serial.println("To use these values add them to the code:"); + Serial.print(" motor.sensor_direction="); + Serial.print(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW"); + Serial.println(";"); + Serial.print(" motor.zero_electric_angle="); + Serial.print(motor.zero_electric_angle, 4); + Serial.println(";"); - Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)"); _delay(1000); Serial.println("If motor is not moving the alignment procedure was not successfull!!"); } diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fce552c6..b72728bc 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -44,7 +44,9 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/KV // 1/sqrt(2) - rms value - KV_rating = _KV*_SQRT2; + KV_rating = NOT_SET; + if (_isset(_KV)) + KV_rating = _KV*_SQRT2; // save phase inductance phase_inductance = _inductance; @@ -90,6 +92,13 @@ void BLDCMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); @@ -124,20 +133,13 @@ void BLDCMotor::enable() FOC functions */ // FOC initialization function -int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) { +int BLDCMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); @@ -165,7 +167,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction exit_flag *= alignCurrentSense(); } } - else SIMPLEFOC_DEBUG("MOT: No current sense."); + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } } if(exit_flag){ @@ -211,7 +213,7 @@ int BLDCMotor::alignSensor() { if(!exit_flag) return exit_flag; // if unknown natural direction - if(!_isset(sensor_direction)){ + if(sensor_direction==Direction::UNKNOWN){ // find natural direction // move one electrical revolution forward @@ -250,10 +252,11 @@ int BLDCMotor::alignSensor() { // check pole pair number 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 + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - } else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip dir calib."); } // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -274,7 +277,7 @@ int BLDCMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -303,8 +306,8 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = limit_volt; // check if the zero found if(monitor_port){ - if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + if(sensor->needsSearch()) { SIMPLEFOC_DEBUG("MOT: Error: Not found!"); } + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -418,7 +421,8 @@ void BLDCMotor::move(float new_target) { // angle set point shaft_angle_sp = target; // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit); // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage @@ -543,12 +547,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { case FOCModulationType::SinePWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 366f4600..a1f196ab 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -49,7 +49,7 @@ class BLDCMotor: public FOCMotor * Function initializing FOC algorithm * and aligning sensor's and motors' zero position */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8865f57c..491a20ee 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -56,6 +56,13 @@ void StepperMotor::init() { } P_angle.limit = velocity_limit; + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + _delay(500); // enable motor SIMPLEFOC_DEBUG("MOT: Enable driver."); @@ -92,20 +99,13 @@ void StepperMotor::enable() FOC functions */ // FOC initialization function -int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) { +int StepperMotor::initFOC() { int exit_flag = 1; motor_status = FOCMotorStatus::motor_calibrating; // align motor if necessary // alignment necessary for encoders! - if(_isset(zero_electric_offset)){ - // abosolute zero offset provided - no need to align - zero_electric_angle = zero_electric_offset; - // set the sensor direction - default CW - sensor_direction = _sensor_direction; - } - // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle _delay(500); @@ -114,7 +114,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - }else SIMPLEFOC_DEBUG("MOT: No sensor."); + } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -177,10 +177,13 @@ int StepperMotor::alignSensor() { 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 + } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } - }else SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } else { + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + } // zero electric angle not known if(!_isset(zero_electric_angle)){ @@ -200,7 +203,7 @@ int StepperMotor::alignSensor() { // stop everything setPhaseVoltage(0, 0, 0); _delay(200); - }else SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); } return exit_flag; } @@ -229,7 +232,7 @@ int StepperMotor::absoluteZeroSearch() { // check if the zero found if(monitor_port){ if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!"); - else SIMPLEFOC_DEBUG("MOT: Success!"); + else { SIMPLEFOC_DEBUG("MOT: Success!"); } } return !sensor->needsSearch(); } @@ -307,7 +310,8 @@ void StepperMotor::move(float new_target) { // angle set point shaft_angle_sp = target; // calculate velocity set point - shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage @@ -357,12 +361,9 @@ void StepperMotor::move(float new_target) { void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation + float _sa, _ca; + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - float _ca = _cos(angle_el); - float _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 36f4fe3f..7eda3167 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -54,12 +54,8 @@ class StepperMotor: public FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + int initFOC() override; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index becfa499..609162c2 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -38,8 +38,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // if motor angle provided function returns signed value of the current // determine the sign of the current // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 - if(motor_electrical_angle) - sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; + if(motor_electrical_angle) { + float ct; + float st; + _sincos(motor_electrical_angle, &st, &ct); + sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + } // return current magnitude return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); } @@ -78,8 +82,9 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ } // calculate park transform - float ct = _cos(angle_el); - float st = _sin(angle_el); + float ct; + float st; + _sincos(angle_el, &st, &ct); DQCurrent_s return_current; return_current.d = i_alpha * ct + i_beta * st; return_current.q = i_beta * ct - i_alpha * st; diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 7f48037f..d1427bcf 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -82,8 +82,10 @@ float FOCMotor::electricalAngle(){ // function implementing the monitor_port setter void FOCMotor::useMonitoring(Print &print){ monitor_port = &print; //operate on the address of print + #ifndef SIMPLEFOC_DISABLE_DEBUG SimpleFOCDebug::enable(&print); SIMPLEFOC_DEBUG("MOT: Monitor enabled!"); + #endif } // utility function intended to be used with serial plotter to monitor motor variables diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index c499df4a..318be99a 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -104,12 +104,8 @@ class FOCMotor * and aligning sensor's and motors' zero position * * - If zero_electric_offset parameter is set the alignment procedure is skipped - * - * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. - * @param sensor_direction sensor natural direction - default is CW - * */ - virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + virtual int initFOC()=0; /** * Function running FOC algorithm in real-time * it calculates the gets motor angle and sets the appropriate voltages @@ -155,6 +151,7 @@ class FOCMotor // state variables float target; //!< current target value - depends of the controller + float feed_forward_velocity = 0.0f; //!< current feed forward velocity float shaft_angle;//!< current motor angle float electrical_angle;//!< current electrical angle float shaft_velocity;//!< current motor velocity @@ -208,7 +205,7 @@ class FOCMotor // sensor related variabels float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available - int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW + Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration /** * Function providing BLDCMotor class with the diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index 28511762..a80fa438 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -18,7 +18,12 @@ void Sensor::update() { float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; - // TODO handle overflow - we do need to reset vel_angle_prev_ts + if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts + vel_angle_prev = angle_prev; + vel_full_rotations = full_rotations; + vel_angle_prev_ts = angle_prev_ts; + return velocity; + } 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; diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index b8834e8e..c77eb911 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -42,6 +42,7 @@ enum Pullup : uint8_t { * */ class Sensor{ + friend class SmoothingSensor; public: /** * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 11a1810f..4cb09863 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,34 +1,30 @@ #include "foc_utils.h" -// int array instead of float array -// 4x200 points per 360 deg -// 2x storage save (int 2Byte float 4 Byte ) -// sin*10000 -const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; // function approximating the sine calculation by using fixed size array -// ~40us (float array) -// ~50us (int array) -// precision +-0.005 -// it has to receive an angle in between 0 and 2PI -float _sin(float a){ - if(a < _PI_2){ - //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873f* a)]; // float array optimized - return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized - }else if(a < _PI){ - // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized - return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized - }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized - } else { - // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized +// uses a 65 element lookup table and interpolation +// thanks to @dekutree for his work on optimizing this +__attribute__((weak)) float _sin(float a){ + // 16bit integer array for sine lookup. interpolation is used for better precision + // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size + // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) + static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; + unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + int t1, t2, frac = i & 0xff; + i = (i >> 8) & 0xff; + if (i < 64) { + t1 = sine_array[i]; t2 = sine_array[i+1]; + } + else if(i < 128) { + t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + } + else if(i < 192) { + t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; } + else { + t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + } + return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } // function approximating cosine calculation by using fixed size array @@ -36,15 +32,21 @@ float _sin(float a){ // ~56us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI -float _cos(float a){ +__attribute__((weak)) float _cos(float a){ float a_sin = a + _PI_2; a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; return _sin(a_sin); } +__attribute__((weak)) void _sincos(float a, float* s, float* c){ + *s = _sin(a); + *c = _cos(a); +} + + // normalizing radian angle to [0,2PI] -float _normalizeAngle(float angle){ +__attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } @@ -57,15 +59,13 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { // square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root -float _sqrtApprox(float number) {//low in fat - long i; - float y; +__attribute__((weak)) float _sqrtApprox(float number) {//low in fat // float x; // const float f = 1.5F; // better precision // x = number * 0.5F; - y = number; - i = * ( long * ) &y; + float y = number; + long i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index e1e0b153..0efe3b59 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,6 +12,7 @@ #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) #define _UNUSED(v) (void) (v) +#define _powtwo(x) (1 << (x)) // utility defines #define _2_SQRT3 1.15470053838f @@ -28,7 +29,7 @@ #define _PI_6 0.52359877559f #define _RPM_TO_RADS 0.10471975512f -#define NOT_SET -12345.0 +#define NOT_SET -12345.0f #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 @@ -71,6 +72,13 @@ float _sin(float a); * @param a angle in between 0 and 2PI */ float _cos(float a); +/** + * Function returning both sine and cosine of the angle in one call. + * Internally it uses the _sin and _cos functions, but you may wish to + * provide your own implementation which is more optimized. + */ +void _sincos(float a, float* s, float* c); + /** * normalizing radian angle to [0,2PI] diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 7aa8865f..ee6be9f9 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -593,6 +593,7 @@ bool Commander::isSentinel(char ch) else if (ch == '\r') { printVerbose(F("Warn: \\r detected! \n")); + return true; // lets still consider it to end the line... } return false; } diff --git a/src/communication/Commander.h b/src/communication/Commander.h index 3633a510..91e3dc45 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -240,6 +240,7 @@ class Commander */ void motion(FOCMotor* motor, char* user_cmd, char* separator = (char *)" "); + bool isSentinel(char ch); private: // Subscribed command callback variables CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number @@ -294,7 +295,6 @@ class Commander void printError(); - bool isSentinel(char ch); }; diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 9ae13362..f9691fd3 100644 --- a/src/communication/StepDirListener.h +++ b/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) || defined(CORE_TEENSY) || defined(NRF52_SERIES) +#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR) #define PinStatus int #endif diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index d0592ef2..635535fa 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -62,6 +62,8 @@ int GenericCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; + if (!initialized) return 0; + // // set phase A active and phases B and C down // driver->setPwm(voltage, 0, 0); // _delay(200); diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 6a41484a..492b3ac9 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -1,4 +1,5 @@ #include "InlineCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // InlineCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -93,6 +94,13 @@ int InlineCurrentSense::driverAlign(float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: No driver linked!"); + return 0; + } + + if (!initialized) return 0; + if(_isset(pinA)){ // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 21df16e1..a706beeb 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -1,4 +1,5 @@ #include "LowsideCurrentSense.h" +#include "communication/SimpleFOCDebug.h" // LowsideCurrentSensor constructor // - shunt_resistor - shunt resistor value // - gain - current-sense op-amp gain @@ -35,6 +36,12 @@ LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int // Lowside sensor init function int LowsideCurrentSense::init(){ + + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: Driver not linked!"); + return 0; + } + // configure ADC variables params = _configureADCLowSide(driver->params,pinA,pinB,pinC); // if init failed return fail @@ -89,10 +96,12 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted int LowsideCurrentSense::driverAlign(float voltage){ - + int exit_flag = 1; if(skip_align) return exit_flag; + if (!initialized) return 0; + if(_isset(pinA)){ // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index dd336416..3df9dff6 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -2,7 +2,7 @@ #include "../../../drivers/hardware_api.h" #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "esp32_adc_driver.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 2e956770..a2f58ac3 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -6,7 +6,8 @@ #include "freertos/task.h" #include "rom/ets_sys.h" #include "esp_attr.h" -#include "esp_intr.h" +//#include "esp_intr.h" // deprecated +#include "esp_intr_alloc.h" #include "soc/rtc_io_reg.h" #include "soc/rtc_cntl_reg.h" #include "soc/sens_reg.h" diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index 7bcd23aa..d84c2fd5 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -10,6 +10,7 @@ #include "hardware/dma.h" #include "hardware/irq.h" #include "hardware/pwm.h" +#include "hardware/adc.h" /* Singleton instance of the ADC engine */ @@ -24,6 +25,7 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-( // like this we either have to block interrupts, or of course have the chance of reading across // new ADC conversions, which probably won't improve the accuracy. + _UNUSED(cs_params); if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) { return engine.lastResults.raw[pinA-26]*engine.adc_conv; @@ -35,6 +37,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params) { void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) { + _UNUSED(driver_params); + if( _isset(pinA) ) engine.addPin(pinA); if( _isset(pinB) ) @@ -163,7 +167,6 @@ bool RP2040ADCEngine::init() { 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); @@ -241,6 +244,7 @@ void RP2040ADCEngine::start() { void RP2040ADCEngine::stop() { adc_run(false); + irq_set_enabled(DMA_IRQ_0, false); dma_channel_abort(readDMAChannel); // if (triggerPWMSlice>=0) // dma_channel_abort(triggerDMAChannel); diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h index b673fc30..ae28bb26 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h @@ -22,6 +22,9 @@ * Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz. * After starting the engine it will continuously sample and provide new values at the configured rate. * + * The default sampling rate is 20kHz, which is suitable for 2 channels assuming you a 5kHz main loop speed (a new measurement is used per + * main loop iteration). + * * Low-side sensing is currently not supported. * * The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current @@ -74,7 +77,7 @@ class RP2040ADCEngine { ADCResults getLastResults(); // TODO find a better API and representation for this - int samples_per_second = 0; // leave at 0 to convert in tight loop + int samples_per_second = 20000; // 20kHz default (assuming 2 shunts and 5kHz loop speed), set to 0 to convert in tight loop float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float //int triggerPWMSlice = -1; diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 5ad3653c..8a7fbbec 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__AVR_ATmega2560__) || defined(AVR_ATmega1280) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega2560 or Arduino/ATmega1280") +#pragma message("") + + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 385f6e64..53fb1086 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -2,6 +2,10 @@ #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega328 ATmega168 ATmega328PB") +#pragma message("") + #define _PWM_FREQUENCY 32000 #define _PWM_FREQUENCY_MAX 32000 #define _PWM_FREQUENCY_MIN 4000 diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b9836e16..b4ad3101 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -3,6 +3,10 @@ #if defined(__AVR_ATmega32U4__) +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/ATmega32U4") +#pragma message("") + // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ // High PWM frequency diff --git a/src/drivers/hardware_specific/due_mcu.cpp b/src/drivers/hardware_specific/due_mcu.cpp index 80ef82d9..4397114b 100644 --- a/src/drivers/hardware_specific/due_mcu.cpp +++ b/src/drivers/hardware_specific/due_mcu.cpp @@ -2,6 +2,12 @@ #if defined(__arm__) && defined(__SAM3X8E__) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Due") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index 4f8ad131..10497876 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,6 +5,13 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") +#pragma message("") + + #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" @@ -81,6 +88,7 @@ typedef struct ESP32MCPWMDriverParams { mcpwm_unit_t mcpwm_unit; mcpwm_operator_t mcpwm_operator1; mcpwm_operator_t mcpwm_operator2; + float deadtime; } ESP32MCPWMDriverParams; diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 76027459..a454c052 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -2,6 +2,10 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) ) +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP32 LEDC driver") +#pragma message("") + #include "driver/ledc.h" #define _PWM_FREQUENCY 25000 // 25khz diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp index fecb712c..0dc23c10 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp @@ -2,6 +2,10 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif + // define bldc motor slots array bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A @@ -41,7 +45,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_config_t pwm_config; pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings @@ -49,10 +53,28 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm if (_isset(dead_zone)){ // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + + // When using hardware deadtime, setting the phase_state parameter is not supported. + #if SIMPLEFOC_ESP32_HW_DEADTIME == true + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_type_t pwm_mode; + if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver + else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); + #else // Software deadtime + for (int i = 0; i < 3; i++){ + if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside + else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver + + if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside + else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver + } + #endif + } _delay(100); @@ -369,7 +391,10 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { .pwm_frequency = pwm_frequency, .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit + .mcpwm_unit = m_slot.mcpwm_unit, + .mcpwm_operator1 = m_slot.mcpwm_operator1, + .mcpwm_operator2 = m_slot.mcpwm_operator2, + .deadtime = _isset(dead_zone) ? dead_zone : 0 }; return params; } @@ -381,15 +406,26 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // se the PWM on the slot timers + // set the PWM on the slot timers // transform duty cycle from [0,1] to [0,100.0] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); - _UNUSED(phase_state); + #if SIMPLEFOC_ESP32_HW_DEADTIME == true + // Hardware deadtime does deadtime insertion internally + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); + _UNUSED(phase_state); + #else + float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); + mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); + #endif } #endif diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 15b9c82d..23d94110 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -2,6 +2,12 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266) + +#pragma message("") +#pragma message("SimpleFOC: compiling for ESP8266") +#pragma message("") + + #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 50000 // 50khz diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index d92004e9..b6bc2f06 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,7 +1,8 @@ + #include "../hardware_api.h" // if the mcu doen't have defiend analogWrite -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(analogWrite) __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif diff --git a/src/drivers/hardware_specific/nrf52_mcu.cpp b/src/drivers/hardware_specific/nrf52_mcu.cpp index f53eada0..a20b828a 100644 --- a/src/drivers/hardware_specific/nrf52_mcu.cpp +++ b/src/drivers/hardware_specific/nrf52_mcu.cpp @@ -3,6 +3,10 @@ #if defined(NRF52_SERIES) +#pragma message("") +#pragma message("SimpleFOC: compiling for NRF52") +#pragma message("") + #define PWM_CLK (16000000) #define PWM_FREQ (40000) diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index b426f0ed..ad16646a 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -3,6 +3,12 @@ #if defined(TARGET_PORTENTA_H7) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Portenta_H7") +#pragma message("") + + #include "pwmout_api.h" #include "pinDefinitions.h" #include "platform/mbed_critical.h" diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp new file mode 100644 index 00000000..f90a4c56 --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.cpp @@ -0,0 +1,609 @@ + +#include "./renesas.h" + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Arduino/Renesas (UNO R4)") +#pragma message("") + + + +#include "communication/SimpleFOCDebug.h" +#include "FspTimer.h" + +#define GPT_OPEN (0x00475054ULL) + +/* + We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit) + Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary. + + So each timer channel can handle one half-bridge, using either a single (3-PWM) or + two complimentary PWM signals (6-PWM). + + For 1-PWM through 4-PWM, we need as many channels as PWM signals, and we can use + either output A or B of the timer (we can set the polarity) - but not both. + + For 6-PWM we need 3 channels, and use both outputs A and B of each channel, then + we can do hardware dead-time. + Or we can use seperate channels for high and low side, with software dead-time. + Each phase can be either hardware (1 channel) or software (2 channels) dead-time + individually, they don't all have to be one or the other. + + Selected channels can be started together, so we can keep the phases in sync for + low-side current sensing and software 6-PWM. + + The event system should permit low side current sensing without needing interrupts, + but this will be handled by the current sense driver. + + Supported: + - arbitrary PWM frequencies between 1Hz (minimum we can set with our integer based API) + and around 48kHz (more would be possible but the range will be low) + - PWM range at 24kHz (default) is 1000 + - PWM range at 48kHz is 500 + - polarity setting is supported, in all modes + - phase state setting is supported, in 3-PWM, 6-PWM hardware dead-time and 6-PWM software dead-time + + TODOs: + - change setDutyCycle to use register access for speed + - add event system support for low-side current sensing + - perhaps add support to reserve timers used also in + Arduino Pwm.h code, for compatibility with analogWrite() + - check if there is a better way for phase-state setting + */ + + +// track which channels are used +bool channel_used[GPT_HOWMANY] = { false }; + + +struct RenesasTimerConfig { + timer_cfg_t timer_cfg; + gpt_instance_ctrl_t ctrl; + gpt_extended_cfg_t ext_cfg; + gpt_extended_pwm_cfg_t pwm_cfg; + gpt_io_pin_t duty_pin; +}; + +struct ClockDivAndRange { + timer_source_div_t clk_div; + uint32_t range; +}; + +ClockDivAndRange getClockDivAndRange(uint32_t pwm_frequency, uint8_t timer_channel) { + ClockDivAndRange result; + uint32_t max_count = (timer_channel < GTP32_HOWMANY)? 4294967295 : 65535; + uint32_t freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD); + float range = (float) freq_hz / ((float) pwm_frequency * 2.0f); + if(range / 1.0 < max_count) { + result.range = (uint32_t) (range / 1.0); + result.clk_div = TIMER_SOURCE_DIV_1; + } + else if (range / 2.0 < max_count) { + result.range = (uint32_t) (range / 2.0); + result.clk_div = TIMER_SOURCE_DIV_2; + } + else if(range / 4.0 < max_count) { + result.range = (uint32_t) (range / 4.0); + result.clk_div = TIMER_SOURCE_DIV_4; + } + else if(range / 8.0 < max_count) { + result.range = (uint32_t) (range / 8.0 ); + result.clk_div = TIMER_SOURCE_DIV_8; + } + else if(range / 16.0 < max_count) { + result.range = (uint32_t) (range / 16.0 ); + result.clk_div = TIMER_SOURCE_DIV_16; + } + else if (range / 32.0 < max_count) { + result.range = (uint32_t) (range / 32.0 ); + result.clk_div = TIMER_SOURCE_DIV_32; + } + else if(range / 64.0 < max_count) { + result.range = (uint32_t) (range / 64.0 ); + result.clk_div = TIMER_SOURCE_DIV_64; + } + else if(range / 128.0 < max_count) { + result.range = (uint32_t) (range / 128.0 ); + result.clk_div = TIMER_SOURCE_DIV_128; + } + else if(range / 256.0 < max_count) { + result.range = (uint32_t) (range / 256.0 ); + result.clk_div = TIMER_SOURCE_DIV_256; + } + else if(range / 512.0 < max_count) { + result.range = (uint32_t) (range / 512.0 ); + result.clk_div = TIMER_SOURCE_DIV_512; + } + else if(range / 1024.0 < max_count) { + result.range = (uint32_t) (range / 1024.0 ); + result.clk_div = TIMER_SOURCE_DIV_1024; + } + else { + SimpleFOCDebug::println("DRV: PWM frequency too low"); + } + return result; +}; + + +bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool active_high, bool complementary = false, bool complementary_active_high = true) { + uint8_t pin = params->pins[index]; + uint8_t pin_C; + std::array pinCfgs = getPinCfgs(pin, PIN_CFG_REQ_PWM); + std::array pinCfgs_C; + if(pinCfgs[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs[0])) { + SIMPLEFOC_DEBUG("DRV: AGT timer not supported"); + return false; + } + // get the timer channel + uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]); + // check its not used + if (channel_used[timer_channel]) { + SIMPLEFOC_DEBUG("DRV: channel in use"); + return false; + } + + if (complementary) { + pin_C = params->pins[index+1]; + pinCfgs_C = getPinCfgs(pin_C, PIN_CFG_REQ_PWM); + if(pinCfgs_C[0] == 0) { + SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin_C); + return false; + } + if (IS_PIN_AGT_PWM(pinCfgs_C[0]) || GET_CHANNEL(pinCfgs_C[0])!=timer_channel) { + SIMPLEFOC_DEBUG("DRV: comp. channel different"); + return false; + } + } + TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B; + if (complementary) { + TimerPWMChannel_t pwm_output_C = IS_PWM_ON_A(pinCfgs_C[0]) ? CHANNEL_A : CHANNEL_B; + if (pwm_output_C != CHANNEL_A || pwm_output != CHANNEL_B) { + SIMPLEFOC_DEBUG("DRV: output A/B mismatch"); + return false; + } + } + + // configure GPIO pin + fsp_err_t err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if ((err == FSP_SUCCESS) && complementary) + err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin_C].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1)); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin config failed"); + return false; + } + + + // configure timer channel - frequency / top value + ClockDivAndRange timings = getClockDivAndRange(params->pwm_frequency, timer_channel); + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("---PWM Config---"); + SimpleFOCDebug::println("DRV: pwm pin: ", pin); + if (complementary) + SimpleFOCDebug::println("DRV: compl. pin: ", pin_C); + SimpleFOCDebug::println("DRV: pwm channel: ", timer_channel); + SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println(complementary?"A+B":((pwm_output==CHANNEL_A)?"A":"B")); + SimpleFOCDebug::println("DRV: pwm freq: ", (int)params->pwm_frequency); + SimpleFOCDebug::println("DRV: pwm range: ", (int)timings.range); + SimpleFOCDebug::println("DRV: pwm clkdiv: ", timings.clk_div); + #endif + + RenesasTimerConfig* t = new RenesasTimerConfig(); + // configure timer channel - count mode + t->timer_cfg.channel = timer_channel; + t->timer_cfg.mode = TIMER_MODE_TRIANGLE_WAVE_SYMMETRIC_PWM; + t->timer_cfg.source_div = timings.clk_div; + t->timer_cfg.period_counts = timings.range; + t->timer_cfg.duty_cycle_counts = 0; + t->timer_cfg.p_callback = nullptr; + t->timer_cfg.p_context = nullptr; + t->timer_cfg.p_extend = &(t->ext_cfg); + t->timer_cfg.cycle_end_ipl = BSP_IRQ_DISABLED; + t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR; + + t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg); + t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED; + t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR; + t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0; + t->pwm_cfg.output_disable = GPT_OUTPUT_DISABLE_NONE; + t->pwm_cfg.adc_trigger = GPT_ADC_TRIGGER_NONE; + t->pwm_cfg.dead_time_count_up = 0; + t->pwm_cfg.dead_time_count_down = 0; + t->pwm_cfg.adc_a_compare_match = 0; + t->pwm_cfg.adc_b_compare_match = 0; + t->pwm_cfg.interrupt_skip_source = GPT_INTERRUPT_SKIP_SOURCE_NONE; + t->pwm_cfg.interrupt_skip_count = GPT_INTERRUPT_SKIP_COUNT_0; + t->pwm_cfg.interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE; + t->pwm_cfg.gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + t->pwm_cfg.gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED; + // configure dead-time if both outputs are used + if (complementary) { + uint32_t dt = params->dead_zone * timings.range; + t->pwm_cfg.dead_time_count_up = dt; + t->pwm_cfg.dead_time_count_down = dt; + } + + // configure timer channel - outputs and polarity + t->ext_cfg.gtior_setting.gtior = 0L; + if (!complementary) { + if(pwm_output == CHANNEL_A) { + t->duty_pin = GPT_IO_PIN_GTIOCA; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01; + // t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0; + // t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0; + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCB; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtioca.output_enabled = false; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; + } + } + else { + t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB; + t->ext_cfg.gtioca.output_enabled = true; + t->ext_cfg.gtiocb.output_enabled = true; + t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (!complementary_active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.oadflt = !complementary_active_high ? 0x00 : 0x01; + t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10); + t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01; + } + + // lets stop the timer in case its running + if (GPT_OPEN == t->ctrl.open) { + if (R_GPT_Stop(&(t->ctrl)) != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: timer stop failed"); + return false; + } + } + + memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t)); + err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg)); + if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) { + SIMPLEFOC_DEBUG("DRV: open failed"); + return false; + } + #if defined(SIMPLEFOC_RESENSAS_DEBUG) + if (err == FSP_ERR_ALREADY_OPEN) { + SimpleFOCDebug::println("DRV: timer already open"); + } + #endif + err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: period set failed"); + return false; + } + err = R_GPT_OutputEnable(&(t->ctrl), t->duty_pin); + if (err != FSP_SUCCESS) { + SIMPLEFOC_DEBUG("DRV: pin enable failed"); + return false; + } + + channel_used[timer_channel] = true; + params->timer_config[index] = t; + params->channels[index] = timer_channel; + if (complementary) { + params->timer_config[index+1] = t; + params->channels[index+1] = timer_channel; + } + + return true; +} + + +// start the timer channels for the motor, synchronously +bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) { + uint32_t mask = 0; + for (int i = 0; i < num_channels; i++) { + // RenesasTimerConfig* t = params->timer_config[i]; + // if (R_GPT_Start(&(t->ctrl)) != FSP_SUCCESS) { + // SIMPLEFOC_DEBUG("DRV: timer start failed"); + // return false; + // } + mask |= (1 << params->channels[i]); +#if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: starting timer: ", params->channels[i]); +#endif + } + params->timer_config[0]->ctrl.p_reg->GTSTR |= mask; + #if defined(SIMPLEFOC_RENESAS_DEBUG) + SimpleFOCDebug::println("DRV: timers started"); + #endif + return true; +} + + +// check if the given pins are on the same timer channel +bool isHardware6Pwm(const int pin1, const int pin2) { + std::array pinCfgs1 = getPinCfgs(pin1, PIN_CFG_REQ_PWM); + std::array pinCfgs2 = getPinCfgs(pin2, PIN_CFG_REQ_PWM); + if(pinCfgs1[0] == 0 || pinCfgs2[0] == 0) + return false; + if (IS_PIN_AGT_PWM(pinCfgs1[0]) || IS_PIN_AGT_PWM(pinCfgs2[0])) + return false; + uint8_t timer_channel1 = GET_CHANNEL(pinCfgs1[0]); + uint8_t timer_channel2 = GET_CHANNEL(pinCfgs2[0]); + return timer_channel1==timer_channel2; +} + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 1); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (!success) + success &= startTimerChannels(params, 2); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA; params->pins[1] = pinB; params->pins[2] = pinC; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 3); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pin1A; params->pins[1] = pin1B; params->pins[2] = pin2A; params->pins[3] = pin2B; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + bool success = true; + success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, SIMPLEFOC_PWM_ACTIVE_HIGH); + if (success) + success = startTimerChannels(params, 4); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + +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){ + RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams; + params->pins[0] = pinA_h; params->pins[1] = pinA_l; params->pins[2] = pinB_h; params->pins[3] = pinB_l; params->pins[4] = pinC_h; params->pins[5] = pinC_l; + params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency; + params->dead_zone = (dead_zone==NOT_SET)?RENESAS_DEFAULT_DEAD_ZONE:dead_zone; + + bool success = true; + if (isHardware6Pwm(pinA_h, pinA_l)) + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 1, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); // reverse polarity on low side gives desired active high/low behaviour + } + + if (isHardware6Pwm(pinB_h, pinB_l)) + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 3, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } + + if (isHardware6Pwm(pinC_h, pinC_l)) + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + else { + success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); + success &= configureTimerPin(params, 5, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); + } + + if (success) + success = startTimerChannels(params, 6); + if (!success) + return SIMPLEFOC_DRIVER_INIT_FAILED; + return params; +} + + + + +void _writeDutyCycle1PWM(float dc_a, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + uint32_t duty_cycle_counts = (uint32_t)(dc_1a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + duty_cycle_counts = (uint32_t)(dc_1b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + duty_cycle_counts = (uint32_t)(dc_2a * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + t = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + duty_cycle_counts = (uint32_t)(dc_2b * (float)(t->timer_cfg.period_counts)); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } +} + + + +void _setSinglePhaseState(RenesasTimerConfig* hi, RenesasTimerConfig* lo, PhaseState state) { + gpt_gtior_setting_t gtior; + gtior.gtior = hi->ctrl.p_reg->GTIOR; + bool on = (state==PHASE_ON) || (state==PHASE_HI); + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA_AND_GTIOCB) { + bool ch = false; + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + ch = true; + } // B is high side + on = (state==PHASE_ON) || (state==PHASE_LO); + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + ch = true; + } + if (ch) + hi->ctrl.p_reg->GTIOR = gtior.gtior; + return; + } + + if (hi->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (hi->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + hi->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + + gtior.gtior = lo->ctrl.p_reg->GTIOR; + on = (state==PHASE_ON) || (state==PHASE_LO); + if (lo->duty_pin == GPT_IO_PIN_GTIOCA) { + if (gtior.gtior_b.oae != on) { + gtior.gtior_b.oae = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + else if (lo->duty_pin == GPT_IO_PIN_GTIOCB) { + if (gtior.gtior_b.obe != on) { + gtior.gtior_b.obe = on; + lo->ctrl.p_reg->GTIOR = gtior.gtior; + } + } + +} + + +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0]; + RenesasTimerConfig* t1 = ((RenesasHardwareDriverParams*)params)->timer_config[1]; + uint32_t dt = (uint32_t)(((RenesasHardwareDriverParams*)params)->dead_zone * (float)(t->timer_cfg.period_counts)); + uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts)); + bool hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[0] == ((RenesasHardwareDriverParams*)params)->channels[1]; + uint32_t dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[0]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[2]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[3]; + duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[2] == ((RenesasHardwareDriverParams*)params)->channels[3]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[1]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + + t = ((RenesasHardwareDriverParams*)params)->timer_config[4]; + t1 = ((RenesasHardwareDriverParams*)params)->timer_config[5]; + duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts)); + hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[4] == ((RenesasHardwareDriverParams*)params)->channels[5]; + dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0; + _setSinglePhaseState(t, t1, phase_state[2]); + if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) { + // error + } + if (!hw_deadtime) { + if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) { + // error + } + } + +} + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/renesas/renesas.h b/src/drivers/hardware_specific/renesas/renesas.h new file mode 100644 index 00000000..91bacdce --- /dev/null +++ b/src/drivers/hardware_specific/renesas/renesas.h @@ -0,0 +1,28 @@ +#pragma once + + +#include "../../hardware_api.h" + + +#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA) + +// uncomment to enable debug output from Renesas driver +// can set this as build-flag in Arduino IDE or PlatformIO +#define SIMPLEFOC_RENESAS_DEBUG + +#define RENESAS_DEFAULT_PWM_FREQUENCY 24000 +#define RENESAS_DEFAULT_DEAD_ZONE 0.05f + +struct RenesasTimerConfig; + +typedef struct RenesasHardwareDriverParams { + uint8_t pins[6]; + uint8_t channels[6]; + RenesasTimerConfig* timer_config[6]; + long pwm_frequency; + float dead_zone; +} RenesasHardwareDriverParams; + + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index a2970c72..eee5797b 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -4,15 +4,22 @@ */ #if defined(TARGET_RP2040) + +#pragma message("") +#pragma message("SimpleFOC: compiling for RP2040") +#pragma message("") + + #define SIMPLEFOC_DEBUG_RP2040 #include "../../hardware_api.h" #include "./rp2040_mcu.h" #include "hardware/pwm.h" +#include "hardware/clocks.h" #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 -#define _PWM_FREQUENCY_MIN 5000 +#define _PWM_FREQUENCY_MIN 1 @@ -30,11 +37,12 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para params->pins[index] = pin; params->slice[index] = slice; params->chan[index] = chan; - 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 > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 + uint32_t sysclock_hz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS) * 1000; + uint32_t factor = 4096 * 2 * pwm_frequency; + uint32_t div = sysclock_hz / factor; + if (sysclock_hz % factor !=0) div+=1; + if (div < 16) div = 16; + uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); SimpleFOCDebug::print(pin); @@ -44,9 +52,17 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para SimpleFOCDebug::print((int)chan); SimpleFOCDebug::print(" frequency "); SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" divisor "); + SimpleFOCDebug::print((int)(div>>4)); + SimpleFOCDebug::print("."); + SimpleFOCDebug::print((int)(div&0xF)); SimpleFOCDebug::print(" top value "); - SimpleFOCDebug::println(wrapvalue); + SimpleFOCDebug::println((int)wrapvalue); #endif + if (wrapvalue < 999) + SimpleFOCDebug::println("Warning: PWM resolution is low."); + pwm_set_clkdiv_int_frac(slice, div>>4, div&0xF); + pwm_set_phase_correct(slice, true); pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; if (invert) { diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h index c3b4fe51..bbfb3873 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -2,6 +2,8 @@ #pragma once +#include "Arduino.h" + #if defined(TARGET_RP2040) diff --git a/src/drivers/hardware_specific/samd/samd21_mcu.cpp b/src/drivers/hardware_specific/samd/samd21_mcu.cpp index d9bb5b9a..d59a3098 100644 --- a/src/drivers/hardware_specific/samd/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd21_mcu.cpp @@ -7,6 +7,10 @@ #ifdef _SAMD21_ +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD21") +#pragma message("") + #ifndef TCC3_CH0 #define TCC3_CH0 NOT_ON_TIMER diff --git a/src/drivers/hardware_specific/samd/samd51_mcu.cpp b/src/drivers/hardware_specific/samd/samd51_mcu.cpp index 4bdf036b..71bf0b81 100644 --- a/src/drivers/hardware_specific/samd/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd/samd51_mcu.cpp @@ -6,6 +6,11 @@ #if defined(_SAMD51_)||defined(_SAME51_) +#pragma message("") +#pragma message("SimpleFOC: compiling for SAMD51/SAME51") +#pragma message("") + + // expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency. // for custom boards or overclockers you can override it using this define. diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index d16808f6..65dad9f4 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,6 +4,10 @@ #if defined(_STM32_DEF_) +#pragma message("") +#pragma message("SimpleFOC: compiling for STM32") +#pragma message("") + //#define SIMPLEFOC_STM32_DEBUG @@ -287,6 +291,11 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* // 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); + if (dead_time>255) dead_time = 255; + if (dead_time==0 && dead_zone>0) { + dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large + SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max"); + } LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp index 742d79df..3a7b5de5 100644 --- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp @@ -10,6 +10,10 @@ #if defined(__arm__) && defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)) +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 3.x") +#pragma message("") + // pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c #if defined(__MK20DX128__) #define FTM0_CH0_PIN 22 @@ -206,7 +210,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - Stepper motor - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); // transform duty cycle from [0,1] to [0,255] // phase A analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index c961ad06..322d5a34 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -7,6 +7,12 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + // half_cycle of the PWM variable int half_cycle = 0; @@ -290,7 +296,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - Stepper motor - 6PWM setting // - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 96057d82..fa4b8e73 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -99,29 +99,23 @@ void Encoder::handleIndex() { } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { - // do nothing for Encoder + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + noInterrupts(); + angle_prev_ts = pulse_timestamp; + long copy_pulse_counter = pulse_counter; + interrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = copy_pulse_counter / (int)cpr; + angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); } /* Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getAngle(); -} -// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost -float Encoder::getMechanicalAngle(){ - return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); -} - -float Encoder::getAngle(){ - return _2PI * (pulse_counter) / ((float)cpr); -} -double Encoder::getPreciseAngle(){ - return _2PI * (pulse_counter) / ((double)cpr); -} -int32_t Encoder::getFullRotations(){ - return pulse_counter / (int)cpr; + return _2PI * (pulse_counter) / ((float)cpr); } @@ -131,6 +125,11 @@ int32_t Encoder::getFullRotations(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + noInterrupts(); + long copy_pulse_counter = pulse_counter; + long copy_pulse_timestamp = pulse_timestamp; + interrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -139,8 +138,8 @@ float Encoder::getVelocity(){ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6f; - long dN = pulse_counter - prev_pulse_counter; + float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f; + long dN = copy_pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) // dN - impulses received @@ -161,7 +160,7 @@ float Encoder::getVelocity(){ prev_timestamp_us = timestamp_us; // save velocity calculation variables prev_Th = Th; - prev_pulse_counter = pulse_counter; + prev_pulse_counter = copy_pulse_counter; return velocity; } diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 91427a83..af6a5ab6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,12 +61,8 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getAngle() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; virtual void update() override; /** diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 7d1d9f64..38767d59 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -94,8 +94,16 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { -float HallSensor::getSensorAngle() { - return getAngle(); +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void HallSensor::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + noInterrupts(); + angle_prev_ts = pulse_timestamp; + long last_electric_rotations = electric_rotations; + int8_t last_electric_sector = electric_sector; + interrupts(); + angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -104,8 +112,8 @@ float HallSensor::getSensorAngle() { Shaft angle calculation TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getMechanicalAngle() { - return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; +float HallSensor::getSensorAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } /* @@ -113,38 +121,18 @@ float HallSensor::getMechanicalAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ + noInterrupts(); + long last_pulse_timestamp = pulse_timestamp; long last_pulse_diff = pulse_diff; - if (last_pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > last_pulse_diff) ) { // last velocity isn't accurate if too old + interrupts(); + if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old return 0; } else { - float vel = direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); - // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 - if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero - return vel; + return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); } } - - -float HallSensor::getAngle() { - return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; -} - - -double HallSensor::getPreciseAngle() { - return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; -} - - -int32_t HallSensor::getFullRotations() { - return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); -} - - - - - // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index d6b4493a..ad50c7d5 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -53,14 +53,12 @@ class HallSensor: public Sensor{ int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation + /** Interrupt-safe update */ + void update() override; /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index af93b8cc..ac2b273b 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -28,7 +28,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) - cpr = pow(2, _bit_resolution); + cpr = _powtwo(_bit_resolution); // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits @@ -48,7 +48,7 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ // angle read register of the magnetic sensor angle_register_msb = config.angle_register; // register maximum value (counts per revolution) - cpr = pow(2, config.bit_resolution); + cpr = _powtwo(config.bit_resolution); int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; @@ -95,7 +95,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { // notify the device that is aboout to be read wire->beginTransmission(chip_address); wire->write(angle_reg_msb); - wire->endTransmission(false); + currWireError = wire->endTransmission(false); // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index fa7ec96b..f8b189fa 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -51,6 +51,9 @@ class MagneticSensorI2C: public Sensor{ /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin , byte scl_pin ); + /** current error code from Wire endTransmission() call **/ + uint8_t currWireError = 0; + private: float cpr; //!< Maximum range of the magnetic sensor uint16_t lsb_used; //!< Number of bits used in LSB register diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 02798ad3..cf211644 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -56,10 +56,21 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + pulse_timestamp = _micros(); this->Sensor::init(); // call base class init } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void MagneticSensorPWM::update() { + if (is_interrupt_based) + noInterrupts(); + Sensor::update(); + angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication + if (is_interrupt_based) + interrupts(); +} + // get current angle (rad) float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor @@ -73,7 +84,8 @@ float MagneticSensorPWM::getSensorAngle(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way - pulse_length_us = pulseIn(pinPWM, HIGH); + pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse + pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable? } return pulse_length_us; } @@ -84,7 +96,10 @@ void MagneticSensorPWM::handlePWM() { unsigned long now_us = _micros(); // if falling edge, calculate the pulse length - if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; + if (!digitalRead(pinPWM)) { + pulse_length_us = now_us - last_call_us; + pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp + } // save the currrent timestamp for the next call last_call_us = now_us; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 77e82459..48492c84 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -32,6 +32,9 @@ class MagneticSensorPWM: public Sensor{ void init(); int pinPWM; + + // Interrupt-safe update + void update() override; // get current angle (rad) float getSensorAngle() override; @@ -62,6 +65,7 @@ class MagneticSensorPWM: public Sensor{ // time tracking variables unsigned long last_call_us; // unsigned long pulse_length_us; + unsigned long pulse_timestamp; }; diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index 7341c89a..52febc38 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -31,13 +31,13 @@ MagneticSensorSPIConfig_s MA730_SPI = { // cs - SPI chip select pin // _bit_resolution sensor resolution bit number // _angle_register - (optional) angle read register - default 0x3FFF -MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ +MagneticSensorSPI::MagneticSensorSPI(int cs, int _bit_resolution, int _angle_register){ chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) - cpr = pow(2,_bit_resolution); + cpr = _powtwo(_bit_resolution); spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; @@ -52,7 +52,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ // angle read register of the magnetic sensor angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) - cpr = pow(2, config.bit_resolution); + cpr = _powtwo(config.bit_resolution); spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index a77e7698..03ebde44 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -30,7 +30,7 @@ class MagneticSensorSPI: public Sensor{ * @param bit_resolution sensor resolution bit number * @param angle_register (optional) angle read register - default 0x3FFF */ - MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0); + MagneticSensorSPI(int cs, int bit_resolution, int angle_register = 0); /** * MagneticSensorSPI class constructor * @param config SPI config