console_azel.c 3.58 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
2
3
/*
	Command generator for az/el positions

4
5
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h>
#include <signal.h>
#include <ctype.h>
#include <inttypes.h>
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <errno.h>


#include "setpoint.h"
41
#include "command_server.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

char *command_host = "localhost";
int command_port = 11000;
int cmd_port = 11041;
char *az_command_spg = "Azimuth_Setpoint";
char *el_command_spg = "Elevation_Setpoint";

int go = 0;
float sp_az = 0;
float sp_el = 0;

typedef union {
	uint32_t u;
	float f;
} float32_t;


59
int handle_cmd(char *command)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
60
61
62
{
	char *aaz, *ael, *ptr;
	
63
64
	printf("cmd: '%s'\n", command);
	aaz = strtok_r(command, " \t", &ptr);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
65
66
67
	ael = strtok_r(NULL, " \t", &ptr);
	
	if (!aaz || !ael)
68
		return -1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
	printf("aaz: '%s' ael: '%s'\n", aaz, ael);
	
	
	while (ael[strlen(ael)-1] == '\n' ||
	    ael[strlen(ael)-1] == '\r')
		ael[strlen(ael)-1] = 0;
	
	sscanf(aaz, "%f", &sp_az);
	sscanf(ael, "%f", &sp_el);
	
	printf("%g %g\n", sp_az, sp_el);
	
	sp_az *= M_PI / 180.0;
	sp_el *= M_PI / 180.0;

	printf("%g %g\n", sp_az, sp_el);

	go = 1;
	
88
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
90
91
92
93
94
}

int main(int argc, char **argv)
{
	struct setpoint_command *sp_command_az = NULL;
	struct setpoint_command *sp_command_el = NULL;
95
	struct command_server *cmd_srv;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
96
97
98
99
100
101
102
103
104

	time_t lastt = 0;

	signal(SIGPIPE, SIG_IGN);

	if (argc == 2 || argc == 4) {
		command_host = argv[argc - 1];
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
	do {
		sp_command_az = setpoint_command_init(command_host, command_port,
		    az_command_spg, "console/azel");
		if (!sp_command_az) {
			printf("Could not open connection for az commands\n");
			sleep(1);
		}
	} while (!sp_command_az);
	do {
		sp_command_el = setpoint_command_init(command_host, command_port,
		    el_command_spg, "console/azel");
		if (!sp_command_el) {
			printf("Could not open connection for el commands\n");
			sleep(1);
		}
	} while (!sp_command_el);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
122

123
124
125
126

	cmd_srv = command_server_create(cmd_port, 0, 100);
	if (!cmd_srv) {
		printf("Could not open listen port for commands\n");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
127
	} else {
128
		command_server_handler_raw_set(cmd_srv, handle_cmd);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
	}

	while (1)
	{
		time_t t;
		int high = 0;
		fd_set fdset_rx;
		struct timeval tv;
		
		t = time(NULL);
		if (t != lastt) {
			lastt = t;
		
			if (go) {
				printf("Setpoint: %f %f %e %e\n",
				    sp_az / M_PI * 180.0, sp_el / M_PI * 180.0,
				    sp_az, sp_el);

				setpoint_command_setpoint(sp_command_az, sp_az);
				setpoint_command_setpoint(sp_command_el, sp_el);
				setpoint_command_setpoint(sp_command_az, sp_az);
				setpoint_command_setpoint(sp_command_el, sp_el);
				go = 0;
			}
		}

		FD_ZERO(&fdset_rx);
	
157
		command_server_fdset_add(cmd_srv, &fdset_rx, &high);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
159
160
161
162
163

		tv.tv_sec = 1;
		tv.tv_usec = 0;

		select(high, &fdset_rx, NULL, NULL, &tv);

164
		command_server_fdset_handle(cmd_srv, &fdset_rx);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
166
167
	}

}