source: trunk/software/tty/readtty_power.c

Last change on this file was 12, checked in by bouyer, 5 years ago

Host software for the v2 firmware
File size: 6.3 KB
Line 
1#include <stdio.h>
2#include <stdlib.h>
3#include <unistd.h>
4#include <string.h>
5#include <errno.h>
6#include <err.h>
7#include <fcntl.h>
8#include <signal.h>
9#include <termios.h>
10#include <sys/ioctl.h>
11#include <sys/select.h>
12
13struct scale {
14        float read;
15        float result;
16};
17
18struct scale current_scale;
19struct scale voltage_scale;
20float current_r = 0;
21
22const float current_ra[] = {0.2, 0.1, 0.05, 0.02};
23
24volatile int quit;
25const char *prog;
26
27static void
28set_quit(int s)
29{
30        quit = 1;
31}
32
33static void
34usage()
35{
36        errx(1, "usage: %s -c[1|2|3|4] -v[1|2|3] [-n] <device>", prog);
37}
38
39static void
40get_cal_data(const char *b, int ci, int vi)
41{
42        int cr[4], vr[3];
43        float ca[4], vv[3];
44        int ret;
45
46        ret = sscanf(b, "cal_data %d %f %d %f %d %f %d %f %d %f %d %f %d %f",
47            &cr[0], &ca[0], &cr[1], &ca[1], &cr[2], &ca[2], &cr[3], &ca[3],
48            &vr[0], &vv[0], &vr[1], &vv[1], &vr[2], &vv[2]);
49        if (ret != 14) {
50                errx(1, "cal_data: converted only %d\n", ret);
51        }
52        for (int i = 0; i < 4; i++) {
53                printf("cr %d ca %f\n", cr[i], ca[i]);
54        }
55        for (int i = 0; i < 3; i++) {
56                printf("vr %d vv %f\n", vr[i], vv[i]);
57        }
58        current_scale.read = cr[ci - 1];
59        current_scale.result = ca[ci - 1];
60        voltage_scale.read = vr[vi - 1];
61        voltage_scale.result = vv[vi - 1];
62}
63
64static void
65readline(int d, char *buf, int l)
66{
67        int i;
68        int bytes;
69        do {
70                for (i = 0; i < l; i++) {
71                        do {
72                                bytes = read(d, &buf[i], 1);
73                                if (quit)
74                                        return;
75                        } while ((bytes < 0 && errno == EAGAIN) || buf[i] == '\r');
76                        if (bytes != 1) {
77                                fprintf(stderr, "readline: read %d\n", bytes);
78                                err(1, "read line");
79                        }
80                        if (buf[i] == '\n')
81                                break;
82                }
83        } while (i == 0);
84        if (buf[i] != '\n') {
85                warnx("realine: end of line not found");
86        }
87        buf[i] = '\0';
88}
89
90int
91main(int argc, char * const argv[])
92{
93        int d;
94        int bytes;
95        int nfds;
96        int lineno = 0;
97        int nopt = 0;
98        char buf[256];
99        int i;
100        fd_set rfds;
101        char gpio[3], current[4], tension[4];
102        char *endp;
103        int gpioi, currenti, tensioni;
104        float current_a, tension_v;
105        int val, gpio_val;
106        int c_opt = 0, v_opt = 0;
107        extern char *optarg;
108        extern int optind;
109        int ch;
110        enum {
111                INIT,
112                GPIO,
113                CURRENT,
114                TENSION,
115                MARK
116        } state;
117
118        struct termios t, ot;
119
120        prog = argv[0];
121        state = INIT;
122        gpioi = currenti = tensioni = 0;
123
124        while ((ch = getopt(argc, argv, "np:c:v:")) != -1) {
125                switch (ch) {
126                case 'n':
127                        nopt = 1;
128                        break;
129                case 'c':
130                        c_opt = atoi(optarg);
131                        if (c_opt < 1 || c_opt > 4)
132                                usage();
133                        current_r = current_ra[c_opt  -1];
134                        break;
135                case 'v':
136                        v_opt = atoi(optarg);
137                        if (v_opt < 1 || v_opt > 3)
138                                usage();
139                        break;
140                default: 
141                        usage();
142                }
143        }
144        argc -= optind;
145        argv += optind;
146
147        if (argc != 1 || c_opt == 0 || v_opt == 0) 
148                usage();
149
150        d = open(argv[0], O_RDWR | O_NOCTTY | O_NONBLOCK, 0);
151        if (d < 0) {
152                err(1, "open %s", argv[0]);
153        }
154        fprintf(stderr, "o\n");
155
156        if (tcgetattr(d, &ot) < 0) {
157                err(1, "tcgetattr");
158        }
159        fprintf(stderr, "tg\n");
160
161        quit = 0;
162
163        if (signal(SIGINT, set_quit) == SIG_ERR) {
164                err(1, "signal(SIGINT)");
165        }
166        if (signal(SIGQUIT, set_quit) == SIG_ERR) {
167                err(1, "signal(SIGQUIT)");
168        }
169        if (signal(SIGPIPE, set_quit) == SIG_ERR) {
170                err(1, "signal(SIGPIPE)");
171        }
172        if (signal(SIGTERM, set_quit) == SIG_ERR) {
173                err(1, "signal(SIGTERM)");
174        }
175
176        t = ot;
177        cfmakeraw(&t);
178        t.c_cflag |= CLOCAL;
179        t.c_cflag &= ~CRTSCTS;
180        cfsetspeed(&t, B921600);
181
182        if (tcsetattr(d, TCSANOW | TCSAFLUSH, &t) < 0) {
183                err(1, "tcsetattr");
184        }
185        fprintf(stderr, "tty ready\n");
186        do {
187                write(d, "M0\n", 3);
188                readline(d, buf, sizeof(buf));
189                if (quit)
190                        goto quit;
191        } while (memcmp(buf, "OK", 2) != 0);
192        write(d, "C\n", 2);
193        readline(d, buf, sizeof(buf));
194        if (quit)
195                goto quit;
196        printf("cal_data: %s\n", buf);
197        get_cal_data(buf, c_opt, v_opt);
198        printf("cal_data selected: %f %f, %f %f\n",
199            current_scale.read, current_scale.result,
200            voltage_scale.read, voltage_scale.result);
201
202        /* start measures */
203        write(d, "M1\n", 3);
204        FD_ZERO(&rfds);
205        while (quit == 0) {
206                FD_SET(d, &rfds);
207                nfds = select(d+1, &rfds, NULL, NULL, NULL);
208                if (nfds > 0 && FD_ISSET(d, &rfds)) {
209                        bytes = read(d, buf, sizeof(buf));
210                        if (bytes < 0 && errno != EAGAIN) {
211                                warn("read");
212                        }
213                        for (i = 0; i < bytes; i++) {
214                                switch(state) {
215                                case INIT:
216                                        /* look for an X mark */
217                                        if (buf[i] == 'X') {
218                                                state = GPIO;
219                                                gpioi = 0;
220                                        }
221                                        break;
222                                case GPIO:
223                                        gpio[gpioi] = buf[i];
224                                        gpioi++;
225                                        if (gpioi < 2)
226                                                break;
227                                        gpio[gpioi] = '\0';
228                                        gpio_val = strtol(gpio, &endp, 16);
229                                        if (*endp != '\0') {
230                                                fprintf(stderr,
231                                                    "gpio error: %c\n",
232                                                    *endp);
233                                                state = INIT;
234                                        } else {
235                                                state = TENSION;
236                                                tensioni = 0;
237                                        }
238                                        break;
239                                case CURRENT:
240                                        current[currenti] = buf[i];
241                                        currenti++;
242                                        if (currenti < 3)
243                                                break;
244                                        current[currenti] = '\0';
245                                        val = strtol(current, &endp, 16);
246                                        if (*endp != '\0') {
247                                                fprintf(stderr,
248                                                    "current error: %c\n",
249                                                    *endp);
250                                                state = INIT;
251                                        } else {
252                                                if (val == 4095) {
253                                                        fprintf(stderr,
254                                                          "current saturation\n"
255                                                          );
256                                                }
257                                                state = MARK;
258                                                current_a = val / current_scale.read * current_scale.result;
259                                                /* correct for in our resistor */
260                                                tension_v -= current_a * current_r;
261                                                if (nopt) {
262                                                        printf("%8f ", (float)lineno / 5000);
263                                                        lineno++;
264                                                }
265                                                printf("%3d %8f %8f %8f\n",
266                                                    gpio_val,
267                                                    current_a,
268                                                    tension_v,
269                                                    current_a * tension_v);
270                                        }
271                                        break;
272                                case TENSION:
273                                        tension[tensioni] = buf[i];
274                                        tensioni++;
275                                        if (tensioni < 3)
276                                                break;
277                                        tension[tensioni] = '\0';
278                                        val = strtol(tension, &endp, 16);
279                                        if (*endp != '\0') {
280                                                fprintf(stderr,
281                                                    "tension error: %c\n",
282                                                    *endp);
283                                                state = INIT;
284                                        } else {
285                                                if (val == 4095) {
286                                                        fprintf(stderr,
287                                                          "tension saturation\n"
288                                                          );
289                                                }
290                                                state = CURRENT;
291                                                currenti = 0;
292                                                tension_v = val / voltage_scale.read * voltage_scale.result;
293                                        }
294                                        break;
295                                case MARK:
296                                        if (buf[i] != 'X') {
297                                                fprintf(stderr,
298                                                    "mark error: %c\n",
299                                                    buf[i]);
300                                                state = INIT;
301                                        } else {
302                                                state = GPIO;
303                                                gpioi = 0;
304                                        }
305                                        break;
306                                }
307                        }
308                } else if (nfds < 0) {
309                        warn("select");
310                }
311        }
312quit:
313        /* stop measures */
314        write(d, "M0\n", 3);
315        if (fflush(stdout) == EOF) {
316                warn("fflush");
317        }
318        if (tcsetattr(d, TCSANOW, &ot) < 0) {
319                err(1, "restore tcsetattr");
320        }
321        exit(0);
322}
Note: See TracBrowser for help on using the repository browser.