Diff: main.cpp

Revision:

56:c681001dfa46

Parent:

55:3568b78dc203

Child:

58:7316c5a4c417

--- a/main.cpp	Wed Jan 25 02:41:17 2017 +0000
+++ b/main.cpp	Wed Jan 25 22:19:55 2017 +0000
@@ -47,11 +47,8 @@
 }
 
 void commutate() {
-    /*safety checks*/
-    if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
-        /*do this even in disabled state, to keep integral down*/
-        go_disabled();
-    } else if (!control_enabled) {
+    /*safety checks, do we do anything this cycle?*/
+    if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
         go_enabled();
     }
     
@@ -77,9 +74,6 @@
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
     /*PI controller*/
-    control.d_integral *= 1.0f - INTEGRAL_DECAY;
-    control.q_integral *= 1.0f - INTEGRAL_DECAY;
-    
     control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
     control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
         
@@ -117,6 +111,12 @@
     vb = vb - voff;
     vc = vc - voff;
     
+    /*safety checks, reset integral*/
+    if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
+        /*do this even in disabled state, to keep integral down*/
+        go_disabled();
+    }
+    
     /*output to timers*/
     set_dtc(io.a, 0.5f + 0.5f * va);
     set_dtc(io.b, 0.5f + 0.5f * vb);