console_suntracker.c 6 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
2
3
/*
	Command generator for sun tracking

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

	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>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38
39
#include <stdbool.h>

40
#include <command/command.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41

Jeroen Vreeken's avatar
Jeroen Vreeken committed
42
43
44
45
46
47
48
49
50
#include <aalib/aalib.h>
#include <utils/weather.h>
#include <utils/dt_model.h>

#include <dt_port_numbers.h>
#include <utils/dt_host.h>
#include <utils/command_server.h>
#include <utils/status_server.h>
#include <log/log.h>
51
52


Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
54
char *command_host = "localhost";
int command_port = CONSOLE_COMMAND_PORT;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
55
56
int stat_port = CONSOLE_SUN_STAT_PORT;
int cmd_port = CONSOLE_SUN_CMD_PORT;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
59
char *az_command_spg = "Azimuth_Setpoint";
char *el_command_spg = "Elevation_Setpoint";

Jeroen Vreeken's avatar
Jeroen Vreeken committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
bool dt_model_enable = false;
bool switch_enabled = false;

struct compensation_switch {
	char *name;
	bool *value;
};

struct compensation_switch compensation_switches[] = {
	{ "enabled", &switch_enabled },
	{ "dt_model", &dt_model_enable },
	{ NULL, NULL }
};

struct weather *weather;


77
static int handle_cmd(char *name, char *val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
78
{
79
	int i;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
80

81
82
83
84
85
86
87
88
	printf("name %s value %s\n", name, val);
	for (i = 0; compensation_switches[i].name; i++) {
		if (!strcmp(name, compensation_switches[i].name)) {
			printf("found switch %s\n", name);
			if (val[0] == '1') {
				*compensation_switches[i].value = true;
			} else {
				*compensation_switches[i].value = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
90
91
			}
		}
	}
92
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
93
94
95
}


96
void output(struct status_server *stat_srv)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
{
	static time_t last = 0;
	time_t now;
	char statline[800];
	int ret, i;
	char *statlinepos;
	
	now = time(NULL);
	if (now <= last)
		return;
	
	last = now;
	
	statlinepos = statline;
	for (i = 0; compensation_switches[i].name; i++) {
		if (i) {
			ret = sprintf(statlinepos, ",");
			statlinepos += ret;
		}
		ret = sprintf(statlinepos,
		    "%s=%d",
		    compensation_switches[i].name,
		    *compensation_switches[i].value);
		statlinepos += ret;
	}
	sprintf(statlinepos, "\n");

124
	status_server_send(stat_srv, statline);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
125
126
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
127
128
129

int main(int argc, char **argv)
{
130
131
	struct command *sp_command_az = NULL;
	struct command *sp_command_el = NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
	struct weather *weather;
133
	struct command_server *cmd_srv;
134
	struct status_server *stat_srv;
135
	double az, el, prev_az = 0;;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
136
137
138
139
140

	time_t lastt = 0;

	signal(SIGPIPE, SIG_IGN);

141
142
143
	log_client_start(dt_host_console(), CONSOLE_LOG_PORT_IN, 
	    LOG_T_DEBUG, LOG_T_INFO, "console/suntracker");

Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
	do {
145
146
147
		sp_command_az = command_open_simple(command_host, command_port,
		    az_command_spg, "console/suntracker", 
		    COMMAND_VALUE_TYPE_FLOAT);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
148
149
150
151
152
153
		if (!sp_command_az) {
			printf("Could not open connection for az commands\n");
			sleep(1);
		}
	} while (!sp_command_az);
	do {
154
155
156
		sp_command_el = command_open_simple(command_host, command_port,
		    el_command_spg, "console/suntracker",
		    COMMAND_VALUE_TYPE_FLOAT);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
157
158
159
160
161
		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
162

163
164
	dt_model_init();

Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
	aalib_init();
Jeroen Vreeken's avatar
Jeroen Vreeken committed
166

167
168
	cmd_srv = command_server_create(cmd_port, 0, 100);
	if (!cmd_srv) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
169
170
		printf("Could not open listen port for commands\n");
	} else {
171
		command_server_handler_set(cmd_srv, handle_cmd);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
172
	}
173
174
	stat_srv = status_server_create(stat_port, 0, 100);
	if (!cmd_srv) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
175
176
177
		printf("Could not open listen port for status\n");
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
178
	weather = weather_create("Dwingeloo");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
179
180
181
182
	if (!weather) {
		fprintf(stderr, "Could not create weather handle\n");
		exit(-1);
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
183
184
185
186

	while (1)
	{
		time_t t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
187
188
189
		int high = 0;
		fd_set fdset_rx;
		struct timeval tv;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
		
		t = time(NULL);
		if (t != lastt) {
			lastt = t;
			t += 5;
		
			aalib(0, t, 
			    weather_get_temperature(weather),
			    weather_get_pressure(weather),
			    &az, &el);

			az -= 180.0;

			printf("%s", ctime(&t));
			printf("Setpoint: %d %f %f\n",
			    (unsigned int)t, az, el);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
207
208
209
210
211
212
213
214
215
			if (dt_model_enable) {
				double daz, del;
				
				daz = dt_model_azimuth_delta(az, el);
				del = dt_model_elevation_delta(az, el);
				printf("model: %g %g\n", daz, del);
				az += daz;
				el += del;
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
216

Jeroen Vreeken's avatar
Jeroen Vreeken committed
217
			if (switch_enabled) {
218
219
220
221
222
223
				/* Detect 'jump' at north crossing */
				if (fabs(prev_az - az) > 180.0) {
					log_send(LOG_T_WARNING,
					    "Disabling tracker to prevent large azimuth jump");
					switch_enabled = false;
				} else {
224
225
226
227
228
229
230
					struct command_entry entry;
					
					entry.type = COMMAND_PTYPE_SETPOINT_TIME;
					entry.t.tv_sec = t;
					entry.t.tv_nsec = 0;
					entry.value.f = az * 2 * M_PI / 360.0;
					command_send(sp_command_az, &entry);
231
232
					if (el < 0.0)
						el = 0.0;
233
234
					entry.value.f = el * 2 * M_PI / 360.0;
					command_send(sp_command_el, &entry);
235
				}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
236
			} else {
237
				struct command_entry entry;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
238
239
				/* not enabled, send idle just in case we were
				   still moving */
240
241
242
243
				entry.type = COMMAND_PTYPE_SPEED;
				entry.value.f = 0.0;
				command_send(sp_command_az, &entry);
				command_send(sp_command_el, &entry);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
244
			}
245
			prev_az = az;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
246
247
248
249
		}

		FD_ZERO(&fdset_rx);
	
250
		command_server_fdset_add(cmd_srv, &fdset_rx, &high);
251
		status_server_fdset_add(stat_srv, &fdset_rx, &high);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
252
253
254
255
256
257
		
		tv.tv_sec = 1;
		tv.tv_usec = 0;

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

258
		command_server_fdset_handle(cmd_srv, &fdset_rx);
259
		status_server_fdset_handle(stat_srv, &fdset_rx);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
260
		
261
		output(stat_srv);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
262
263
	}
}