00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <stdio.h>
00026 #include <string.h>
00027 #include <aversive/error.h>
00028
00029 #include <blocking_detection_manager.h>
00030
00032 void bd_init(struct blocking_detection * bd)
00033 {
00034 uint8_t flags;
00035 IRQ_LOCK(flags);
00036 memset(bd, 0, sizeof(*bd));
00037 IRQ_UNLOCK(flags);
00038 }
00039
00040
00041 void bd_set_current_thresholds(struct blocking_detection * bd,
00042 int32_t k1, int32_t k2,
00043 uint32_t i_thres, uint16_t cpt_thres)
00044 {
00045 uint8_t flags;
00046 IRQ_LOCK(flags);
00047 bd->k1 = k1;
00048 bd->k2 = k2;
00049 bd->i_thres = i_thres;
00050 bd->cpt_thres = cpt_thres;
00051 bd->cpt = 0;
00052 IRQ_UNLOCK(flags);
00053 }
00054
00055
00056 void bd_set_speed_threshold(struct blocking_detection * bd,
00057 uint16_t speed)
00058 {
00059 uint8_t flags;
00060 IRQ_LOCK(flags);
00061 bd->speed_thres = speed;
00062 IRQ_UNLOCK(flags);
00063 }
00064
00066 void bd_reset(struct blocking_detection * bd)
00067 {
00068 uint8_t flags;
00069 IRQ_LOCK(flags);
00070 bd->cpt = 0;
00071 IRQ_UNLOCK(flags);
00072 }
00073
00074
00075
00077 void bd_manage_from_speed_cmd(struct blocking_detection * bd,
00078 int32_t speed, int32_t cmd)
00079 {
00080 int32_t i=0;
00081
00082
00083 if ( bd->cpt_thres ) {
00084 i = bd->k1 * cmd - bd->k2 * speed;
00085 if (ABS(i) > bd->i_thres &&
00086 (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
00087 if (bd->cpt == bd->cpt_thres - 1)
00088 WARNING(E_BLOCKING_DETECTION_MANAGER,
00089 "BLOCKING cmd=%ld, speed=%ld i=%ld",
00090 cmd, speed, i);
00091 if(bd->cpt < bd->cpt_thres)
00092 bd->cpt++;
00093 }
00094 else {
00095 bd->cpt=0;
00096 }
00097 #if BD_DEBUG
00098 if (bd->debug_cpt++ == BD_DEBUG) {
00099 DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
00100 cmd, speed, i);
00101 bd->debug_cpt = 0;
00102 }
00103 }
00104 #endif
00105 }
00106
00108 void bd_manage_from_pos_cmd(struct blocking_detection * bd,
00109 int32_t pos, int32_t cmd)
00110 {
00111 int32_t speed = (pos - bd->prev_pos);
00112 bd_manage_from_speed_cmd(bd, speed, cmd);
00113 bd->prev_pos = pos;
00114 }
00115
00117 void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
00118 {
00119 bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs), cs_get_out(cs));
00120 }
00121
00123 uint8_t bd_get(struct blocking_detection * bd)
00124 {
00125 uint8_t ret, flags;
00126 IRQ_LOCK(flags);
00127 ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));
00128 IRQ_UNLOCK(flags);
00129 return ret;
00130 }