/
motion.c
120 lines (99 loc) · 2.17 KB
/
motion.c
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
#include <avr/io.h>
#include <stdint.h>
#include <stdlib.h>
#include <avr/eeprom.h>
#include "motion.h"
volatile static uint16_t adcval[2];
volatile static uint8_t mtd_state = MTD_STATE_ENABLED;
volatile static uint8_t val_new = 0;
volatile static uint16_t mtd_treshold;
void mtd_init()
{
mtd_treshold = 0;
mtd_treshold += eeprom_read_byte (MTD_EEPROM_BYTE);
mtd_enable();
}
uint8_t mtd_get_treshold()
{
if (mtd_treshold > 0xff)
return 0xff;
return mtd_treshold & 0xff;
}
uint16_t filter (uint8_t in_idx, uint16_t in_val)
{
static int32_t fval[2];
static uint8_t init_done = 0;
int32_t delta[2];
if (init_done != 0x03)
{
fval[in_idx] = in_val * 65536;
init_done |= (0x01 << in_idx);
return 0xffff;
}
delta[in_idx] = ((in_val * 65536) - fval[in_idx]);
fval[in_idx] += delta[in_idx] / 1024;
return (abs(delta[in_idx] / 65536));
}
uint16_t motion_check()
{
uint8_t i;
uint16_t tmp;
for (i=0;i<2;i++)
{
if (!(val_new & (i+1)))
continue;
val_new &= ~(i+1);
tmp = filter (i, adcval[i]);
if (tmp > mtd_treshold)
return tmp;
}
return 0;
}
void mtd_set_treshold (uint8_t in_t)
{
mtd_treshold = in_t;
eeprom_write_byte(MTD_EEPROM_BYTE, in_t);
}
void mtd_enable()
{
mtd_state |= MTD_STATE_ENABLED;
}
void mtd_disable()
{
mtd_state &= ~(MTD_STATE_ENABLED);
}
void motiond_tick ()
{
static uint8_t mystate = 0;
if (!(mtd_state & MTD_STATE_ENABLED)) return;
switch (mystate)
{
case 0x00:
ADMUX = (_BV(REFS0) | _BV(REFS1) | 0x01); // int. 2.56V ref, left adjust result, channel 1
ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0));
break;
case 0x01:
if (ADCSRA & _BV(ADSC)) return; /* conversion is not done yet */
val_new |= 0x01;
adcval[0] = ADC;
break;
case 0x02:
ADMUX = (_BV(REFS0) | _BV(REFS1) | 0x02); // int. 2.56V ref, left adjust result, channel 2
ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0));
break;
case 0x03:
if (ADCSRA & _BV(ADSC)) return; /* conversion is not done yet */
val_new |= 0x02;
adcval[1] = ADC;
mystate = 0;
break;
default:
mystate = 0;
return;
}
mystate++;
}
uint16_t get_adc (uint8_t in_num)
{
return adcval[in_num];
}