From: Lelik P. Korchagin Date: Mon, 30 Oct 2023 18:54:03 +0000 (+0300) Subject: * reworked with flight_data_record X-Git-Url: https://git.keenfalcon.ru/?a=commitdiff_plain;h=79011a64cd4af67a3daa4903ed0aa1390c2cddc2;p=fpvtools.git * reworked with flight_data_record --- diff --git a/fpvplay.c b/fpvplay.c index c5f375e..1a1e397 100644 --- a/fpvplay.c +++ b/fpvplay.c @@ -4,161 +4,84 @@ # include # include # include -# include # include # include # include # include +# include -struct fpv +struct flight_data_record { - uint32_t len; - uint8_t type; - uint32_t ms; - uint8_t pad[8]; -} __attribute__((packed)); + uint32_t len; + uint8_t type; + uint32_t ms; + uint8_t pad[8]; + uint8_t data[8*1024*1024]; +} __attribute__((packed)); -size_t out(int fd, struct sockaddr_in *to, uint8_t *data, size_t len) +static const char short_options[] = "s:e:t:"; +static const struct option long_options[] = { - int res = 0; - - while (len > 0) { - size_t n = len; + { "start", required_argument, NULL, 's' }, + { "end", required_argument, NULL, 'e' }, + { "to", required_argument, NULL, 't' }, + { 0, 0, NULL, 0 } +}; - if (n > 1400) { - n = 1400; - } +static int opt_s = 0; +static int opt_e = 0; +static char *opt_t = "127.0.0.1"; - if ((n = sendto(fd, data, n, 0, (struct sockaddr*)to, sizeof(struct sockaddr_in))) > 0) { - len -= n; +int get(int file, uint8_t *data, size_t length) +{ + while (length > 0) { + int n = read(file, data, length); + if (n > 0) { data += n; - res += n; + length -= n; + } else if (errno == EINTR || errno == EAGAIN) { + continue; } else { - if (errno != EAGAIN) { - printf("write: %s\n", strerror(errno)); - } else { - continue; - } - break; + return -1; } } - return res; + return 0; } -enum -{ - NUT_CODED_SLICE_NON_IDR = 1, // Coded slice of a non-IDR picture - NUT_CODED_SLICE_IDR = 5, // Coded slice of an IDR picture - NUT_SPS = 7, // Sequence parameter set - NUT_PPS = 8 // Picture parameter set -}; - -struct parameter_set +int get_record(int file, struct flight_data_record *r) { - uint8_t data[256]; - size_t length; - int found; -}; - -size_t out_v(int fd, struct sockaddr_in *to, uint8_t *data, int len) -{ - static struct parameter_set sps = { .data = { 0, }, .length = 0, .found = 0 }; - static struct parameter_set pps = { .data = { 0, }, .length = 0, .found = 0 }; - static struct parameter_set *ps = NULL; - - static struct { uint8_t data[4]; int length; } marker = { .length = 0, .data = { 0, } }; - struct { uint8_t data[4096]; int length; } pkt = { .length = 0 }; - int p; - - for (p = 0; p < len; p++) { - int c = data[p]; - - if (marker.length == 4) { - if (memcmp(marker.data, "\x00\x00\x00\x01", 4) == 0) { - switch (c & 0x1F) { - case NUT_SPS: - memset(ps = &sps, 0, sizeof(sps)); - sps.found = 1; - break; - - case NUT_PPS: - memset(ps = &pps, 0, sizeof(pps)); - pps.found = 1; - break; - - case NUT_CODED_SLICE_NON_IDR: - case NUT_CODED_SLICE_IDR: - if (sps.found == 0 && sps.length > 0) { - int i; for (i = 0; i < sps.length; i++) { - if (pkt.length < sizeof(pkt.data)) { - pkt.data[pkt.length++] = sps.data[i]; - } - } - } - sps.found = 0; - - if (pps.found == 0 && pps.length > 0) { - int i; for (i = 0; i < pps.length; i++) { - if (pkt.length < sizeof(pkt.data)) { - pkt.data[pkt.length++] = pps.data[i]; - } - } - } - pps.found = 0; - - default: - ps = NULL; - break; - } - - marker.data[0] = 0; - marker.data[1] = 0; - marker.data[2] = 0; - marker.data[3] = 0; - marker.length = 0; - } else { - marker.data[0] = marker.data[1]; - marker.data[1] = marker.data[2]; - marker.data[2] = marker.data[3]; - marker.data[3] = c & 0xFF; - } - } else { - marker.data[marker.length++] = c & 0xFF;; - } + int n; - if (ps != NULL) { - if (ps->length < sizeof(ps->data)) { - ps->data[ps->length++] = c & 0xFF; - } else { - memset(ps, 0, sizeof(*ps)); - ps = NULL; - } - } - if (pkt.length < sizeof(pkt.data)) { - pkt.data[pkt.length++] = c & 0xFF; - } + n = get(file, (uint8_t*)r, sizeof(*r) - sizeof(r->data)); + if (n < 0) { + return -1; } - return out(fd, to, pkt.data, pkt.length); + n = get(file, r->data, r->len); + if (n < 0) { + return -1; + } + return 0; } -size_t in(int fd, uint8_t *data, size_t len) +size_t xmit(int fd, struct sockaddr_in *to, uint8_t *data, size_t len) { int res = 0; while (len > 0) { - size_t n = read(fd, data, len); - if (n > 0) { - len -= n; + size_t n = len; + + if (n > 1400) { + n = 1400; + } + + if ((n = sendto(fd, data, n, 0, (struct sockaddr*)to, sizeof(struct sockaddr_in))) > 0) { + len -= n; data += n; res += n; } else { - if (n < 0) { - if (errno != EAGAIN) { - printf("read: %s\n", strerror(errno)); - } else { - continue; - } + if (errno == EAGAIN) { + continue; } break; } @@ -166,27 +89,15 @@ size_t in(int fd, uint8_t *data, size_t len) return res; } -static const char short_options[] = "s:e:t:"; -static const struct option long_options[] = -{ - { "start", required_argument, NULL, 's' }, - { "end", required_argument, NULL, 'e' }, - { "to", required_argument, NULL, 't' }, - { 0, 0, NULL, 0 } -}; - -static int opt_s = -1; -static int opt_e = -1; -static char *opt_t = "127.255.255.255"; - int main(int argc, char *argv[]) { - struct fpv fpv; - static uint8_t data[8 * 1024 * 1024]; - int i; + struct sockaddr_in all; + int on = 1; + int udp; for (;;) { int c; + int i; if ((c = getopt_long_only(argc, argv, short_options, long_options, &i)) == -1) { break; @@ -203,7 +114,6 @@ int main(int argc, char *argv[]) case 't': opt_t = optarg; - break; case 0: break; @@ -213,80 +123,72 @@ int main(int argc, char *argv[]) } } - - struct sockaddr_in all; - int on = 1; - int udp; - memset(&all, 0, sizeof(all)); all.sin_family = AF_INET; all.sin_addr.s_addr = inet_addr(opt_t); all.sin_port = htons(0); udp = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - setsockopt(udp, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)); + if (udp == -1) { + return 1; + } - if ((argc - optind) < 1) { + if (setsockopt(udp, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) == -1) { return 1; } - for (i = 0; (optind + i) < argc; i++) { - int fd = open(argv[optind + i], O_RDONLY); + for (int i = 0; (optind + i) < argc; i++) { + int file = open(argv[optind + i], O_RDONLY); + int play = 0; int ms = 0; - if (fd >= 0) { - while (1) { - if (in(fd, (uint8_t*)&fpv, sizeof(fpv)) > 0) { - int play = 1; - - if (opt_s > 0 && fpv.ms < opt_s) { - play = 0; - } - - if (opt_e > 0 && fpv.ms >= opt_e) { - break; - } - - if (play) { - int sleep = fpv.ms - ms; - if (sleep > 0) { - usleep(sleep * 1000); - } - } - ms = fpv.ms; - - if (in(fd, data, fpv.len) > 0) { - if (!play) { - continue; - } - - switch (fpv.type) { - case 0: - all.sin_port = htons(5000); - break; - - case 2: - all.sin_port = htons(14550); - break; - - case 5: - all.sin_port = htons(5003); - break; - - default: - continue; - } - - out(udp, &all, data, fpv.len); - } else { - break; - } - } else { - break; + if (file < 0) { + continue; + } + + while (1) { + static struct flight_data_record r; // flight_data_record is too large to be allocated ot the top of the stack !!! + + if (get_record(file, &r) < 0) { + break; + } + + if (opt_s > 0 && r.ms < opt_s) { + continue; + } + + if (opt_e > 0 && r.ms >= opt_e) { + break; + } + + if (play) { + int sleep = r.ms - ms; + if (sleep > 0) { + usleep(sleep * 1000); } } - close(fd); + ms = r.ms; + + switch (r.type) { + case 0: + all.sin_port = htons(5000); + break; + + case 2: + all.sin_port = htons(14550); + break; + + case 5: + all.sin_port = htons(5003); + break; + + default: + continue; + } + + xmit(udp, &all, r.data, r.len); } + close(file); } return 0; }