Root/
Source at commit a11670bb7bb6e506db72463fea6738100b22c9a7 created 11 years 3 months ago. By Werner Almesberger, tornado/fw/sim/alg.c: accept data from standard input; fix "process" | |
---|---|
1 | #include <stdbool.h> |
2 | #include <stdint.h> |
3 | #include <stdlib.h> |
4 | #include <stdio.h> |
5 | #include <math.h> |
6 | |
7 | |
8 | #define G 28 /* 1 Earth gravity in accelerometer units */ |
9 | #define MID 512 /* accelerometer middle value */ |
10 | |
11 | #define S 1000 /* sample rate (samples/s) */ |
12 | #define F 2 /* rotational speed in Hz */ |
13 | #define R 0.46 /* radius of accelerometer orbit */ |
14 | |
15 | #define H (G/2) /* hysteresis, in accelerometer units */ |
16 | |
17 | |
18 | /* noise pattern: frequency, amplitude pairs */ |
19 | |
20 | static struct noise { |
21 | int f; /* frequency (in samples) */ |
22 | int a; /* amplitude (in accelerometer units) */ |
23 | } noise[] = { |
24 | { 1, G/3 }, /* general noise */ |
25 | { S/F/10, 5*G }, /* hits caused by the chain "jumping" */ |
26 | { 0, } |
27 | }; |
28 | |
29 | |
30 | static uint16_t sample(double t) |
31 | { |
32 | int fz = R*4*M_PI*M_PI*F*F/9.81*G; |
33 | int fg = cos(2*M_PI*F*t)*G; |
34 | int f = fz+fg; |
35 | const struct noise *n; |
36 | |
37 | for (n = noise; n->f; n++) |
38 | if (!(random() % n->f)) |
39 | f += random() % (2*n->a) - n->a; |
40 | return f+MID; |
41 | } |
42 | |
43 | |
44 | #define E_SHIFT 3 /* ~ 0.1 */ |
45 | #define M_SHIFT 10 /* ~ 1/S */ |
46 | |
47 | |
48 | static void process(unsigned v) |
49 | { |
50 | static uint16_t e = MID << E_SHIFT; |
51 | static uint32_t m = MID << M_SHIFT; |
52 | static bool up = 0; |
53 | int d; |
54 | |
55 | e = v+(e-(e >> E_SHIFT)); |
56 | m = v+(m-(m >> M_SHIFT)); |
57 | d = (e >> E_SHIFT)-(m >> M_SHIFT); |
58 | if (up) { |
59 | if (d < -H) |
60 | up = 0; |
61 | } else { |
62 | if (d > H) |
63 | up = 1; |
64 | } |
65 | printf("%d %d %d %d %d\n", |
66 | v, e >> E_SHIFT, m >> M_SHIFT, d, up); |
67 | } |
68 | |
69 | |
70 | static void usage(const char *name) |
71 | { |
72 | fprintf(stderr, "usage: %s [seconds]\n", name); |
73 | exit(1); |
74 | } |
75 | |
76 | |
77 | int main(int argc, char **argv) |
78 | { |
79 | double t; |
80 | char *end; |
81 | int i; |
82 | unsigned v; |
83 | |
84 | switch (argc) { |
85 | case 1: |
86 | while (scanf("%u", &v) == 1) |
87 | process(v); |
88 | break; |
89 | case 2: |
90 | t = strtod(argv[1], &end); |
91 | if (*end) |
92 | usage(*argv); |
93 | for (i = 0; i != t*S; i++) { |
94 | v = sample((double) i/S); |
95 | process(v); |
96 | } |
97 | break; |
98 | default: |
99 | usage(*argv); |
100 | } |
101 | |
102 | return 0; |
103 | } |
104 |
Branches:
master
tornado-v1