# include <unistd.h>
# include <string.h>
# include <errno.h>
-# include <getopt.h>
# include <sys/types.h>
# include <sys/stat.h>
# include <fcntl.h>
# include <arpa/inet.h>
+# include <getopt.h>
-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;
}
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;
case 't':
opt_t = optarg;
- break;
case 0:
break;
}
}
-
- 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;
}
projects / fpvtools.git / commitdiff