Diff: main.cpp

Revision:

193:3abadeecf908

Parent:

188:43f50a4cc040

Child:

194:05e119bc5a78

--- a/main.cpp	Sat Feb 10 03:58:36 2018 +0000
+++ b/main.cpp	Sat Feb 10 05:35:49 2018 +0000
@@ -15,7 +15,6 @@
 #include "BREMSConfig.h"
 
 #include "layout.h"
-#include "defaults.h"
 #include "derived.h"
 #include "main.h"
 
@@ -31,7 +30,8 @@
 
 void commutate() {  
     /*safety checks, do we do anything this cycle?*/
-    if (!control.enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
+    //if (!control.enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
+    if (!control.enabled && checks_passed()) {
         go_enabled();
     }
     
@@ -44,7 +44,7 @@
     loop_counter++;
     
     /*update position, sin, cos*/
-    foc.p = io.pos->GetElecPosition() - POS_OFFSET;
+    foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
     float sin_p = sinf(foc.p);
     float cos_p = cosf(foc.p);
     
@@ -57,8 +57,8 @@
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
     /*PI controller*/
-    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);
+    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);
         
     float d_err = control.d_ref - control.d_filtered;
     float q_err = control.q_ref - control.q_filtered;
@@ -66,24 +66,24 @@
     control.d_integral += d_err * KI_D;
     control.q_integral += q_err * KI_Q;
     
-    constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, INTEGRAL_MAX);
+    constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX);
     
-    foc.vd_decouple = -Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f;
-    foc.vq_decouple = Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f;
+    foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f;
+    foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f;
     
     constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
     
     foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
     foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
     
-    constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + OVERMODULATION_FACTOR);
+    constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR);
     
     if (!control.enabled) {
         foc.vd = 0.0f;
         foc.vq = 0.0f;
     }
     
-    float pv = foc.p + read.w / V_PHASE_SWIZZLE;
+    float pv = foc.p + read.w / _V_PHASE_SWIZZLE;
     float sin_pv = sinf(pv);
     float cos_pv = cosf(pv);
     
@@ -102,7 +102,8 @@
     vc = vc - voff;
     
     /*safety checks, reset integral*/
-    if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
+    //if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
+    if (!checks_passed()) {
         /*do this even in disabled state, to keep integral down*/
         go_disabled();
     }
@@ -113,12 +114,13 @@
     set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
     
     /*log data*/
-    if (ENABLE_LOGGING) log();
+    if (_ENABLE_LOGGING) log();
 }
 
 void slow_loop() {
     float x = io.throttle_in->get_throttle();
     x = control.throttle_filter->update(x);
+    control.user_cmd = io.throttle_in->get_usecs();
     control.torque_percent = th->map(x, read.w);
     dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
 }
@@ -131,8 +133,8 @@
     packet[3] = control.q_ref / 2.0f + 128.0f;
     packet[4] = control.q_filtered / 2.0f + 128.0f;
     packet[5] = 255.0f * control.torque_percent;
-    packet[6] = 255.0f / (1.0f + OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
-    packet[7] = 255.0f / (1.0f + OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
+    packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
+    packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
     io.logger->log(packet);
 }