From 2a89d6383e2c2d2656cb22effd5dbd9bfc8e6389 Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Mon, 29 Dec 2025 12:18:25 +0100 Subject: [PATCH] Add drv8323 and drv8320 drivers (untested) --- src/drivers/drv832x/drv832x.cpp | 182 ++++++++++++++++ src/drivers/drv832x/drv832x.h | 278 ++++++++++++++++++++++++ src/drivers/drv832x/drv832x_registers.h | 128 +++++++++++ 3 files changed, 588 insertions(+) create mode 100644 src/drivers/drv832x/drv832x.cpp create mode 100644 src/drivers/drv832x/drv832x.h create mode 100644 src/drivers/drv832x/drv832x_registers.h diff --git a/src/drivers/drv832x/drv832x.cpp b/src/drivers/drv832x/drv832x.cpp new file mode 100644 index 0000000..df34de2 --- /dev/null +++ b/src/drivers/drv832x/drv832x.cpp @@ -0,0 +1,182 @@ + + +#include "./drv832x.h" + +#define BIT_READ_MASK 0x07FF + + +void DRV832xDriver3PWM::init(SPIClass* _spi) { + DRV832xDriver::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV832xDriver::setPWMMode(DRV832x_PWMMode::PWM3_Mode); + BLDCDriver3PWM::init(); +}; + + +void DRV832xDriver6PWM::init(SPIClass* _spi) { + DRV832xDriver::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV832xDriver::setPWMMode(DRV832x_PWMMode::PWM6_Mode); // default mode is 6-PWM + BLDCDriver6PWM::init(); +}; + +void DRV832xDriver::init(SPIClass* _spi) { + // TODO make SPI speed configurable + spi = _spi; + settings = SPISettings(1000000, MSBFIRST, SPI_MODE1); + + //setup pins + pinMode(cs, OUTPUT); + digitalWrite(cs, HIGH); // switch off + + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices + spi->begin(); + + if (_isset(nFault)) { + pinMode(nFault, INPUT); + // TODO add interrupt handler on the nFault pin if configured + // add configuration for how to handle faults... idea: interrupt handler calls a callback, depending on the type of fault + // consider what would be a useful configuration in practice? What do we want to do on a fault, e.g. over-temperature for example? + + //attachInterrupt(digitalPinToInterrupt(nFault), handleInterrupt, PinStatus::FALLING); + } +}; + + + + +uint16_t DRV832xDriver::readSPI(uint8_t addr) { + digitalWrite(cs, 0); + spi->beginTransaction(settings); + addr &= 0xf; + uint16_t data = (addr<<11)|0x8000; + uint16_t result = spi->transfer16(data); + spi->endTransaction(); + digitalWrite(cs, 1); +// Serial.print("SPI Read Result: "); +// Serial.print(data, HEX); +// Serial.print(" -> "); +// Serial.println(result, HEX); + return result; +} + + +uint16_t DRV832xDriver::writeSPI(uint8_t addr, uint16_t value) { + digitalWrite(cs, 0); + spi->beginTransaction(settings); + addr &= 0xf; + value &= BIT_READ_MASK; + uint16_t data = (addr<<11)|value; + uint16_t result = spi->transfer16(data); + spi->endTransaction(); + digitalWrite(cs, 1); +// Serial.print("SPI Write Result: "); +// Serial.print(data, HEX); +// Serial.print(" -> "); +// Serial.println(result, HEX); + return result; +} + +#define def_getset(name, type, address, register_type, register_name) \ +type DRV832xDriver::get##name() {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + return static_cast(data.register_name);\ +}\ +\ +void DRV832xDriver::set##name(type value) {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + data.register_name = value;\ + delayMicroseconds(1);\ + writeSPI(address, data.reg);\ +} + +#define def_isset(name, address, register_type, register_name) \ +bool DRV832xDriver::is##name() {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + return static_cast(data.register_name);\ +}\ +\ +void DRV832xDriver::set##name(bool value) {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + data.register_name = value;\ + delayMicroseconds(1);\ + writeSPI(address, data.reg);\ +} + +DRV832xStatus DRV832xDriver::getStatus() { + Fault data0; + VGS data1; + uint16_t result = readSPI(Fault_Status_ADDR); + data0.reg = (result & BIT_READ_MASK); + delayMicroseconds(1); // delay at least 400ns between operations + result = readSPI(VGS_Status_ADDR); + data1.reg = (result & BIT_READ_MASK); + return DRV832xStatus(data0, data1); +} + + +void DRV832xDriver::clearFault() { + uint16_t result = readSPI(Driver_Control_ADDR); + Driver_Control data; + data.reg = (result & BIT_READ_MASK); + data.CLR_FLT |= 1; + delayMicroseconds(1); // delay at least 400ns between operations + result = writeSPI(Driver_Control_ADDR, data.reg); +} + + +bool DRV832xDriver::isRegistersLocked(){ + uint16_t result = readSPI(Gate_Drive_HS_ADDR); + Gate_Drive_HS data; + data.reg = (result & BIT_READ_MASK); + return data.LOCK==LOCK_LOCK; +} +void DRV832xDriver::setRegistersLocked(bool lock){ + uint16_t result = readSPI(Gate_Drive_HS_ADDR); + Gate_Drive_HS data; + data.reg = (result & BIT_READ_MASK); + data.LOCK = lock?LOCK_LOCK:LOCK_UNLOCK; + delayMicroseconds(1); // delay at least 400ns between operations + result = writeSPI(Gate_Drive_HS_ADDR, data.reg); +} + +def_isset(Brake, Driver_Control_ADDR, Driver_Control, BRAKE) +def_isset(Coast, Driver_Control_ADDR, Driver_Control, COAST) +def_isset(OnePWMInvertDirection, Driver_Control_ADDR, Driver_Control, _1PWM_DIR) +def_isset(OnePWMAsynchronousRectification, Driver_Control_ADDR, Driver_Control, _1PWM_COM) +def_getset(PWMMode, DRV832x_PWMMode, Driver_Control_ADDR, Driver_Control, PWM_MODE) +def_isset(OvertemperatureReporting, Driver_Control_ADDR, Driver_Control, OTW_REP) +def_isset(DisableGateDriveFault, Driver_Control_ADDR, Driver_Control, DIS_GDF) +def_isset(DisableChargePumpUndervoltage, Driver_Control_ADDR, Driver_Control, DIS_CPUV) + +def_getset(VDSLevel, DRV832x_VDS_LVL, OCP_Control_ADDR, OCP_Control, VDS_LVL); +def_getset(OCPMode, DRV832x_OCPMode, OCP_Control_ADDR, OCP_Control, OCP_MODE) +def_getset(OCPDeglitchTime, DRV832x_OCPDeglitch, OCP_Control_ADDR, OCP_Control, OCP_DEG) +def_getset(DeadTime, DRV832x_DEADTIME, OCP_Control_ADDR, OCP_Control, DEAD_TIME); + +def_getset(HighSideChargeCurrent, DRV832x_IDRIVEP, Gate_Drive_HS_ADDR, Gate_Drive_HS, IDRIVEP_HS); +def_getset(HighSideDischargeCurrent, DRV832x_IDRIVEN, Gate_Drive_HS_ADDR, Gate_Drive_HS, IDRIVEN_HS); +def_getset(LowSideChargeCurrent, DRV832x_IDRIVEP, Gate_Drive_LS_ADDR, Gate_Drive_LS, IDRIVEP_LS); +def_getset(LowSideDischargeCurrent, DRV832x_IDRIVEN, Gate_Drive_LS_ADDR, Gate_Drive_LS, IDRIVEN_LS); + +def_getset(DriveTime, DRV832x_TDRIVE, Gate_Drive_LS_ADDR, Gate_Drive_LS, TDRIVE); +def_isset(CycleByCycle, Gate_Drive_LS_ADDR, Gate_Drive_LS, CBC); +def_isset(RetryTime, OCP_Control_ADDR, OCP_Control, TRETRY); + +def_getset(CurrentSenseOvercurrentSensitivity, DRV832x_CS_VSEN_LVL, CSA_Control_ADDR, CSA_Control, SEN_LVL); +def_isset(CurrentSenseCalibrateA, CSA_Control_ADDR, CSA_Control, CSA_CAL_A); +def_isset(CurrentSenseCalibrateB, CSA_Control_ADDR, CSA_Control, CSA_CAL_B); +def_isset(CurrentSenseCalibrateC, CSA_Control_ADDR, CSA_Control, CSA_CAL_C); +def_isset(CurrentSenseOvercurrentDisable, CSA_Control_ADDR, CSA_Control, DIS_SEN); +def_getset(CurrentSenseGain, DRV832x_CSAGain, CSA_Control_ADDR, CSA_Control, CSA_GAIN) +def_isset(CurrentSenseOvercurrentResistor, CSA_Control_ADDR, CSA_Control, LS_REF); +def_isset(CurrentSenseBidirectionnal, CSA_Control_ADDR, CSA_Control, VREF_DIV); +def_isset(CurrentSenseFET, CSA_Control_ADDR, CSA_Control, CSA_FET); + + diff --git a/src/drivers/drv832x/drv832x.h b/src/drivers/drv832x/drv832x.h new file mode 100644 index 0000000..e6a46b1 --- /dev/null +++ b/src/drivers/drv832x/drv832x.h @@ -0,0 +1,278 @@ + + +#ifndef SIMPLEFOC_DRV832x +#define SIMPLEFOC_DRV832x + + +#include "Arduino.h" +#include +#include +#include + +#include "./drv832x_registers.h" + + +enum DRV832x_PWMMode { + PWM6_Mode = 0b00, //default + PWM3_Mode = 0b01, + PWM1_Mode = 0b10, + Independant_Mode = 0b11 +}; + +enum DRV832x_IDRIVEP { + IDRIVEP_10mA, + IDRIVEP_30mA, + IDRIVEP_60mA, + IDRIVEP_80mA, + IDRIVEP_120mA, + IDRIVEP_140mA, + IDRIVEP_170mA, + IDRIVEP_190mA, + IDRIVEP_260mA, + IDRIVEP_330mA, + IDRIVEP_370mA, + IDRIVEP_440mA, + IDRIVEP_570mA, + IDRIVEP_680mA, + IDRIVEP_820mA, + IDRIVEP_1000mA, //default +}; + +enum DRV832x_IDRIVEN { + IDRIVEN_20mA, + IDRIVEN_60mA, + IDRIVEN_120mA, + IDRIVEN_160mA, + IDRIVEN_240mA, + IDRIVEN_280mA, + IDRIVEN_340mA, + IDRIVEN_380mA, + IDRIVEN_520mA, + IDRIVEN_660mA, + IDRIVEN_740mA, + IDRIVEN_880mA, + IDRIVEN_1140mA, + IDRIVEN_1360mA, + IDRIVEN_1640mA, + IDRIVEN_2000mA, //default +}; + +enum DRV832x_LOCK +{ + LOCK_LOCK=0b110, + LOCK_UNLOCK=0b011 //default +}; + +enum DRV832x_TDRIVE +{ + TDRIVE_500ns, + TDRIVE_1000ns, + TDRIVE_2000ns, + TDRIVE_4000ns //default +}; + +enum DRV832x_DEADTIME +{ + DEADTIME_50ns, + DEADTIME_100ns, //default + DEADTIME_200ns, + DEADTIME_400ns +}; + +enum DRV832x_OCPMode +{ + OCPMode_LATCH_FAULT, + OCPMode_RETRY_FAULT, //default + OCPMode_REPORT, + OCPMode_IGNORE +}; + +enum DRV832x_OCPDeglitch +{ + OCPDeglitch_2us, + OCPDeglitch_4us,//default + OCPDeglitch_6us, + OCPDeglitch_8us +}; + +enum DRV832x_VDS_LVL +{ + VDS_LVL_60mV, + VDS_LVL_130mV, + VDS_LVL_200mV, + VDS_LVL_260mV, + VDS_LVL_310mV, + VDS_LVL_450mV, + VDS_LVL_530mV, + VDS_LVL_600mV, + VDS_LVL_680mV, + VDS_LVL_750mV, //default + VDS_LVL_940mV, + VDS_LVL_1130mV, + VDS_LVL_1300mV, + VDS_LVL_1500mV, + VDS_LVL_1700mV, + VDS_LVL_1880mV, +}; + +enum DRV832x_CSAGain +{ + CSAGain_5, + CSAGain_10, + CSAGain_20,//default + CSAGain_40 +}; + +enum DRV832x_CS_VSEN_LVL +{ + CSAGain_250mV, + CSAGain_500mV, + CSAGain_750mV, + CSAGain_1000mV//default +}; + +class DRV832xFault { +public: + DRV832xFault(Fault status) : fault_reg(status) {}; + ~DRV832xFault() {}; + + bool isFault() { return fault_reg.FAULT; }; + bool isOverCurrent() { return fault_reg.VDS_OCP; }; + bool isGateDriveFault() { return fault_reg.GDF; }; + bool isUndervoltageLockout() { return fault_reg.UVLO; }; + bool isOverTemperature() { return fault_reg.OTSD; }; + bool isOverCurrent_Ah() {return fault_reg.VDS_HA; }; + bool isOverCurrent_Al() {return fault_reg.VDS_LA; }; + bool isOverCurrent_Bh() {return fault_reg.VDS_HB; }; + bool isOverCurrent_Bl() {return fault_reg.VDS_LB; }; + bool isOverCurrent_Ch() {return fault_reg.VDS_HC; }; + bool isOverCurrent_Cl() {return fault_reg.VDS_LC; }; + + Fault fault_reg; +}; + + +class DRV832xVGS { +public: + DRV832xVGS(VGS status) : vgs_reg(status) {}; + ~DRV832xVGS() {}; + + + bool isOverCurrent_As() { return vgs_reg.SA_OC==0b1; }; + bool isOverCurrent_Bs() { return vgs_reg.SB_OC==0b1; }; + bool isOverCurrent_Cs() { return vgs_reg.SC_OC==0b1; }; + bool isOverTemperatureWarning() { return vgs_reg.OTW==0b1; }; + bool isChargePumpUndervoltage() { return vgs_reg.CPUV==0b1; }; + bool isGateDriveFault_Ah() { return vgs_reg.VGS_HA==0b1; }; + bool isGateDriveFault_Al() { return vgs_reg.VGS_LA==0b1; }; + bool isGateDriveFault_Bh() { return vgs_reg.VGS_HB==0b1; }; + bool isGateDriveFault_Bl() { return vgs_reg.VGS_LB==0b1; }; + bool isGateDriveFault_Ch() { return vgs_reg.VGS_HC==0b1; }; + bool isGateDriveFault_Cl() { return vgs_reg.VGS_LC==0b1; }; + + VGS vgs_reg; +}; + + + +class DRV832xStatus : public DRV832xFault, public DRV832xVGS { + public: + DRV832xStatus(Fault faults, VGS vgs) : DRV832xFault(faults), DRV832xVGS(vgs) {}; + ~DRV832xStatus() {}; +}; + +#define decl_getset(name, type) type get##name(); void set##name(type); +#define decl_isset(name) bool is##name(); void set##name(bool); + +class DRV832xDriver { + + public: + DRV832xDriver(int cs, bool currentLimit = false, int nFault = NOT_SET) : currentLimit(currentLimit), cs(cs), nFault(nFault), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE1) {}; + virtual ~DRV832xDriver() {}; + + virtual void init(SPIClass* _spi = &SPI); + + void clearFault(); // TODO check for fault condition methods + + DRV832xStatus getStatus(); + + decl_isset(RegistersLocked); + + //Driver control + decl_isset(Brake); + decl_isset(Coast); + decl_isset(OnePWMInvertDirection); + decl_isset(OnePWMAsynchronousRectification); + decl_getset(PWMMode, DRV832x_PWMMode); + decl_isset(OvertemperatureReporting); + decl_isset(DisableGateDriveFault); + decl_isset(DisableChargePumpUndervoltage); + + //OCP Control + decl_getset(VDSLevel, DRV832x_VDS_LVL); + decl_getset(OCPMode, DRV832x_OCPMode); + decl_getset(OCPDeglitchTime, DRV832x_OCPDeglitch); + decl_getset(DeadTime, DRV832x_DEADTIME); + + //Drive current + decl_getset(HighSideChargeCurrent, DRV832x_IDRIVEP); + decl_getset(HighSideDischargeCurrent, DRV832x_IDRIVEN); + decl_getset(LowSideChargeCurrent, DRV832x_IDRIVEP); + decl_getset(LowSideDischargeCurrent, DRV832x_IDRIVEN); + + //Misc drive + decl_getset(DriveTime, DRV832x_TDRIVE); + decl_isset(CycleByCycle); + decl_isset(RetryTime); + + //Current sense (DRV8323 only) + decl_getset(CurrentSenseOvercurrentSensitivity, DRV832x_CS_VSEN_LVL); + decl_isset(CurrentSenseCalibrateA); + decl_isset(CurrentSenseCalibrateB); + decl_isset(CurrentSenseCalibrateC); + decl_isset(CurrentSenseOvercurrentDisable); + decl_getset(CurrentSenseGain, DRV832x_CSAGain); + decl_isset(CurrentSenseOvercurrentResistor); + decl_isset(CurrentSenseBidirectionnal); + decl_isset(CurrentSenseFET); + + private: + uint16_t readSPI(uint8_t addr); + uint16_t writeSPI(uint8_t addr, uint16_t value); + + bool currentLimit; + int cs; + int nFault; + SPIClass* spi; + SPISettings settings; +}; + +#undef decl_getset +#undef decl_isset + +class DRV832xDriver3PWM : public DRV832xDriver, public BLDCDriver3PWM { + + public: + DRV832xDriver3PWM(int phA,int phB,int phC, int cs, bool currentLimit = false, int en = NOT_SET, int nFault = NOT_SET) : + DRV832xDriver(cs, currentLimit, nFault), BLDCDriver3PWM(phA, phB, phC, en) { enable_active_high=false; }; + virtual ~DRV832xDriver3PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + + +class DRV832xDriver6PWM : public DRV832xDriver, public BLDCDriver6PWM { + + public: + DRV832xDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int cs, bool currentLimit = false, int en = NOT_SET, int nFault = NOT_SET) : + DRV832xDriver(cs, currentLimit, nFault), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) { enable_active_high=false; }; + virtual ~DRV832xDriver6PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + +#endif diff --git a/src/drivers/drv832x/drv832x_registers.h b/src/drivers/drv832x/drv832x_registers.h new file mode 100644 index 0000000..b6af386 --- /dev/null +++ b/src/drivers/drv832x/drv832x_registers.h @@ -0,0 +1,128 @@ + + +#pragma once + + +#define Fault_Status_ADDR 0x0 +#define VGS_Status_ADDR 0x1 +#define Driver_Control_ADDR 0x2 +#define Gate_Drive_HS_ADDR 0x3 +#define Gate_Drive_LS_ADDR 0x4 +#define OCP_Control_ADDR 0x5 +#define CSA_Control_ADDR 0x6 + + +//Read-only +typedef union { + struct + { + + bool VDS_LC:1, //Overcurrent on C low-side mosfet + VDS_HC:1, //Overcurrent on C high-side mosfet + VDS_LB:1, //Overcurrent on B low-side mosfet + VDS_HB:1, //Overcurrent on B high-side mosfet + VDS_LA:1, //Overcurrent on A low-side mosfet + VDS_HA:1, //Overcurrent on A high-side mosfet + OTSD:1, //Overtemperature shutdown fault + UVLO:1, //undervoltage lockout fault + GDF:1, //Gate drive fault + VDS_OCP:1, //VDS monitor overcurrent fault + FAULT:1; //Mirrors nFAULT pin + //uint8_t :5; + }; + uint16_t reg : 11; +} Fault; + +//Read-only +typedef union { + struct + { + + bool VGS_LC:1, //Gate drive fault on C low-side mosfet + VGS_HC:1, //Gate drive fault on C high-side mosfet + VGS_LB:1, //Gate drive fault on B low-side mosfet + VGS_HB:1, //Gate drive fault on B high-side mosfet + VGS_LA:1, //Gate drive fault on A low-side mosfet + VGS_HA:1, //Gate drive fault on A high-side mosfet + CPUV:1, //Charge-pump undervoltage fault + OTW:1, //Overtemp warning + SC_OC:1, //Overcurrent on phase C sense amplifier + SB_OC:1, //Overcurrent on phase B sense amplifier + SA_OC:1; //Overcurrent on phase A sense amplifier + //uint8_t :5; + }; + uint16_t reg : 11; +} VGS; + +typedef union { + struct + { + + bool CLR_FLT:1, //Clear faults, automatically clears itself + BRAKE:1, //Turn on all low-side mosfets in 1PWM + COAST:1, //Put mosfet in Hi-Z + _1PWM_DIR:1, //Invert 1PWM dir pin + _1PWM_COM:1; //1PWM asynchronous rectification + uint8_t PWM_MODE:2; //00 = 6PWM, 01=3PWM, 10=1PWM, 11=Independant + bool OTW_REP:1, //Overtemp warning report on nFAULT + DIS_GDF:1, //Disable gate drive fault + DIS_CPUV:1, //Disable charge pump undervoltage lockout fault + :1; + //uint8_t :5; + }; + uint16_t reg : 11; +} Driver_Control; + +typedef union { + struct + { + uint8_t IDRIVEN_HS:4, //high side discharge current + IDRIVEP_HS:4, //high side charge current + LOCK:3; //110 = lock registers, 011b = unlock registers + //uint8_t :5; + }; + uint16_t reg : 11; +} Gate_Drive_HS; + +typedef union { + struct + { + uint8_t IDRIVEN_LS:4, //low side discharge current + IDRIVEP_LS:4, //low side charge current + TDRIVE:2; //gate current peak duration + bool CBC:1; //cycle-by-cycle operation + //uint8_t :5; + }; + uint16_t reg : 11; +} Gate_Drive_LS; + +typedef union { + struct + { + uint8_t VDS_LVL:4, //vds protection voltage + OCP_DEG:2, //overcurrent deglitch time + OCP_MODE:2, //overcurrent fault registering + DEAD_TIME:2; //dead-time + bool TRETRY:1; //VDS_OCP and SEN_OCP retry time + //uint8_t :5; + }; + uint16_t reg : 11; +} OCP_Control; + +//Only for DRV8323 +typedef union { + struct + { + uint8_t SEN_LVL:2; //Sense OCP voltage (value*0.25) V + bool CSA_CAL_C:1, //Short inputs of current sense for offset cal + CSA_CAL_B:1, //Short inputs of current sense for offset cal + CSA_CAL_A:1, //Short inputs of current sense for offset cal + DIS_SEN:1; //Disable sense overcurrent fault + uint8_t CSA_GAIN:2; //(2^value)*5 V/V sensitivity + bool LS_REF:1, //VDS OCP measurement source + VREF_DIV:1, //Unidirectional current sense + CSA_FET:1; //Change current sense amp positive input + //uint8_t :5; + }; + uint16_t reg : 11; +} CSA_Control; \ No newline at end of file