git.droids-corp.org
/
aversive.git
/ commitdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
| commitdiff |
tree
raw
|
patch
|
inline
| side by side (from parent 1:
876789a
)
fix possible overflow in blocking detection, and reindent
author
zer0
<zer0@carbon.local>
Tue, 4 May 2010 17:13:46 +0000
(19:13 +0200)
committer
zer0
<zer0@carbon.local>
Tue, 4 May 2010 17:13:46 +0000
(19:13 +0200)
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
patch
|
blob
|
history
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
patch
|
blob
|
history
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
8b5f547
..
1e3808b
100644
(file)
--- a/
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
+++ b/
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
@@
-71,6
+71,15
@@
void bd_reset(struct blocking_detection * bd)
IRQ_UNLOCK(flags);
}
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)
/** function to be called periodically */
void bd_manage_from_speed_cmd(struct blocking_detection * bd,
int32_t speed, int32_t cmd)
@@
-87,7
+96,7
@@
void bd_manage_from_speed_cmd(struct blocking_detection * bd,
* has the same sign than i */
if ((uint32_t)ABS(i) > bd->i_thres &&
(bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
* 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
)) {
+
same_sign(i, cmd
)) {
if (bd->cpt == bd->cpt_thres - 1)
WARNING(E_BLOCKING_DETECTION_MANAGER,
"BLOCKING cmd=%ld, speed=%ld i=%ld",
if (bd->cpt == bd->cpt_thres - 1)
WARNING(E_BLOCKING_DETECTION_MANAGER,
"BLOCKING cmd=%ld, speed=%ld i=%ld",
diff --git
a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
index
27b7897
..
4afce09
100644
(file)
--- a/
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
+++ b/
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
@@
-1,6
+1,6
@@
-/*
+/*
* Copyright Droids Corporation (2007)
* 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
* 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
@@
-30,14
+30,14
@@
#include <control_system_manager.h>
#include <control_system_manager.h>
-/* detect blocking based on motor current.
+/* detect blocking based on motor current.
* triggers the blocking if:
* triggers the blocking if:
- * - the current in the motor is a above a threshold
+ * - the current in the motor is a above a threshold
* during n tests
* - the speed is below the threshold (if specified)
*
* We suppose that i = k1.V - k2.w
* during n tests
* - the speed is below the threshold (if specified)
*
* We suppose that i = k1.V - k2.w
- * (V is the voltage applied on the motor, and w the current speed
+ * (V is the voltage applied on the motor, and w the current speed
* of the motor)
*/
* of the motor)
*/
@@
-59,34
+59,34
@@
struct blocking_detection {
};
/** init module, give the cs as parameter */
};
/** init module, give the cs as parameter */
-void bd_init(struct blocking_detection *
bd);
+void bd_init(struct blocking_detection *bd);
-/** thresholds for current-based blocking detection. If cpt_thres
+/** thresholds for current-based blocking detection. If cpt_thres
* is 0, disable it. */
* is 0, disable it. */
-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);
/** speed threshold: if speed is above it, disable
uint32_t i_thres, uint16_t cpt_thres);
/** speed threshold: if speed is above it, disable
- *
blocking_detection. */
-void bd_set_speed_threshold(struct blocking_detection *
bd,
+ * blocking_detection. */
+void bd_set_speed_threshold(struct blocking_detection *
bd,
uint16_t speed);
/** reset the blocking */
uint16_t speed);
/** reset the blocking */
-void bd_reset(struct blocking_detection *
bd);
+void bd_reset(struct blocking_detection *bd);
-/** function to be called periodically
n
, when we use cs structure */
-void bd_manage_from_cs(struct blocking_detection *
bd, struct cs *cs);
+/** function to be called periodically, when we use cs structure */
+void bd_manage_from_cs(struct blocking_detection *bd, struct cs *cs);
/** function to be called periodically, when we use values directly */
/** function to be called periodically, when we use values directly */
-void bd_manage_from_pos_cmd(struct blocking_detection *
bd,
+void bd_manage_from_pos_cmd(struct blocking_detection *
bd,
int32_t pos, int32_t cmd);
/** function to be called periodically, when we use values directly */
int32_t pos, int32_t cmd);
/** function to be called periodically, when we use values directly */
-void bd_manage_from_speed_cmd(struct blocking_detection *
bd,
+void bd_manage_from_speed_cmd(struct blocking_detection *
bd,
int32_t speed, int32_t cmd);
/** get value of blocking detection */
int32_t speed, int32_t cmd);
/** get value of blocking detection */
-uint8_t bd_get(struct blocking_detection *
bd);
+uint8_t bd_get(struct blocking_detection *bd);
#endif
#endif