X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Fblocking_detection_manager%2Fblocking_detection_manager.c;h=1e3808bb1d5e1ac699ae6de9b151cb8f21cfc0a6;hp=cdb2b58cca1c31b6350a68ba6431030a26b99e7a;hb=bf5060a8c2deade2516d1cfeccae7137e740b4c4;hpb=ccc6954bb046671b9e28c5806db5121c1eef49c0

diff --git a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
index cdb2b58..1e3808b 100644
--- a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
+++ b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation (2007)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -38,8 +38,8 @@ void bd_init(struct blocking_detection * bd)
 }
 
 /* thresholds */
-void bd_set_current_thresholds(struct blocking_detection * bd, 
-			       int32_t k1, int32_t k2, 
+void bd_set_current_thresholds(struct blocking_detection * bd,
+			       int32_t k1, int32_t k2,
 			       uint32_t i_thres, uint16_t cpt_thres)
 {
 	uint8_t flags;
@@ -53,7 +53,7 @@ void bd_set_current_thresholds(struct blocking_detection * bd,
 }
 
 /* speed threshold */
-void bd_set_speed_threshold(struct blocking_detection * bd, 
+void bd_set_speed_threshold(struct blocking_detection * bd,
 			    uint16_t speed)
 {
 	uint8_t flags;
@@ -71,45 +71,57 @@ void bd_reset(struct blocking_detection * bd)
 	IRQ_UNLOCK(flags);
 }
 
-
+static inline  uint8_t same_sign(int32_t a, int32_t b)
+{
+	if (a >= 0 && b >= 0)
+		return 1;
+	if (a < 0 && b < 0)
+		return 1;
+	return 0;
+}
 
 /** function to be called periodically */
-void bd_manage_from_speed_cmd(struct blocking_detection * bd, 
-			      int32_t speed, int32_t cmd)	   
+void bd_manage_from_speed_cmd(struct blocking_detection * bd,
+			      int32_t speed, int32_t cmd)
 {
-	int32_t i=0;
+	int32_t i = 0;
 
-	/* 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;
-		}
+	/* disabled */
+	if (bd->cpt_thres == 0)
+		return;
+
+	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) &&
+	    same_sign(i, cmd)) {
+		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;
+	}
 #if BD_DEBUG
-		if (bd->debug_cpt++ == BD_DEBUG) {
-			DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
-			      cmd, speed, i);
-			bd->debug_cpt = 0;
-		}
+	if (bd->debug_cpt++ == BD_DEBUG) {
+		DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
+		      cmd, speed, i);
+		bd->debug_cpt = 0;
 	}
 #endif
 }
 
 /** function to be called periodically */
-void bd_manage_from_pos_cmd(struct blocking_detection * bd, 
-			    int32_t pos, int32_t cmd)   
+void bd_manage_from_pos_cmd(struct blocking_detection * bd,
+			    int32_t pos, int32_t cmd)
 {
 	int32_t speed = (pos - bd->prev_pos);
-	bd_manage_from_speed_cmd(bd, speed, cmd);	
+	bd_manage_from_speed_cmd(bd, speed, cmd);
 	bd->prev_pos = pos;
 }