diff --git a/src/server/backends/SRT/srt_drive.c b/src/server/backends/SRT/srt_drive.c index 2dd32a5fb1d3f62e2ad34b927dcf814116e85f97..6424b3a8ee7efb18476153386285761936a88a37 100644 --- a/src/server/backends/SRT/srt_drive.c +++ b/src/server/backends/SRT/srt_drive.c @@ -26,11 +26,12 @@ #include <gmodule.h> #include <string.h> - #include <math.h> #include <backend.h> #include <ack.h> +#include <net.h> + #define MSG "SRT DRIVE: " @@ -722,7 +723,7 @@ static double srt_drive_motor_cmd_eval(gchar *cmd) * @brief command the drive motors */ -static void srt_drive_cmd_motors(double *az_cnt, double *el_cnt) +static int srt_drive_cmd_motors(double *az_cnt, double *el_cnt) { const int azc = (int) (*az_cnt); const int elc = (int) (*el_cnt); @@ -757,8 +758,12 @@ static void srt_drive_cmd_motors(double *az_cnt, double *el_cnt) s.eta_msec = 0; ack_status_slew(PKT_TRANS_ID_UNDEF, &s); - g_message("Azimuth: %f sec/count", g_timer_elapsed(tm, NULL) / (*az_cnt)); g_free(cmd); + + if ((*az_cnt) == 0.0) + return -1; + + g_message("Azimuth: %f sec/count", g_timer_elapsed(tm, NULL) / (*az_cnt)); } /* elevation drive */ @@ -780,11 +785,17 @@ static void srt_drive_cmd_motors(double *az_cnt, double *el_cnt) s.eta_msec = 0; ack_status_slew(PKT_TRANS_ID_UNDEF, &s); - g_message("Elevation: %f sec/count", g_timer_elapsed(tm, NULL) /(*el_cnt)); g_free(cmd); + + if ((*el_cnt) == 0.0) + return -1; + + g_message("Elevation: %f sec/count", g_timer_elapsed(tm, NULL) / (*el_cnt)); } g_timer_destroy(tm); + + return 0; } @@ -796,6 +807,8 @@ static void srt_drive_cmd_motors(double *az_cnt, double *el_cnt) static int srt_drive_move(void) { + int ret; + double az_cnt; double el_cnt; @@ -816,9 +829,21 @@ static int srt_drive_move(void) return 0; be_shared_comlink_acquire(); - srt_drive_cmd_motors(&d_az_cnt, &d_el_cnt); + ret = srt_drive_cmd_motors(&d_az_cnt, &d_el_cnt); be_shared_comlink_release(); + if (ret) { + + net_server_broadcast_message("Somebody crashed into a wall, " + "initiating recovery procedure.", + NULL); + + g_warning(MSG "timeout occured, starting recovery procedure"); + + be_park_telescope(); + return 0; + } + /* update current sensor count */ srt.pos.az_cnts += d_az_cnt; srt.pos.el_cnts += d_el_cnt; @@ -887,10 +912,6 @@ static gpointer srt_drive_thread(gpointer data) static int srt_drive_moveto(double az, double el) { - double az_cnts; - double el_cnts; - - ack_moveto_azel(PKT_TRANS_ID_UNDEF, az, el); az += srt_drive_az_tilt_corr(az, el); @@ -975,6 +996,11 @@ static gpointer srt_park_thread(gpointer data) ack_status_move(PKT_TRANS_ID_UNDEF, &s); ack_status_slew(PKT_TRANS_ID_UNDEF, &s); + + net_server_broadcast_message("Telescope is now at park position.", + NULL); + + g_thread_exit(NULL); } @@ -1068,6 +1094,9 @@ static void srt_drive_notify_pos_update(void) G_MODULE_EXPORT void be_park_telescope(void) { + net_server_broadcast_message("Moving telescope to park position, " + "please stand by.", NULL); + g_message(MSG "parking telescope"); g_thread_new(NULL, srt_park_thread, NULL);