Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 383:49a02dea8ae3 MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Combine variables in a structure to improve readability of code Add function to compensate offsets from calculated to real sector center position
author ideenmodellierer
date Thu, 17 Oct 2019 21:15:42 +0200
parents 695434a6dcf6
children 427ae9f8e28e
comparison
equal deleted inserted replaced
382:14fd5f35cb50 383:49a02dea8ae3
25 25
26 #define SECTOR_MAX 30 /* maximum number of sectors */ 26 #define SECTOR_MAX 30 /* maximum number of sectors */
27 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ 27 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
28 28
29 static detectionState_t detectionState = DETECT_NOTHING; 29 static detectionState_t detectionState = DETECT_NOTHING;
30 30 static SSector sectorDetection;
31 uint8_t curSector;
32 static uint8_t targetSector;
33 static float sectorSize;
34 static float sectorwindow;
35 static uint8_t sectorCount;
36
37 static float sector_upperborder;
38 static float sector_lowerborder;
39 31
40 32
41 uint8_t GetSectorForPitch(float pitch) 33 uint8_t GetSectorForPitch(float pitch)
42 { 34 {
43 static uint8_t lastsector = 0xFF; 35 static uint8_t lastsector = 0;
44 float newPitch; 36 float newPitch;
45 uint8_t sector = 0; 37 uint8_t sector = 0;
46 38
47 39 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */
48 newPitch = pitch + (sectorwindow / 2.0); /* do not use negativ values */
49 if (newPitch < 0.0) /* clip value */ 40 if (newPitch < 0.0) /* clip value */
50 { 41 {
51 newPitch = 0.0; 42 newPitch = 0.0;
52 } 43 }
53 if (newPitch > sectorwindow) /* clip value */ 44 if (newPitch > sectorDetection.window) /* clip value */
54 { 45 {
55 newPitch = sectorwindow; 46 newPitch = sectorDetection.window;
56 }
57
58 if(lastsector == 0xFF) /* First call of function => make sure a new sector is set */
59 {
60 sector_lowerborder = SECTOR_BORDER;
61 sector_upperborder = SECTOR_BORDER * -1.0;
62
63 } 47 }
64 48
65 /* switch to other sector? */ 49 /* switch to other sector? */
66 if((newPitch > sector_upperborder) || (newPitch <= sector_lowerborder)) 50 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
67 { 51 {
68 sector = (uint16_t) newPitch / sectorSize; 52 sector = (uint16_t) newPitch / sectorDetection.size;
69 sector_lowerborder = sector * sectorSize - SECTOR_HYSTERY; 53 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
70 sector_upperborder = (sector + 1) * sectorSize + SECTOR_HYSTERY; 54 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
71 lastsector = sector; 55 lastsector = sector;
72 } 56 }
73 57
74 return lastsector; 58 return lastsector;
75 } 59 }
76 60
77 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors) 61 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
78 { 62 {
79 if(numOfSectors == CUSTOMER_DEFINED_VIEWS) 63 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
80 { 64 {
81 sectorCount = t7_GetEnabled_customviews(); 65 sectorDetection.count = t7_GetEnabled_customviews();
82 if(sectorCount > 7) 66 if(sectorDetection.count > 7)
83 { 67 {
84 sectorCount = 7; /* more views are hard to manually control */ 68 sectorDetection.count = 7; /* more views are hard to manually control */
85 } 69 }
86 } 70 }
87 else 71 else
88 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) 72 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
89 { 73 {
90 sectorCount = numOfSectors; 74 sectorDetection.count = numOfSectors;
91 } 75 }
92 76
93 if(sectorCount == SECTOR_MAX) 77 if(sectorDetection.count == SECTOR_MAX)
94 { 78 {
95 sectorwindow = SECTOR_WINDOW_MAX; 79 sectorDetection.window = SECTOR_WINDOW_MAX;
96 } 80 }
97 else 81 else
98 { 82 {
99 sectorwindow = SECTOR_WINDOW; 83 sectorDetection.window = SECTOR_WINDOW;
100 } 84 }
101 85
102 sectorSize = sectorwindow / sectorCount; 86 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
103 87 sectorDetection.size = sectorDetection.window / sectorDetection.count;
88 sectorDetection.center = 0;
89
90 /* reset border values */
91 sectorDetection.lowerborder = SECTOR_BORDER;
92 sectorDetection.upperborder = SECTOR_BORDER * -1.0;
104 /* get the current sector */ 93 /* get the current sector */
105 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 94 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
106 targetSector = curSector; 95 sectorDetection.target = sectorDetection.current;
96 /* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
97 sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
98
107 } 99 }
108 100
109 void InitMotionDetection(void) 101 void InitMotionDetection(void)
110 { 102 {
111 targetSector = 0; 103 sectorDetection.target = 0;
112 curSector = 0; 104 sectorDetection.current = 0;
113 sectorSize = 0; 105 sectorDetection.size = 0;
114 sectorCount = 0; 106 sectorDetection.count = 0;
115 107
116 switch(settingsGetPointer()->MotionDetection) 108 switch(settingsGetPointer()->MotionDetection)
117 { 109 {
118 case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS); 110 case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS);
119 break; 111 break;
136 128
137 /* only change sector if reading is stable */ 129 /* only change sector if reading is stable */
138 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 130 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
139 if(lastTargetSector == newTargetSector) 131 if(lastTargetSector == newTargetSector)
140 { 132 {
141 targetSector = newTargetSector; 133 sectorDetection.target = newTargetSector;
142 } 134 }
143 lastTargetSector = newTargetSector; 135 lastTargetSector = newTargetSector;
144 if(targetSector != curSector) 136 if(sectorDetection.target != sectorDetection.current)
145 { 137 {
146 if(targetSector > curSector) 138 if(sectorDetection.target > sectorDetection.current)
147 { 139 {
148 curSector++; 140 sectorDetection.current++;
149 PitchEvent = DETECT_POS_PITCH; 141 PitchEvent = DETECT_POS_PITCH;
150 } 142 }
151 else 143 else
152 { 144 {
153 curSector--; 145 sectorDetection.current--;
154 PitchEvent = DETECT_NEG_PITCH; 146 PitchEvent = DETECT_NEG_PITCH;
155 } 147 }
156 } 148 }
157 return PitchEvent; 149 return PitchEvent;
158 } 150 }
200 detectionState_t detectPitch(float currentPitch) 192 detectionState_t detectPitch(float currentPitch)
201 { 193 {
202 static uint8_t lastSector = 0; 194 static uint8_t lastSector = 0;
203 static uint8_t startSector = 0; 195 static uint8_t startSector = 0;
204 static uint8_t stableCnt = 0; 196 static uint8_t stableCnt = 0;
197
198 uint8_t curSector;
205 199
206 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */ 200 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */
207 { 201 {
208 detectionState = DETECT_NOTHING; 202 detectionState = DETECT_NOTHING;
209 } 203 }