introduce minimum angle to detect motion

Author: Richard Unger

src/BLDCMotor.cpp

@@ -113,8 +113,10 @@ int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
     // added the shaft_angle update
     sensor->update();
     shaft_angle = shaftAngle();
-  }else 
+  }else {
+    exit_flag = 0; // no FOC without sensor
     SIMPLEFOC_DEBUG("MOT: No sensor.");
+  }
 
   // aligning the current sensor - can be skipped
   // checks if driver phases are the same as current sense phases
@@ -201,7 +203,8 @@ int BLDCMotor::alignSensor() {
     setPhaseVoltage(0, 0, 0);
     _delay(200);
     // determine the direction the sensor moved
-    if (mid_angle == end_angle) {
+    float moved =  fabs(mid_angle - end_angle);
+    if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
       SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
       return 0; // failed calibration
     } else if (mid_angle < end_angle) {
@@ -212,7 +215,6 @@ int BLDCMotor::alignSensor() {
       sensor_direction = Direction::CW;
     }
     // check pole pair number
-    float moved =  fabs(mid_angle - end_angle);
     if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
       SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
     } else 

src/common/base_classes/Sensor.cpp

@@ -17,7 +17,8 @@ void Sensor::update() {
  /** get current angular velocity (rad/s) */
 float Sensor::getVelocity() {
     // calculate sample time
-    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
+    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f;
+    // TODO handle overflow - we do need to reset vel_angle_prev_ts
     if (Ts<minDeltaT) return velocity; // don't update velocity if deltaT is too small
 
     velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;

src/common/foc_utils.h

@@ -32,6 +32,8 @@
 #define _ACTIVE 1
 #define _NC (NOT_SET)
 
+#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
+
 // dq current structure
 struct DQCurrent_s
 {