Current Sense Init FAIL

Jose_Aldahir_Espinos · January 30, 2026, 12:47am

Hi, I tried to test the current sense in my custom PCB.

STM32G431
AS5600
MN4006 380KV
DRV8305
Shunts 2m ohm

And when I start the code I have this issue:

I test a different code only to see the ADC of SO1, SO2, and SO3 applying some PWM in the phases and I have:

So I don’t know why the current sense doesn’t init.

These are my codes:

Current control:

#include <SimpleFOC.h>

#include <Wire.h>

#include <SPI.h>




#define CS_DRV   PA4

#define EN_DRV   PC9




// PWM

#define PWM_U    PA8

#define PWM_V    PA9

#define PWM_W    PA10




// Shunts (según tu esquema)

#define SO1_PIN  PC4

#define SO2_PIN  PC5

#define SO3_PIN  PB2




HardwareSerial Serial2(PA3, PA2);




// Commander: corriente objetivo (A, escala depende de CS_GAIN real)

float target_current = 0.0f;

Commander command = Commander(Serial2);

void doTarget(char* cmd) { command.scalar(&target_current, cmd); }




// Encoder

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);




// Motor/Driver

BLDCMotor motor = BLDCMotor(12);

BLDCDriver3PWM driver = BLDCDriver3PWM(PWM_U, PWM_V, PWM_W, EN_DRV);




// Shunt=2mΩ; CSA gain real desconocida (default).

// Si CSA=20V/V => 25. Si 40 => 12.5. Si 10 => 50.

static const float CS_GAIN_EST = 25.0f;

// 

LowsideCurrentSense current_sense = LowsideCurrentSense(CS_GAIN_EST, SO1_PIN, SO2_PIN, SO3_PIN);




// DRV8305 SPI

void drvWrite(uint8_t reg, uint16_t data) {

  uint16_t packet = ((reg & 0x7F) << 11) | (data & 0x7FF);

  digitalWrite(CS_DRV, LOW);

  SPI.transfer16(packet);

  digitalWrite(CS_DRV, HIGH);

}

uint16_t drvRead(uint8_t reg) {

  uint16_t packet = (0x8000 | ((reg & 0x7F) << 11));

  digitalWrite(CS_DRV, LOW);

  uint16_t response = SPI.transfer16(packet);

  digitalWrite(CS_DRV, HIGH);

  return response;

}




// Rampa de corriente para evitar picos

static float iq_cmd = 0.0f;

static uint32_t last_us = 0;




void setup() {

  Serial2.begin(115200);

  delay(20);

  Serial2.println("\n== SimpleFOC DRV8305 + AS5600 (FOC CURRENT, 3-shunt) ==");




  // I2C AS5600

  Wire.setSDA(PC11);

  Wire.setSCL(PC8);

  Wire.begin();

  Wire.setClock(100000);

  sensor.init();




  SimpleFOCDebug::enable(&Serial2);




  // SPI DRV8305

  pinMode(CS_DRV, OUTPUT);

  digitalWrite(CS_DRV, HIGH);

  SPI.begin();

  SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE1));

  delay(10);




  // Tus writes (por ahora igual)

  drvWrite(0x03, 0x003);

  delay(5);

  drvWrite(0x07, 0x2FF);

  delay(5);




  Serial2.print("DRV FLT=0x"); Serial2.println(drvRead(0x0F), HEX);




  // Driver

  driver.voltage_power_supply = 12.0f;

  driver.voltage_limit = 6.0f;

  driver.init();




  // Link

  motor.linkDriver(&driver);

  motor.linkSensor(&sensor);




  // Current sense

  current_sense.linkDriver(&driver);

  bool cs_ok = current_sense.init();

  motor.linkCurrentSense(&current_sense);

  Serial2.print("CurrentSense init: "); Serial2.println(cs_ok ? "OK" : "FAIL");




  // FOC config

  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;




  motor.voltage_limit = 6.0f;

  motor.voltage_sensor_align = 3.0f;




  motor.controller = MotionControlType::torque;

  motor.torque_controller = TorqueControlType::foc_current;




  // Límites conservadores (sube después)

  motor.current_limit = 1.0f;

  motor.velocity_limit = 200.0f;




  // PID corriente (estables)

  motor.PID_current_q.P = 1.2f;

  motor.PID_current_q.I = 80.0f;

  motor.PID_current_d.P = 1.2f;

  motor.PID_current_d.I = 80.0f;

  motor.LPF_current_q.Tf = 0.004f;

  motor.LPF_current_d.Tf = 0.004f;




  motor.useMonitoring(Serial2);




  // Init

  motor.init();

  int ok = motor.initFOC();

  Serial2.print("initFOC: "); Serial2.println(ok ? "OK" : "FAIL");




  command.add('T', doTarget, "target current [A]");

  Serial2.println("Ready. Use: T0.2 T0.5 T1.0");




  last_us = micros();

}




void loop() {

  motor.loopFOC();




  // Slew

  uint32_t now = micros();

  float dt = (now - last_us) * 1e-6f;

  last_us = now;




  const float IQ_SLEW = 2.0f; // A/s

  float diff = target_current - iq_cmd;

  float step = IQ_SLEW * dt;

  if (diff >  step) diff =  step;

  if (diff < -step) diff = -step;

  iq_cmd += diff;




  // Clamp

  if (iq_cmd > motor.current_limit) iq_cmd = motor.current_limit;

  if (iq_cmd < -motor.current_limit) iq_cmd = -motor.current_limit;




  motor.move(iq_cmd);

  command.run();




  // (opcional) monitoreo fault

  static uint32_t tmr = 0;

  if (millis() - tmr > 200) {

    tmr = millis();

    uint16_t flt = drvRead(0x0F);

    if (flt != 0) {

      Serial2.print("DRV FAULT FLT=0x"); Serial2.println(flt, HEX);

      motor.disable();

    }

  }

}

And the test of SO1, 2 and 3:

#include <Arduino.h>

#include <SPI.h>




// ===== Ajusta a tu hardware =====

#define CS_DRV   PA4

#define EN_DRV   PC9




#define PWM_U    PA8

#define PWM_V    PA9

#define PWM_W    PA10




#define SO1_PIN  PC4

#define SO2_PIN  PC5

#define SO3_PIN  PB2




HardwareSerial Serial2(PA3, PA2);




// 

static inline void drvWrite(uint8_t reg, uint16_t data) {

  uint16_t packet = ((reg & 0x7F) << 11) | (data & 0x7FF);

  digitalWrite(CS_DRV, LOW);

  SPI.transfer16(packet);

  digitalWrite(CS_DRV, HIGH);

}

static inline uint16_t drvRead(uint8_t reg) {

  uint16_t packet = (0x8000 | ((reg & 0x7F) << 11));

  digitalWrite(CS_DRV, LOW);

  uint16_t response = SPI.transfer16(packet);

  digitalWrite(CS_DRV, HIGH);

  return response;

}




// ===== ADC avg =====

static int adcAvg(int pin, int n=32) {

  long s = 0;

  for (int i=0;i<n;i++) s += analogRead(pin);

  return (int)(s / n);

}




// ===== PWM =====

static void pwmSetup() {

  pinMode(EN_DRV, OUTPUT);

  digitalWrite(EN_DRV, HIGH);




  pinMode(PWM_U, OUTPUT);

  pinMode(PWM_V, OUTPUT);

  pinMode(PWM_W, OUTPUT);




  analogWriteResolution(12); // 0..4095

}

static void pwmAllOff() {

  analogWrite(PWM_U, 0);

  analogWrite(PWM_V, 0);

  analogWrite(PWM_W, 0);

}

static void pwmSetU(uint16_t duty) {

  analogWrite(PWM_U, duty);

  analogWrite(PWM_V, 0);

  analogWrite(PWM_W, 0);

}




// ===== Inyección NO-bloqueante =====

static bool injecting = false;

static uint16_t inj_duty = 0;

static uint32_t inj_end_ms = 0;




static void startInject(uint16_t duty, uint32_t ms) {

  injecting = true;

  inj_duty = duty;

  inj_end_ms = millis() + ms;

  pwmSetU(inj_duty);

}




static void serviceInject() {

  if (injecting) {

    if ((int32_t)(millis() - inj_end_ms) >= 0) {

      injecting = false;

      pwmAllOff();

    } else {

      // mantener PWM activo

      pwmSetU(inj_duty);

    }

  }

}




void setup() {

  Serial2.begin(115200);

  delay(50);

  Serial2.println("\n== DRV8305 CSA TEST (non-blocking) ==");




  analogReadResolution(12);




  // SPI

  pinMode(CS_DRV, OUTPUT);

  digitalWrite(CS_DRV, HIGH);

  SPI.begin();

  SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE1));

  delay(10);




  // Opcional: tus writes básicos

  drvWrite(0x03, 0x003);

  delay(5);

  drvWrite(0x07, 0x2FF);

  delay(5);




  // PWM

  pwmSetup();

  pwmAllOff();




  Serial2.print("FLT=0x"); Serial2.println(drvRead(0x0F), HEX);




  Serial2.println("Comandos por Serial2:");

  Serial2.println("  1) Inject 5% (205) por 800ms");

  Serial2.println("  2) Inject 12% (490) por 800ms");

  Serial2.println("  3) Inject 20% (820) por 600ms");

  Serial2.println("  0) PWM OFF");

}




void loop() {

  serviceInject();




  // imprimir cada 20ms SIEMPRE (aun durante pulso)

  static uint32_t t = 0;

  if (millis() - t > 20) {

    t = millis();




    uint16_t flt = drvRead(0x0F);

    int so1 = adcAvg(SO1_PIN, 16);

    int so2 = adcAvg(SO2_PIN, 16);

    int so3 = adcAvg(SO3_PIN, 16);




    Serial2.print(injecting ? "INJ " : "IDLE");

    Serial2.print(" duty="); Serial2.print(injecting ? inj_duty : 0);

    Serial2.print(" FLT=0x"); Serial2.print(flt, HEX);

    Serial2.print(" SO1="); Serial2.print(so1);

    Serial2.print(" SO2="); Serial2.print(so2);

    Serial2.print(" SO3="); Serial2.println(so3);

  }




  // comandos

  if (Serial2.available()) {

    char c = Serial2.read();

    if (c == '0') {

      injecting = false;

      pwmAllOff();

      Serial2.println("PWM OFF");

    } else if (c == '1') {

      Serial2.println("Inject 5% duty (205) for 800ms");

      startInject(205, 800);

    } else if (c == '2') {

      Serial2.println("Inject 12% duty (490) for 800ms");

      startInject(490, 800);

    } else if (c == '3') {

      Serial2.println("Inject 20% duty (820) for 600ms");

      startInject(820, 600);

    }

  }

}

Antun_Skuric · February 1, 2026, 8:22am

Hi @Jose_Aldahir_Espinos,

I’m not sure to follow completely your testing code. Let’s just try something a bit simpler before we deep dive in all the possible reasons why the alignment could be failing.

The failing message basically says that the currents are too low. Can it be that your gain is not correct


// Shunt=2mΩ; CSA gain real desconocida (default).
// Si CSA=20V/V => 25. Si 40 => 12.5. Si 10 => 50.
static const float CS_GAIN_EST = 25.0f;

LowsideCurrentSense current_sense = LowsideCurrentSense(CS_GAIN_EST, SO1_PIN, SO2_PIN, SO3_PIN);

You can also call the constructor with your amplifier gain + your shunt, it will automatically calculate the equivalent gain

LowsideCurrentSense current_sense = LowsideCurrentSense(0.002f,  DRV_CSA, SO1_PIN, SO2_PIN, SO3_PIN);

Also, I’d suggest calling current_sense.init() between motor.init() and motor.initFOC(). If you call it before the driver is not enabled so it might be reading some random values.

Another nice way to test if your driver and current measurements are good is by using this example code:

github.com/simplefoc/Arduino-FOC

examples/utils/current_sense_test/align_current_sense/align_current_sense.ino

dev

/**
 * This is an example code for visual aligning current sense and the driver phases as well 
 * it is used to test the current sense implementation.
 * 
 * In this example it uses the BLDCMotor and BLDCDriver3PWM classes to control a BLDC motor
 * and the InlineCurrentSense class to read the phase currents.
 * > In your application you can use any other motor, driver and current sense implementation.
 * > The rest of the code will stay the same
 * 
 * The example uses the teleplot (https://teleplot.fr) service to visualize the phase currents and voltages.
 * Its really awesome tool and you can use it to visualize any data you want.  
 */
#include <SimpleFOC.h>

// BLDC motor & driver instance
// NOTE: replace with your motor and driver configuration
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(6, 10, 5, 8);

// Current sense instance

This file has been truncated. show original

Jose_Aldahir_Espinos · February 4, 2026, 7:02pm

Hi @Antun_Skuric

Thanks for your reply, I test the code that you provided, and these are the results.

I put 10 on the gain (it’s the default value in the datasheet)

I don’t know if this it’s ok, can you tell me that?

I appreciate it.

Antun_Skuric · February 4, 2026, 7:18pm

Hey can you please use teleplot to visualise the signals

Jose_Aldahir_Espinos · February 4, 2026, 7:32pm

Sure:

Antun_Skuric · February 4, 2026, 7:40pm

Your current measurements are very low. Can you raise the voltage limit a bit. You’re searching to have sinusoidal shaped current measurements.

Jose_Aldahir_Espinos · February 4, 2026, 7:55pm

With motor.voltage_limit = 4.0
If I put 5.0 the motor spin 1s and the driver put on the nFAULT LED.

L=4.16

Jose_Aldahir_Espinos · February 4, 2026, 7:59pm

I’m testing with a Power Supply with 12V and 3.5A limit, if I put more than L=4.16 the driver put on the nFAULT LED, and if I rise the current my motor gets to hot, its a MN4006 380KV

Antun_Skuric · February 4, 2026, 8:06pm

Ok thanks. Your courent measurements seem to be off and very very noisy.

You have 2mOhm shunt right and what amp gain do you have?

EDIT: I see that you’ve put 10. Can you make it higher?

Jose_Aldahir_Espinos · February 4, 2026, 9:33pm

If I change the gain to 40 and yes I have 2mOhm res shunt