Mercurial > public > ostc4

annotate Discovery/Src/motion.c @ 370:77cdfbdaca8c MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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
author ideenmodellierer
date Tue, 13 Aug 2019 21:13:54 +0200
parents bdf978d2a5d4
children fca370f847f8
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
1 /*
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
2 * motion.c
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
3 *
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
4 * Created on: 20.05.2019
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
5 * Author: Thorsten Sonntag
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
6 */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
7
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
8 #include <stdint.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
9 #include <string.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
10 #include <math.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
11 #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
parents: 363
diff changeset
12 #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
parents: 363
diff changeset
13 #include "t7.h"
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
14
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
15 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
16 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
17 #define MOVE_DELTA_SPEED 4 /* Delta speed needed to identify a valid movement */
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
parents: 363
diff changeset
18 #define PITCH_DELTA_COUNT 10 /* Delta count needed to identify a valid minima / maxima */
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
parents: 363
diff changeset
19 #define PITCH_DELTA_END 10 /* Delta allowed between start and end position */
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
parents: 363
diff changeset
20
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
parents: 363
diff changeset
21
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
parents: 363
diff changeset
22 #define SECTOR_WINDOW 40.0 /* Pitch window which is used for custom view projection */
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
parents: 363
diff changeset
23 #define SECTOR_HYSTERY 3 /* Additional offset to avoid fast changing displays */
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
parents: 363
diff changeset
24 #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */
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
parents: 363
diff changeset
25 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
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
parents: 363
diff changeset
26
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
27
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
28 static detectionState_t detectionState = DETECT_NOTHING;
362
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
29
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
parents: 363
diff changeset
30 static uint8_t curSector;
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
parents: 363
diff changeset
31 static uint8_t targetSector;
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
parents: 363
diff changeset
32 static uint8_t sectorSize;
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
parents: 363
diff changeset
33 static uint8_t sectorCount;
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
parents: 363
diff changeset
34
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
parents: 363
diff changeset
35 SSector PitchSector[10]; /* max number of enabled custom views */
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
parents: 363
diff changeset
36
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
parents: 363
diff changeset
37
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
parents: 363
diff changeset
38 uint8_t GetSectorForPitch(float pitch)
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
parents: 363
diff changeset
39 {
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
parents: 363
diff changeset
40 static float lastPitch = 1000;
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
parents: 363
diff changeset
41 float newPitch;
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
parents: 363
diff changeset
42 uint8_t index;
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
parents: 363
diff changeset
43 uint8_t sector = 0;
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
parents: 363
diff changeset
44
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
parents: 363
diff changeset
45 if(lastPitch == 1000) /* init at first call */
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
parents: 363
diff changeset
46 {
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
parents: 363
diff changeset
47 lastPitch = pitch;
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
parents: 363
diff changeset
48 }
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
parents: 363
diff changeset
49
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
parents: 363
diff changeset
50 newPitch = lastPitch + (pitch / SECTOR_FILTER);
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
parents: 363
diff changeset
51
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
parents: 363
diff changeset
52 for(index = 1; index < sectorCount; index++)
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
parents: 363
diff changeset
53 {
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
parents: 363
diff changeset
54 if((pitch < PitchSector[index].upperlimit) && (pitch > PitchSector[index].lowerlimit ))
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
parents: 363
diff changeset
55 {
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
parents: 363
diff changeset
56 sector = index;
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
parents: 363
diff changeset
57 break;
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
parents: 363
diff changeset
58 }
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
parents: 363
diff changeset
59 }
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
parents: 363
diff changeset
60 lastPitch = newPitch;
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
parents: 363
diff changeset
61 return sector;
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
parents: 363
diff changeset
62 }
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
parents: 363
diff changeset
63
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
parents: 363
diff changeset
64 void DefinePitchSectors(float centerPitch)
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
parents: 363
diff changeset
65 {
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
parents: 363
diff changeset
66 uint8_t index;
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
parents: 363
diff changeset
67
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
parents: 363
diff changeset
68 sectorCount = t7_GetEnabled_customviews();
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
parents: 363
diff changeset
69 sectorSize = SECTOR_WINDOW / sectorCount;
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
parents: 363
diff changeset
70
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
parents: 363
diff changeset
71 PitchSector[0].upperlimit = centerPitch + (SECTOR_WINDOW / 2);
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
parents: 363
diff changeset
72 PitchSector[0].lowerlimit = PitchSector[0].upperlimit - sectorSize - SECTOR_HYSTERY;
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
parents: 363
diff changeset
73
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
parents: 363
diff changeset
74 for(index = 1; index < sectorCount; index++)
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
parents: 363
diff changeset
75 {
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
parents: 363
diff changeset
76 PitchSector[index].upperlimit = PitchSector[0].upperlimit - index * sectorSize + SECTOR_HYSTERY;
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
parents: 363
diff changeset
77 PitchSector[index].lowerlimit = PitchSector[0].upperlimit - (index + 1) * sectorSize - SECTOR_HYSTERY;
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
parents: 363
diff changeset
78 }
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
parents: 363
diff changeset
79
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
parents: 363
diff changeset
80 PitchSector[0].upperlimit = SECTOR_BORDER;
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
parents: 363
diff changeset
81 PitchSector[index - 1].lowerlimit = SECTOR_BORDER * -1.0;
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
parents: 363
diff changeset
82
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
parents: 363
diff changeset
83 /* get the current sector */
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
parents: 363
diff changeset
84 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
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
parents: 363
diff changeset
85 targetSector = curSector;
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
parents: 363
diff changeset
86 }
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
parents: 363
diff changeset
87
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
parents: 363
diff changeset
88 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
parents: 363
diff changeset
89 {
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
parents: 363
diff changeset
90 targetSector = 0;
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
parents: 363
diff changeset
91 curSector = 0;
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
parents: 363
diff changeset
92 sectorSize = 0;
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
parents: 363
diff changeset
93 sectorCount = 0;
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
parents: 363
diff changeset
94 DefinePitchSectors(0);
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
parents: 363
diff changeset
95 }
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
parents: 363
diff changeset
96
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
parents: 363
diff changeset
97 /* Map the current pitch value to a sector and create button event in case the sector is left */
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
parents: 363
diff changeset
98 detectionState_t detectSectorButtonEvent(float curPitch)
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
parents: 363
diff changeset
99 {
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
parents: 363
diff changeset
100 static uint8_t lastTargetSector = 0;
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
parents: 363
diff changeset
101 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
parents: 363
diff changeset
102 uint8_t PitchEvent = DETECT_NOTHING;
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
parents: 363
diff changeset
103
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
parents: 363
diff changeset
104 /* only change sector if reading is stable */
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
parents: 363
diff changeset
105 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
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
parents: 363
diff changeset
106 if(lastTargetSector == 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
parents: 363
diff changeset
107 {
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
parents: 363
diff changeset
108 targetSector = 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
parents: 363
diff changeset
109 }
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
parents: 363
diff changeset
110 lastTargetSector = 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
parents: 363
diff changeset
111 if(targetSector != curSector)
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
parents: 363
diff changeset
112 {
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
parents: 363
diff changeset
113 if(targetSector > curSector)
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
parents: 363
diff changeset
114 {
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
parents: 363
diff changeset
115 curSector++;
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
parents: 363
diff changeset
116 PitchEvent = DETECT_POS_PITCH;
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
parents: 363
diff changeset
117 }
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
parents: 363
diff changeset
118 else
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
parents: 363
diff changeset
119 {
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
parents: 363
diff changeset
120 curSector--;
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
parents: 363
diff changeset
121 PitchEvent = DETECT_NEG_PITCH;
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
parents: 363
diff changeset
122 }
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
parents: 363
diff changeset
123 }
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
parents: 363
diff changeset
124 return PitchEvent;
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
parents: 363
diff changeset
125 }
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
parents: 363
diff changeset
126
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
parents: 363
diff changeset
127
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
parents: 363
diff changeset
128 /* Detect if user is generating an pitch including return to starting position */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
129 /* 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
parents: 363
diff changeset
130 detectionState_t detectPitch(float currentPitch)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
131 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
132 static uint8_t stableCnt = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
133 static float lastPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
134 static float startPitch = 0.0;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
135 float curSpeed;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
136
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
parents: 363
diff changeset
137
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
parents: 363
diff changeset
138 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
139 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
140 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
141 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
142
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
143 curSpeed = currentPitch - lastPitch;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
144
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
145 /* feed value into state machine */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
146 switch (detectionState)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
147 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
148 case DETECT_NOTHING: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) /* detect a stable condition before evaluating for the next move */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
149 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
150 stableCnt++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
151 }
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
parents: 363
diff changeset
152 else
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
parents: 363
diff changeset
153 {
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
parents: 363
diff changeset
154 stableCnt=0;
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
parents: 363
diff changeset
155 }
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
parents: 363
diff changeset
156
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
157 if(stableCnt > STABLE_STATE_COUNT)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
158 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
159 detectionState = DETECT_START;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
160 stableCnt = 0;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
161 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
162 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
163 case DETECT_START: if(fabsf(curSpeed) > MOVE_DELTA_SPEED)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
164 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
165 if(curSpeed > 0)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
166 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
167 detectionState = DETECT_POS_MOVE;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
168 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
169 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
170 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
171 detectionState = DETECT_NEG_MOVE;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
172 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
173 stableCnt = 0;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
174 startPitch = lastPitch;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
175 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
176 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
177 case DETECT_NEG_MOVE:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
178 case DETECT_POS_MOVE: if(fabsf(curSpeed) > MOVE_DELTA_SPEED )
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
179 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
180 stableCnt++;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
181 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
182 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
183 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
184 if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
185 {
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
parents: 363
diff changeset
186 if(fabsf(startPitch - currentPitch) > PITCH_DELTA_COUNT)
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
187 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
188 detectionState++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
189 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
190 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
191 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
192 detectionState = DETECT_NOTHING;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
193 }
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
194 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
195 else
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
196 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
197 detectionState = DETECT_NOTHING;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
198 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
199 stableCnt = 0;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
200 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
201 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
202 case DETECT_MINIMA:
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
parents: 363
diff changeset
203 case DETECT_MAXIMA: /* stay at maximum for short time to add a pattern for user interaction */
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
parents: 363
diff changeset
204 if(fabsf(curSpeed) < MOVE_DELTA_SPEED )
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
205 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
206 stableCnt++;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
207 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
208 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
209 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
210 if(stableCnt > 0)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
211 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
212 detectionState++;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
213 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
214 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
215 {
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
216 detectionState = DETECT_NOTHING;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
217 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
218 stableCnt = 0;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
219 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
220 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
221 case DETECT_RISEBACK:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
222 case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
223 {
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
parents: 363
diff changeset
224 if(fabsf(startPitch - currentPitch) < PITCH_DELTA_END)
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
225 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
226 detectionState++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
227 }
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
228 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
229 stableCnt++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
230 break;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
231 default:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
232 detectionState = DETECT_NOTHING;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
233 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
234
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
235 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
236 if(stableCnt > STABLE_STATE_TIMEOUT)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
237 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
238 detectionState = DETECT_NOTHING;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
239 stableCnt = 0;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
240 }
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
241
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
242 lastPitch = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
243 return detectionState;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
244 }