From 6a929d4236303cb3aec7dc1a83c30bd1e899c853 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Thu, 12 Sep 2019 18:11:32 -0700
Subject: [PATCH] improve oscilloscope functionality

---
 Firmware/MotorControl/low_level.cpp |  5 -----
 Firmware/MotorControl/motor.cpp     | 25 +++++++++++++++++++++++++
 Firmware/MotorControl/odrive_main.h |  2 +-
 tools/odrive/shell.py               |  5 +++--
 tools/odrive/utils.py               |  6 ++++++
 5 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index 3d02ee73..82e54049 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -426,11 +426,6 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
     // Only one conversion in sequence, so only rank1
     uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
     vbus_voltage = ADCValue * voltage_scale;
-    if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) {
-        if (oscilloscope_pos >= OSCILLOSCOPE_SIZE)
-            oscilloscope_pos = 0;
-        oscilloscope[oscilloscope_pos++] = vbus_voltage;
-    }
 }
 
 static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index fd628c89..17306e59 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -410,6 +410,31 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
         return false; // error set inside enqueue_modulation_timings
     log_timing(TIMING_LOG_FOC_CURRENT);
 
+    if (axis_->axis_num_ == 0) {
+
+        // Edit these to suit your capture needs
+        float trigger_data = ictrl.v_current_control_integral_d;
+        float trigger_threshold = 0.5f;
+        float sample_data = Ialpha;
+
+        static bool ready = false;
+        static bool capturing = false;
+        if (trigger_data < trigger_threshold) {
+            ready = true;
+        }
+        if (ready && trigger_data >= trigger_threshold) {
+            capturing = true;
+            ready = false;
+        }
+        if (capturing) {
+            oscilloscope[oscilloscope_pos] = sample_data;
+            if (++oscilloscope_pos >= OSCILLOSCOPE_SIZE) {
+                oscilloscope_pos = 0;
+                capturing = false;
+            }
+        }
+    }
+
     return true;
 }
 
diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h
index 677fb996..e9d4978d 100644
--- a/Firmware/MotorControl/odrive_main.h
+++ b/Firmware/MotorControl/odrive_main.h
@@ -94,7 +94,7 @@ constexpr size_t AXIS_COUNT = 2;
 extern Axis *axes[AXIS_COUNT];
 
 // if you use the oscilloscope feature you can bump up this value
-#define OSCILLOSCOPE_SIZE 128
+#define OSCILLOSCOPE_SIZE 4096
 extern float oscilloscope[OSCILLOSCOPE_SIZE];
 extern size_t oscilloscope_pos;
 
diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py
index c25278e3..cb9cd25f 100644
--- a/tools/odrive/shell.py
+++ b/tools/odrive/shell.py
@@ -5,7 +5,7 @@ import threading
 import fibre
 import odrive
 import odrive.enums
-from odrive.utils import start_liveplotter, dump_errors
+from odrive.utils import start_liveplotter, dump_errors, oscilloscope_dump
 #from odrive.enums import * # pylint: disable=W0614
 
 def print_banner():
@@ -77,7 +77,8 @@ def launch_shell(args, logger, app_shutdown_token):
 
     interactive_variables = {
         'start_liveplotter': start_liveplotter,
-        'dump_errors': dump_errors
+        'dump_errors': dump_errors,
+        'oscilloscope_dump': oscilloscope_dump
     }
 
     # Expose all enums from odrive.enums
diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py
index f5ce0c7f..9ffe20e6 100755
--- a/tools/odrive/utils.py
+++ b/tools/odrive/utils.py
@@ -60,6 +60,12 @@ def dump_errors(odrv, clear=False):
             else:
                 print(prefix + _VT100Colors['green'] + "no error" + _VT100Colors['default'])
 
+def oscilloscope_dump(odrv, num_vals, filename='oscilloscope.csv'):
+    with open(filename, 'w') as f:
+        for x in range(num_vals):
+            f.write(str(odrv.get_oscilloscope_val(x)))
+            f.write('\n')
+
 data_rate = 10
 plot_rate = 10
 num_samples = 1000