changes to get it working on hv driver

nmscode · nmscode · commit 7559ba5aeb26 · 2024-07-03T01:00:37.000-04:00
diff --git a/src/HFIBLDCMotor.cpp b/src/HFIBLDCMotor.cpp
@@ -364,7 +364,7 @@ void IRAM_ATTR HFIBLDCMotor::process_hfi(){
   current_meas.d = ABcurrent.alpha * _ca + ABcurrent.beta * _sa;
   current_meas.q = ABcurrent.beta * _ca - ABcurrent.alpha * _sa;
 
-  if(current_meas.d+current_meas.q>ocp_protection_limit){
+  if(fabs(current_meas.d)+fabs(current_meas.q)>ocp_protection_limit){
     ocp_cycles_counter+=1;
   }
 
@@ -431,23 +431,29 @@ void IRAM_ATTR HFIBLDCMotor::process_hfi(){
     i_beta_prev=ABcurrent.beta;
 
     flux_observer_angle=_atan2(flux_beta,flux_alpha);
-    if(flux_observer_angle<0) {flux_observer_angle+=_2PI;}
-    
-    bemf=(polarity_correction*(voltage.q -phase_resistance * current_meas.q));
-    flux_observer_velocity=((bemf*KV_rating*_SQRT3*_2PI)/(60.0f));
+    if(polarity_correction<0){
+      flux_observer_angle-=_PI;
+    }
+    while(flux_observer_angle<0){
+      flux_observer_angle+=_2PI;
+    }
+    float iir_coef=0.99;
+    //
+    bemf=bemf*(iir_coef)+(1.0-iir_coef)*(polarity_correction*(voltage.q - phase_resistance * current_meas.q));
+    flux_observer_velocity = flux_observer_velocity*iir_coef+(1.0-iir_coef)*((bemf*KV_rating*_SQRT3*_2PI)/(60.0f)); 
 
     if(bemf>bemf_threshold || bemf<-bemf_threshold){
       bemf_count+=2;
     }else{
       bemf_count-=2;
       if(bemf_count<0) {bemf_count=0;}
     }
-    if(bemf_count>100){ // use flux observer after 
+    if(bemf_count>fo_hysteresis_threshold){ // use flux observer after 
       bemf_count+=1;
-      if(bemf_count>200){bemf_count=200;}
+      if(bemf_count>2*fo_hysteresis_threshold){bemf_count=2*fo_hysteresis_threshold;}
     }
 
-    if(bemf_count>102){
+    if(bemf_count>fo_hysteresis_threshold){
       // sensorless_out=flux_observer_angle;
       // sensorless_velocity = flux_observer_velocity;
       hfi_v_act=0;
@@ -531,31 +537,36 @@ void IRAM_ATTR HFIBLDCMotor::process_hfi(){
     current_err.q = polarity_correction * current_setpoint.q - current_meas.q;
     current_err.d = polarity_correction * current_setpoint.d - current_meas.d;
 
+    if (bemf_count <= fo_hysteresis_threshold)
+    {
+      sensorless_out = hfi_angle;
+      sensorless_velocity = hfi_velocity;
+    }else
+    {
+      sensorless_out = flux_observer_angle;
+      sensorless_velocity = flux_observer_velocity;
+      usedFOlast = true;
+    }
     
     voltage_pid.q = PID_current_q(current_err.q, Ts, Ts_div);
     voltage_pid.d = PID_current_d(current_err.d, Ts, Ts_div);
     
+    //voltage_pid.q+= bemf;//+delta_current.q/Ts*Lq+sensorless_velocity*pole_pairs*polarity_correction * current_setpoint.d*Ld;
+    //voltage_pid.d-=sensorless_velocity*pole_pairs*polarity_correction * current_setpoint.q*Lq;//
+
 
     // lowpass does a += on the first arg
-    LOWPASS(voltage.q, voltage_pid.q, 0.34f);
-    LOWPASS(voltage.d, voltage_pid.d, 0.34f);
+    // LOWPASS(voltage.q, voltage_pid.q, 0.34f);
+    // LOWPASS(voltage.d, voltage_pid.d, 0.34f);
+
+    voltage.q=voltage_pid.q;//+bemf*pole_pairs+sensorless_velocity*pole_pairs*polarity_correction * current_setpoint.d*Ld;
+    voltage.d=voltage_pid.d;//-sensorless_velocity*pole_pairs*polarity_correction * current_setpoint.q*Lq;;
   
     voltage.d = _constrain(voltage.d ,-voltage_limit, voltage_limit);
     voltage.q = _constrain(voltage.q ,-voltage_limit, voltage_limit);
   
     voltage.d += hfi_v_act;
 
-    if (bemf_count < 100)
-    {
-      sensorless_out = hfi_angle;
-      sensorless_velocity = hfi_velocity;
-    }else
-    {
-      sensorless_out = flux_observer_angle;
-      sensorless_velocity = flux_observer_velocity;
-      usedFOlast = true;
-    }
-    
     sensorless_out_prev = sensorless_out;
   }
 
diff --git a/src/HFIBLDCMotor.h b/src/HFIBLDCMotor.h
@@ -50,13 +50,14 @@ class HFIBLDCMotor: public FOCMotor
 
     float Ts = 1.0f/60000.0f;
     float Ts_L = Ts * ( 1 / Lq - 1 / Ld );
-    float current_bandwidth = 300;
+    float current_bandwidth = 1000;
     float polarity_max_pos=0;
     float polarity_max_neg=0;
     float polarity_detection=0;
     float polarity_alignment_voltage=0.5;
     float bemf_threshold=5;
     float deadtime_compensation=0.0;
+    float fo_hysteresis_threshold=200;
 
     DQCurrent_s current_meas;
     DQCurrent_s current_high;
@@ -182,6 +183,7 @@ class HFIBLDCMotor: public FOCMotor
     float Ts_pp_div = 1.0f / (Ts * pole_pairs);
     float Ts_div = 1.0f / Ts;
     float predivAngleest = 1.0f / (hfi_v * Ts * ( 1.0f / Lq - 1.0f / Ld ) );
+    float fo_prev = 0;
 
 };
 
