Context Navigation

readtty_power.c @ 16

Last change on this file since 16 was 12, checked in by bouyer, 5 years ago
Host software for the v2 firmware
File size: 6.3 KB

Rev	Line
[1]	1	#include <stdio.h>
	2	#include <stdlib.h>
	3	#include <unistd.h>
[12]	4	#include <string.h>
[1]	5	#include <errno.h>
[12]	6	#include <err.h>
[1]	7	#include <fcntl.h>
	8	#include <signal.h>
	9	#include <termios.h>
	10	#include <sys/ioctl.h>
[12]	11	#include <sys/select.h>
[1]	12
	13	struct scale {
	14	float read;
	15	float result;
	16	};
	17
[12]	18	struct scale current_scale;
	19	struct scale voltage_scale;
[1]	20	float current_r = 0;
	21
[12]	22	const float current_ra[] = {0.2, 0.1, 0.05, 0.02};
[1]	23
	24	volatile int quit;
	25	const char *prog;
	26
	27	static void
	28	set_quit(int s)
	29	{
	30	quit = 1;
	31	}
	32
	33	static void
	34	usage()
	35	{
[12]	36	errx(1, "usage: %s -c[1\|2\|3\|4] -v[1\|2\|3] [-n] <device>", prog);
[1]	37	}
[12]	38
	39	static void
	40	get_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
	64	static void
	65	readline(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
[1]	90	int
	91	main(int argc, char * const argv[])
	92	{
	93	int d;
	94	int bytes;
	95	int nfds;
[12]	96	int lineno = 0;
	97	int nopt = 0;
[1]	98	char buf[256];
	99	int i;
	100	fd_set rfds;
[12]	101	char gpio[3], current[4], tension[4];
[1]	102	char *endp;
[12]	103	int gpioi, currenti, tensioni;
[1]	104	float current_a, tension_v;
[12]	105	int val, gpio_val;
	106	int c_opt = 0, v_opt = 0;
[1]	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;
[12]	122	gpioi = currenti = tensioni = 0;
[1]	123
[12]	124	while ((ch = getopt(argc, argv, "np:c:v:")) != -1) {
[1]	125	switch (ch) {
[12]	126	case 'n':
	127	nopt = 1;
	128	break;
[1]	129	case 'c':
[12]	130	c_opt = atoi(optarg);
	131	if (c_opt < 1 \|\| c_opt > 4)
	132	usage();
	133	current_r = current_ra[c_opt -1];
[1]	134	break;
	135	case 'v':
[12]	136	v_opt = atoi(optarg);
	137	if (v_opt < 1 \|\| v_opt > 3)
	138	usage();
[1]	139	break;
	140	default:
	141	usage();
	142	}
	143	}
	144	argc -= optind;
	145	argv += optind;
	146
[12]	147	if (argc != 1 \|\| c_opt == 0 \|\| v_opt == 0)
[1]	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");
[12]	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);
[1]	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 */
[12]	217	if (buf[i] == 'X') {
[1]	218	state = GPIO;
[12]	219	gpioi = 0;
	220	}
[1]	221	break;
	222	case GPIO:
[12]	223	gpio[gpioi] = buf[i];
	224	gpioi++;
	225	if (gpioi < 2)
	226	break;
	227	gpio[gpioi] = '\0';
	228	gpio_val = strtol(gpio, &endp, 16);
[1]	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;
[12]	258	current_a = val / current_scale.read * current_scale.result;
[1]	259	/* correct for in our resistor */
	260	tension_v -= current_a * current_r;
[12]	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);
[1]	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;
[12]	292	tension_v = val / voltage_scale.read * voltage_scale.result;
[1]	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;
[12]	303	gpioi = 0;
[1]	304	}
	305	break;
	306	}
	307	}
	308	} else if (nfds < 0) {
	309	warn("select");
	310	}
	311	}
[12]	312	quit:
	313	/* stop measures */
	314	write(d, "M0\n", 3);
	315	if (fflush(stdout) == EOF) {
	316	warn("fflush");
	317	}
[1]	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.

Context Navigation

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

Download in other formats: