1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
|
/*
* MOVThread.cpp
*
* Created on: 29 May 2020
* Author: Ralim
*/
#include "BMA223.hpp"
#include "BSP.h"
#include "FreeRTOS.h"
#include "I2C_Wrapper.hpp"
#include "LIS2DH12.hpp"
#include "MMA8652FC.hpp"
#include "MSA301.h"
#include "QC3.h"
#include "SC7A20.hpp"
#include "Settings.h"
#include "TipThermoModel.h"
#include "cmsis_os.h"
#include "history.hpp"
#include "main.hpp"
#include "power.hpp"
#include "stdlib.h"
#include "task.h"
#define MOVFilter 8
uint8_t accelInit = 0;
TickType_t lastMovementTime = 0;
void detectAccelerometerVersion() {
DetectedAccelerometerVersion = 99;
#ifdef ACCEL_MMA
if (MMA8652FC::detect()) {
if (MMA8652FC::initalize()) {
DetectedAccelerometerVersion = 1;
}
} else
#endif
#ifdef ACCEL_LIS
if (LIS2DH12::detect()) {
// Setup the ST Accelerometer
if (LIS2DH12::initalize()) {
DetectedAccelerometerVersion = 2;
}
} else
#endif
#ifdef ACCEL_BMA
if (BMA223::detect()) {
// Setup the ST Accelerometer
if (BMA223::initalize()) {
DetectedAccelerometerVersion = 3;
}
} else
#endif
#ifdef ACCEL_MSA
if (MSA301::detect()) {
// Setup the MSA301 Accelerometer
if (MSA301::initalize()) {
DetectedAccelerometerVersion = 4;
}
} else
#endif
#ifdef ACCEL_SC7
if (SC7A20::detect()) {
// Setup the SC7A20 Accelerometer
if (SC7A20::initalize()) {
DetectedAccelerometerVersion = 5;
}
} else
#endif
{
// disable imu sensitivity
systemSettings.sensitivity = 0;
}
}
inline void readAccelerometer(int16_t &tx, int16_t &ty, int16_t &tz, Orientation &rotation) {
#ifdef ACCEL_LIS
if (DetectedAccelerometerVersion == 2) {
LIS2DH12::getAxisReadings(tx, ty, tz);
rotation = LIS2DH12::getOrientation();
} else
#endif
#ifdef ACCEL_MMA
if (DetectedAccelerometerVersion == 1) {
MMA8652FC::getAxisReadings(tx, ty, tz);
rotation = MMA8652FC::getOrientation();
} else
#endif
#ifdef ACCEL_BMA
if (DetectedAccelerometerVersion == 3) {
BMA223::getAxisReadings(tx, ty, tz);
rotation = BMA223::getOrientation();
} else
#endif
#ifdef ACCEL_MSA
if (DetectedAccelerometerVersion == 4) {
MSA301::getAxisReadings(tx, ty, tz);
rotation = MSA301::getOrientation();
} else
#endif
#ifdef ACCEL_SC7
if (DetectedAccelerometerVersion == 5) {
SC7A20::getAxisReadings(tx, ty, tz);
rotation = SC7A20::getOrientation();
} else
#endif
{
// do nothing :(
}
}
void startMOVTask(void const *argument __unused) {
detectAccelerometerVersion();
osDelay(TICKS_100MS / 2); // wait ~50ms for setup of accel to finalise
lastMovementTime = 0;
// Mask 2 seconds if we are in autostart so that if user is plugging in and
// then putting in stand it doesnt wake instantly
if (systemSettings.autoStartMode)
osDelay(2 * TICKS_SECOND);
int16_t datax[MOVFilter] = {0};
int16_t datay[MOVFilter] = {0};
int16_t dataz[MOVFilter] = {0};
uint8_t currentPointer = 0;
int16_t tx = 0, ty = 0, tz = 0;
int32_t avgx, avgy, avgz;
if (systemSettings.sensitivity > 9)
systemSettings.sensitivity = 9;
Orientation rotation = ORIENTATION_FLAT;
for (;;) {
int32_t threshold = 1500 + (9 * 200);
threshold -= systemSettings.sensitivity * 200; // 200 is the step size
readAccelerometer(tx, ty, tz, rotation);
if (systemSettings.OrientationMode == 2) {
if (rotation != ORIENTATION_FLAT) {
OLED::setRotation(rotation == ORIENTATION_LEFT_HAND); // link the data through
}
}
datax[currentPointer] = (int32_t)tx;
datay[currentPointer] = (int32_t)ty;
dataz[currentPointer] = (int32_t)tz;
if (!accelInit) {
for (uint8_t i = currentPointer + 1; i < MOVFilter; i++) {
datax[i] = (int32_t)tx;
datay[i] = (int32_t)ty;
dataz[i] = (int32_t)tz;
}
accelInit = 1;
}
currentPointer = (currentPointer + 1) % MOVFilter;
avgx = avgy = avgz = 0;
// calculate averages
for (uint8_t i = 0; i < MOVFilter; i++) {
avgx += datax[i];
avgy += datay[i];
avgz += dataz[i];
}
avgx /= MOVFilter;
avgy /= MOVFilter;
avgz /= MOVFilter;
// Sum the deltas
int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz));
// So now we have averages, we want to look if these are different by more
// than the threshold
// If movement has occurred then we update the tick timer
if (error > threshold) {
lastMovementTime = xTaskGetTickCount();
}
osDelay(TICKS_100MS); // Slow down update rate
}
}
|
