From 9973900313207975f9a57138b86e949c0168d7ce Mon Sep 17 00:00:00 2001 From: Inhar Andueza Date: Wed, 21 Mar 2018 16:01:42 +0100 Subject: [PATCH 1/2] To merge FTC in github --- src/ikSensorDiagnoser/ikSensorDiagnoser.c | 35 ++++++++++++++--------- src/ikSensorDiagnoser/ikSensorDiagnoser.h | 2 ++ 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/ikSensorDiagnoser/ikSensorDiagnoser.c b/src/ikSensorDiagnoser/ikSensorDiagnoser.c index 14846ee..1ca8303 100644 --- a/src/ikSensorDiagnoser/ikSensorDiagnoser.c +++ b/src/ikSensorDiagnoser/ikSensorDiagnoser.c @@ -35,6 +35,7 @@ void ikSensorDiagnoser_initParams(ikSensorDiagnoserParams *params) { /* set default parameter values */ params->nStepsToFault = 1; params->tolerance = 1.0; + params->nIgnoredSteps = 0; } int ikSensorDiagnoser_init(ikSensorDiagnoser *self, const ikSensorDiagnoserParams *params) { @@ -59,24 +60,30 @@ int ikSensorDiagnoser_init(ikSensorDiagnoser *self, const ikSensorDiagnoserParam for (i = 0; i < 3; i++) self->ok[i] = self->n; return err; + + self->nSteps = params->nIgnoredSteps; /* ################################################*/ } void ikSensorDiagnoser_step(ikSensorDiagnoser *self, int ok[3], const double signals[3]) { int i,j; int _ok[3] = {0,0,0}; - - /* check tolerances */ - for(i = 0; i < 3; i++) { - j = i + 1; - if(j > 2) j = 0; - if(self->tol > fabs(signals[i] - signals[j])) { - if (self->ok[i] && self->ok[j]) { - _ok[i] = 1; - _ok[j] = 1; - } - } - } - + static int _t = 0; /*################################################*/ + /* check tolerances */ + for(i = 0; i < 3; i++) { + j = i + 1; + if(j > 2) j = 0; + if(self->tol > fabs(signals[i] - signals[j])) { + if (0nSteps && _tnSteps){ /*################################################*/ + _ok[i] = 1; /*################################################*/ + _ok[j] = 1; /*################################################*/ + _t++; /*################################################*/ + } /*################################################*/ + if (self->ok[i] && self->ok[j]) { + _ok[i] = 1; + _ok[j] = 1; + } + } + } /* compute steps left for fault detection */ for(i = 0; i < 3; i++) { self->ok[i]--; @@ -90,7 +97,7 @@ void ikSensorDiagnoser_step(ikSensorDiagnoser *self, int ok[3], const double sig void ikSensorDiagnoser_getOutput(const ikSensorDiagnoser *self, int ok[3]) { int i; - + for(i = 0; i < 3; i++) ok[i] = self->ok[i] > 0; } diff --git a/src/ikSensorDiagnoser/ikSensorDiagnoser.h b/src/ikSensorDiagnoser/ikSensorDiagnoser.h index 833846e..a0cd45e 100644 --- a/src/ikSensorDiagnoser/ikSensorDiagnoser.h +++ b/src/ikSensorDiagnoser/ikSensorDiagnoser.h @@ -55,6 +55,7 @@ extern "C" { int ok[3]; int n; double tol; + int nSteps;/* ################################################*/ /* @endcond */ } ikSensorDiagnoser; @@ -65,6 +66,7 @@ extern "C" { typedef struct ikSensorDiagnoserParams { int nStepsToFault; /** Date: Wed, 28 Mar 2018 10:38:15 +0200 Subject: [PATCH 2/2] merge-atzeko prest --- src/ikSensorDiagnoser/ikSensorDiagnoser.c | 34 +++++++++++------------ src/ikSensorDiagnoser/ikSensorDiagnoser.h | 3 +- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/src/ikSensorDiagnoser/ikSensorDiagnoser.c b/src/ikSensorDiagnoser/ikSensorDiagnoser.c index 1ca8303..d6ee676 100644 --- a/src/ikSensorDiagnoser/ikSensorDiagnoser.c +++ b/src/ikSensorDiagnoser/ikSensorDiagnoser.c @@ -35,7 +35,6 @@ void ikSensorDiagnoser_initParams(ikSensorDiagnoserParams *params) { /* set default parameter values */ params->nStepsToFault = 1; params->tolerance = 1.0; - params->nIgnoredSteps = 0; } int ikSensorDiagnoser_init(ikSensorDiagnoser *self, const ikSensorDiagnoserParams *params) { @@ -55,34 +54,26 @@ int ikSensorDiagnoser_init(ikSensorDiagnoser *self, const ikSensorDiagnoserParam self->tol = 1.0; if (!err) err = -2; } - /* initilise fault states */ for (i = 0; i < 3; i++) self->ok[i] = self->n; - + return err; - self->nSteps = params->nIgnoredSteps; /* ################################################*/ } void ikSensorDiagnoser_step(ikSensorDiagnoser *self, int ok[3], const double signals[3]) { int i,j; int _ok[3] = {0,0,0}; - static int _t = 0; /*################################################*/ /* check tolerances */ for(i = 0; i < 3; i++) { j = i + 1; if(j > 2) j = 0; - if(self->tol > fabs(signals[i] - signals[j])) { - if (0nSteps && _tnSteps){ /*################################################*/ - _ok[i] = 1; /*################################################*/ - _ok[j] = 1; /*################################################*/ - _t++; /*################################################*/ - } /*################################################*/ - if (self->ok[i] && self->ok[j]) { - _ok[i] = 1; - _ok[j] = 1; - } + if(self->tol > fabs(signals[i] - signals[j])) { + if (self->ok[i] && self->ok[j]) { + _ok[i] = 1; + _ok[j] = 1; } + } } /* compute steps left for fault detection */ for(i = 0; i < 3; i++) { @@ -90,7 +81,16 @@ void ikSensorDiagnoser_step(ikSensorDiagnoser *self, int ok[3], const double sig self->ok[i] = self->ok[i] > 0 ? self->ok[i] : 0; if (_ok[i]) self->ok[i] = self->n; } - + + /* + #################################################################### + */ + if(self->ResetSignal > 0){ + for(i = 0; i < 3; i++) { + self->ok[i] = 1; + self->ok[i] = self->n; + } + } /* set outputs */ ikSensorDiagnoser_getOutput(self, ok); } @@ -98,7 +98,7 @@ void ikSensorDiagnoser_step(ikSensorDiagnoser *self, int ok[3], const double sig void ikSensorDiagnoser_getOutput(const ikSensorDiagnoser *self, int ok[3]) { int i; - for(i = 0; i < 3; i++) ok[i] = self->ok[i] > 0; + for(i = 0; i < 3; i++) ok[i] = self->ok[i] > 0; } /* @endcond */ diff --git a/src/ikSensorDiagnoser/ikSensorDiagnoser.h b/src/ikSensorDiagnoser/ikSensorDiagnoser.h index a0cd45e..18aec9e 100644 --- a/src/ikSensorDiagnoser/ikSensorDiagnoser.h +++ b/src/ikSensorDiagnoser/ikSensorDiagnoser.h @@ -55,7 +55,7 @@ extern "C" { int ok[3]; int n; double tol; - int nSteps;/* ################################################*/ + int ResetSignal; /* @endcond */ } ikSensorDiagnoser; @@ -66,7 +66,6 @@ extern "C" { typedef struct ikSensorDiagnoserParams { int nStepsToFault; /**