diff --git a/src/HybridStepperMotor.cpp b/src/HybridStepperMotor.cpp new file mode 100644 index 00000000..2e70965f --- /dev/null +++ b/src/HybridStepperMotor.cpp @@ -0,0 +1,588 @@ +#include "HybridStepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + +// HybridStepperMotor(int pp) +// - pp - pole pair number +// - R - motor phase resistance +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) + : FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV * _SQRT2; + // save phase inductance + phase_inductance = _inductance; + + // torque control type is voltage by default + // current and foc_current not supported yet + torque_controller = TorqueControlType::voltage; +} + +/** + Link the driver which controls the motor +*/ +void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +{ + driver = _driver; +} + +// init hardware pins +int HybridStepperMotor::init() +{ + if (!driver || !driver->initialized) + { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return 0; + } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + + // sanity check for the voltage limit configuration + if (voltage_limit > driver->voltage_limit) + voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if (voltage_sensor_align > voltage_limit) + voltage_sensor_align = voltage_limit; + + // update the controller limits + if (_isset(phase_resistance)) + { + // velocity control loop controls current + PID_velocity.limit = current_limit; + } + else + { + PID_velocity.limit = voltage_limit; + } + 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."); + enable(); + _delay(500); + + motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; +} + +// disable motor driver +void HybridStepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable driver + driver->disable(); + // motor status update + enabled = 0; +} +// enable motor driver +void HybridStepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + // motor status update + enabled = 1; +} + +/** + * FOC functions + */ +int HybridStepperMotor::initFOC() { + int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + + // align motor if necessary + // alignment necessary for encoders! + // sensor and motor alignment - can be skipped + // by setting motor.sensor_direction and motor.zero_electric_angle + if(sensor){ + exit_flag *= alignSensor(); + // added the shaft_angle update + sensor->update(); + shaft_angle = shaftAngle(); + + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + current_sense->driver_type = DriverType::Hybrid; + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } + + if(exit_flag){ + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; + }else{ + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; + disable(); + } + + return exit_flag; +} + +// Calibrate the motor and current sense phases +int HybridStepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + +// Encoder alignment to electrical 0 angle +int HybridStepperMotor::alignSensor() +{ + int exit_flag = 1; // success + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // if unknown natural direction + if(sensor_direction==Direction::UNKNOWN) { + // check if sensor needs zero search + if (sensor->needsSearch()) + exit_flag = absoluteZeroSearch(); + // stop init if not found index + if (!exit_flag) + return exit_flag; + + // find natural direction + // move one electrical revolution forward + for (int i = 0; i <= 500; i++) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + // take and angle in the middle + sensor->update(); + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >= 0; i--) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + sensor->update(); + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) + { + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); + return 0; // failed calibration + } + else if (mid_angle < end_angle) + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); + sensor_direction = Direction::CCW; + } + else + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); + sensor_direction = Direction::CW; + } + // check pole pair number + float moved = fabs(mid_angle - end_angle); + if (fabs(moved * pole_pairs - _2PI) > 0.5f) + { // 0.5f is arbitrary number it can be lower or higher! + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved); + } + else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } + else + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + + // zero electric angle not known + if (!_isset(zero_electric_angle)) + { + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + _delay(20); + if (monitor_port) + { + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + } + else + SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + return exit_flag; +} + +// Encoder alignment the absolute zero angle +// - to the index +int HybridStepperMotor::absoluteZeroSearch() +{ + + SIMPLEFOC_DEBUG("MOT: Index search..."); + // search the absolute zero with small velocity + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; + velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; + while (sensor->needsSearch() && shaft_angle < _2PI) + { + angleOpenloop(1.5f * _2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->update(); + } + // disable motor + setPhaseVoltage(0, 0, 0); + // reinit the limits + velocity_limit = limit_vel; + 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!"); + } + return !sensor->needsSearch(); +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void HybridStepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + + // if open-loop do nothing + if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; + + // if disabled do nothing + if(!enabled) return; + + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void HybridStepperMotor::move(float new_target) { + + // set internal target variable + if(_isset(new_target) ) target = new_target; + + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; + // estimate the motor current if phase reistance available and current_sense not available + if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; + + // upgrade the current based voltage limit + switch (controller) { + case MotionControlType::torque: + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } + break; + case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. + // angle set point + shaft_angle_sp = target; + // calculate velocity set point + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = _constrain(shaft_velocity_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 + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity: + // velocity set point - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity_openloop: + // velocity control in open loop - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + case MotionControlType::angle_openloop: + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. + shaft_angle_sp = target; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + } +} + +void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +{ + angle_el = _normalizeAngle(angle_el); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + float center; + + switch (foc_modulation) { + case FOCModulationType::Trapezoid_120: + case FOCModulationType::Trapezoid_150: + // not handled + Ua = 0; + Ub = 0; + Uc = 0; + break; + case FOCModulationType::SinePWM: + // C phase is fixed at half-rail to provide bias point for A, B legs + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + center = driver->voltage_limit / 2; + + Ua += center; + Ub += center; + Uc = center; + break; + + case FOCModulationType::SpaceVectorPWM: + // C phase moves in order to increase max bias on coils + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + float Umin = fmin(fmin(Ua, Ub), 0); + float Umax = fmax(fmax(Ua, Ub), 0); + float Vo = -(Umin + Umax)/2 + driver->voltage_limit/2; + + Ua = Ua + Vo; + Ub = Ub + Vo; + Uc = Vo; + } + driver->setPwm(Ua, Ub, Uc); + +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +float HybridStepperMotor::velocityOpenloop(float target_velocity) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to achieve target velocity + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts); + // for display purposes + shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +float HybridStepperMotor::angleOpenloop(float target_angle) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts)) + { + shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts; + shaft_velocity = velocity_limit; + } + else + { + shaft_angle = target_angle; + shaft_velocity = 0; + } + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} diff --git a/src/HybridStepperMotor.h b/src/HybridStepperMotor.h new file mode 100644 index 00000000..d75ef8cd --- /dev/null +++ b/src/HybridStepperMotor.h @@ -0,0 +1,113 @@ +/** + * @file HybridStepperMotor.h + * + */ + +#ifndef HybridStepperMotor_h +#define HybridStepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class HybridStepperMotor : public FOCMotor +{ +public: + /** + HybridStepperMotor class constructor + @param pp pole pair number + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] + */ + HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver handle for hardware peripheral control + */ + void linkDriver(BLDCDriver *driver); + + /** + * BLDCDriver link: + */ + BLDCDriver *driver; + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC() override; + + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform + + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + +private: + int alignCurrentSense(); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroSearch(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + float velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + float angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + +#endif diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..b4f48f29 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -98,6 +98,7 @@ void loop() { #include "BLDCMotor.h" #include "StepperMotor.h" +#include "HybridStepperMotor.h" #include "sensors/Encoder.h" #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4813dc3b..85ebd2c5 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -59,6 +59,22 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } + if (driver_type == DriverType::Hybrid){ + ABCurrent_s return_ABcurrent; + //ia + ib + ic = 0 + if(current.a == 0){ + return_ABcurrent.alpha = -current.c - current.b; + return_ABcurrent.beta = current.b; + }else if(current.b == 0){ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = -current.c - current.a; + }else{ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + } + return return_ABcurrent; + } + // otherwise it's a BLDC motor and // calculate clarke transform float i_alpha, i_beta; @@ -141,10 +157,18 @@ int CurrentSense::driverAlign(float voltage, bool modulation_centered){ if (!initialized) return 0; // check if stepper or BLDC - if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); - else - return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + switch(driver_type){ + case DriverType::BLDC: + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + case DriverType::Stepper: + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); + case DriverType::Hybrid: + return alignHybridDriver(voltage, (BLDCDriver*)driver, modulation_centered); + default: + // driver type not supported + SIMPLEFOC_DEBUG("CS: Cannot align driver type!"); + return 0; + } } @@ -473,3 +497,239 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive } + + + +int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ + + _UNUSED(modulation_centered); + + bool phases_switched = 0; + bool phases_inverted = 0; + + // first find the middle phase, which will be set to the C phase + // set phase A active and phases B active, and C down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f && fabs(c.c) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // find the highest magnitude in c + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cc[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cc[i]) continue; // current not measured + if(cc[i] > max_c){ + max_c = cc[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cc[j]) continue; // current not measured + float ratio = max_c / cc[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 0: // phase A is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + break; + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + // switch phase B and C + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is positive and invert the gain if so + // current c should be negative + if( _sign(c.c) > 0 ){ + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else{ + // c - middle phase is not measured + SIMPLEFOC_DEBUG("CS: Middle phase not measured!"); + if(_isset(pinC)){ + // switch the missing phase with the phase C + if(!_isset(pinA)){ + SIMPLEFOC_DEBUG("CS: Switch (A)NC-C"); + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + }else if(!_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch (B)NC-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + } + } + } + + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + + // Find the phase A + + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), 0, 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I on the phase C + + // find the highest magnitude in A + // and make sure it's around the same as the C current (if the phase C is measured) + + float ca[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.a && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-C currents not equal!"); + return 0; + } + }else if(c.b && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + }else{ + // if the current are equal + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + }else{ + // if they are not equal take the highest as A + if (fabs(c.a) < fabs(c.b)){ + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + + } + } + } + // if we get here, phase A is aligned + // check if the current is negative and invert the gain if so + if( _sign(c.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // check the phase B + // find the highest magnitude in c + // and make sure it's around the same as the C current (if the phase C is measured) + float cb[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.b && c.c){ + // if b and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.b) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + } + } + + // check if b has good polarity + if( _sign(c.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index f3d15b31..3d9ec1b5 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -135,6 +135,11 @@ class CurrentSense{ * Function used to align the current sense with the Stepper motor driver */ int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); + /** + * Function used to align the current sense with the Hybrid motor driver + */ + int alignHybridDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); + /** * Function used to read the average current values over N samples */ diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 15ba1c47..263460b3 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -15,7 +15,8 @@ enum PhaseState : uint8_t { enum DriverType{ UnknownDriver=0, BLDC=1, - Stepper=2 + Stepper=2, + Hybrid=3 }; /** diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h index 055f20db..5c7b1a28 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -3,6 +3,8 @@ #define STM32_CURRENTSENSE_MCU_DEF #include "../../hardware_api.h" #include "../../../common/foc_utils.h" +#include "../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../drivers/hardware_specific/stm32/stm32_timerutils.h" #if defined(_STM32_DEF_) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 880fb3d6..f0ef07c4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -41,6 +41,14 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -86,6 +94,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -101,52 +116,63 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif + } + + - // first channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - // second channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1 + i; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC))); + #endif + return -1; + } } - cs_params->adc_handle = &hadc; - return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } - + return 0; } + extern "C" { void ADC1_2_IRQHandler(void) { diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h index b0f4f83b..fbcf4e99 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h @@ -5,12 +5,10 @@ #if defined(STM32F1xx) #include "stm32f1xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 7d31f966..b52b48bd 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index 5320cc3e..4c465425 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -9,15 +9,26 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -80,6 +91,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -95,65 +113,57 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used - #ifdef SIMPLEFOC_STM32_DEBUG - // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); - #endif - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; } - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(i == 0 ? "A" : i == 1 ? "B" : "C"); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } extern "C" { diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index 71071a56..ca73f181 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -5,13 +5,11 @@ #if defined(STM32F4xx) #include "stm32f4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32f4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index d3c7bd7f..85d142bc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp index 04575397..8f25bf63 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -9,15 +9,26 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -80,12 +91,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++]; TIM_TypeDef *instance_to_check = timer_to_check->Instance; - // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; - // if(TRGO_already_configured) continue; - uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -106,42 +121,23 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used - #ifdef SIMPLEFOC_STM32_DEBUG - // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); - #endif - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif - return -1; } - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } @@ -156,21 +152,36 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) + +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h index 0a3614b5..ebd9e75e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -4,12 +4,10 @@ #if defined(STM32F7xx) #include "stm32f7xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32f7_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index e4b9c850..868aafd0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -28,7 +28,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 84a9560c..7fd38921 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -8,15 +8,26 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -130,6 +141,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -145,62 +163,62 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif - return -1; } - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } + extern "C" { void ADC1_2_IRQHandler(void) { diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index 2298b17c..34fb947e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -6,13 +6,11 @@ #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) #include "stm32g4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32g4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 9be98860..a4d66b60 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -36,7 +36,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index 8f7889c4..bc74f095 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -8,22 +8,32 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif return -1; } - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); @@ -128,6 +138,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels @@ -144,60 +161,59 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + }else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif - return -1; } - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } extern "C" { diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h index 0317b74b..2004ff69 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -6,13 +6,11 @@ #if defined(STM32L4xx) #include "stm32l4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32l4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index ed55b482..4ddc5484 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -35,7 +35,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; }