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"
592
+ − 17 #include "base.h"
359
+ − 18
+ − 19 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
363
+ − 20 #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
+ − 21
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 22 #define SECTOR_MAX 24 /* maximum number of sectors */
378
+ − 23 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
592
+ − 24 #define SECTOR_MAX_CNT 5 /* max number of views used for sector control */
359
+ − 25
627
+ − 26
+ − 27 typedef enum
+ − 28 {
+ − 29 MOTION_DELTA_STABLE = 0,
+ − 30 MOTION_DELTA_JITTER,
+ − 31 MOTION_DELTA_RAISE,
+ − 32 MOTION_DELTA_RAISE_FAST,
+ − 33 MOTION_DELTA_FALL,
+ − 34 MOTION_DELTA_FALL_FAST
+ − 35 } MotionDeltaState_t;
551
+ − 36
625
+ − 37 #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */
+ − 38 #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */
+ − 39 #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */
627
+ − 40 #define MOTION_DELTA_FAST_LEVEL 6.0 /* Movement causing a fast change detected */
551
+ − 41
+ − 42 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
+ − 43
627
+ − 44 #define MOTION_FOCUS_LIMIT 0.5 /* +/- value which defines the border of the focus area */
+ − 45 #define MOTION_FOCUS_USE_SECTOR 0.4 /* +/- value for the focus area used to map secors to views */
+ − 46 #define MOTION_FOCUS_SCROLL_IDLE 0.3 /* +/- value for starting generation of scroll events */
+ − 47
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 48 detectionState_t detectionState = DETECT_NOTHING;
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
diff
changeset
+ − 49 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
+ − 50
551
+ − 51 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */
+ − 52 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */
+ − 53
+ − 54 static uint8_t focusCnt = 0;
+ − 55 static uint8_t inFocus = 0;
592
+ − 56 static uint8_t sectorMap[SECTOR_MAX_CNT];
551
+ − 57
611
+ − 58 static uint8_t suspendMotionDetectionSec = 0;
+ − 59
551
+ − 60 void resetMotionDeltaHistory()
+ − 61 {
+ − 62 motionDeltaHistoryIdx = 0;
+ − 63 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory));
+ − 64 }
+ − 65
+ − 66 void evaluateMotionDelta(float roll, float pitch, float yaw)
+ − 67 {
+ − 68 static float lastValue[3] = {0.0,0.0,0.0};
+ − 69 uint8_t nextIndex = motionDeltaHistoryIdx + 1;
+ − 70 uint8_t axis;
+ − 71 float curValue;
+ − 72
+ − 73 if(nextIndex == MOTION_DELTA_HISTORY_SIZE)
+ − 74 {
+ − 75 nextIndex = 0;
+ − 76 }
+ − 77 for(axis=0; axis < 3; axis++)
+ − 78 {
+ − 79 switch(axis)
+ − 80 {
+ − 81 case MOTION_HISTORY_ROLL: curValue = roll;
+ − 82 break;
+ − 83 case MOTION_HISTORY_PITCH: curValue = pitch;
+ − 84 break;
+ − 85 default:
+ − 86 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */
+ − 87 {
+ − 88 lastValue[MOTION_HISTORY_YAW] -= 360;
+ − 89 }
+ − 90 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */
+ − 91 {
+ − 92 lastValue[MOTION_HISTORY_YAW] += 360;
+ − 93 }
+ − 94 curValue = yaw;
+ − 95 break;
+ − 96 }
+ − 97 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL)
+ − 98 {
+ − 99 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE;
+ − 100 }
+ − 101 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL)
+ − 102 {
+ − 103 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER;
+ − 104 }
+ − 105 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL)
+ − 106 {
+ − 107 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE;
+ − 108 }
+ − 109 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
+ − 110 {
+ − 111 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
+ − 112 }
627
+ − 113
+ − 114 if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL)
+ − 115 {
+ − 116 motionDeltaHistory[axis][nextIndex]++;
+ − 117 }
+ − 118
551
+ − 119 lastValue[axis] = curValue;
+ − 120 }
+ − 121 motionDeltaHistoryIdx = nextIndex;
+ − 122 }
+ − 123
+ − 124 SDeltaHistory GetDeltaHistory(uint8_t stepback)
+ − 125 {
578
+ − 126 uint8_t loop;
551
+ − 127 uint8_t index = motionDeltaHistoryIdx;
+ − 128
+ − 129 SDeltaHistory result = {0,0,0};
+ − 130
627
+ − 131 loop = stepback + 1; /* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */
551
+ − 132 if(stepback < MOTION_DELTA_HISTORY_SIZE)
+ − 133 {
+ − 134 while(loop != 0) /* find requested entry */
+ − 135 {
+ − 136 loop--;
+ − 137 index--;
+ − 138 if(index == 0)
+ − 139 {
+ − 140 index = MOTION_DELTA_HISTORY_SIZE - 1;
+ − 141 }
+ − 142 }
+ − 143 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index];
+ − 144 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index];
+ − 145 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
+ − 146 }
+ − 147 return result;
+ − 148 }
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
625
+ − 150 uint8_t GetSectorForFocus(float focusOffset)
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
+ − 151 {
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 uint8_t sector = 0;
627
+ − 153 float compare = -1.0 * MOTION_FOCUS_USE_SECTOR + sectorDetection.size ; /* start with first sector upper limit */
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
627
+ − 155 while(compare <= MOTION_FOCUS_USE_SECTOR)
381
+ − 156 {
625
+ − 157 if(focusOffset > compare)
+ − 158 {
+ − 159 sector++;
+ − 160 }
+ − 161 else
+ − 162 {
+ − 163 break;
+ − 164 }
627
+ − 165 compare += sectorDetection.size;
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
+ − 166 }
627
+ − 167 if(sector >= sectorDetection.count)
381
+ − 168 {
627
+ − 169 sector = sectorDetection.count - 1;
381
+ − 170 }
625
+ − 171 return sector;
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
+ − 172 }
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
+ − 173
625
+ − 174 void DefineSectorCount(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
+ − 175 {
371
+ − 176 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
+ − 177 {
384
+ − 178 if(settingsGetPointer()->design == 3) /* Big font view ? */
+ − 179 {
+ − 180 sectorDetection.count = t3_GetEnabled_customviews();
+ − 181 }
+ − 182 else
+ − 183 {
+ − 184 sectorDetection.count = t7_GetEnabled_customviews();
+ − 185 }
592
+ − 186 if(sectorDetection.count > SECTOR_MAX_CNT)
371
+ − 187 {
592
+ − 188 sectorDetection.count = SECTOR_MAX_CNT; /* more views are hard to manually control */
371
+ − 189 }
+ − 190 }
+ − 191 else
373
+ − 192 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
371
+ − 193 {
383
+ − 194 sectorDetection.count = numOfSectors;
371
+ − 195 }
627
+ − 196 sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count;
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
+ − 197 }
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
592
+ − 199
+ − 200 uint8_t GetCVForSector(uint8_t selSector)
+ − 201 {
+ − 202 if(selSector < sectorDetection.count)
+ − 203 {
+ − 204 return sectorMap[selSector];
+ − 205 }
+ − 206 else
+ − 207 {
+ − 208 return 0;
+ − 209 }
+ − 210 }
625
+ − 211
592
+ − 212 void MapCVToSector()
+ − 213 {
625
+ − 214 uint8_t ViewIndex = 0;
627
+ − 215 memset(sectorMap, 0, sizeof(sectorMap));
592
+ − 216
627
+ − 217 while(ViewIndex < (sectorDetection.count / 2)) /* define center sector */
+ − 218 {
+ − 219 ViewIndex++;
+ − 220 }
592
+ − 221
+ − 222 if(settingsGetPointer()->design == 3) /* Big font view ? */
+ − 223 {
595
+ − 224 t3_set_customview_to_primary();
625
+ − 225 sectorMap[ViewIndex] = t3_change_customview(ACTION_END);
592
+ − 226 }
+ − 227 else
+ − 228 {
595
+ − 229 t7_set_customview_to_primary();
625
+ − 230 sectorMap[ViewIndex] = t7_change_customview(ACTION_END);
595
+ − 231
592
+ − 232 }
+ − 233
625
+ − 234 ViewIndex++;
627
+ − 235 while(sectorMap[ViewIndex] == 0)
592
+ − 236 {
+ − 237 if(settingsGetPointer()->design == 3) /* Big font view ? */
+ − 238 {
625
+ − 239 sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER);
592
+ − 240 }
+ − 241 else
+ − 242 {
625
+ − 243 sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
592
+ − 244 }
625
+ − 245 ViewIndex++;
627
+ − 246 if(ViewIndex == sectorDetection.count)
+ − 247 {
+ − 248 ViewIndex = 0;
+ − 249 }
592
+ − 250 }
+ − 251
+ − 252 }
+ − 253
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
+ − 254 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
+ − 255 {
383
+ − 256 sectorDetection.target = 0;
+ − 257 sectorDetection.current = 0;
+ − 258 sectorDetection.size = 0;
+ − 259 sectorDetection.count = 0;
371
+ − 260
+ − 261 switch(settingsGetPointer()->MotionDetection)
+ − 262 {
625
+ − 263 case MOTION_DETECT_SECTOR: DefineSectorCount(CUSTOMER_DEFINED_VIEWS);
592
+ − 264 MapCVToSector();
371
+ − 265 break;
625
+ − 266 case MOTION_DETECT_MOVE: DefineSectorCount(SECTOR_MAX);
371
+ − 267 break;
625
+ − 268 case MOTION_DETECT_SCROLL: DefineSectorCount(SECTOR_SCROLL);
373
+ − 269 break;
371
+ − 270 default:
+ − 271 break;
+ − 272 }
+ − 273
551
+ − 274 resetMotionDeltaHistory();
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
+ − 275 }
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
+ − 276
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
+ − 277 /* Map the current pitch value to a sector and create button event in case the sector is left */
625
+ − 278 detectionState_t detectSectorButtonEvent(float focusOffset)
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
+ − 279 {
642
+ − 280 static uint8_t lastTargetSector = 0xFF;
648
+ − 281 static float lastfocusOffset = 1000.0;
642
+ − 282
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
+ − 283 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
+ − 284
625
+ − 285 newTargetSector = GetSectorForFocus(focusOffset);
592
+ − 286
642
+ − 287 /* take a small hysteresis into account to avoid fast display changes (flicker) */
+ − 288 if((newTargetSector != lastTargetSector) && (fabsf(focusOffset - lastfocusOffset) > (sectorDetection.size / 3)))
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
+ − 289 {
642
+ − 290 lastfocusOffset = focusOffset;
648
+ − 291 lastTargetSector = newTargetSector;
642
+ − 292 if(settingsGetPointer()->design == 3) /* Big font view ? */
+ − 293 {
+ − 294 t3_select_customview(GetCVForSector(newTargetSector));
+ − 295 }
+ − 296 else
+ − 297 {
+ − 298 t7_select_customview(GetCVForSector(newTargetSector));
+ − 299 }
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
+ − 300 }
592
+ − 301 return DETECT_NOTHING;
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
+ − 302 }
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
+ − 303
373
+ − 304 /* Check if pitch is not in center position and trigger a button action if needed */
625
+ − 305 detectionState_t detectScrollButtonEvent(float focusOffset)
373
+ − 306 {
378
+ − 307 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
+ − 308
373
+ − 309 uint8_t PitchEvent = DETECT_NOTHING;
+ − 310
+ − 311 if(delayscroll == 0)
+ − 312 {
627
+ − 313 if(focusOffset > MOTION_FOCUS_SCROLL_IDLE)
373
+ − 314 {
625
+ − 315 PitchEvent = DETECT_POS_PITCH;
574
+ − 316 delayscroll = 7;
373
+ − 317 }
627
+ − 318 if(focusOffset < (-1.0 * MOTION_FOCUS_SCROLL_IDLE))
+ − 319 {
+ − 320 PitchEvent = DETECT_NEG_PITCH;
+ − 321 delayscroll = 7;
+ − 322 }
373
+ − 323 }
+ − 324 else
+ − 325 {
+ − 326 delayscroll--;
+ − 327 }
+ − 328 return PitchEvent;
+ − 329 }
+ − 330
+ − 331
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
+ − 332 /* Detect if user is generating an pitch including return to starting position */
359
+ − 333 /* 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
+ − 334 detectionState_t detectPitch(float currentPitch)
359
+ − 335 {
578
+ − 336 static int8_t lastStart = 0;
551
+ − 337 uint8_t exit = 0;
578
+ − 338 int8_t step = 0;
574
+ − 339 uint8_t duration = 0;
551
+ − 340 SDeltaHistory test;
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
+ − 341
578
+ − 342 if(lastStart < 0)
+ − 343 {
+ − 344 detectionState = DETECT_NOTHING;
+ − 345 lastStart = 0;
+ − 346 }
+ − 347 else
+ − 348 {
+ − 349 detectionState = DETECT_START;
+ − 350 }
+ − 351 step = lastStart;
+ − 352 do
551
+ − 353 {
+ − 354 test = GetDeltaHistory(step);
574
+ − 355 duration++;
551
+ − 356 switch (detectionState)
+ − 357 {
+ − 358 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE)
+ − 359 {
+ − 360 exit = 1;
578
+ − 361 lastStart = -2;
551
+ − 362 }
+ − 363 else
+ − 364 {
+ − 365 detectionState = DETECT_START;
578
+ − 366 lastStart = -1;
551
+ − 367 }
+ − 368 break;
+ − 369 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE)
+ − 370 {
+ − 371 detectionState = DETECT_POS_MOVE;
578
+ − 372 lastStart = step;
551
+ − 373 }
578
+ − 374 else
551
+ − 375 if(test.pitch == MOTION_DELTA_FALL)
+ − 376 {
+ − 377 detectionState = DETECT_NEG_MOVE;
578
+ − 378 lastStart = step;
+ − 379 }
+ − 380 else
+ − 381 {
+ − 382 lastStart = -1;
551
+ − 383 }
574
+ − 384 duration = 0;
551
+ − 385 break;
627
+ − 386 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE) || (test.pitch == MOTION_DELTA_RAISE_FAST))
578
+ − 387 {
+ − 388 detectionState++;
+ − 389 }
+ − 390 break;
627
+ − 391 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL) || (test.pitch == MOTION_DELTA_FALL_FAST))
551
+ − 392 {
+ − 393 detectionState++;
+ − 394 }
+ − 395 break;
+ − 396 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL)
+ − 397 {
+ − 398 detectionState = DETECT_FALLBACK;
+ − 399 }
+ − 400 break;
+ − 401 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE)
+ − 402 {
+ − 403 detectionState = DETECT_RISEBACK;
+ − 404 }
+ − 405 break;
+ − 406 case DETECT_RISEBACK:
+ − 407 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE)
+ − 408 {
578
+ − 409 if(duration > 4) /* avoid detection triggered by short moves */
574
+ − 410 {
+ − 411 detectionState++;
+ − 412 }
578
+ − 413 exit = 1;
+ − 414 lastStart = -2;
551
+ − 415 }
+ − 416 break;
+ − 417 default:
+ − 418 detectionState = DETECT_NOTHING;
+ − 419 exit = 1;
+ − 420 break;
+ − 421 }
578
+ − 422 step--;
+ − 423 } while((step >= 0) && (!exit));
+ − 424
+ − 425 if((lastStart < MOTION_DELTA_HISTORY_SIZE))
+ − 426 {
+ − 427 lastStart++; /* prepare value for next iteration (history index will be increased) */
+ − 428 }
+ − 429 else
+ − 430 {
+ − 431 lastStart = -1;
551
+ − 432 }
+ − 433 if((detectionState != DETECT_POS_PITCH) && (detectionState != DETECT_NEG_PITCH)) /* nothing found */
359
+ − 434 {
+ − 435 detectionState = DETECT_NOTHING;
+ − 436 }
551
+ − 437 else /* dont detect the same event twice */
+ − 438 {
+ − 439 resetMotionDeltaHistory();
+ − 440 }
+ − 441 return detectionState;
+ − 442 }
363
+ − 443
551
+ − 444 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord)
+ − 445 {
+ − 446 pCoord->x = ((cosf(yaw) * cosf(pitch)) * pCoord->x + (cosf(yaw)*sinf(pitch)*sinf(roll) - (sinf(yaw)* cosf(roll))) * pCoord->y + (cosf(yaw)*sinf(pitch)*cosf(roll) + sinf(yaw)*sinf(roll)) * pCoord->z);
+ − 447 pCoord->y = ((sinf(yaw) * cosf(pitch)) * pCoord->x + (sinf(yaw)*sinf(pitch)*sinf(roll) + cosf(yaw) * cosf(roll)) * pCoord->y + ( sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll))* pCoord->z);
+ − 448 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z);
+ − 449 }
+ − 450
+ − 451 SCoord CoordAdd(SCoord cA, SCoord cB)
+ − 452 {
+ − 453 SCoord result;
+ − 454
+ − 455 result.x = cA.x + cB.x;
+ − 456 result.y = cA.y + cB.y;
+ − 457 result.z = cA.z + cB.z;
+ − 458 return result;
+ − 459 }
+ − 460
+ − 461 SCoord CoordSub(SCoord cA, SCoord cB)
+ − 462 {
+ − 463 SCoord result;
373
+ − 464
551
+ − 465 result.x = cA.x - cB.x;
+ − 466 result.y = cA.y - cB.y;
+ − 467 result.z = cA.z - cB.z;
+ − 468 return result;
+ − 469 }
+ − 470
+ − 471 SCoord CoordCross(SCoord cA, SCoord cB)
+ − 472 {
+ − 473 SCoord result;
+ − 474
+ − 475 result.x = (cA.y * cB.z) - (cA.z * cB.y);
+ − 476 result.y = (cA.z * cB.x) - (cA.x * cB.z);
+ − 477 result.z = (cA.x * cB.y) - (cA.y * cB.x);
+ − 478
+ − 479 return result;
+ − 480
+ − 481 }
+ − 482
+ − 483 SCoord CoordMulF(SCoord op, float factor)
+ − 484 {
+ − 485 SCoord result;
+ − 486 result.x = (op.x * factor);
+ − 487 result.y = (op.y * factor);
+ − 488 result.z = (op.z * factor);
+ − 489
+ − 490 return result;
+ − 491 }
373
+ − 492
551
+ − 493 SCoord CoordDivF(SCoord op, float factor)
+ − 494 {
+ − 495 SCoord result;
+ − 496 result.x = (op.x / factor);
+ − 497 result.y = (op.y / factor);
+ − 498 result.z = (op.z / factor);
+ − 499
+ − 500 return result;
+ − 501 }
+ − 502
+ − 503 float CoordDot(SCoord cA, SCoord cB)
+ − 504 {
+ − 505 float result;
+ − 506
+ − 507 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z;
+ − 508 return result;
+ − 509 }
+ − 510
+ − 511 void calibrateViewport(float roll, float pitch, float yaw)
+ − 512 {
+ − 513 SSettings* pSettings = settingsGetPointer();
+ − 514
621
+ − 515 pSettings->viewPitch = pitch + 180;
+ − 516 pSettings->viewRoll = roll+ 180;
551
+ − 517 pSettings->viewYaw = yaw;
+ − 518 }
+ − 519
+ − 520
625
+ − 521 float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis)
551
+ − 522 {
+ − 523 uint8_t retval = 0;
+ − 524 float angleYaw;
+ − 525 float anglePitch;
+ − 526 float angleRoll;
+ − 527 float distance = 0;
+ − 528 float _a, _b;
+ − 529 SCoord u,v,n;
627
+ − 530 float r = 0.0;
+ − 531 float focusLimit = 0;
551
+ − 532
+ − 533 SCoord refVec;
+ − 534 SCoord axis_1;
+ − 535 SCoord axis_2;
+ − 536 SCoord curVec;
+ − 537 SCoord resultVec;
+ − 538
627
+ − 539 SDeltaHistory movementDelta;
+ − 540
551
+ − 541 SSettings* pSettings = settingsGetPointer();
+ − 542
621
+ − 543 roll += 180;
+ − 544 pitch += 180;
+ − 545
551
+ − 546 /* calculate base vector taking calibration delta into account yaw (heading) */
621
+ − 547 float compYaw;
574
+ − 548
625
+ − 549 if(enableAxis & MOTION_ENABLE_YAW)
+ − 550 {
+ − 551 compYaw = 360.0 - yaw; /* turn to 0° */
+ − 552 compYaw += pSettings->viewYaw; /* consider calib yaw value */
+ − 553 compYaw += yaw;
+ − 554
+ − 555 if (compYaw < 0.0)
+ − 556 {
+ − 557 compYaw = 360.0 + compYaw;
+ − 558 }
574
+ − 559
625
+ − 560 if (compYaw > 360.0)
+ − 561 {
+ − 562 compYaw = compYaw - 360.0;
+ − 563 }
+ − 564 if (compYaw > 360.0)
+ − 565 {
+ − 566 compYaw = compYaw - 360.0;
+ − 567 }
+ − 568 angleYaw = pSettings->viewYaw * M_PI / 180.0;
+ − 569 }
+ − 570 else
551
+ − 571 {
625
+ − 572 compYaw = 0.0;
+ − 573 angleYaw = 0.0;
381
+ − 574 }
551
+ − 575
625
+ − 576 if(enableAxis & MOTION_ENABLE_PITCH)
381
+ − 577 {
625
+ − 578 anglePitch = pSettings->viewPitch * M_PI / 180.0;
373
+ − 579 }
625
+ − 580 else
574
+ − 581 {
625
+ − 582 anglePitch = 0;
574
+ − 583 }
625
+ − 584 if(enableAxis & MOTION_ENABLE_ROLL)
+ − 585 {
+ − 586 angleRoll = pSettings->viewRoll * M_PI / 180.0;
+ − 587 }
+ − 588 else
+ − 589 {
+ − 590 angleRoll = 0;
+ − 591 }
551
+ − 592
+ − 593 refVec.x = 0;
+ − 594 refVec.y = 0;
+ − 595 refVec.z = 1.0;
+ − 596
+ − 597 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
+ − 598
+ − 599 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
+ − 600 axis_1.x = 0;
+ − 601 if(refVec.y >=0)
+ − 602 {
+ − 603 axis_2.y = 1; /* => Spawn y == refVec y */
+ − 604 }
+ − 605 else axis_1.y = -1;
+ − 606 axis_1.z = -1.0 * refVec.y / refVec.z;
+ − 607 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */
+ − 608
+ − 609 /* check if detection plane is correct */
+ − 610 u = CoordSub(axis_1,refVec);
+ − 611 v = CoordSub(axis_2,refVec);
+ − 612 n = CoordCross(u,v);
+ − 613
+ − 614 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001))
+ − 615 {
+ − 616 retval = 2;
+ − 617 }
+ − 618 else
+ − 619 {
625
+ − 620 if(enableAxis & MOTION_ENABLE_PITCH)
+ − 621 {
+ − 622 anglePitch = pitch * M_PI / 180.0;
+ − 623 }
+ − 624 else
+ − 625 {
+ − 626 anglePitch = 0.0;
+ − 627 }
+ − 628 if(enableAxis & MOTION_ENABLE_ROLL)
+ − 629 {
+ − 630 angleRoll = roll * M_PI / 180.0;
+ − 631 }
+ − 632 else
+ − 633 {
+ − 634 angleRoll = 0.0;
+ − 635 }
+ − 636 if(enableAxis & MOTION_ENABLE_YAW)
+ − 637 {
+ − 638 angleYaw = compYaw * M_PI / 180.0;
+ − 639 }
+ − 640 else
+ − 641 {
+ − 642 angleYaw = 0.0;
+ − 643 }
+ − 644
551
+ − 645 curVec.x = 0;
+ − 646 curVec.y = 0;
+ − 647 curVec.z = 1.0;
+ − 648 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
+ − 649
+ − 650 _a = CoordDot(curVec,n);
+ − 651 _b = CoordDot(refVec,n);
+ − 652
+ − 653 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */
+ − 654 {
+ − 655 retval = 3;
+ − 656 }
+ − 657 else
+ − 658 {
+ − 659 r=_a/_b;
+ − 660 if(r<0.00||r>1.40) /* are we looking into wrong direction? */
+ − 661 {
+ − 662 retval = 4;
+ − 663 }
+ − 664 }
+ − 665 distance = retval * 1.0; /* just for debugging */
+ − 666 if(retval == 0)
+ − 667 {
+ − 668 /* start calculating the matchpoint */
+ − 669 curVec = CoordMulF(curVec,r);
+ − 670 resultVec = CoordSub(refVec,curVec);
+ − 671
+ − 672 /* calculate the distance between reference and actual vector */
+ − 673 resultVec.x = resultVec.x * resultVec.x;
+ − 674 resultVec.y = resultVec.y * resultVec.y;
+ − 675 resultVec.z = resultVec.z * resultVec.z;
+ − 676
+ − 677 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0))
+ − 678 {
+ − 679 distance = 0.0;
+ − 680 }
+ − 681 else
+ − 682 {
+ − 683 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
+ − 684 }
+ − 685 }
+ − 686 }
+ − 687
627
+ − 688 movementDelta = GetDeltaHistory(0);
+ − 689
+ − 690 if(inFocus == 0) /* consider option to use smaller spot to detect focus state */
+ − 691 {
+ − 692 focusLimit = MOTION_FOCUS_LIMIT - (((pSettings->viewPortMode >> 5) & 0x03) / 10.0);
+ − 693 }
+ − 694 else
+ − 695 {
+ − 696 focusLimit = MOTION_FOCUS_LIMIT; /* use standard spot to detect diver interactions */
+ − 697 }
+ − 698
+ − 699 if((distance <= focusLimit) && (movementDelta.yaw != MOTION_DELTA_RAISE_FAST) && (movementDelta.yaw != MOTION_DELTA_FALL_FAST)) /* handle focus counter to avoid fast in/out focus changes */
551
+ − 700 {
+ − 701 if(focusCnt < 10)
+ − 702 {
+ − 703 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
+ − 704 {
+ − 705 resetMotionDeltaHistory();
+ − 706 }
+ − 707 focusCnt++;
+ − 708 }
574
+ − 709 if((focusCnt == 10) && (inFocus == 0))
551
+ − 710 {
+ − 711 inFocus = 1;
+ − 712 }
+ − 713 }
+ − 714 else
+ − 715 {
627
+ − 716 if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5))
+ − 717 {
+ − 718 focusCnt--;
+ − 719 }
625
+ − 720 if(focusCnt >= 5) /* Reset focus faster then setting focus */
551
+ − 721 {
+ − 722 focusCnt--;
+ − 723 }
+ − 724 else
+ − 725 {
574
+ − 726 focusCnt = 0;
551
+ − 727 inFocus = 0;
+ − 728 }
+ − 729 }
627
+ − 730 if ((r<1) && (retval == 0)) /* add direction information to distance */
+ − 731 {
+ − 732 distance *= -1.0;
+ − 733 }
551
+ − 734 return distance;
359
+ − 735 }
551
+ − 736 uint8_t viewInFocus(void)
+ − 737 {
+ − 738 return inFocus;
+ − 739 }
+ − 740 void resetFocusState(void)
+ − 741 {
+ − 742 inFocus = 0;
+ − 743 }
611
+ − 744
642
+ − 745 uint8_t viewDetectionSuspended(void)
+ − 746 {
+ − 747 uint8_t retVal = 0;
+ − 748
+ − 749 if(suspendMotionDetectionSec)
+ − 750 {
+ − 751 retVal = 1;
+ − 752 }
+ − 753 return retVal;
+ − 754 }
+ − 755
611
+ − 756 void suspendMotionDetection(uint8_t seconds)
+ − 757 {
+ − 758 suspendMotionDetectionSec = seconds * 10; /* detection function is called every 100ms */
+ − 759 }
+ − 760
+ − 761 void HandleMotionDetection(void)
+ − 762 {
+ − 763 detectionState_t pitchstate = DETECT_NOTHING;
618
+ − 764 static uint8_t wasInFocus = 0;
625
+ − 765 float focusOffset = 0.0;
611
+ − 766
+ − 767 evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
625
+ − 768 if(viewInFocus())
+ − 769 {
+ − 770 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, (MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW));
+ − 771 }
+ − 772 else
+ − 773 {
+ − 774 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL);
+ − 775 }
611
+ − 776 if(viewInFocus())
+ − 777 {
+ − 778 wasInFocus = 1;
+ − 779 set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03);
+ − 780
+ − 781 if(suspendMotionDetectionSec == 0) /* suspend detection while diver is manually operating the OSTC */
+ − 782 {
+ − 783 switch(settingsGetPointer()->MotionDetection)
+ − 784 {
+ − 785 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch);
+ − 786 break;
625
+ − 787 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset);
611
+ − 788 break;
627
+ − 789 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(fabs(focusOffset));
611
+ − 790 break;
+ − 791 default:
+ − 792 pitchstate = DETECT_NOTHING;
+ − 793 break;
+ − 794 }
+ − 795 }
+ − 796
+ − 797 if(DETECT_NEG_PITCH == pitchstate)
+ − 798 {
+ − 799 StoreButtonAction((uint8_t)ACTION_PITCH_NEG);
+ − 800 }
+ − 801 if(DETECT_POS_PITCH == pitchstate)
+ − 802 {
+ − 803 StoreButtonAction((uint8_t)ACTION_PITCH_POS);
+ − 804 }
+ − 805 }
+ − 806 else
+ − 807 {
+ − 808 if(wasInFocus)
+ − 809 {
+ − 810 wasInFocus = 0;
+ − 811 if(suspendMotionDetectionSec == 0)
+ − 812 {
+ − 813 if(settingsGetPointer()->design == 7)
+ − 814 {
+ − 815 t7_set_customview_to_primary();
+ − 816 }
+ − 817 else
+ − 818 {
+ − 819 t3_set_customview_to_primary();
+ − 820 }
+ − 821 }
+ − 822 }
+ − 823 set_Backlight_Boost(0);
+ − 824 }
+ − 825 if(suspendMotionDetectionSec != 0)
+ − 826 {
+ − 827 suspendMotionDetectionSec--;
+ − 828 }
+ − 829 }
+ − 830
+ − 831