359
+ − 1 /*
+ − 2 * motion.c
+ − 3 *
+ − 4 * Created on: 20.05.2019
+ − 5 * Author: Thorsten Sonntag
+ − 6 */
+ − 7
+ − 8 #include <stdint.h>
+ − 9 #include <string.h>
373
+ − 10 #include <stdlib.h>
359
+ − 11 #include <math.h>
+ − 12 #include "motion.h"
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 13 #include "data_central.h"
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 14 #include "t7.h"
384
+ − 15 #include "t3.h"
371
+ − 16 #include "settings.h"
359
+ − 17
+ − 18 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
363
+ − 19 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 20
373
+ − 21 #define SECTOR_WINDOW 80.0 /* Pitch window which is used for custom view projection */
381
+ − 22 #define SECTOR_WINDOW_MAX 120.0 /* Pitch window which will be greater than the divers field of view */
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 23 #define SECTOR_HYSTERY 2 /* Additional offset to avoid fast changing displays */
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 24 #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 25 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 26
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 27 #define SECTOR_MAX 24 /* maximum number of sectors */
378
+ − 28 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
359
+ − 29
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 30 detectionState_t detectionState = DETECT_NOTHING;
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 31 SSector sectorDetection;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 32
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 33
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 34 uint8_t GetSectorForPitch(float pitch)
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 35 {
383
+ − 36 static uint8_t lastsector = 0;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 37 float newPitch;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 38 uint8_t sector = 0;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 39
383
+ − 40 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */
381
+ − 41 if (newPitch < 0.0) /* clip value */
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 42 {
381
+ − 43 newPitch = 0.0;
+ − 44 }
383
+ − 45 if (newPitch > sectorDetection.window) /* clip value */
381
+ − 46 {
383
+ − 47 newPitch = sectorDetection.window;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 48 }
381
+ − 49
+ − 50 /* switch to other sector? */
383
+ − 51 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
381
+ − 52 {
383
+ − 53 sector = (uint16_t) newPitch / sectorDetection.size;
+ − 54 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
+ − 55 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
381
+ − 56 lastsector = sector;
+ − 57 }
+ − 58
+ − 59 return lastsector;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 60 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 61
371
+ − 62 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 63 {
371
+ − 64 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
+ − 65 {
384
+ − 66 if(settingsGetPointer()->design == 3) /* Big font view ? */
+ − 67 {
+ − 68 sectorDetection.count = t3_GetEnabled_customviews();
+ − 69 }
+ − 70 else
+ − 71 {
+ − 72 sectorDetection.count = t7_GetEnabled_customviews();
+ − 73 }
383
+ − 74 if(sectorDetection.count > 7)
371
+ − 75 {
383
+ − 76 sectorDetection.count = 7; /* more views are hard to manually control */
371
+ − 77 }
+ − 78 }
+ − 79 else
373
+ − 80 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
371
+ − 81 {
383
+ − 82 sectorDetection.count = numOfSectors;
371
+ − 83 }
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 84
383
+ − 85 if(sectorDetection.count == SECTOR_MAX)
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 86 {
383
+ − 87 sectorDetection.window = SECTOR_WINDOW_MAX;
381
+ − 88 }
+ − 89 else
+ − 90 {
383
+ − 91 sectorDetection.window = SECTOR_WINDOW;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 92 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 93
383
+ − 94 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
+ − 95 sectorDetection.size = sectorDetection.window / sectorDetection.count;
+ − 96 sectorDetection.center = 0;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 97
383
+ − 98 /* reset border values */
+ − 99 sectorDetection.lowerborder = SECTOR_BORDER;
+ − 100 sectorDetection.upperborder = SECTOR_BORDER * -1.0;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 101 /* get the current sector */
383
+ − 102 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+ − 103 sectorDetection.target = sectorDetection.current;
+ − 104 /* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
+ − 105 sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
+ − 106
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 107 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 108
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 109 void InitMotionDetection(void)
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 110 {
383
+ − 111 sectorDetection.target = 0;
+ − 112 sectorDetection.current = 0;
+ − 113 sectorDetection.size = 0;
+ − 114 sectorDetection.count = 0;
371
+ − 115
+ − 116 switch(settingsGetPointer()->MotionDetection)
+ − 117 {
+ − 118 case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS);
+ − 119 break;
+ − 120 case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX);
+ − 121 break;
373
+ − 122 case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL);
+ − 123 break;
371
+ − 124 default:
+ − 125 break;
+ − 126 }
+ − 127
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 128 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 129
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 130 /* Map the current pitch value to a sector and create button event in case the sector is left */
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 131 detectionState_t detectSectorButtonEvent(float curPitch)
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 132 {
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 133 static uint8_t lastTargetSector = 0;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 134 uint8_t newTargetSector;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 135 uint8_t PitchEvent = DETECT_NOTHING;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 136
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 137 /* only change sector if reading is stable */
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 138 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 139 if(lastTargetSector == newTargetSector)
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 140 {
383
+ − 141 sectorDetection.target = newTargetSector;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 142 }
381
+ − 143 lastTargetSector = newTargetSector;
383
+ − 144 if(sectorDetection.target != sectorDetection.current)
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 145 {
383
+ − 146 if(sectorDetection.target > sectorDetection.current)
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 147 {
383
+ − 148 sectorDetection.current++;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 149 PitchEvent = DETECT_POS_PITCH;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 150 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 151 else
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 152 {
383
+ − 153 sectorDetection.current--;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 154 PitchEvent = DETECT_NEG_PITCH;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 155 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 156 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 157 return PitchEvent;
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 158 }
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 159
373
+ − 160 /* Check if pitch is not in center position and trigger a button action if needed */
+ − 161 detectionState_t detectScrollButtonEvent(float curPitch)
+ − 162 {
378
+ − 163 static uint8_t delayscroll = 0; /* slow down the number of scroll events */
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 164
373
+ − 165 uint8_t PitchEvent = DETECT_NOTHING;
+ − 166 uint8_t newSector;
+ − 167
+ − 168 if(delayscroll == 0)
+ − 169 {
+ − 170 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+ − 171 /* for scroll detection the motion windoe is split into 6 sectors => set event accoring to the sector number*/
+ − 172 switch(newSector)
+ − 173 {
+ − 174 case 0:
+ − 175 case 1: PitchEvent = DETECT_POS_PITCH;
+ − 176 break;
378
+ − 177 case 5:
+ − 178 case 6: PitchEvent = DETECT_NEG_PITCH;
373
+ − 179 break;
+ − 180 default:
+ − 181 break;
+ − 182 }
+ − 183 if(PitchEvent != DETECT_NOTHING)
+ − 184 {
+ − 185 delayscroll = 5;
+ − 186 }
+ − 187 }
+ − 188 else
+ − 189 {
+ − 190 delayscroll--;
+ − 191 }
+ − 192 return PitchEvent;
+ − 193 }
+ − 194
+ − 195
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 196 /* Detect if user is generating an pitch including return to starting position */
359
+ − 197 /* This is done by feeding the past movements value per value into a state machine */
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 198 detectionState_t detectPitch(float currentPitch)
359
+ − 199 {
373
+ − 200 static uint8_t lastSector = 0;
+ − 201 static uint8_t startSector = 0;
359
+ − 202 static uint8_t stableCnt = 0;
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 203
383
+ − 204 uint8_t curSector;
+ − 205
370
77cdfbdaca8c
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
diff
changeset
+ − 206 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */
359
+ − 207 {
+ − 208 detectionState = DETECT_NOTHING;
+ − 209 }
363
+ − 210
373
+ − 211 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+ − 212
+ − 213 /* feed value into state machine */
+ − 214 switch (detectionState)
+ − 215 {
381
+ − 216 case DETECT_NOTHING: if(curSector != lastSector) /* detect a stable condition before evaluating for the next move */
373
+ − 217 {
+ − 218 stableCnt=0;
+ − 219 }
+ − 220
+ − 221 if(stableCnt > STABLE_STATE_COUNT)
+ − 222 {
+ − 223 detectionState = DETECT_START;
+ − 224 stableCnt = 0;
+ − 225 startSector = lastSector;
+ − 226 }
+ − 227 break;
+ − 228 case DETECT_START: if(curSector != lastSector)
+ − 229 {
+ − 230 if(abs(curSector - startSector) > 1)
+ − 231 {
+ − 232 if(curSector > lastSector)
+ − 233 {
+ − 234 detectionState = DETECT_POS_MOVE;
+ − 235 }
+ − 236 else
+ − 237 {
+ − 238 detectionState = DETECT_NEG_MOVE;
+ − 239 }
+ − 240 stableCnt = 0;
+ − 241 startSector = lastSector;
+ − 242 }
+ − 243 }
+ − 244 break;
+ − 245 case DETECT_NEG_MOVE:
381
+ − 246 case DETECT_POS_MOVE: if(curSector == lastSector) /* Moved to a max? */
373
+ − 247 {
378
+ − 248 if(abs(startSector - curSector) > 2)
373
+ − 249 {
378
+ − 250 detectionState++;
+ − 251 stableCnt = 0;
373
+ − 252 }
381
+ − 253 if(stableCnt > 2)
373
+ − 254 {
381
+ − 255 detectionState = DETECT_NOTHING;
373
+ − 256 stableCnt = 0;
+ − 257 }
+ − 258 }
+ − 259 break;
381
+ − 260 case DETECT_MAXIMA:
+ − 261 case DETECT_MINIMA: if(curSector != lastSector) /* reset timeout detection */
+ − 262 {
+ − 263 detectionState++;
+ − 264 stableCnt = 0;
+ − 265 }
+ − 266 break;
373
+ − 267 case DETECT_RISEBACK:
381
+ − 268 case DETECT_FALLBACK:
+ − 269 if(curSector == lastSector) /* check if we are back at start position at end of movement */
373
+ − 270 {
381
+ − 271 if(abs(startSector - curSector) <= 1)
373
+ − 272 {
381
+ − 273 if(stableCnt > 2)
+ − 274 {
+ − 275 detectionState++;
+ − 276 stableCnt = 0;
+ − 277 }
373
+ − 278 }
+ − 279 }
381
+ − 280 break;
373
+ − 281 default:
+ − 282 detectionState = DETECT_NOTHING;
+ − 283 break;
381
+ − 284 }
+ − 285 if(detectionState != DETECT_START)
+ − 286 {
+ − 287 stableCnt++;
373
+ − 288 }
+ − 289 lastSector = curSector;
+ − 290 if(stableCnt > STABLE_STATE_TIMEOUT)
+ − 291 {
+ − 292 detectionState = DETECT_NOTHING;
+ − 293 stableCnt = 0;
+ − 294 }
+ − 295
359
+ − 296 return detectionState;
+ − 297 }