- /* if current-based blocking_detection enabled */
- if ( bd->cpt_thres ) {
- i = bd->k1 * cmd - bd->k2 * speed;
- if ((uint32_t)ABS(i) > bd->i_thres &&
- (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
- if (bd->cpt == bd->cpt_thres - 1)
- WARNING(E_BLOCKING_DETECTION_MANAGER,
- "BLOCKING cmd=%ld, speed=%ld i=%ld",
- cmd, speed, i);
- if (bd->cpt < bd->cpt_thres)
- bd->cpt++;
- }
- else {
- bd->cpt=0;
- }
+ i = bd->k1 * cmd - bd->k2 * speed;
+
+ /* if i is above threshold, speed is below threshold, and cmd
+ * has the same sign than i */
+ if ((uint32_t)ABS(i) > bd->i_thres &&
+ (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
+ (i * cmd > 0)) {
+ if (bd->cpt == bd->cpt_thres - 1)
+ WARNING(E_BLOCKING_DETECTION_MANAGER,
+ "BLOCKING cmd=%ld, speed=%ld i=%ld",
+ cmd, speed, i);
+ if (bd->cpt < bd->cpt_thres)
+ bd->cpt++;
+ }
+ else {
+ bd->cpt=0;
+ }