diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp index 2917791..33dc475 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.cpp +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -37,10 +37,45 @@ int STSPIN32G4::init(){ // TODO init fault monitor + bootstrap_charge(); + return isReady() ? 1 : 0; }; - +void STSPIN32G4::enable() +{ + bootstrap_charge(); + BLDCDriver6PWM::enable(); +} + + +void STSPIN32G4::bootstrap_charge() +{ + //store phase states, enable low side drive + static const int num_phases = sizeof(phase_state)/sizeof(phase_state[0]); + PhaseState phase_state_before[num_phases]; + bool none_disabled = true; + for (size_t i = 0; i < num_phases; i++) + { + phase_state_before[i] = phase_state[i]; + if (phase_state[i] == PHASE_HI || phase_state[i] == PHASE_OFF) + { + none_disabled = false; + } + } + if (none_disabled) + { + return; + } + setPhaseState(PHASE_LO, PHASE_LO, PHASE_LO); + setPwm(0,0,0); + _delay(bootstrap_capacitor_charge_time); + //restore phase states + for (size_t i = 0; i < num_phases; i++) + { + phase_state[i] = phase_state_before[i]; + } +} void STSPIN32G4::wake() { digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index 3df8137..ed12acf 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -116,8 +116,13 @@ class STSPIN32G4 : public BLDCDriver6PWM { STSPIN32G4(); ~STSPIN32G4(); + unsigned int bootstrap_capacitor_charge_time = 10; //time to let the bootstrap capacitors charge for, in ms. During his time, brakes are applied. This is not necessary if the motor is rotating, as the BEMF allows the caps to charge. + int init() override; + void enable() override; + + void bootstrap_charge(); void wake(); void sleep(); bool isReady();