speedometer_firmware/speed_detect.c at master · greentea-ssl/speedometer_firmware · GitHub

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 "speed_detect.h"


void SpeedDetect_Init(SpeedDetect_t* h)
{
    h->init.th_high[0] = 150;
    h->init.th_high[1] = 150;
    h->init.th_low[0] = 100;
    h->init.th_low[1] = 100;

    h->init.timeout_threshold = 112500; // 0.09[m] / lowspeed[m/s] * 250E+6[Hz]

    uint16_t init_val = 300;

    for(int ch = 0; ch < 2; ch++)
    {
        h->maf_sum[ch] = init_val*MAF_LEN;
        for(int i = 0; i < MAF_LEN; i++)
        {
            h->maf_buf[ch][i] = init_val;
        }
    }
    h->maf_buf_cursor = 0;

    h->bin_state[0] = false;
    h->bin_state[1] = false;

    h->mes_state = MES_STATE_STANDBY;

}


void SpeedDetect_UpdateResponse(SpeedDetect_t* h, uint16_t* buf, uint32_t length)
{
    uint32_t ch_len = length / 2;
    uint16_t u[2];
    uint16_t y[2];
    SpeedDetect_Init_t* init = &h->init;

    for(int i = 0; i < ch_len; i++)
    {
        for(int ch = 0; ch < 2; ch++)
        {
            u[ch] = buf[i*2 + ch];
            h->maf_sum[ch] -= h->maf_buf[ch][h->maf_buf_cursor];
            h->maf_sum[ch] += u[ch];
            h->maf_buf[ch][h->maf_buf_cursor] = u[ch];
            y[ch] = h->maf_sum[ch] >> MAF_DIV_SHIFT;

            #if 0 // 物理Highレベルで検出か?
            if(h->bin_state[ch] && y[ch] < init->th_low)
            {
                h->bin_state[ch] = false;
            }
            else if(y[ch] > init->th_high)
            {
                h->bin_state[ch] = true;
            }
            #else
            if(!h->bin_state[ch] && y[ch] < init->th_low[ch])
            {
                h->bin_state[ch] = true;
            }
            else if(y[ch] > init->th_high[ch])
            {
                h->bin_state[ch] = false;
            }
            #endif
        }
        h->maf_buf_cursor = ++h->maf_buf_cursor & MAF_CURSOR_MASK;

        switch(h->mes_state)
        {
            case MES_STATE_STANDBY:
            if(h->bin_state[0])
            {
                h->mes_start_ch = 0;
                h->mes_period_counter = 0;
                h->mes_state = MES_STATE_MEASURING;
            }
            else if(h->bin_state[1])
            {
                h->mes_start_ch = 1;
                h->mes_period_counter = 0;
                h->mes_state = MES_STATE_MEASURING;
            }
            break;

            case MES_STATE_MEASURING:
            h->mes_period_counter++;
            if(h->mes_period_counter >= init->timeout_threshold)
            {
                h->mes_period_counter = 0;
                h->mes_state = MES_STATE_ENDING;
                break;
            }
            if(h->mes_start_ch == 0 && h->bin_state[1])
            {
                h->mes_state = MES_STATE_ENDING;
            }
            else if(h->mes_start_ch == 1 && h->bin_state[0])
            {
                h->mes_state = MES_STATE_ENDING;
            }
            break;

            case MES_STATE_ENDING:
            if(!h->bin_state[0] && !h->bin_state[1])
            {
                h->mes_state = MES_STATE_STANDBY;
            }
            break;
        }

    }

}