X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_monitor.c;h=9265a1992ac439294dbec034e1b0b2ca0a204b84;hp=1c9b7189fffd8bb0388a92acdf201c82d7c6c8ad;hb=d7c2d358ed8a1afc9f0ba2bd830b10f6b56dc7b1;hpb=09771c644de54ae354e8f98af7ba74289b3c0fcc diff --git a/aoview/aoview_monitor.c b/aoview/aoview_monitor.c index 1c9b7189..9265a199 100644 --- a/aoview/aoview_monitor.c +++ b/aoview/aoview_monitor.c @@ -31,6 +31,7 @@ aoview_monitor_disconnect(void) aoview_serial_close(monitor_serial); monitor_serial = NULL; } + aoview_log_new(); } static void @@ -64,81 +65,132 @@ aoview_parse_pos(double *target, char *source) *target = r; } -static void -aoview_monitor_parse(char *line) +gboolean +aoview_monitor_parse(const char *input_line) { char *saveptr; char *words[64]; int nword; - struct aostate state; + char line_buf[8192], *line; + struct aodata data; - printf ("%s\n", line); + /* avoid smashing our input parameter */ + strncpy (line_buf, input_line, sizeof (line_buf)-1); + line_buf[sizeof(line_buf) - 1] = '\0'; + line = line_buf; for (nword = 0; nword < 64; nword++) { words[nword] = strtok_r(line, " \t\n", &saveptr); line = NULL; if (words[nword] == NULL) break; } - if (nword < 26) - return; + if (nword < 36) + return FALSE; if (strcmp(words[0], "CALL") != 0) - return; - aoview_parse_string(state.callsign, sizeof (state.callsign), words[1]); - aoview_parse_int(&state.serial, words[3]); - aoview_parse_int(&state.rssi, words[5]); - aoview_parse_string(state.state, sizeof (state.state), words[9]); - aoview_parse_int(&state.tick, words[10]); - aoview_parse_int(&state.accel, words[12]); - aoview_parse_int(&state.pres, words[14]); - aoview_parse_int(&state.temp, words[16]); - aoview_parse_int(&state.batt, words[18]); - aoview_parse_int(&state.drogue, words[20]); - aoview_parse_int(&state.main, words[22]); - if (strcmp (words[26], "unlocked") != 0 && nword >= 29) { - sscanf(words[26], "%d:%d:%d", &state.gps_time.hour, &state.gps_time.minute, &state.gps_time.second); - aoview_parse_pos(&state.lat, words[27]); - aoview_parse_pos(&state.lon, words[28]); - sscanf(words[29], "%dm", &state.alt); + return FALSE; + aoview_parse_string(data.callsign, sizeof (data.callsign), words[1]); + aoview_parse_int(&data.serial, words[3]); + + aoview_parse_int(&data.rssi, words[5]); + aoview_parse_string(data.state, sizeof (data.state), words[9]); + aoview_parse_int(&data.tick, words[10]); + aoview_parse_int(&data.accel, words[12]); + aoview_parse_int(&data.pres, words[14]); + aoview_parse_int(&data.temp, words[16]); + aoview_parse_int(&data.batt, words[18]); + aoview_parse_int(&data.drogue, words[20]); + aoview_parse_int(&data.main, words[22]); + aoview_parse_int(&data.flight_accel, words[24]); + aoview_parse_int(&data.ground_accel, words[26]); + aoview_parse_int(&data.flight_vel, words[28]); + aoview_parse_int(&data.flight_pres, words[30]); + aoview_parse_int(&data.ground_pres, words[32]); + aoview_parse_int(&data.gps.nsat, words[34]); + if (strcmp (words[36], "unlocked") == 0) { + data.gps.gps_connected = 1; + data.gps.gps_locked = 0; + data.gps.gps_time.hour = data.gps.gps_time.minute = data.gps.gps_time.second = 0; + data.gps.lat = data.gps.lon = 0; + data.gps.alt = 0; + } else if (nword >= 40) { + data.gps.gps_locked = 1; + data.gps.gps_connected = 1; + sscanf(words[36], "%d:%d:%d", &data.gps.gps_time.hour, &data.gps.gps_time.minute, &data.gps.gps_time.second); + aoview_parse_pos(&data.gps.lat, words[37]); + aoview_parse_pos(&data.gps.lon, words[38]); + sscanf(words[39], "%dm", &data.gps.alt); } else { - state.gps_time.hour = state.gps_time.minute = state.gps_time.second = 0; - state.lat = state.lon = 0; - state.alt = 0; + data.gps.gps_connected = 0; + data.gps.gps_locked = 0; + data.gps.gps_time.hour = data.gps.gps_time.minute = data.gps.gps_time.second = 0; + data.gps.lat = data.gps.lon = 0; + data.gps.alt = 0; } - aoview_state_notify(&state); + if (nword >= 46) { + data.gps.gps_extended = 1; + sscanf(words[40], "%lfm/s", &data.gps.ground_speed); + sscanf(words[41], "%d", &data.gps.course); + sscanf(words[42], "%lfm/s", &data.gps.climb_rate); + sscanf(words[43], "%lf", &data.gps.hdop); + sscanf(words[44], "%d", &data.gps.h_error); + sscanf(words[45], "%d", &data.gps.v_error); + } else { + data.gps.gps_extended = 0; + data.gps.ground_speed = 0; + data.gps.course = 0; + data.gps.climb_rate = 0; + data.gps.hdop = 0; + data.gps.h_error = 0; + data.gps.v_error = 0; + } + aoview_state_notify(&data); + return TRUE; } -static gboolean -aoview_monitor_callback(void *user_data) +static void +aoview_monitor_callback(gpointer user_data, + struct aoview_serial *serial, + gint revents) { int c; - if (!monitor_serial) - return FALSE; - - for (;;) { - c = aoview_serial_getc(monitor_serial); - if (c == -1) - break; - if (c == '\r') - continue; - if (c == '\n') { - monitor_line[monitor_pos] = '\0'; - if (monitor_pos) - aoview_monitor_parse(monitor_line); - monitor_pos = 0; - } else if (monitor_pos < MONITOR_LEN) - monitor_line[monitor_pos++] = c; + if (revents & (G_IO_HUP|G_IO_ERR)) { + aoview_monitor_disconnect(); + return; + } + if (revents & G_IO_IN) { + for (;;) { + c = aoview_serial_getc(serial); + if (c == -1) + break; + if (c == '\r') + continue; + if (c == '\n') { + monitor_line[monitor_pos] = '\0'; + if (monitor_pos) { + if (aoview_monitor_parse(monitor_line)) { + aoview_log_set_serial(aostate.data.serial); + if (aoview_log_get_serial()) + aoview_log_printf ("%s\n", monitor_line); + } + } + monitor_pos = 0; + } else if (monitor_pos < MONITOR_LEN) + monitor_line[monitor_pos++] = c; + } } - return TRUE; } -void +gboolean aoview_monitor_connect(char *tty) { aoview_monitor_disconnect(); monitor_serial = aoview_serial_open(tty); + if (!monitor_serial) + return FALSE; + aoview_table_clear(); + aoview_state_reset(); aoview_serial_set_callback(monitor_serial, - aoview_monitor_callback, - monitor_serial, - NULL); + aoview_monitor_callback); + return TRUE; }