Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 625:028d8f3a9410

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Switch Sector / Scroll detection to vector implementation: The vector implementation is mor robust against influences of roll and yaw to the pitch angle. To improve the performance the old sector based implementation has been replaced. In addition after entering focus state the roll angle is ignored. To improve readability and due to removal of some no longer needed function codes, function names have been updated
author Ideenmodellierer
date Mon, 08 Feb 2021 21:35:46 +0100
parents 6826731ff2be
children 189f945ae4ba
comparison
equal deleted inserted replaced
624:930f1bbe0ac2 625:028d8f3a9410
17 #include "base.h" 17 #include "base.h"
18 18
19 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ 19 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
20 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */ 20 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
21 21
22 #define SECTOR_WINDOW 30.0 /* Pitch window which is used for custom view projection */
23 #define SECTOR_WINDOW_MAX 120.0 /* Pitch window which will be greater than the divers field of view */
24 #define SECTOR_HYSTERY 2 /* Additional offset to avoid fast changing displays */
25 #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */
26 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
27
28 #define SECTOR_MAX 24 /* maximum number of sectors */ 22 #define SECTOR_MAX 24 /* maximum number of sectors */
29 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ 23 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
30 #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 */
31 25
32 #define MOTION_DELTA_STABLE 0 26 #define MOTION_DELTA_STABLE 0
33 #define MOTION_DELTA_JITTER 1 27 #define MOTION_DELTA_JITTER 1
34 #define MOTION_DELTA_RAISE 2 28 #define MOTION_DELTA_RAISE 2
35 #define MOTION_DELTA_FALL 3 29 #define MOTION_DELTA_FALL 3
36 30
37 #define MOTION_DELTA_JITTER_LEVEL 3.0 /* lower values are considered as stable */ 31 #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */
38 #define MOTION_DELTA_RAISE_LEVEL 6.0 /* Movement causing a significant change detected */ 32 #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */
39 #define MOTION_DELTA_FALL_LEVEL -6.0 /* Movement causing a significant change detected */ 33 #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */
40 34
41 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */ 35 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
42 36
43 detectionState_t detectionState = DETECT_NOTHING; 37 detectionState_t detectionState = DETECT_NOTHING;
44 SSector sectorDetection; 38 SSector sectorDetection;
135 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index]; 129 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
136 } 130 }
137 return result; 131 return result;
138 } 132 }
139 133
140 uint8_t GetSectorForPitch(float pitch) 134 uint8_t GetSectorForFocus(float focusOffset)
141 { 135 {
142 static uint8_t lastsector = 0;
143 float newPitch;
144 uint8_t sector = 0; 136 uint8_t sector = 0;
145 137 float compare = 0.1;
146 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */ 138
147 if (newPitch < 0.0) /* clip value */ 139 while(compare <= 0.5)
148 { 140 {
149 newPitch = 0.0; 141 if(focusOffset > compare)
150 } 142 {
151 if (newPitch > sectorDetection.window) /* clip value */ 143 sector++;
152 { 144 }
153 newPitch = sectorDetection.window; 145 else
154 } 146 {
155 147 break;
156 /* switch to other sector? */ 148 }
157 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder)) 149 compare += 0.1;
158 { 150 }
159 sector = (uint16_t) newPitch / sectorDetection.size; 151 if(sector > sectorDetection.count)
160 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY; 152 {
161 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY; 153 sector = sectorDetection.count;
162 lastsector = sector; 154 }
163 } 155 return sector;
164 156 }
165 return lastsector; 157
166 } 158 void DefineSectorCount(uint8_t numOfSectors)
167
168 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
169 { 159 {
170 if(numOfSectors == CUSTOMER_DEFINED_VIEWS) 160 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
171 { 161 {
172 if(settingsGetPointer()->design == 3) /* Big font view ? */ 162 if(settingsGetPointer()->design == 3) /* Big font view ? */
173 { 163 {
185 else 175 else
186 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) 176 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
187 { 177 {
188 sectorDetection.count = numOfSectors; 178 sectorDetection.count = numOfSectors;
189 } 179 }
190
191 if(sectorDetection.count == SECTOR_MAX)
192 {
193 sectorDetection.window = SECTOR_WINDOW_MAX;
194 }
195 else
196 {
197 sectorDetection.window = SECTOR_WINDOW;
198 }
199
200 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
201 sectorDetection.size = sectorDetection.window / sectorDetection.count;
202 sectorDetection.center = 0;
203
204 /* reset border values */
205 sectorDetection.lowerborder = SECTOR_BORDER;
206 sectorDetection.upperborder = SECTOR_BORDER * -1.0;
207 /* get the current sector */
208 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
209 sectorDetection.target = sectorDetection.current;
210 /* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
211 sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
212
213 } 180 }
214 181
215 182
216 uint8_t GetCVForSector(uint8_t selSector) 183 uint8_t GetCVForSector(uint8_t selSector)
217 { 184 {
222 else 189 else
223 { 190 {
224 return 0; 191 return 0;
225 } 192 }
226 } 193 }
194
227 void MapCVToSector() 195 void MapCVToSector()
228 { 196 {
229 uint8_t centerView = 0; 197 uint8_t ViewIndex = 0;
230 198
231 memset(sectorMap, 0, sizeof(sectorMap)); 199 memset(sectorMap, 0, sizeof(sectorMap));
232 200
233 switch(sectorDetection.count)
234 {
235 case 1: centerView = 0; break;
236 case 2: centerView = 0; break;
237 case 3: centerView = 1; break;
238 case 4: centerView = 1; break;
239 case 5: centerView = 2; break;
240 default: centerView = sectorDetection.count / 2 - 1;
241 break;
242 }
243 if(settingsGetPointer()->design == 3) /* Big font view ? */ 201 if(settingsGetPointer()->design == 3) /* Big font view ? */
244 { 202 {
245 t3_set_customview_to_primary(); 203 t3_set_customview_to_primary();
246 sectorMap[centerView] = t3_change_customview(ACTION_END); 204 sectorMap[ViewIndex] = t3_change_customview(ACTION_END);
247 } 205 }
248 else 206 else
249 { 207 {
250 t7_set_customview_to_primary(); 208 t7_set_customview_to_primary();
251 sectorMap[centerView] = t7_change_customview(ACTION_END); 209 sectorMap[ViewIndex] = t7_change_customview(ACTION_END);
252 210
253 } 211 }
254 212
255 centerView++; 213 ViewIndex++;
256 while(sectorMap[centerView] == 0) 214 while(ViewIndex < sectorDetection.count)
257 { 215 {
258 if(settingsGetPointer()->design == 3) /* Big font view ? */ 216 if(settingsGetPointer()->design == 3) /* Big font view ? */
259 { 217 {
260 sectorMap[centerView] = t3_change_customview(ACTION_BUTTON_ENTER); 218 sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER);
261 } 219 }
262 else 220 else
263 { 221 {
264 sectorMap[centerView] = t7_change_customview(ACTION_BUTTON_ENTER); 222 sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
265 } 223 }
266 centerView++; 224 ViewIndex++;
267 if(centerView == sectorDetection.count)
268 {
269 centerView = 0;
270 }
271 } 225 }
272 226
273 } 227 }
274 228
275 void InitMotionDetection(void) 229 void InitMotionDetection(void)
276 { 230 {
277 float sensorPitch = settingsGetPointer()->viewPitch - 180.0; /* calib values are stored as 360° values. Sensor uses +/- 180° */
278 sectorDetection.target = 0; 231 sectorDetection.target = 0;
279 sectorDetection.current = 0; 232 sectorDetection.current = 0;
280 sectorDetection.size = 0; 233 sectorDetection.size = 0;
281 sectorDetection.count = 0; 234 sectorDetection.count = 0;
282 235
283 switch(settingsGetPointer()->MotionDetection) 236 switch(settingsGetPointer()->MotionDetection)
284 { 237 {
285 case MOTION_DETECT_SECTOR: DefinePitchSectors(sensorPitch,CUSTOMER_DEFINED_VIEWS); 238 case MOTION_DETECT_SECTOR: DefineSectorCount(CUSTOMER_DEFINED_VIEWS);
286 MapCVToSector(); 239 MapCVToSector();
287 break; 240 break;
288 case MOTION_DETECT_MOVE: DefinePitchSectors(sensorPitch,SECTOR_MAX); 241 case MOTION_DETECT_MOVE: DefineSectorCount(SECTOR_MAX);
289 break; 242 break;
290 case MOTION_DETECT_SCROLL: DefinePitchSectors(sensorPitch,SECTOR_SCROLL); 243 case MOTION_DETECT_SCROLL: DefineSectorCount(SECTOR_SCROLL);
291 break; 244 break;
292 default: 245 default:
293 break; 246 break;
294 } 247 }
295 248
296 resetMotionDeltaHistory(); 249 resetMotionDeltaHistory();
297 } 250 }
298 251
299 /* Map the current pitch value to a sector and create button event in case the sector is left */ 252 /* Map the current pitch value to a sector and create button event in case the sector is left */
300 detectionState_t detectSectorButtonEvent(float curPitch) 253 detectionState_t detectSectorButtonEvent(float focusOffset)
301 { 254 {
302 uint8_t newTargetSector; 255 uint8_t newTargetSector;
303 256
304 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 257 newTargetSector = GetSectorForFocus(focusOffset);
305 258
306 if(settingsGetPointer()->design == 3) /* Big font view ? */ 259 if(settingsGetPointer()->design == 3) /* Big font view ? */
307 { 260 {
308 t3_select_customview(GetCVForSector(newTargetSector)); 261 t3_select_customview(GetCVForSector(newTargetSector));
309 } 262 }
314 267
315 return DETECT_NOTHING; 268 return DETECT_NOTHING;
316 } 269 }
317 270
318 /* Check if pitch is not in center position and trigger a button action if needed */ 271 /* Check if pitch is not in center position and trigger a button action if needed */
319 detectionState_t detectScrollButtonEvent(float curPitch) 272 detectionState_t detectScrollButtonEvent(float focusOffset)
320 { 273 {
321 static uint8_t delayscroll = 0; /* slow down the number of scroll events */ 274 static uint8_t delayscroll = 0; /* slow down the number of scroll events */
322 275
323 uint8_t PitchEvent = DETECT_NOTHING; 276 uint8_t PitchEvent = DETECT_NOTHING;
324 uint8_t newSector;
325 277
326 if(delayscroll == 0) 278 if(delayscroll == 0)
327 { 279 {
328 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 280 if(focusOffset > 0.3)
329 /* for scroll detection the motion window is split into 6 sectors => set event accoring to the sector number*/ 281 {
330 switch(newSector) 282 PitchEvent = DETECT_POS_PITCH;
331 {
332 case 0: PitchEvent = DETECT_POS_PITCH;
333 break;
334 case 6: PitchEvent = DETECT_NEG_PITCH;
335 break;
336 default:
337 break;
338 }
339 if(PitchEvent != DETECT_NOTHING)
340 {
341 delayscroll = 7; 283 delayscroll = 7;
342 } 284 }
343 } 285 }
344 else 286 else
345 { 287 {
536 pSettings->viewRoll = roll+ 180; 478 pSettings->viewRoll = roll+ 180;
537 pSettings->viewYaw = yaw; 479 pSettings->viewYaw = yaw;
538 } 480 }
539 481
540 482
541 float checkViewport(float roll, float pitch, float yaw) 483 float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis)
542 { 484 {
543 static float freezeRoll = 0;
544 static float freezeYaw = 0;
545
546 uint8_t retval = 0; 485 uint8_t retval = 0;
547 float angleYaw; 486 float angleYaw;
548 float anglePitch; 487 float anglePitch;
549 float angleRoll; 488 float angleRoll;
550 float distance = 0; 489 float distance = 0;
555 SCoord refVec; 494 SCoord refVec;
556 SCoord axis_1; 495 SCoord axis_1;
557 SCoord axis_2; 496 SCoord axis_2;
558 SCoord curVec; 497 SCoord curVec;
559 SCoord resultVec; 498 SCoord resultVec;
560 SDeltaHistory test;
561 499
562 SSettings* pSettings = settingsGetPointer(); 500 SSettings* pSettings = settingsGetPointer();
563 501
564 roll += 180; 502 roll += 180;
565 pitch += 180; 503 pitch += 180;
566 504
567 /* calculate base vector taking calibration delta into account yaw (heading) */ 505 /* calculate base vector taking calibration delta into account yaw (heading) */
568 float compYaw; 506 float compYaw;
569 507
570 compYaw = 360.0 - yaw; /* turn to 0° */ 508 if(enableAxis & MOTION_ENABLE_YAW)
571 compYaw += pSettings->viewYaw; /* consider calib yaw value */ 509 {
572 compYaw += yaw; 510 compYaw = 360.0 - yaw; /* turn to 0° */
573 511 compYaw += pSettings->viewYaw; /* consider calib yaw value */
574 if (compYaw < 0.0) 512 compYaw += yaw;
575 { 513
576 compYaw = 360.0 + compYaw; 514 if (compYaw < 0.0)
577 } 515 {
578 516 compYaw = 360.0 + compYaw;
579 if (compYaw > 360.0) 517 }
580 { 518
581 compYaw = compYaw - 360.0; 519 if (compYaw > 360.0)
582 } 520 {
583 if (compYaw > 360.0) 521 compYaw = compYaw - 360.0;
584 { 522 }
585 compYaw = compYaw - 360.0; 523 if (compYaw > 360.0)
586 } 524 {
587 angleYaw = pSettings->viewYaw * M_PI / 180.0; 525 compYaw = compYaw - 360.0;
588 anglePitch = pSettings->viewPitch * M_PI / 180.0; 526 }
589 angleRoll = pSettings->viewRoll * M_PI / 180.0; 527 angleYaw = pSettings->viewYaw * M_PI / 180.0;
528 }
529 else
530 {
531 compYaw = 0.0;
532 angleYaw = 0.0;
533 }
534
535 if(enableAxis & MOTION_ENABLE_PITCH)
536 {
537 anglePitch = pSettings->viewPitch * M_PI / 180.0;
538 }
539 else
540 {
541 anglePitch = 0;
542 }
543 if(enableAxis & MOTION_ENABLE_ROLL)
544 {
545 angleRoll = pSettings->viewRoll * M_PI / 180.0;
546 }
547 else
548 {
549 angleRoll = 0;
550 }
590 551
591 refVec.x = 0; 552 refVec.x = 0;
592 refVec.y = 0; 553 refVec.y = 0;
593 refVec.z = 1.0; 554 refVec.z = 1.0;
594 555
595 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec); 556 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
596
597 anglePitch = pitch * M_PI / 180.0;
598 angleRoll = roll * M_PI / 180.0;
599 angleYaw = yaw * M_PI / 180.0;
600 557
601 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */ 558 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
602 axis_1.x = 0; 559 axis_1.x = 0;
603 if(refVec.y >=0) 560 if(refVec.y >=0)
604 { 561 {
617 { 574 {
618 retval = 2; 575 retval = 2;
619 } 576 }
620 else 577 else
621 { 578 {
622 angleYaw = compYaw * M_PI / 180.0; 579 if(enableAxis & MOTION_ENABLE_PITCH)
623 anglePitch = pitch * M_PI / 180.0; 580 {
624 angleRoll = roll * M_PI / 180.0; 581 anglePitch = pitch * M_PI / 180.0;
582 }
583 else
584 {
585 anglePitch = 0.0;
586 }
587 if(enableAxis & MOTION_ENABLE_ROLL)
588 {
589 angleRoll = roll * M_PI / 180.0;
590 }
591 else
592 {
593 angleRoll = 0.0;
594 }
595 if(enableAxis & MOTION_ENABLE_YAW)
596 {
597 angleYaw = compYaw * M_PI / 180.0;
598 }
599 else
600 {
601 angleYaw = 0.0;
602 }
603
625 curVec.x = 0; 604 curVec.x = 0;
626 curVec.y = 0; 605 curVec.y = 0;
627 curVec.z = 1.0; 606 curVec.z = 1.0;
628 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec); 607 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
629 608
643 } 622 }
644 } 623 }
645 distance = retval * 1.0; /* just for debugging */ 624 distance = retval * 1.0; /* just for debugging */
646 if(retval == 0) 625 if(retval == 0)
647 { 626 {
648
649 /* start calculating the matchpoint */ 627 /* start calculating the matchpoint */
650 curVec = CoordMulF(curVec,r); 628 curVec = CoordMulF(curVec,r);
651 resultVec = CoordSub(refVec,curVec); 629 resultVec = CoordSub(refVec,curVec);
652 630
653 /* calculate the distance between reference and actual vector */ 631 /* calculate the distance between reference and actual vector */
677 focusCnt++; 655 focusCnt++;
678 } 656 }
679 if((focusCnt == 10) && (inFocus == 0)) 657 if((focusCnt == 10) && (inFocus == 0))
680 { 658 {
681 inFocus = 1; 659 inFocus = 1;
682 freezeRoll = roll; 660 }
683 freezeYaw = yaw; 661 }
684 } 662 else
685 } 663 {
686 else 664 if(focusCnt >= 5) /* Reset focus faster then setting focus */
687 { 665 {
688 if(focusCnt >= 5) /* Reset focus faster then setting focus */
689 {
690 if(pSettings->MotionDetection != MOTION_DETECT_MOVE) /* only apply extended focus for methods using absolute pitch values */
691 {
692 test = GetDeltaHistory(0);
693 if((test.yaw == MOTION_DELTA_STABLE) && (test.roll == MOTION_DELTA_STABLE))
694 {
695 if((fabsf(freezeRoll - roll) < MOTION_DELTA_JITTER_LEVEL) && (fabsf(freezeYaw - yaw) < MOTION_DELTA_JITTER_LEVEL))
696 {
697 focusCnt++;
698 }
699 }
700 else
701 {
702 if(freezeRoll != 0.0)
703 {
704 focusCnt = 1;
705 }
706 }
707 }
708 focusCnt--; 666 focusCnt--;
709 } 667 }
710 else 668 else
711 { 669 {
712 focusCnt = 0; 670 focusCnt = 0;
713 inFocus = 0; 671 inFocus = 0;
714 freezeRoll = 0;
715 freezeYaw = 0;
716 } 672 }
717 } 673 }
718 return distance; 674 return distance;
719 } 675 }
720 uint8_t viewInFocus(void) 676 uint8_t viewInFocus(void)
733 689
734 void HandleMotionDetection(void) 690 void HandleMotionDetection(void)
735 { 691 {
736 detectionState_t pitchstate = DETECT_NOTHING; 692 detectionState_t pitchstate = DETECT_NOTHING;
737 static uint8_t wasInFocus = 0; 693 static uint8_t wasInFocus = 0;
694 float focusOffset = 0.0;
738 695
739 evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading); 696 evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
740 checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading); 697 if(viewInFocus())
741 698 {
699 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, (MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW));
700 }
701 else
702 {
703 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL);
704 }
742 if(viewInFocus()) 705 if(viewInFocus())
743 { 706 {
744 wasInFocus = 1; 707 wasInFocus = 1;
745 set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03); 708 set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03);
746 709
748 { 711 {
749 switch(settingsGetPointer()->MotionDetection) 712 switch(settingsGetPointer()->MotionDetection)
750 { 713 {
751 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch); 714 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch);
752 break; 715 break;
753 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(stateRealGetPointer()->lifeData.compass_pitch); 716 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset);
754 break; 717 break;
755 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(stateRealGetPointer()->lifeData.compass_pitch); 718 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(focusOffset);
756 break; 719 break;
757 default: 720 default:
758 pitchstate = DETECT_NOTHING; 721 pitchstate = DETECT_NOTHING;
759 break; 722 break;
760 } 723 }