Root/tornado/fw/sim/alg.c

Source at commit ce9caf0f3baa50d46a52d28b7afc215770b24158 created 11 years 16 days ago.
By Werner Almesberger, tornado/fw/sim/alg.c: tune EWMA weights; correct sample rate for simulation
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 2000 /* 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
20static 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
30static 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 6 /* ~ 0.06 */
45#define M_SHIFT 10 /* ~ 2/S */
46
47
48static 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
70static void usage(const char *name)
71{
72    fprintf(stderr, "usage: %s [seconds]\n", name);
73    exit(1);
74}
75
76
77int 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

Archive Download this file

Branches:
master
tornado-v1



interactive