Mercurial > public > ostc4
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 } |