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

4
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013, 2014
5
	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

	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>

39
40
#include <command/command.h>
#include <log/log.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
42
#include <dt_port_numbers.h>
#include <utils/dt_host.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
43

Jeroen Vreeken's avatar
Jeroen Vreeken committed
44
#include <utils/command_server.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
45
46
47
48
49
50
51
52
53
54
55
56

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;


57
int handle_cmd(char *command)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
58
59
60
{
	char *aaz, *ael, *ptr;
	
61
62
	printf("cmd: '%s'\n", command);
	aaz = strtok_r(command, " \t", &ptr);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
63
64
65
	ael = strtok_r(NULL, " \t", &ptr);
	
	if (!aaz || !ael)
66
		return -1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
	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;
	
86
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
87
88
89
90
}

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

	time_t lastt = 0;

	signal(SIGPIPE, SIG_IGN);

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

103
104
105
	log_client_start(dt_host_console(), CONSOLE_LOG_PORT_IN, 
	    LOG_T_DEBUG, LOG_T_INFO, "console_azel");

Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
	do {
107
108
		sp_command_az = command_open_simple(command_host, command_port,
		    az_command_spg, "console/azel", COMMAND_VALUE_TYPE_FLOAT);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
109
110
111
112
113
114
		if (!sp_command_az) {
			printf("Could not open connection for az commands\n");
			sleep(1);
		}
	} while (!sp_command_az);
	do {
115
116
		sp_command_el = command_open_simple(command_host, command_port,
		    el_command_spg, "console/azel", COMMAND_VALUE_TYPE_FLOAT);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
117
118
119
120
121
122
		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
123

124
125
126
127

	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
128
	} else {
129
		command_server_handler_raw_set(cmd_srv, handle_cmd);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
131
132
133
134
135
136
137
138
139
140
141
142
143
	}

	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) {
144
145
				struct command_entry entry;
				
Jeroen Vreeken's avatar
Jeroen Vreeken committed
146
147
148
149
				printf("Setpoint: %f %f %e %e\n",
				    sp_az / M_PI * 180.0, sp_el / M_PI * 180.0,
				    sp_az, sp_el);

150
151
152
153
154
155
156
157
				entry.type = COMMAND_PTYPE_SETPOINT;
				entry.value.f = sp_az;
				command_send(sp_command_az, &entry);
				command_send(sp_command_az, &entry);
				entry.value.f = sp_el;
				command_send(sp_command_el, &entry);
				command_send(sp_command_el, &entry);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
159
160
161
162
163
				go = 0;
			}
		}

		FD_ZERO(&fdset_rx);
	
164
		command_server_fdset_add(cmd_srv, &fdset_rx, &high);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
166
167
168
169
170

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

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

171
		command_server_fdset_handle(cmd_srv, &fdset_rx);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
172
173
174
	}

}