From 7bb6d58c6012c760a1a073effd13aee9b8ac7843 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Tue, 2 Feb 2021 21:59:15 +0100
Subject: [PATCH] implement last_n error check

---
 src/main.cpp | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 8638d20..cbf536a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,7 +4,6 @@
 /*
   TODO: 
   - correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state
-  - implement second blind
   - implement drive to position
   - implement failure detection
   - implement homie
@@ -66,6 +65,8 @@ struct blindmodel
   unsigned long lastreadtime=0;
   float position=0; //0 is furthest open. positive is down (closing). unit is mm. estimated position
 
+  int8_t last_n=0; //-1 means not defined/unknown
+
   float length_clear; //length of clear part in position units
   float length_opaque; //lengt of opaque part in position units
   float start_first_clear; //distance from end marker to beginning of first clear section
@@ -121,8 +122,9 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B
 unsigned long last_motor_send=0;
 #define MOTOR_UPDATE_INTERVAL 100
 
-#define DIFF_ERROR_FACTOR 0.35 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed
-#define ERRORCODE_POSITIONDIFFTOOHIGH 1
+#define DIFF_ERROR_FACTOR 0.4 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed
+#define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
+#define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
 
 int getFitered(int* values,uint8_t size);
 void classifySensorValue(blindmodel &blind);
@@ -203,7 +205,6 @@ void loop() {
   checkButton(button2);
   
 
-
   //Manual movement by button
   manualMoveHandler(button1, blind1);
   manualMoveHandler(button2, blind2);
@@ -413,8 +414,12 @@ void estimatePosition(blindmodel &blind) {
             blind.position = blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n
             float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
             blind.position += _filterdelay_correction; //correct for filter delay
-            Serial.print("n=");
-             Serial.println(_n);
+            Serial.print("nearest n="); Serial.println(_n); 
+
+            if (blind.last_n !=-1 && abs(blind.last_n-_n>1)) { //if last_n is known and if n is not the next (error)
+              setError(blind, ERRORCODE_N_NOT_NEXT);
+            }
+            blind.last_n=_n; //for error check
 
             Serial.print("posCorrection="); Serial.print(_filterdelay_correction); 
 
@@ -491,7 +496,7 @@ void checkModes(blindmodel &blind) {
             //blind.mode=MODE_IDLE;
             blind.mode=MODE_MEASURE_SPEED; //next measure speed
             blind.mode_find_end_state=0; //reset
-            
+            blind.last_n=-1; //unknown
           }
           break;
       }