Commit 436abeb5 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'jeroen' into controller_bus

Conflicts:
	controller/block/block_servo_state.c
parents 7a2c1190 760f76de
../../controller/controller/block_setpoint_generator.h
\ No newline at end of file
......@@ -53,6 +53,10 @@
#include "log.h"
#define TIME_OFFSET 2
#define EL_HORIZON -1.0
#define NEXTPASS_RANGE_T (3600 * 24)
#define COORD_NR 50
char *command_host = "localhost";
int command_port = CONSOLE_COMMAND_PORT;
......@@ -64,32 +68,191 @@ char *el_command_spg = "Elevation_Setpoint";
char tle_str[139] =
"1 25544U 98067A 11346.36157800 .00019501 00000-0 25689-3 0 6081"
"2 25544 51.6432 344.3748 0024836 155.4957 344.3191 15.58092544748721";
char *name = NULL;
struct location *location;
struct predict *predict = NULL;
bool set_tle(char newtle[139])
static bool passknown = false;
time_t delay;
static double az_aos = 0;
static time_t t_aos;
static time_t t_los;
struct status_server *stat_srv;
void nextpass(struct predict *predict)
{
time_t t, tnow = time(NULL);
double JD;
struct ln_date date;
int i;
double prev_az, az, el;
bool aos = false, los = false;
bool cross_west = false, cross_east = false;
int t_west_1 = 0, t_east_1 = 0, t_west_2 = 0, t_east_2 = 0;
time_t t_step;
char statline[2000];
char *statlinepos;
if (delay > tnow)
return;
for (i = TIME_OFFSET; i < NEXTPASS_RANGE_T; i++) {
t = tnow + i;
ln_get_date_from_timet(&t, &date);
JD = ln_get_julian_day(&date);
predict_calc_azel(predict, JD, &az, &el);
az -= 180.0;
if (el >= EL_HORIZON && !aos) {
log_send(LOG_T_INFO,
"Next pass starts in %d seconds, az=%f", i, az);
aos = true;
prev_az = az;
az_aos = az;
t_aos = tnow + i;
}
if (el < EL_HORIZON && aos) {
log_send(LOG_T_INFO,
"Next pass ends in %d seconds", i);
los = true;
t_los = tnow + i;
break;
}
if (aos && !los) {
// printf("%d az: %f\n", (int)t, az);
/* this is a pass */
if (az < -90.0 && prev_az > 90.0) {
/* crossed north */
prev_az -= 360.0;
}
if (az > 90.0 && prev_az < -90.0) {
/* crossed north */
prev_az += 360.0;
}
if (az <= 90.0 && prev_az > 90.0)
cross_west = true;
if (az >= -90.0 && prev_az < -90.0)
cross_east = true;
if (cross_east) {
t_west_2++;
} else {
t_west_1++;
}
if (cross_west) {
t_east_2++;
} else {
t_east_1++;
}
}
}
if (!aos) {
log_send(LOG_T_WARNING,
"No pass within %d seconds, will calculate again after %d seconds",
NEXTPASS_RANGE_T, NEXTPASS_RANGE_T / 2);
delay = tnow + NEXTPASS_RANGE_T / 2;
if (stat_srv)
status_server_send(stat_srv, "pass=unknown\n");
return;
}
delay = 0;
log_send(LOG_T_INFO,
"cross_west: %d, cross_east: %d, t_w_1: %d, t_w_2: %d, t_e_1: %d, t_e_2: %d",
cross_west, cross_east, t_west_1, t_west_2, t_east_1, t_east_2);
if (t_west_1 >= t_west_2 &&
t_west_1 >= t_east_1 &&
t_west_1 >= t_east_2) {
/* if we begin with start az we might go through east late */
if (az_aos < -90) {
az_aos += 360;
}
} else if (t_west_2 >= t_west_1 &&
t_west_2 >= t_east_1 &&
t_west_2 >= t_east_2) {
/* if we begin with start az we go through east in wrong way first */
az_aos += 360;
} else if (t_east_1 >= t_west_1 &&
t_east_1 >= t_west_2 &&
t_east_1 >= t_east_2) {
/* if we begin with start az we might go through west late */
if (az_aos > 90) {
az_aos -= 360;
}
} else {
az_aos -= 360;
}
if (az_aos > 270.0)
az_aos = 270.0;
if (az_aos < -270.0)
az_aos = -270.0;
t_step = (t_los - t_aos) / COORD_NR;
if (!t_step)
t_step = 1;
statlinepos = statline;
statlinepos += sprintf(statlinepos, "pass=");
for (i = t_aos; i < t_los; i += t_step) {
t = i;
ln_get_date_from_timet(&t, &date);
JD = ln_get_julian_day(&date);
predict_calc_azel(predict, JD, &az, &el);
az -= 180.0;
statlinepos += sprintf(statlinepos, "%lld:%d:%d ",
(long long)t, (int)round(az), (int)round(el));
}
sprintf(statlinepos, "\n");
if (stat_srv)
status_server_send(stat_srv, statline);
passknown = true;
}
bool set_tle(char *newtle)
{
double lat, lon, alt;
char processed_tle[139];
char *new_name;
lat = location_get_latitude(location);
lon = location_get_longitude(location);
alt = location_get_altitude(location);
printf("Using lat: %f, lon: %f, alt: %f\n", lat, lon, alt);
log_send(LOG_T_DEBUG, "Using lat: %f, lon: %f, alt: %f",
lat, lon, alt);
if (predict_tle_check(newtle)) {
printf("Invallid TLE\n");
if (predict_tle_check(newtle, processed_tle, &new_name)) {
log_send(LOG_T_ERROR, "Invallid TLE");
return false;
}
strcpy(tle_str, newtle);
strncpy(tle_str, processed_tle, 139);
if (name) {
free(name);
name = NULL;
}
if (new_name)
name = new_name;
else
name = strdup("unknown");
log_send(LOG_T_INFO, "Switching to new TLE for satellite '%s'",
name ? name : "unknown");
if (predict)
predict_free(predict);
predict = predict_create(tle_str, lat, lon, alt);
log_send(LOG_T_INFO, "Switching to new TLE");
passknown = false;
delay = 0;
nextpass(predict);
return true;
}
......@@ -128,10 +291,10 @@ int handle_cmd(char *name, char *val)
{
int i;
printf("name %s value %s\n", name, val);
log_send(LOG_T_DEBUG, "name %s value %s", name, val);
for (i = 0; compensation_switches[i].name; i++) {
if (!strcmp(name, compensation_switches[i].name)) {
printf("found switch %s\n", name);
log_send(LOG_T_DEBUG, "found switch %s", name);
if (val[0] == '1') {
*compensation_switches[i].value = true;
if (!strcmp(name, "enabled")) {
......@@ -143,10 +306,7 @@ int handle_cmd(char *name, char *val)
}
}
if (!strcmp(name, "tle")) {
char newtle[139];
strcpy(newtle, val);
if (!set_tle(newtle))
if (!set_tle(val))
switch_enabled = false;
}
......@@ -167,11 +327,6 @@ void output(struct status_server *stat_srv)
last = now;
// printf("Trace %f %f : %02dh%02dm%04.1f %s%03dd%02dm%04.1f\n",
// hrz.az, hrz.alt,
// hms.hours, hms.minutes, hms.seconds,
// dms.neg ? "-" : " ", dms.degrees, dms.minutes, dms.seconds);
statlinepos = statline;
for (i = 0; compensation_switches[i].name; i++) {
if (i) {
......@@ -184,6 +339,10 @@ void output(struct status_server *stat_srv)
*compensation_switches[i].value);
statlinepos += ret;
}
if (name) {
ret = sprintf(statlinepos, ",name=%s", name);
statlinepos += ret;
}
sprintf(statlinepos, ",tle=%s\n", tle_str);
status_server_send(stat_srv, statline);
......@@ -195,9 +354,7 @@ int main(int argc, char **argv)
struct setpoint_command *sp_command_az = NULL;
struct setpoint_command *sp_command_el = NULL;
struct command_server *cmd_srv;
struct status_server *stat_srv;
time_t lastt = 0;
double lat, lon, alt;
double last_az = 0.0;
dt_model_init();
......@@ -208,10 +365,6 @@ int main(int argc, char **argv)
return -1;
}
lat = location_get_latitude(location);
lon = location_get_longitude(location);
alt = location_get_altitude(location);
printf("Using lat: %f, lon: %f, alt: %f\n", lat, lon, alt);
set_tle(tle_str);
......@@ -229,25 +382,29 @@ int main(int argc, char **argv)
sp_command_el = setpoint_command_init(command_host, command_port,
el_command_spg, "console/sattracker");
if (!sp_command_az || !sp_command_el) {
fprintf(stderr, "Setpoint generator(s) not found\n");
log_send(LOG_T_ERROR, "Setpoint generator(s) not found");
exit(-1);
}
cmd_srv = command_server_create(cmd_port, 0, 100);
if (!cmd_srv) {
printf("Could not open listen port for commands\n");
log_send(LOG_T_ERROR,
"Could not open listen port for commands");
return -1;
} else {
command_server_handler_set(cmd_srv, handle_cmd);
}
stat_srv = status_server_create(stat_port, 0, 100);
if (!cmd_srv) {
printf("Could not open listen port for status\n");
if (!stat_srv) {
log_send(LOG_T_ERROR,
"Could not open listen port for status");
return -1;
}
weather = weather_create("Dwingeloo");
if (!weather) {
fprintf(stderr, "Could not create weather handle\n");
log_send(LOG_T_ERROR, "Could not create weather handle");
exit(-1);
}
......@@ -274,43 +431,64 @@ int main(int argc, char **argv)
predict_calc_azel(predict, JD, &az, &el);
/* predict has an az range from 0 to 360 with 0 being South.
Dwingeloo has an az range from -270 to 270 with 0 being South
*/
az -= 180.0;
if (el < EL_HORIZON) {
if (!passknown)
nextpass(predict);
/* large step? */
if (fabs(az_aos - az) > 180.0) {
log_send(LOG_T_DEBUG,
"Preset to aos position %f -> %f",
az, az_aos);
az += copysign(360.0, az_aos);
}
} else {
/* check if we might have to compensate 360
degrees to prevent large az steps
*/
log_send(LOG_T_DEBUG,
"fabs(%f - %f): %f", last_az,az,fabs(last_az- az));
if (az > 90.0 || az < -90.0) {
/* large step? */
if (fabs(last_az - az) > 200.0) {
log_send(LOG_T_DEBUG,
"Swapping AZ");
az += copysign(360.0, last_az);
}
}
passknown = false;
}
if (t > t_los)
passknown = false;
if (refraction_enable) {
/* Get current weather */
temperature = weather_get_temperature(weather);
pressure = weather_get_pressure(weather);
/* Refraction adjustment */
alt_adj = ln_get_refraction_adj(el, pressure, temperature);
printf("Refraction: %f @ %fC,%fmbar\n", alt_adj, temperature, pressure);
log_send(LOG_T_DEBUG,
"Refraction: %f @ %fC,%fmbar", alt_adj, temperature, pressure);
el += alt_adj;
}
/* libnova has an az range from 0 to 360 with 0 being South.
Dwingeloo has an az range from -270 to 270 with 0 being South
*/
az -= 180.0;
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);
log_send(LOG_T_DEBUG, "model: %g %g", daz, del);
az += daz;
el += del;
}
/* check if we might have compensate 360 degrees to
prevent large az steps
*/
printf("fabs(%f - %f): %f\n", last_az,az,fabs(last_az- az));
if (az > 90.0 || az < -90.0) {
/* large step? */
if (fabs(last_az - az) > 200.0) {
printf("Swapping AZ\n");
az += copysign(360.0, last_az);
}
}
if (!switch_cmd && fabs(last_az - az) > 180.0) {
if (!switch_cmd && fabs(last_az - az) > 90.0) {
if (switch_enabled)
log_send(LOG_T_WARNING,
"Disabling tracker to prevent large azimuth jump");
......@@ -319,7 +497,11 @@ int main(int argc, char **argv)
last_az = az;
printf("Setpoint: %d %f %f %f\n",
/* Temporary limit on elevation */
if (el < 0.0)
el = 0.0;
log_send(LOG_T_DEBUG, "Setpoint: %d %f %f %f",
(unsigned int)t, JD, az, el);
if (switch_enabled) {
......
......@@ -148,9 +148,8 @@ int main (int argc, char **argv)
"Lost connection to controller\n");
printf(buffer);
}
} else {
command_server_fdset_handle(cmd_srv, &fd_rd);
}
command_server_fdset_handle(cmd_srv, &fd_rd);
if (fd_logfile < 0) {
fd_logfile = open(logfile,
......
......@@ -179,9 +179,45 @@ struct predict *predict_create(char *tle, double lat, double lon, double alt)
return predict;
}
bool predict_tle_check(char *tle)
bool predict_tle_check(char *tle, char processed_tle[139], char **name)
{
return !Good_Elements(tle);
char tmp_tle[strlen(tle)+1];
int i;
strcpy(tmp_tle, tle);
for (i = 0; i < strlen(tmp_tle); i++) {
if (tmp_tle[i] == '\n' ||
tmp_tle[i] == '\r') {
strcpy(tmp_tle + i, tmp_tle + i + 1);
i--;
}
}
for (i = 0; i < (int)strlen(tmp_tle) - 137; i++) {
if (Good_Elements(tmp_tle + i)) {
if (processed_tle) {
memcpy(processed_tle, tmp_tle + i, 138);
processed_tle[138] = 0;
}
if (name && i) {
*name = malloc(i+1);
if (*name) {
memcpy(*name, tmp_tle, i);
(*name)[i] = 0;
while (i && (
(*name)[i-1] == ' ' ||
(*name)[i-1] == '\t')) {
(*name)[i-1] = 0;
i--;
}
}
} else {
*name = NULL;
}
return false;
}
}
return true;
}
void predict_free(struct predict *predict)
......
......@@ -5,7 +5,7 @@
struct predict;
bool predict_tle_check(char *tle);
bool predict_tle_check(char *tle, char processed_tle[138], char **name);
struct predict *predict_create(char *tle, double lat, double lon, double alt);
......
......@@ -26,8 +26,7 @@
#include <arpa/inet.h>
#include "tcp_connect.h"
#include "block_setpoint_generator.h"
#include "controller_setpoint_command.h"
struct setpoint_command {
int fd;
......
......@@ -38,7 +38,7 @@
#include "setpoint.h"
#include "tcp_listen.h"
#include "block_setpoint_generator.h"
#include "controller_setpoint_command.h"
#include "status_server.h"
#include "command_server.h"
......
......@@ -26,7 +26,7 @@
#include <stdint.h>
#include <inttypes.h>
#include <arpa/inet.h>
#include "block_setpoint_generator.h"
#include "controller_setpoint_command.h"
#define BUFSIZE 4096
......
......@@ -126,7 +126,7 @@ function azimuth_view(element)
a_v_this.ctx.lineTo( 42, 5);
a_v_this.ctx.lineTo( 50, 5);
a_v_this.ctx.stroke();
//S
a_v_this.ctx.beginPath();
a_v_this.ctx.arc(0,42.5, 2.5, 0, 0.7*Math.PI, true);
a_v_this.ctx.arc(0,47.5, 2.5, -0.3*Math.PI, Math.PI, false);
......@@ -282,3 +282,115 @@ function elevation_view(element)
e_v_this.ctx.stroke();
}
}
function satellite_view(element)
{
var s_v_this = this;
this.canvas = document.getElementById(element);
this.ctx = this.canvas.getContext("2d");
this.ctx.scale(
this.canvas.width / 100,
this.canvas.height / 100);
this.ctx.translate(50,50);
this.coord2xy = function satellite_view_coord2xy(coord) {
var az = coord.az * Math.PI / 180;
var el = 50 - (coord.el/90 * 50);
var x = Math.sin(-az) * el;
var y = Math.cos(-az) * el;
// alert(coord.az + " " + coord.el + " " + az +" "+ el +" "+ x +" "+ y);
return (new function() {
this.x = x;
this.y = y;
});
}
this.draw = function satellite_view_draw(coords)
{
var i;
var coord;
s_v_this.ctx.save();
/* white */
s_v_this.ctx.fillStyle = "rgb(255, 255, 255)";
s_v_this.ctx.fillRect(-50, -50, 100, 100);
/* background */
s_v_this.ctx.strokeStyle = "rgba(0, 0, 0, 0.25)";
s_v_this.ctx.beginPath();
s_v_this.ctx.arc(0, 0, 50, 0, 2 * Math.PI);
s_v_this.ctx.lineTo(-50,0);
s_v_this.ctx.moveTo(0,50);
s_v_this.ctx.lineTo(0,-50);
s_v_this.ctx.stroke();
s_v_this.ctx.beginPath();
s_v_this.ctx.arc(0, 0, 16.7, 0, 2 * Math.PI);
s_v_this.ctx.stroke();
s_v_this.ctx.beginPath();
s_v_this.ctx.arc(0, 0, 33.3, 0, 2 * Math.PI);
s_v_this.ctx.stroke();
s_v_this.ctx.strokeStyle = "rgb(0, 0, 255)";
//N
s_v_this.ctx.beginPath();
s_v_this.ctx.moveTo(-4, -40);
s_v_this.ctx.lineTo(-4, -50);
s_v_this.ctx.lineTo( 4, -40);
s_v_this.ctx.lineTo( 4, -50);
s_v_this.ctx.stroke();
//W
s_v_this.ctx.beginPath();
s_v_this.ctx.moveTo(-50, -5);
s_v_this.ctx.lineTo(-47.5, 5);
s_v_this.ctx.lineTo(-45, -3);
s_v_this.ctx.lineTo(-42.5, 5);
s_v_this.ctx.lineTo(-40, -5);
s_v_this.ctx.stroke();
//E
s_v_this.ctx.beginPath();
s_v_this.ctx.moveTo( 50, -5);
s_v_this.ctx.lineTo( 42, -5);
s_v_this.ctx.lineTo( 42, 0);
s_v_this.ctx.lineTo( 47, 0);
s_v_this.ctx.moveTo( 42, 0);