From aaa5f802de7625090e5f70642a37f4978443ec1d Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 26 Jan 2018 22:34:34 -0800
Subject: [PATCH] configurable search speed and dir, check for already flashed
 calibration

---
 Firmware/MotorControl/low_level.c | 16 +++++++++++-----
 Firmware/MotorControl/low_level.h |  1 +
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c
index 3764fbb7..48fb730e 100644
--- a/Firmware/MotorControl/low_level.c
+++ b/Firmware/MotorControl/low_level.c
@@ -122,10 +122,11 @@ Motor_t motors[] = {
             .use_index = true,
             .index_found = false,
             .calibrated = false,
+            .idx_search_speed = 10.0f,
             .encoder_cpr = ENCODER_CPR,
             .encoder_offset = 0,
             .encoder_state = 0,
-            .motor_dir = 0,   // set by calib_enc_offset
+            .motor_dir = 1,   // 1 or -1
             .phase = 0.0f,    // [rad]
             .pll_pos = 0.0f,  // [rad]
             .pll_vel = 0.0f,  // [rad/s]
@@ -220,10 +221,11 @@ Motor_t motors[] = {
             .use_index = true,
             .index_found = false,
             .calibrated = false,
+            .idx_search_speed = 10.0f,
             .encoder_cpr = ENCODER_CPR,
             .encoder_offset = 0,
             .encoder_state = 0,
-            .motor_dir = 0,   // set by calib_enc_offset
+            .motor_dir = 1,   // 1 or -1
             .phase = 0.0f,    // [rad]
             .pll_pos = 0.0f,  // [rad]
             .pll_vel = 0.0f,  // [rad/s]
@@ -809,6 +811,7 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
 
     int offset = encvaluesum / (num_steps * 2);
     motor->encoder.encoder_offset = offset;
+    motor->encoder.calibrated = true;
     return true;
 }
 
@@ -832,10 +835,13 @@ bool motor_calibration(Motor_t* motor) {
     if (motor->rotor_mode == ROTOR_MODE_ENCODER ||
             motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) {
         if (motor->encoder.use_index && !motor->encoder.index_found)
-            if (!scan_for_enc_idx(motor, 10.0f, calibration_voltage))
+            if (!scan_for_enc_idx(motor,
+                    (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed,
+                    calibration_voltage))
+                return false;
+        if (!motor->encoder.calibrated)
+            if (!calib_enc_offset(motor, calibration_voltage))
                 return false;
-        if (!calib_enc_offset(motor, calibration_voltage))
-            return false;
     }
 
     // Calculate current control gains
diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h
index 07a79688..194e0a49 100644
--- a/Firmware/MotorControl/low_level.h
+++ b/Firmware/MotorControl/low_level.h
@@ -113,6 +113,7 @@ typedef struct {
     bool use_index;
     bool index_found;
     bool calibrated;
+    float idx_search_speed;
     int encoder_cpr;
     int32_t encoder_offset;
     int32_t encoder_state;