make encoder dir find and overspeed check

Firmware/MotorControl/axis.cpp

@@ -137,7 +137,7 @@ bool Axis::run_lockin_spin() {
     lockin_state_ = LOCKIN_STATE_RAMP;
     float x = 0.0f;
     run_control_loop([&]() {
-        float phase = wrap_pm_pi(config_.lockin_ramp_time * x);
+        float phase = wrap_pm_pi(config_.lockin_ramp_distance * x);
         float I_mag = config_.lockin_current * x;
         x += current_meas_period / config_.lockin_ramp_time;
         if (!motor_.update(I_mag, phase))
@@ -146,7 +146,7 @@ bool Axis::run_lockin_spin() {
     });
     
     // Spin states
-    float distance = config_.lockin_ramp_time;
+    float distance = config_.lockin_ramp_distance;
     float phase = wrap_pm_pi(distance);
     float vel = distance / config_.lockin_ramp_time;
 
@@ -154,7 +154,7 @@ bool Axis::run_lockin_spin() {
     auto spin_done = [&](bool vel_override = false) -> bool {
         bool done = false;
         if (config_.lockin_finish_on_vel || vel_override)
-            done = done || vel >= config_.lockin_vel;
+            done = done || fabsf(vel) >= fabsf(config_.lockin_vel);
         if (config_.lockin_finish_on_distance)
             done = done || fabsf(distance) >= fabsf(config_.lockin_finish_distance);
         if (config_.lockin_finish_on_enc_idx)
@@ -298,7 +298,10 @@ void Axis::run_state_machine_loop() {
             case AXIS_STATE_ENCODER_INDEX_SEARCH: {
                 if (!motor_.is_calibrated_)
                     goto invalid_state_label;
+                if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
+                    goto invalid_state_label;
 
+                // TODO: move code body to function in Encoder
                 encoder_.config_.use_index = true;
                 encoder_.index_found_ = false;
 
@@ -306,7 +309,32 @@ void Axis::run_state_machine_loop() {
                 config_.lockin_finish_on_enc_idx = true;
                 status = run_lockin_spin();
                 config_.lockin_finish_on_enc_idx = orig_setting;
-                // status = encoder_.run_index_search();
+            } break;
+
+            case AXIS_STATE_ENCODER_DIR_FIND: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+
+                // TODO: move code body to function in Encoder
+                int32_t init_enc_val = encoder_.shadow_count_;
+                bool orig_setting = config_.lockin_finish_on_distance;
+                config_.lockin_finish_on_distance = true;
+                motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
+                status = run_lockin_spin();
+                config_.lockin_finish_on_distance = orig_setting;
+
+                if (status) {
+                    // Check response and direction
+                    if (encoder_.shadow_count_ > init_enc_val + 8) {
+                        // motor same dir as encoder
+                        motor_.config_.direction = 1;
+                    } else if (encoder_.shadow_count_ < init_enc_val - 8) {
+                        // motor opposite dir as encoder
+                        motor_.config_.direction = -1;
+                    } else {
+                        motor_.config_.direction = 0;
+                    }
+                }
             } break;
 
             case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
@@ -316,13 +344,13 @@ void Axis::run_state_machine_loop() {
             } break;
 
             case AXIS_STATE_LOCKIN_SPIN: {
-                if (!motor_.is_calibrated_)
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                     goto invalid_state_label;
                 status = run_lockin_spin();
             } break;
 
             case AXIS_STATE_SENSORLESS_CONTROL: {
-                if (!motor_.is_calibrated_)
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                         goto invalid_state_label;
                 status = run_lockin_spin(); // TODO: restart if desired
                 if (status) {
@@ -334,7 +362,7 @@ void Axis::run_state_machine_loop() {
             } break;
 
             case AXIS_STATE_CLOSED_LOOP_CONTROL: {
-                if (!motor_.is_calibrated_)
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                     goto invalid_state_label;
                 if (!encoder_.is_ready_)
                     goto invalid_state_label;

Firmware/MotorControl/axis.hpp

@@ -33,6 +33,7 @@ public:
         AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
         AXIS_STATE_CLOSED_LOOP_CONTROL = 8,  //<! run closed loop control
         AXIS_STATE_LOCKIN_SPIN = 9,       //<! run lockin spin
+        AXIS_STATE_ENCODER_DIR_FIND = 10,
     };
 
     struct Config_t {

Firmware/MotorControl/encoder.cpp

@@ -274,6 +274,15 @@ bool Encoder::update() {
         snap_to_zero_vel = true;
     }
 
+    // Check overspeed fault
+    if (config_.overspeed_fault_ratio != 0.0f) { // 0.0f = disabled
+        // TODO: Use separate encoder, motor, controller vel_lim: take min
+        if (fabsf(vel_estimate_) > config_.overspeed_fault_ratio * axis_->controller_.config_.vel_limit) {
+            set_error(ERROR_OVERSPEED);
+            return false;
+        }
+    }
+
     //// run encoder count interpolation
     int32_t corrected_enc = count_in_cpr_ - config_.offset;
     // if we are stopped, make sure we don't randomly drift

Firmware/MotorControl/encoder.hpp

@@ -15,6 +15,7 @@ public:
         ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
         ERROR_ILLEGAL_HALL_STATE = 0x10,
         ERROR_INDEX_NOT_FOUND_YET = 0x20,
+        ERROR_OVERSPEED = 0x40,
     };
 
     enum Mode_t {
@@ -36,7 +37,9 @@ public:
         float offset_float = 0.0f; // Sub-count phase alignment offset
         float calib_range = 0.02f;
         float bandwidth = 1000.0f;
+        float overspeed_fault_ratio = 1.2f; // ratio of vel_lim, 0.0f = disabled
         bool find_idx_on_lockin = false;
+        bool idx_search_unidirectional = false;
     };
 
     Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -98,6 +101,7 @@ public:
                 make_protocol_property("mode", &config_.mode),
                 make_protocol_property("use_index", &config_.use_index),
                 make_protocol_property("pre_calibrated", &config_.pre_calibrated),
+                make_protocol_property("overspeed_fault_ratio", &config_.overspeed_fault_ratio),
                 make_protocol_property("idx_search_speed", &config_.idx_search_speed),
                 make_protocol_property("cpr", &config_.cpr),
                 make_protocol_property("offset", &config_.offset),
@@ -105,7 +109,8 @@ public:
                 make_protocol_property("bandwidth", &config_.bandwidth,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
                 make_protocol_property("calib_range", &config_.calib_range),
-                make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin)
+                make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin),
+                make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional)
             )
         );
     }

Firmware/MotorControl/motor.hpp

@@ -58,7 +58,7 @@ public:
         float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
         float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
         float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
-        int32_t direction = 1;                // 1 or -1
+        int32_t direction = 0;                // 1 or -1 (0 = unspecified)
         MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
         // Read out max_allowed_current to see max supported value for current_lim.
         // float current_lim = 70.0f; //[A]

tools/odrive/enums.py

@@ -11,6 +11,7 @@ AXIS_STATE_ENCODER_INDEX_SEARCH = 6
 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
 AXIS_STATE_CLOSED_LOOP_CONTROL = 8
 AXIS_STATE_LOCKIN_SPIN = 9
+AXIS_STATE_ENCODER_DIR_FIND = 10
 
 AXIS_ERROR_NONE = 0
 AXIS_ERROR_INVALID_STATE = 1