source: trunk/software/tty/readtty_power.c @ 12

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <err.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <sys/select.h>

struct scale {
        float read;
        float result;
};

struct scale current_scale;
struct scale voltage_scale;
float current_r = 0;

const float current_ra[] = {0.2, 0.1, 0.05, 0.02};

volatile int quit;
const char *prog;

static void
set_quit(int s)
{
        quit = 1;
}

static void
usage()
{
        errx(1, "usage: %s -c[1|2|3|4] -v[1|2|3] [-n] <device>", prog);
}

static void
get_cal_data(const char *b, int ci, int vi)
{
        int cr[4], vr[3];
        float ca[4], vv[3];
        int ret;

        ret = sscanf(b, "cal_data %d %f %d %f %d %f %d %f %d %f %d %f %d %f",
            &cr[0], &ca[0], &cr[1], &ca[1], &cr[2], &ca[2], &cr[3], &ca[3],
            &vr[0], &vv[0], &vr[1], &vv[1], &vr[2], &vv[2]);
        if (ret != 14) {
                errx(1, "cal_data: converted only %d\n", ret);
        }
        for (int i = 0; i < 4; i++) {
                printf("cr %d ca %f\n", cr[i], ca[i]);
        }
        for (int i = 0; i < 3; i++) {
                printf("vr %d vv %f\n", vr[i], vv[i]);
        }
        current_scale.read = cr[ci - 1];
        current_scale.result = ca[ci - 1];
        voltage_scale.read = vr[vi - 1];
        voltage_scale.result = vv[vi - 1];
}

static void
readline(int d, char *buf, int l)
{
        int i;
        int bytes;
        do {
                for (i = 0; i < l; i++) {
                        do {
                                bytes = read(d, &buf[i], 1);
                                if (quit)
                                        return;
                        } while ((bytes < 0 && errno == EAGAIN) || buf[i] == '\r');
                        if (bytes != 1) {
                                fprintf(stderr, "readline: read %d\n", bytes);
                                err(1, "read line");
                        }
                        if (buf[i] == '\n')
                                break;
                }
        } while (i == 0);
        if (buf[i] != '\n') {
                warnx("realine: end of line not found");
        }
        buf[i] = '\0';
}

int
main(int argc, char * const argv[])
{
        int d;
        int bytes;
        int nfds;
        int lineno = 0;
        int nopt = 0;
        char buf[256];
        int i;
        fd_set rfds;
        char gpio[3], current[4], tension[4];
        char *endp;
        int gpioi, currenti, tensioni;
        float current_a, tension_v;
        int val, gpio_val;
        int c_opt = 0, v_opt = 0;
        extern char *optarg;
        extern int optind;
        int ch;
        enum {
                INIT,
                GPIO,
                CURRENT,
                TENSION,
                MARK
        } state;

        struct termios t, ot;

        prog = argv[0];
        state = INIT;
        gpioi = currenti = tensioni = 0;

        while ((ch = getopt(argc, argv, "np:c:v:")) != -1) {
                switch (ch) {
                case 'n':
                        nopt = 1;
                        break;
                case 'c':
                        c_opt = atoi(optarg);
                        if (c_opt < 1 || c_opt > 4)
                                usage();
                        current_r = current_ra[c_opt  -1];
                        break;
                case 'v':
                        v_opt = atoi(optarg);
                        if (v_opt < 1 || v_opt > 3)
                                usage();
                        break;
                default: 
                        usage();
                }
        }
        argc -= optind;
        argv += optind;

        if (argc != 1 || c_opt == 0 || v_opt == 0) 
                usage();

        d = open(argv[0], O_RDWR | O_NOCTTY | O_NONBLOCK, 0);
        if (d < 0) {
                err(1, "open %s", argv[0]);
        }
        fprintf(stderr, "o\n");

        if (tcgetattr(d, &ot) < 0) {
                err(1, "tcgetattr");
        }
        fprintf(stderr, "tg\n");

        quit = 0;

        if (signal(SIGINT, set_quit) == SIG_ERR) {
                err(1, "signal(SIGINT)");
        }
        if (signal(SIGQUIT, set_quit) == SIG_ERR) {
                err(1, "signal(SIGQUIT)");
        }
        if (signal(SIGPIPE, set_quit) == SIG_ERR) {
                err(1, "signal(SIGPIPE)");
        }
        if (signal(SIGTERM, set_quit) == SIG_ERR) {
                err(1, "signal(SIGTERM)");
        }

        t = ot;
        cfmakeraw(&t);
        t.c_cflag |= CLOCAL;
        t.c_cflag &= ~CRTSCTS;
        cfsetspeed(&t, B921600);

        if (tcsetattr(d, TCSANOW | TCSAFLUSH, &t) < 0) {
                err(1, "tcsetattr");
        }
        fprintf(stderr, "tty ready\n");
        do {
                write(d, "M0\n", 3);
                readline(d, buf, sizeof(buf));
                if (quit)
                        goto quit;
        } while (memcmp(buf, "OK", 2) != 0);
        write(d, "C\n", 2);
        readline(d, buf, sizeof(buf));
        if (quit)
                goto quit;
        printf("cal_data: %s\n", buf);
        get_cal_data(buf, c_opt, v_opt);
        printf("cal_data selected: %f %f, %f %f\n",
            current_scale.read, current_scale.result,
            voltage_scale.read, voltage_scale.result);

        /* start measures */
        write(d, "M1\n", 3);
        FD_ZERO(&rfds);
        while (quit == 0) {
                FD_SET(d, &rfds);
                nfds = select(d+1, &rfds, NULL, NULL, NULL);
                if (nfds > 0 && FD_ISSET(d, &rfds)) {
                        bytes = read(d, buf, sizeof(buf));
                        if (bytes < 0 && errno != EAGAIN) {
                                warn("read");
                        }
                        for (i = 0; i < bytes; i++) {
                                switch(state) {
                                case INIT:
                                        /* look for an X mark */
                                        if (buf[i] == 'X') {
                                                state = GPIO;
                                                gpioi = 0;
                                        }
                                        break;
                                case GPIO:
                                        gpio[gpioi] = buf[i];
                                        gpioi++;
                                        if (gpioi < 2)
                                                break;
                                        gpio[gpioi] = '\0';
                                        gpio_val = strtol(gpio, &endp, 16);
                                        if (*endp != '\0') {
                                                fprintf(stderr,
                                                    "gpio error: %c\n",
                                                    *endp);
                                                state = INIT;
                                        } else {
                                                state = TENSION;
                                                tensioni = 0;
                                        }
                                        break;
                                case CURRENT:
                                        current[currenti] = buf[i];
                                        currenti++;
                                        if (currenti < 3)
                                                break;
                                        current[currenti] = '\0';
                                        val = strtol(current, &endp, 16);
                                        if (*endp != '\0') {
                                                fprintf(stderr,
                                                    "current error: %c\n",
                                                    *endp);
                                                state = INIT;
                                        } else {
                                                if (val == 4095) {
                                                        fprintf(stderr,
                                                          "current saturation\n"
                                                          );
                                                }
                                                state = MARK;
                                                current_a = val / current_scale.read * current_scale.result;
                                                /* correct for in our resistor */
                                                tension_v -= current_a * current_r;
                                                if (nopt) {
                                                        printf("%8f ", (float)lineno / 5000);
                                                        lineno++;
                                                }
                                                printf("%3d %8f %8f %8f\n",
                                                    gpio_val,
                                                    current_a,
                                                    tension_v,
                                                    current_a * tension_v);
                                        }
                                        break;
                                case TENSION:
                                        tension[tensioni] = buf[i];
                                        tensioni++;
                                        if (tensioni < 3)
                                                break;
                                        tension[tensioni] = '\0';
                                        val = strtol(tension, &endp, 16);
                                        if (*endp != '\0') {
                                                fprintf(stderr,
                                                    "tension error: %c\n",
                                                    *endp);
                                                state = INIT;
                                        } else {
                                                if (val == 4095) {
                                                        fprintf(stderr,
                                                          "tension saturation\n"
                                                          );
                                                }
                                                state = CURRENT;
                                                currenti = 0;
                                                tension_v = val / voltage_scale.read * voltage_scale.result;
                                        }
                                        break;
                                case MARK:
                                        if (buf[i] != 'X') {
                                                fprintf(stderr,
                                                    "mark error: %c\n",
                                                    buf[i]);
                                                state = INIT;
                                        } else {
                                                state = GPIO;
                                                gpioi = 0;
                                        }
                                        break;
                                }
                        }
                } else if (nfds < 0) {
                        warn("select");
                }
        }
quit:
        /* stop measures */
        write(d, "M0\n", 3);
        if (fflush(stdout) == EOF) {
                warn("fflush");
        }
        if (tcsetattr(d, TCSANOW, &ot) < 0) {
                err(1, "restore tcsetattr");
        }
        exit(0);
}