comparison Discovery/Src/motion.c @ 627:189f945ae4ba

Improve in / out of focus transitions: Added a function which detects if the yaw value changed significant while in focus state. This event is typically pointing to a arm movment => diver is no longer focussing at the OSTC. Improve custom view mapping: Added a direction indicator to the focus distance value (now +/- values instead of absolut values before). With this modification custom view sectors may have the double size and are threrfore easier to be controlled.
author Ideenmodellierer
date Fri, 12 Feb 2021 21:43:27 +0100
parents 028d8f3a9410
children c737cf5d9067
comparison
equal deleted inserted replaced
626:3e1a0e267f38 627:189f945ae4ba
21 21
22 #define SECTOR_MAX 24 /* maximum number of sectors */ 22 #define SECTOR_MAX 24 /* maximum number of sectors */
23 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ 23 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
24 #define SECTOR_MAX_CNT 5 /* max number of views used for sector control */ 24 #define SECTOR_MAX_CNT 5 /* max number of views used for sector control */
25 25
26 #define MOTION_DELTA_STABLE 0 26
27 #define MOTION_DELTA_JITTER 1 27 typedef enum
28 #define MOTION_DELTA_RAISE 2 28 {
29 #define MOTION_DELTA_FALL 3 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;
30 36
31 #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */ 37 #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */
32 #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */ 38 #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */
33 #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */ 39 #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */
40 #define MOTION_DELTA_FAST_LEVEL 6.0 /* Movement causing a fast change detected */
34 41
35 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */ 42 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
43
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 */
36 47
37 detectionState_t detectionState = DETECT_NOTHING; 48 detectionState_t detectionState = DETECT_NOTHING;
38 SSector sectorDetection; 49 SSector sectorDetection;
39 50
40 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */ 51 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */
97 } 108 }
98 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL) 109 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
99 { 110 {
100 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL; 111 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
101 } 112 }
113
114 if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL)
115 {
116 motionDeltaHistory[axis][nextIndex]++;
117 }
118
102 lastValue[axis] = curValue; 119 lastValue[axis] = curValue;
103 } 120 }
104 motionDeltaHistoryIdx = nextIndex; 121 motionDeltaHistoryIdx = nextIndex;
105 } 122 }
106 123
109 uint8_t loop; 126 uint8_t loop;
110 uint8_t index = motionDeltaHistoryIdx; 127 uint8_t index = motionDeltaHistoryIdx;
111 128
112 SDeltaHistory result = {0,0,0}; 129 SDeltaHistory result = {0,0,0};
113 130
114 stepback++; /* motionDeltaHistoryIdx is pointing to future entry => step back one to get the latest */ 131 loop = stepback + 1; /* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */
115 loop = stepback;
116 if(stepback < MOTION_DELTA_HISTORY_SIZE) 132 if(stepback < MOTION_DELTA_HISTORY_SIZE)
117 { 133 {
118 while(loop != 0) /* find requested entry */ 134 while(loop != 0) /* find requested entry */
119 { 135 {
120 loop--; 136 loop--;
132 } 148 }
133 149
134 uint8_t GetSectorForFocus(float focusOffset) 150 uint8_t GetSectorForFocus(float focusOffset)
135 { 151 {
136 uint8_t sector = 0; 152 uint8_t sector = 0;
137 float compare = 0.1; 153 float compare = -1.0 * MOTION_FOCUS_USE_SECTOR + sectorDetection.size ; /* start with first sector upper limit */
138 154
139 while(compare <= 0.5) 155 while(compare <= MOTION_FOCUS_USE_SECTOR)
140 { 156 {
141 if(focusOffset > compare) 157 if(focusOffset > compare)
142 { 158 {
143 sector++; 159 sector++;
144 } 160 }
145 else 161 else
146 { 162 {
147 break; 163 break;
148 } 164 }
149 compare += 0.1; 165 compare += sectorDetection.size;
150 } 166 }
151 if(sector > sectorDetection.count) 167 if(sector >= sectorDetection.count)
152 { 168 {
153 sector = sectorDetection.count; 169 sector = sectorDetection.count - 1;
154 } 170 }
155 return sector; 171 return sector;
156 } 172 }
157 173
158 void DefineSectorCount(uint8_t numOfSectors) 174 void DefineSectorCount(uint8_t numOfSectors)
175 else 191 else
176 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) 192 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
177 { 193 {
178 sectorDetection.count = numOfSectors; 194 sectorDetection.count = numOfSectors;
179 } 195 }
196 sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count;
180 } 197 }
181 198
182 199
183 uint8_t GetCVForSector(uint8_t selSector) 200 uint8_t GetCVForSector(uint8_t selSector)
184 { 201 {
193 } 210 }
194 211
195 void MapCVToSector() 212 void MapCVToSector()
196 { 213 {
197 uint8_t ViewIndex = 0; 214 uint8_t ViewIndex = 0;
198
199 memset(sectorMap, 0, sizeof(sectorMap)); 215 memset(sectorMap, 0, sizeof(sectorMap));
216
217 while(ViewIndex < (sectorDetection.count / 2)) /* define center sector */
218 {
219 ViewIndex++;
220 }
200 221
201 if(settingsGetPointer()->design == 3) /* Big font view ? */ 222 if(settingsGetPointer()->design == 3) /* Big font view ? */
202 { 223 {
203 t3_set_customview_to_primary(); 224 t3_set_customview_to_primary();
204 sectorMap[ViewIndex] = t3_change_customview(ACTION_END); 225 sectorMap[ViewIndex] = t3_change_customview(ACTION_END);
209 sectorMap[ViewIndex] = t7_change_customview(ACTION_END); 230 sectorMap[ViewIndex] = t7_change_customview(ACTION_END);
210 231
211 } 232 }
212 233
213 ViewIndex++; 234 ViewIndex++;
214 while(ViewIndex < sectorDetection.count) 235 while(sectorMap[ViewIndex] == 0)
215 { 236 {
216 if(settingsGetPointer()->design == 3) /* Big font view ? */ 237 if(settingsGetPointer()->design == 3) /* Big font view ? */
217 { 238 {
218 sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER); 239 sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER);
219 } 240 }
220 else 241 else
221 { 242 {
222 sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER); 243 sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
223 } 244 }
224 ViewIndex++; 245 ViewIndex++;
246 if(ViewIndex == sectorDetection.count)
247 {
248 ViewIndex = 0;
249 }
225 } 250 }
226 251
227 } 252 }
228 253
229 void InitMotionDetection(void) 254 void InitMotionDetection(void)
275 300
276 uint8_t PitchEvent = DETECT_NOTHING; 301 uint8_t PitchEvent = DETECT_NOTHING;
277 302
278 if(delayscroll == 0) 303 if(delayscroll == 0)
279 { 304 {
280 if(focusOffset > 0.3) 305 if(focusOffset > MOTION_FOCUS_SCROLL_IDLE)
281 { 306 {
282 PitchEvent = DETECT_POS_PITCH; 307 PitchEvent = DETECT_POS_PITCH;
308 delayscroll = 7;
309 }
310 if(focusOffset < (-1.0 * MOTION_FOCUS_SCROLL_IDLE))
311 {
312 PitchEvent = DETECT_NEG_PITCH;
283 delayscroll = 7; 313 delayscroll = 7;
284 } 314 }
285 } 315 }
286 else 316 else
287 { 317 {
343 { 373 {
344 lastStart = -1; 374 lastStart = -1;
345 } 375 }
346 duration = 0; 376 duration = 0;
347 break; 377 break;
348 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE)) 378 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE) || (test.pitch == MOTION_DELTA_RAISE_FAST))
349 { 379 {
350 detectionState++; 380 detectionState++;
351 } 381 }
352 break; 382 break;
353 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL)) 383 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL) || (test.pitch == MOTION_DELTA_FALL_FAST))
354 { 384 {
355 detectionState++; 385 detectionState++;
356 } 386 }
357 break; 387 break;
358 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL) 388 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL)
487 float anglePitch; 517 float anglePitch;
488 float angleRoll; 518 float angleRoll;
489 float distance = 0; 519 float distance = 0;
490 float _a, _b; 520 float _a, _b;
491 SCoord u,v,n; 521 SCoord u,v,n;
492 float r; 522 float r = 0.0;
523 float focusLimit = 0;
493 524
494 SCoord refVec; 525 SCoord refVec;
495 SCoord axis_1; 526 SCoord axis_1;
496 SCoord axis_2; 527 SCoord axis_2;
497 SCoord curVec; 528 SCoord curVec;
498 SCoord resultVec; 529 SCoord resultVec;
530
531 SDeltaHistory movementDelta;
499 532
500 SSettings* pSettings = settingsGetPointer(); 533 SSettings* pSettings = settingsGetPointer();
501 534
502 roll += 180; 535 roll += 180;
503 pitch += 180; 536 pitch += 180;
642 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z)); 675 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
643 } 676 }
644 } 677 }
645 } 678 }
646 679
647 if(distance < 0.5) /* handle focus counter to avoid fast in/out focus changes */ 680 movementDelta = GetDeltaHistory(0);
681
682 if(inFocus == 0) /* consider option to use smaller spot to detect focus state */
683 {
684 focusLimit = MOTION_FOCUS_LIMIT - (((pSettings->viewPortMode >> 5) & 0x03) / 10.0);
685 }
686 else
687 {
688 focusLimit = MOTION_FOCUS_LIMIT; /* use standard spot to detect diver interactions */
689 }
690
691 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 */
648 { 692 {
649 if(focusCnt < 10) 693 if(focusCnt < 10)
650 { 694 {
651 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */ 695 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
652 { 696 {
659 inFocus = 1; 703 inFocus = 1;
660 } 704 }
661 } 705 }
662 else 706 else
663 { 707 {
708 if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5))
709 {
710 focusCnt--;
711 }
664 if(focusCnt >= 5) /* Reset focus faster then setting focus */ 712 if(focusCnt >= 5) /* Reset focus faster then setting focus */
665 { 713 {
666 focusCnt--; 714 focusCnt--;
667 } 715 }
668 else 716 else
669 { 717 {
670 focusCnt = 0; 718 focusCnt = 0;
671 inFocus = 0; 719 inFocus = 0;
672 } 720 }
721 }
722 if ((r<1) && (retval == 0)) /* add direction information to distance */
723 {
724 distance *= -1.0;
673 } 725 }
674 return distance; 726 return distance;
675 } 727 }
676 uint8_t viewInFocus(void) 728 uint8_t viewInFocus(void)
677 { 729 {
713 { 765 {
714 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch); 766 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch);
715 break; 767 break;
716 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset); 768 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset);
717 break; 769 break;
718 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(focusOffset); 770 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(fabs(focusOffset));
719 break; 771 break;
720 default: 772 default:
721 pitchstate = DETECT_NOTHING; 773 pitchstate = DETECT_NOTHING;
722 break; 774 break;
723 } 775 }