|
| 1 | +#define _GNU_SOURCE |
| 2 | +#include "rotator.h" |
| 3 | +#include "astro.h" |
| 4 | +#include <math.h> |
| 5 | +#include <stdio.h> |
| 6 | +#include <stdlib.h> |
| 7 | +#include <string.h> |
| 8 | + |
| 9 | +#if defined(_WIN32) || defined(_WIN64) |
| 10 | +#include <winsock2.h> |
| 11 | +#include <ws2tcpip.h> |
| 12 | +#include <windows.h> |
| 13 | +#else |
| 14 | +#include <arpa/inet.h> |
| 15 | +#include <netdb.h> |
| 16 | +#include <sys/socket.h> |
| 17 | +#include <sys/time.h> |
| 18 | +#include <unistd.h> |
| 19 | +#endif |
| 20 | + |
| 21 | +typedef struct |
| 22 | +{ |
| 23 | + char host[64]; |
| 24 | + char port[16]; |
| 25 | + char get_fmt[64]; |
| 26 | + char set_fmt[64]; |
| 27 | + char custom_cmd[128]; |
| 28 | + char park_az[16]; |
| 29 | + char park_el[16]; |
| 30 | + char lead_time[16]; |
| 31 | + |
| 32 | + bool auto_steer; |
| 33 | + int steer_mode; |
| 34 | + |
| 35 | + bool connected; |
| 36 | + int sock; |
| 37 | + float cur_az; |
| 38 | + float cur_el; |
| 39 | + bool has_position; |
| 40 | + double last_poll_time; |
| 41 | + double last_send_time; |
| 42 | + char status[128]; |
| 43 | +} RotatorState; |
| 44 | + |
| 45 | +static RotatorState rot = { |
| 46 | + .host = "127.0.0.1", |
| 47 | + .port = "4533", |
| 48 | + .get_fmt = "p", |
| 49 | + .set_fmt = "P %.1f %.1f", |
| 50 | + .custom_cmd = "", |
| 51 | + .park_az = "180.0", |
| 52 | + .park_el = "0.0", |
| 53 | + .lead_time = "30", |
| 54 | + .auto_steer = true, |
| 55 | + .steer_mode = ROTATOR_STEER_POLAR, |
| 56 | + .connected = false, |
| 57 | + .sock = -1, |
| 58 | + .cur_az = 0.0f, |
| 59 | + .cur_el = 0.0f, |
| 60 | + .has_position = false, |
| 61 | + .last_poll_time = 0.0, |
| 62 | + .last_send_time = 0.0, |
| 63 | + .status = "Disconnected"}; |
| 64 | + |
| 65 | +static void Disconnect(void) |
| 66 | +{ |
| 67 | + if (rot.sock != -1) |
| 68 | + { |
| 69 | +#if defined(_WIN32) || defined(_WIN64) |
| 70 | + closesocket((SOCKET)rot.sock); |
| 71 | +#else |
| 72 | + close(rot.sock); |
| 73 | +#endif |
| 74 | + rot.sock = -1; |
| 75 | + } |
| 76 | + rot.connected = false; |
| 77 | +} |
| 78 | + |
| 79 | +static bool ConnectTcp(const char *host, const char *port) |
| 80 | +{ |
| 81 | + Disconnect(); |
| 82 | + |
| 83 | +#if defined(_WIN32) || defined(_WIN64) |
| 84 | + static bool wsa_ready = false; |
| 85 | + if (!wsa_ready) |
| 86 | + { |
| 87 | + WSADATA wsa_data; |
| 88 | + if (WSAStartup(MAKEWORD(2, 2), &wsa_data) != 0) |
| 89 | + { |
| 90 | + snprintf(rot.status, sizeof(rot.status), "WSA startup failed"); |
| 91 | + return false; |
| 92 | + } |
| 93 | + wsa_ready = true; |
| 94 | + } |
| 95 | +#endif |
| 96 | + |
| 97 | + struct addrinfo hints = {0}, *res = NULL, *rp = NULL; |
| 98 | + hints.ai_family = AF_UNSPEC; |
| 99 | + hints.ai_socktype = SOCK_STREAM; |
| 100 | + |
| 101 | + if (getaddrinfo(host, port, &hints, &res) != 0 || !res) |
| 102 | + { |
| 103 | + snprintf(rot.status, sizeof(rot.status), "DNS/host lookup failed"); |
| 104 | + return false; |
| 105 | + } |
| 106 | + |
| 107 | + int sfd = -1; |
| 108 | + for (rp = res; rp != NULL; rp = rp->ai_next) |
| 109 | + { |
| 110 | + sfd = (int)socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); |
| 111 | + if (sfd < 0) |
| 112 | + continue; |
| 113 | + if (connect(sfd, rp->ai_addr, rp->ai_addrlen) == 0) |
| 114 | + break; |
| 115 | +#if defined(_WIN32) || defined(_WIN64) |
| 116 | + closesocket((SOCKET)sfd); |
| 117 | +#else |
| 118 | + close(sfd); |
| 119 | +#endif |
| 120 | + sfd = -1; |
| 121 | + } |
| 122 | + freeaddrinfo(res); |
| 123 | + |
| 124 | + if (sfd < 0) |
| 125 | + { |
| 126 | + snprintf(rot.status, sizeof(rot.status), "Connection failed"); |
| 127 | + return false; |
| 128 | + } |
| 129 | + |
| 130 | +#if defined(_WIN32) || defined(_WIN64) |
| 131 | + DWORD timeout_ms = 150; |
| 132 | + setsockopt(sfd, SOL_SOCKET, SO_RCVTIMEO, (const char *)&timeout_ms, sizeof(timeout_ms)); |
| 133 | + setsockopt(sfd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&timeout_ms, sizeof(timeout_ms)); |
| 134 | +#else |
| 135 | + struct timeval tv = {.tv_sec = 0, .tv_usec = 150000}; |
| 136 | + setsockopt(sfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); |
| 137 | + setsockopt(sfd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)); |
| 138 | +#endif |
| 139 | + |
| 140 | + rot.sock = sfd; |
| 141 | + rot.connected = true; |
| 142 | + snprintf(rot.status, sizeof(rot.status), "Connected to %s:%s", host, port); |
| 143 | + return true; |
| 144 | +} |
| 145 | + |
| 146 | +static bool SendRaw(const char *cmd, char *response, size_t response_len) |
| 147 | +{ |
| 148 | + if (!rot.connected || rot.sock < 0 || !cmd || cmd[0] == '\0') |
| 149 | + return false; |
| 150 | + |
| 151 | + char out[256]; |
| 152 | + size_t cmd_len = strlen(cmd); |
| 153 | + if (cmd_len >= sizeof(out) - 2) |
| 154 | + cmd_len = sizeof(out) - 2; |
| 155 | + memcpy(out, cmd, cmd_len); |
| 156 | + if (cmd_len == 0 || out[cmd_len - 1] != '\n') |
| 157 | + out[cmd_len++] = '\n'; |
| 158 | + out[cmd_len] = '\0'; |
| 159 | + |
| 160 | +#if defined(_WIN32) || defined(_WIN64) |
| 161 | + int sent = send((SOCKET)rot.sock, out, (int)cmd_len, 0); |
| 162 | +#else |
| 163 | + int sent = (int)send(rot.sock, out, cmd_len, 0); |
| 164 | +#endif |
| 165 | + if (sent <= 0) |
| 166 | + { |
| 167 | + snprintf(rot.status, sizeof(rot.status), "Send failed"); |
| 168 | + Disconnect(); |
| 169 | + return false; |
| 170 | + } |
| 171 | + |
| 172 | + { |
| 173 | + char discard[256] = {0}; |
| 174 | + char *resp_buf = response; |
| 175 | + size_t resp_len = response_len; |
| 176 | + if (!resp_buf || resp_len == 0) |
| 177 | + { |
| 178 | + resp_buf = discard; |
| 179 | + resp_len = sizeof(discard); |
| 180 | + } |
| 181 | + |
| 182 | +#if defined(_WIN32) || defined(_WIN64) |
| 183 | + int n = recv((SOCKET)rot.sock, resp_buf, (int)resp_len - 1, 0); |
| 184 | +#else |
| 185 | + int n = (int)recv(rot.sock, resp_buf, resp_len - 1, 0); |
| 186 | +#endif |
| 187 | + if (n <= 0) |
| 188 | + { |
| 189 | + if (response && response_len > 0) |
| 190 | + response[0] = '\0'; |
| 191 | + snprintf(rot.status, sizeof(rot.status), "Read failed"); |
| 192 | + Disconnect(); |
| 193 | + return false; |
| 194 | + } |
| 195 | + resp_buf[n] = '\0'; |
| 196 | + } |
| 197 | + return true; |
| 198 | +} |
| 199 | + |
| 200 | +static bool ParseFirstTwoFloats(const char *s, float *a, float *b) |
| 201 | +{ |
| 202 | + if (!s || !a || !b) |
| 203 | + return false; |
| 204 | + char *end = NULL; |
| 205 | + const char *p = s; |
| 206 | + float vals[2] = {0}; |
| 207 | + int found = 0; |
| 208 | + while (*p && found < 2) |
| 209 | + { |
| 210 | + float v = strtof(p, &end); |
| 211 | + if (end != p) |
| 212 | + { |
| 213 | + vals[found++] = v; |
| 214 | + p = end; |
| 215 | + } |
| 216 | + else |
| 217 | + { |
| 218 | + p++; |
| 219 | + } |
| 220 | + } |
| 221 | + if (found < 2) |
| 222 | + return false; |
| 223 | + *a = vals[0]; |
| 224 | + *b = vals[1]; |
| 225 | + return true; |
| 226 | +} |
| 227 | + |
| 228 | +static void PollPosition(void) |
| 229 | +{ |
| 230 | + if (!rot.connected) |
| 231 | + return; |
| 232 | + if (GetTime() - rot.last_poll_time < 0.5) |
| 233 | + return; |
| 234 | + rot.last_poll_time = GetTime(); |
| 235 | + |
| 236 | + char response[256] = {0}; |
| 237 | + if (!SendRaw(rot.get_fmt, response, sizeof(response))) |
| 238 | + return; |
| 239 | + |
| 240 | + float az = 0.0f, el = 0.0f; |
| 241 | + if (ParseFirstTwoFloats(response, &az, &el)) |
| 242 | + { |
| 243 | + rot.cur_az = az; |
| 244 | + rot.cur_el = el; |
| 245 | + rot.has_position = true; |
| 246 | + snprintf(rot.status, sizeof(rot.status), "OK"); |
| 247 | + } |
| 248 | + else |
| 249 | + { |
| 250 | + snprintf(rot.status, sizeof(rot.status), "Parse failed"); |
| 251 | + } |
| 252 | +} |
| 253 | + |
| 254 | +static bool SetPosition(float az, float el) |
| 255 | +{ |
| 256 | + if (!rot.connected) |
| 257 | + return false; |
| 258 | + char cmd[256]; |
| 259 | + snprintf(cmd, sizeof(cmd), rot.set_fmt, az, el); |
| 260 | + bool ok = SendRaw(cmd, NULL, 0); |
| 261 | + if (ok) |
| 262 | + rot.last_send_time = GetTime(); |
| 263 | + return ok; |
| 264 | +} |
| 265 | + |
| 266 | +void RotatorShutdown(void) { Disconnect(); } |
| 267 | + |
| 268 | +char *RotatorGetHostBuffer(void) { return rot.host; } |
| 269 | +int RotatorGetHostBufferSize(void) { return (int)sizeof(rot.host); } |
| 270 | +char *RotatorGetPortBuffer(void) { return rot.port; } |
| 271 | +int RotatorGetPortBufferSize(void) { return (int)sizeof(rot.port); } |
| 272 | +char *RotatorGetGetFmtBuffer(void) { return rot.get_fmt; } |
| 273 | +int RotatorGetGetFmtBufferSize(void) { return (int)sizeof(rot.get_fmt); } |
| 274 | +char *RotatorGetSetFmtBuffer(void) { return rot.set_fmt; } |
| 275 | +int RotatorGetSetFmtBufferSize(void) { return (int)sizeof(rot.set_fmt); } |
| 276 | +char *RotatorGetCustomCmdBuffer(void) { return rot.custom_cmd; } |
| 277 | +int RotatorGetCustomCmdBufferSize(void) { return (int)sizeof(rot.custom_cmd); } |
| 278 | +char *RotatorGetParkAzBuffer(void) { return rot.park_az; } |
| 279 | +int RotatorGetParkAzBufferSize(void) { return (int)sizeof(rot.park_az); } |
| 280 | +char *RotatorGetParkElBuffer(void) { return rot.park_el; } |
| 281 | +int RotatorGetParkElBufferSize(void) { return (int)sizeof(rot.park_el); } |
| 282 | +const char *RotatorGetStatus(void) { return rot.status; } |
| 283 | +bool RotatorGetAutoSteer(void) { return rot.auto_steer; } |
| 284 | +void RotatorSetAutoSteer(bool enabled) { rot.auto_steer = enabled; } |
| 285 | +int RotatorGetSteerMode(void) { return rot.steer_mode; } |
| 286 | +void RotatorSetSteerMode(int mode) { rot.steer_mode = mode; } |
| 287 | +int RotatorGetLeadTimeSec(void) { return (int)atol(rot.lead_time); } |
| 288 | +void RotatorSetLeadTimeSec(int sec) { snprintf(rot.lead_time, sizeof(rot.lead_time), "%d", sec); } |
| 289 | +char *RotatorGetLeadTimeBuffer(void) { return rot.lead_time; } |
| 290 | +int RotatorGetLeadTimeBufferSize(void) { return (int)sizeof(rot.lead_time); } |
| 291 | +void RotatorConnect(void) { ConnectTcp(rot.host, rot.port); } |
| 292 | +void RotatorDisconnect(void) |
| 293 | +{ |
| 294 | + Disconnect(); |
| 295 | + snprintf(rot.status, sizeof(rot.status), "Disconnected"); |
| 296 | +} |
| 297 | +void RotatorPollNow(void) { PollPosition(); } |
| 298 | +void RotatorSendCustomNow(void) |
| 299 | +{ |
| 300 | + if (rot.custom_cmd[0] != '\0') |
| 301 | + SendRaw(rot.custom_cmd, NULL, 0); |
| 302 | +} |
| 303 | +void RotatorSetParkNow(float az, float el) { SetPosition(az, el); } |
| 304 | + |
| 305 | +void RotatorUpdateControl(UIContext *ctx, bool show_scope_dialog, bool show_polar_dialog, bool polar_lunar_mode, int selected_pass_idx) |
| 306 | +{ |
| 307 | + if (rot.connected) |
| 308 | + PollPosition(); |
| 309 | + |
| 310 | + if (rot.connected && rot.auto_steer && (GetTime() - rot.last_send_time) > 0.25) |
| 311 | + { |
| 312 | + float target_az = 0.0f, target_el = 0.0f; |
| 313 | + bool has_target = false; |
| 314 | + |
| 315 | + if (rot.steer_mode == ROTATOR_STEER_SCOPE && show_scope_dialog) |
| 316 | + { |
| 317 | + target_az = *ctx->scope_az; |
| 318 | + target_el = *ctx->scope_el; |
| 319 | + has_target = true; |
| 320 | + } |
| 321 | + else if (rot.steer_mode == ROTATOR_STEER_POLAR && show_polar_dialog && !polar_lunar_mode && selected_pass_idx >= 0 && selected_pass_idx < num_passes) |
| 322 | + { |
| 323 | + SatPass *p = &passes[selected_pass_idx]; |
| 324 | + int lead_sec = RotatorGetLeadTimeSec(); |
| 325 | + double lead_epoch = (lead_sec > 0) ? (lead_sec / 86400.0) : 0.0; |
| 326 | + double steer_start = p->aos_epoch - lead_epoch; |
| 327 | + if (*ctx->current_epoch >= steer_start && *ctx->current_epoch <= p->los_epoch && p->sat != NULL) |
| 328 | + { |
| 329 | + double t_use = (*ctx->current_epoch < p->aos_epoch) ? p->aos_epoch : *ctx->current_epoch; |
| 330 | + double gmst_use = (t_use == p->aos_epoch) ? epoch_to_gmst(p->aos_epoch) : ctx->gmst_deg; |
| 331 | + Vector3 sat_pos = calculate_position(p->sat, get_unix_from_epoch(t_use)); |
| 332 | + double az = 0.0, el = 0.0; |
| 333 | + get_az_el(sat_pos, gmst_use, home_location.lat, home_location.lon, home_location.alt, &az, &el); |
| 334 | + target_az = (float)az; |
| 335 | + target_el = (float)el; |
| 336 | + has_target = true; |
| 337 | + } |
| 338 | + } |
| 339 | + |
| 340 | + if (has_target) |
| 341 | + { |
| 342 | + while (target_az < 0.0f) |
| 343 | + target_az += 360.0f; |
| 344 | + while (target_az >= 360.0f) |
| 345 | + target_az -= 360.0f; |
| 346 | + if (target_el > 90.0f) |
| 347 | + target_el = 90.0f; |
| 348 | + if (target_el < -90.0f) |
| 349 | + target_el = -90.0f; |
| 350 | + SetPosition(target_az, target_el); |
| 351 | + } |
| 352 | + } |
| 353 | +} |
| 354 | + |
| 355 | +bool RotatorIsConnected(void) { return rot.connected; } |
| 356 | +bool RotatorHasPosition(void) { return rot.has_position; } |
| 357 | +float RotatorGetAz(void) { return rot.cur_az; } |
| 358 | +float RotatorGetEl(void) { return rot.cur_el; } |
0 commit comments