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