Mirrors/OpenSolo
1
0
Fork 0
mirror of https://github.com/OpenSolo/OpenSolo.git synced 2025-06-13 05:37:49 +02:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit, based on .tar.gz file as provided by 3DR , see SOURCE file

Browse Source
This commit is contained in:
Buzz
2017-07-29 18:00:15 +10:00
commit adbe6ebbf3
495 changed files with 110498 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
sololink/flightcode/video/README Normal file
View File

@ -0,0 +1,54 @@
The Solo video pipeline exists in this folder. The pipeline works as follows:
=================
| |
| SOLO |
| |
| vidout |
=================
|
|
|
V
=================
| app_streamer | ==========
| | | | | |
| | ARTOO >-->|----->| mobile |
| | | | |
| -> hdmiout | ==========
=================
|
|
|
V
=================
| MONITOR |
=================
vidout:
vidout is always running on Solo and handles resolution size changes
automatically. It always sends rtp-encapsulated h.264 720p video
to Artoo on port 5550. Depending on network conditions, it will reduce
the framerate of the video. Specifically, if the number of transmit
retries on Solo eclipses 200, the framerate will be reduced based on the
following:
30fps -> 24fps -> 15fps -> 10fps -> 5fps
If the link continues to be bad, the framerate is held at 5fps. Further work
could reduce the GOP or QUANT to reduce bitrate even further. Additional
work should be done to support streaming different resolutions as well.
app_streamer:
app_streamer on Artoo takes data from port 5550 and passes it to
both the hdmiout as well as any connected mobile devices. It sends full
frames at a time to mobile devices, but passes data byte-by-byte to hdmiout.
hdmiout:
hdmiout is always running, but waits to detect a connected HDMI cable before
starting the decoding pipeline. It puts 720p video out on the HDMI port of Artoo
using the mfw_isink gstreamer plugin.
cleanlibs.sh:
cleanlibs is used to remove old Sculpture libraries that are no longer in use.
This is installed as a startup script, but can probably be removed in the future.

45
sololink/flightcode/video/app/Makefile Normal file
View File

@ -0,0 +1,45 @@
# To cross compile:
#
# Set up as usual for bitbake:
# $ . setup-environment build
#
# In the build directory:
# $ bitbake meta-ide-support
# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
#
# Now a make in this directory should work.
VPATH = ../../util ../../ini ../../ini/cpp
INCS = -I../../util -I../../ini -I../../ini/cpp
CFLAGS += -Wall $(INCS)
CXXFLAGS += -Wall $(INCS)
LIBS =
SRCS_CPP = app_streamer.cpp
SRCS_CPP += INIReader.cpp
SRCS_C = util.c
SRCS_C += ini.c
OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
MAIN = app_streamer
all: $(MAIN)
$(MAIN): $(OBJS)
$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
clean:
$(RM) *.o *~ $(MAIN)
BASE := ../../..
fmt:
@python $(BASE)/tools/build/clang-format-run.py --apply
fmt-diff:
@python $(BASE)/tools/build/clang-format-run.py
.PHONY: all clean fmt fmt-diff

334
sololink/flightcode/video/app/app_streamer.cpp Normal file
View File

@ -0,0 +1,334 @@
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <string.h>
#include <syslog.h>
#include <unistd.h>
#include "INIReader.h"
#include "net_wmm.h"
#include "util.h"
// This is the module that runs on the controller, relaying video from Solo to
// the currently connected App.
// App's IP will appear in this file when app connects
static const char *app_ip_filename = "/var/run/solo_app.ip";
// Listen for video packets on this UDP port
static const unsigned my_port = 5550;
// Send video packets to this UDP port (in app)
static const unsigned app_port = 5600;
// Maximum packet size we will forward
// Video packets all fit in one Enet frame (~1500 bytes)
static const unsigned max_packet_bytes = 2000;
// Stats
static unsigned packet_count = 0;
static unsigned byte_count = 0;
// Frame-based transmits
static const unsigned max_frame_packets = 128;
struct pkt {
char buf[max_packet_bytes];
uint16_t len;
};
// Get app IP from file
// Note that returned value is network byte order
static in_addr_t get_app_ip(const char *filename)
{
int fd = open(filename, O_RDONLY);
if (fd < 0)
return htonl(INADDR_NONE);
// max reasonable IP length is 3+1+3+1+3+1+3=15
char ip_buf[40];
memset(ip_buf, 0, sizeof(ip_buf));
ssize_t n_bytes = read(fd, ip_buf, sizeof(ip_buf) - 1);
close(fd);
if (n_bytes <= 0) {
return htonl(INADDR_NONE);
}
// IP address is typically "10.1.1.10\n"; inet_addr accepts the '\n'
return inet_addr(ip_buf);
} // get_app_ip
// Create socket used to send and receive video packets.
// To support receive, bind the socket to the video port, any interface.
// To support send, set the TOS we want for video packets.
static int create_socket(int tos)
{
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
syslog(LOG_ERR, "creating socket");
return -1;
}
// bind to the input port on any interface
struct sockaddr_in my_addr;
memset(&my_addr, 0, sizeof(my_addr));
my_addr.sin_addr.s_addr = htonl(INADDR_ANY);
my_addr.sin_port = htons(my_port);
if (bind(fd, (struct sockaddr *)&my_addr, sizeof(my_addr)) < 0) {
syslog(LOG_ERR, "binding socket");
return -1;
}
// set TOS for output packets
if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) != 0) {
syslog(LOG_ERR, "setting socket options");
return -1;
}
return fd;
} // create_socket
// Create the local socket used to send and video packets to the local stream (HDMI output).
// No need to bind as we just dump data across.
static int create_local_socket(void)
{
int fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
syslog(LOG_ERR, "creating local stream socket");
return -1;
}
return fd;
} // create_local_socket
int main(int argc, const char *argv[])
{
openlog("streamer", LOG_NDELAY, LOG_LOCAL3);
syslog(LOG_INFO, "starting: built " __DATE__ " " __TIME__);
INIReader reader("/etc/sololink.conf");
if (reader.ParseError() < 0) {
syslog(LOG_ERR, "can't load /etc/sololink.conf");
return -1;
}
int app_check_interval_s = reader.GetInteger("video", "vidAppInt", 1);
unsigned app_check_interval_us = app_check_interval_s * 1000000;
int log_interval_s = reader.GetInteger("video", "vidLogInt", 10);
unsigned log_interval_us = log_interval_s * 1000000;
int app_video_tos = reader.GetInteger("video", "vidAppTos", 0xbf);
int fd = create_socket(app_video_tos);
if (fd < 0)
return 1; // error already logged
int local_fd = create_local_socket();
if (local_fd < 0)
return 1; // error already logged
uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
// most recent log time, init to "now" for calculation purposes
// (first message might be a bit off)
uint64_t log_last_us = now_us;
// next log time
uint64_t log_time_us = log_last_us + log_interval_us;
// next time to check for app IP change
uint64_t app_check_us = now_us;
in_addr_t app_ip = htonl(INADDR_NONE);
struct sockaddr_in app_addr;
// Setup the local streaming port
struct sockaddr_in local_addr;
memset(&local_addr, 0, sizeof(local_addr));
local_addr.sin_family = AF_INET;
local_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
local_addr.sin_port = htons(app_port); // Same port as the app
// For sending full frame packets
struct pkt frame_pkts[max_frame_packets];
unsigned frame_pkt_idx = 0;
bool frame_complete = false;
unsigned i;
int small_pkt_count = 0;
// For sequence monitoring
int seq = 0;
int last_seq = 0;
int drops = 0;
const int num_fds = 1;
struct pollfd fd_set[num_fds];
memset(fd_set, 0, sizeof(fd_set));
fd_set[0].fd = fd;
fd_set[0].events = POLLIN;
// handle downlink video packets
while (1) {
// Calculate timeout such that if it expires, we know that it will be
// time to log status or check the app IP (the +1 at the end). E.g if
// log time is 999 usec later than now, delay 1 msec (not zero).
uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
int timeout_ms;
// timeout based on log time or app check time, whichever is sooner
if (log_time_us < app_check_us)
timeout_ms = (log_time_us - now_us) / 1000 + 1;
else
timeout_ms = (app_check_us - now_us) / 1000 + 1;
if (timeout_ms < 0)
// handle race condition where we check times at the bottom of
// this loop, then calculate the delay a bit later (here)
timeout_ms = 0;
// If poll returns with an error, fd_set is unmodified, so let's make
// sure we don't false-detect a ready socket
fd_set[0].revents = 0;
int pollrc = poll(fd_set, num_fds, timeout_ms);
if ((pollrc > 0) && (fd_set[0].revents & POLLIN)) {
char pkt_buf[max_packet_bytes];
ssize_t pkt_bytes = recv(fd, pkt_buf, sizeof(pkt_buf), 0);
now_us = clock_gettime_us(CLOCK_MONOTONIC);
// We shouldn't get any errors from recv. If we do, something new or
// exciting is going on. Log a message, and to avoid soaking the CPU,
// sleep a second before looking for another packet. The sleep will
// disrupt any video that might be playing, but again, we should not
// get any errors here, so the main goal is to let the rest of the
// system run, even at the cost of bad video.
if (pkt_bytes <= 0) {
syslog(LOG_ERR, "recv returned %d", pkt_bytes);
sleep(1);
} else {
if (sendto(local_fd, pkt_buf, pkt_bytes, 0, (struct sockaddr *)&local_addr,
sizeof(local_addr)) != pkt_bytes) {
// Another error we should never see
syslog(LOG_ERR, "sendto local returned error");
sleep(1);
}
// Pull the sequence number off the header
seq = pkt_buf[2] << 8 | pkt_buf[3];
if (seq > (last_seq + 1) && last_seq && seq > last_seq)
drops += (seq - last_seq - 1);
last_seq = seq;
// Copy this packet into the frame packet buffer
memcpy(frame_pkts[frame_pkt_idx].buf, pkt_buf, pkt_bytes);
frame_pkts[frame_pkt_idx].len = pkt_bytes;
// If this is a full frame send it. Or, if its a tiny packet (black screen) send
// it.
// Or if we're out of space, send it all.
if ((pkt_buf[12] == 0x5c && pkt_buf[13] == 0x81) ||
frame_pkt_idx == max_frame_packets - 1)
frame_complete = true;
// We look for small packets. If we get 3 in a row then we start sending them,
// as it could be a black frame. Otherwise, don't send them until we get a
// new frame. 128 bytes consist a "small" packet. Black frames tend to be <100B.
if (pkt_bytes < 128) {
if (small_pkt_count >= 2)
frame_complete = true;
else
++small_pkt_count;
} else if (small_pkt_count > 0)
small_pkt_count = 0;
// stats are for downlink packets (vs. forwarded packets)
packet_count++;
byte_count += pkt_bytes;
// If we have a complete frame or we have no more room, send it along
if (frame_complete) {
if (app_ip != htonl(INADDR_NONE)) {
// Send the entire frame of packets
for (i = 0; i <= frame_pkt_idx; ++i) {
if (sendto(fd, frame_pkts[i].buf, frame_pkts[i].len, 0,
(struct sockaddr *)&app_addr,
sizeof(app_addr)) != frame_pkts[i].len) {
// Another error we should never see, even if the app has
// disconnected.
syslog(LOG_ERR, "sendto returned error");
sleep(1);
}
}
}
// Reset the frame pkt index, frame_complete flag
frame_pkt_idx = 0;
frame_complete = false;
} else {
// Check if our buffer is full, discard if so
++frame_pkt_idx;
if (frame_pkt_idx >= max_frame_packets) {
frame_pkt_idx = 0;
continue;
}
}
} // if (pkt_bytes...)
} else // if ((pollrc > 0) && (fd_set[0].revents & POLLIN))
{
// Timeout or error. In either case, we log or check app if it is
// time then just try again.
now_us = clock_gettime_us(CLOCK_MONOTONIC);
}
// log stats if it is time
if (now_us > log_time_us) {
uint64_t interval_us = now_us - log_last_us;
unsigned packets_sec = (packet_count * 1000000ULL) / interval_us;
unsigned bytes_sec = (byte_count * 1000000ULL) / interval_us;
syslog(LOG_INFO, "pkts=%i bytes=%i drops=%i", packets_sec, bytes_sec, drops);
packet_count = 0;
byte_count = 0;
drops = 0;
log_last_us = now_us;
log_time_us += log_interval_us;
}
// check for change in app IP if it is time
if (now_us > app_check_us) {
in_addr_t ip = get_app_ip(app_ip_filename);
if (app_ip != ip) {
app_ip = ip;
if (app_ip == htonl(INADDR_NONE)) {
syslog(LOG_INFO, "app disconnected");
} else {
memset(&app_addr, 0, sizeof(app_addr));
app_addr.sin_addr.s_addr = app_ip;
app_addr.sin_port = htons(app_port);
struct in_addr app_in_addr = {app_ip};
syslog(LOG_INFO, "app connected at %s", inet_ntoa(app_in_addr));
}
}
app_check_us = now_us + app_check_interval_us;
}
} // while (1)
} // main

24
sololink/flightcode/video/cleanLibs.sh Executable file
View File

@ -0,0 +1,24 @@
#!/bin/bash
RWFS=/mnt/rootfs.rw
LIBDIR=/usr/lib
if [ -e $RWFS/$LIBDIR/sndast* ] ||
[ -e $RWFS/$LIBDIR/libvpu* ] ||
[ -e $RWFS/$LIBDIR/libfsl* ] ||
[ -e $RWFS/$LIBDIR/gstrea* ] ; then
#Remove all the sculpture libraries
rm -rf $RWFS/$LIBDIR/sndast
rm -rf $RWFS/$LIBDIR/libvpu*
rm -rf $RWFS/$LIBDIR/libfslvpu*
rm -rf $RWFS/$LIBDIR/gstreamer*
rm -rf $RWFS/$LIBDIR/.*libfslvpu*
#Remove the gstreamer registry
rm -f ~/.gstreamer-0.10/registry.arm.bin
#Rebuild the gstreamer library
gst-inspect > /dev/null 2>&1
fi

33
sololink/flightcode/video/hdmi/Makefile Normal file
View File

@ -0,0 +1,33 @@
# To cross compile:
#
# Set up as usual for bitbake:
# $ . setup-environment build
#
# In the build directory:
# $ bitbake meta-ide-support
# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
#
# Now a make in this directory should work.
PKGCONFIG = `pkg-config gstreamer-0.10 --cflags --libs`
CFLAGS += $(PKGCONFIG)
CFLAGS += -Wall
all: out
out:
$(CC) $(CFLAGS) hdmiout.c -o hdmiout
clean:
$(RM) *.o *~ hdmiout
BASE := ../../..
fmt:
@python $(BASE)/tools/build/clang-format-run.py --apply
fmt-diff:
@python $(BASE)/tools/build/clang-format-run.py
.PHONY: all clean fmt fmt-diff

318
sololink/flightcode/video/hdmi/hdmiout.c Normal file
View File

@ -0,0 +1,318 @@
#include <gst/gst.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <syslog.h>
#include <netinet/in.h>
#include <arpa/inet.h>
/* For setting the alpha IOCTL */
#define MXCFB_SET_GBL_ALPHA _IOW('F', 0x21, struct mxcfb_gbl_alpha)
#define VID_PORT 5600 // The video port from app_streamer
#define OOB_PORT 5551 // The oob data port
/* Maximum length of an sprop string */
const int sprop_max_len = 256;
GstElement *pipeline;
GstElement *source;
GstElement *depayloader;
GstElement *decoder;
GstElement *sink;
/* Creates a pipeline of the form
* udpsrc ! rtph264depay ! vpudec ! mfw_isink
* and initializes all default parameters
*/
int create_pipeline(int port, char *sprop)
{
/* Build the pipeline */
pipeline = gst_pipeline_new("video_stream");
source = gst_element_factory_make("udpsrc", "vidsrc");
depayloader = gst_element_factory_make("rtph264depay", "depayloader");
decoder = gst_element_factory_make("vpudec", "decoder");
sink = gst_element_factory_make("mfw_isink", "sink");
if (!pipeline || !source || !depayloader || !decoder || !sink) {
g_printerr("One element could not be created. Exiting.\n");
return -1;
}
/* Caps for the udpsrc. rtph264depay needs these */
GstCaps *caps = gst_caps_new_simple("application/x-rtp", "media", G_TYPE_STRING, "video",
"clock-rate", G_TYPE_INT, 90000, "encoding-name",
G_TYPE_STRING, "H264", "sprop-parameter-sets",
G_TYPE_STRING, sprop, "payload", G_TYPE_INT, 96, NULL);
g_object_set(source, "caps", caps, NULL);
gst_caps_unref(caps);
/* Set the udpsrc port */
g_object_set(G_OBJECT(source), "port", port, NULL);
/* For non-choppy video output */
g_object_set(G_OBJECT(decoder), "low-latency", 1, NULL);
/* we add all elements into the pipeline */
gst_bin_add_many(GST_BIN(pipeline), source, depayloader, decoder, sink, NULL);
/* we link the elements together */
gst_element_link_many(source, depayloader, decoder, sink, NULL);
return 0;
}
/* destroys the pipeline when shutting down */
int destroy_pipeline(void)
{
gst_element_set_state(pipeline, GST_STATE_NULL);
g_object_unref(pipeline);
return 0;
}
/* Restarts the pipeline without destroying it. Accepts
* a new sprop parameter as an arguement. This assumes
* the pipeline needs to be restarted but not destroyed when
* a resolution change occurs to the pipe */
int restart_pipeline(char *sprop)
{
gst_element_set_state(pipeline, GST_STATE_NULL);
/* Caps for the udpsrc. rtph264depay needs these */
GstCaps *caps = gst_caps_new_simple("application/x-rtp", "media", G_TYPE_STRING, "video",
"clock-rate", G_TYPE_INT, 90000, "encoding-name",
G_TYPE_STRING, "H264", "sprop-parameter-sets",
G_TYPE_STRING, sprop, "payload", G_TYPE_INT, 96, NULL);
g_object_set(source, "caps", caps, NULL);
gst_caps_unref(caps);
gst_element_set_state(pipeline, GST_STATE_READY);
gst_element_set_state(pipeline, GST_STATE_PLAYING);
return 0;
}
/* Checks the status of the HDMI connection to
* determine if we should start or shut down the
* pipeline
*/
int hdmi_connected(void)
{
char buf[64];
memset(buf, 0, 64);
// Read the cable_state from the sysfs
int fd =
open("/sys/devices/soc0/soc.1/20e0000.hdmi_video/cable_state", O_RDONLY | O_NONBLOCK, 0);
if (fd < 0) {
syslog(LOG_ERR, "Unable to open cable state file");
// By default assume its connected
goto return_connected;
}
int len = 0;
len = read(fd, buf, 64);
if (len <= 0) {
syslog(LOG_ERR, "Unable to read cable state file");
close(fd);
goto return_connected;
}
// Strip the tailing return
buf[len - 1] = 0;
if (!strcmp(buf, "plugout")) {
close(fd);
return 0;
} else
close(fd);
return_connected:
return 1;
}
/* For handling ctrl-c
*/
void int_handler(int sig)
{
destroy_pipeline();
exit(0);
}
/* For setting the vsalpha. This eliminates the need for
* a VSALPHA setting in the environment variables, which
* did not always seem to work
*/
int set_alpha(void)
{
int fd;
struct mxcfb_gbl_alpha {
int enable;
int alpha;
} g_alpha;
g_alpha.alpha = 0; // alpha value
g_alpha.enable = 1;
if ((fd = open("/dev/fb0", O_RDWR)) < 0) {
syslog(LOG_ERR, "Unable to open frame buffer 0");
return -1;
}
if (ioctl(fd, MXCFB_SET_GBL_ALPHA, &g_alpha) < 0) {
syslog(LOG_ERR, "Set global alpha failed");
close(fd);
return -1;
}
close(fd);
return 0;
}
/* Open the out-of-band socket for sending and
* receiving any out-of-band data, such as a new
* resolution or an error count.
*/
int open_socket(void)
{
int fd;
struct sockaddr_in addr;
int tos = 0xFF;
/* create a UDP socket */
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
return -1;
/* Bind the socket to any IP, OOB_PORT */
memset((char *)&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons(OOB_PORT);
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
return -1;
/* Set the socket to VI priority */
if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) < 0)
return -1;
fcntl(fd, F_SETFL, O_NONBLOCK); // set to non-blocking
return fd;
}
/* Looks for data on the OOB port. If available, it
* returns the width, height and framerate of the
* stream.
*/
int oob_receive_sprop(int fd, char *sprop)
{
struct sockaddr_in addr;
socklen_t addrlen = sizeof(addr);
char buf[sprop_max_len];
memset(buf, 0, sprop_max_len);
int recvlen = recvfrom(fd, buf, sprop_max_len, 0, (struct sockaddr *)&addr, &addrlen);
if (recvlen > 0) {
/* Sanity check that we got a null terminated string */
if (buf[sprop_max_len - 1] != 0) {
syslog(LOG_INFO, "Got a bad sprop string, ignoring");
return 0;
}
memset(sprop, 0, sprop_max_len);
strcpy(sprop, buf);
}
return recvlen;
}
/* The main entry point. Arguments are currently only
* for gstreamer
*/
int main(int argc, char *argv[])
{
char sprop[sprop_max_len];
char last_sprop[sprop_max_len];
memset(sprop, 0, sprop_max_len);
memset(last_sprop, 0, sprop_max_len);
int oob_sock_fd;
/* sig handler to shut down the pipeline */
signal(SIGINT, int_handler);
openlog("hdmi", LOG_NDELAY, LOG_LOCAL3);
/* Set the alphablending off for isink */
set_alpha();
/* Sit here and wait until the HDMI is connected */
syslog(LOG_INFO, "Waiting for an HDMI connection");
while (!hdmi_connected())
sleep(1);
/* Initialize GStreamer */
syslog(LOG_INFO, "Initializing HDMI pipeline");
gst_init(&argc, &argv);
/* Open the oob socket */
oob_sock_fd = open_socket();
if (oob_sock_fd <= 0) {
syslog(LOG_ERR, "Unable to open OOB socket");
return -1;
}
/* Wait for some OOB data */
while (1) {
if (oob_receive_sprop(oob_sock_fd, sprop) > 0) {
syslog(LOG_INFO, "Received sprop");
break;
}
sleep(1);
}
memcpy(last_sprop, sprop, sprop_max_len);
/* Create the pipeline */
syslog(LOG_INFO, "Creating HDMI pipeline");
if (create_pipeline(VID_PORT, sprop) < 0)
return -1;
/* Start playing */
syslog(LOG_INFO, "Starting HDMI playing");
gst_element_set_state(pipeline, GST_STATE_PLAYING);
// Check the incoming frame size
while (1) {
// Check the HDMI connection state
if (!hdmi_connected()) {
break; // Exit and let inittab restart us
}
// Look for new data on the OOB port
if (oob_receive_sprop(oob_sock_fd, sprop) > 0) {
/* If this is a new sprop, restart the pipeline.
* We are guaranteed to have a null terminated string
* from the oob_receive_sprop call */
if (strcmp(sprop, last_sprop) != 0) {
syslog(LOG_INFO, "Received new sprop: %s", sprop);
/* Restart the pipeline */
restart_pipeline(sprop);
memset(last_sprop, 0, sizeof(last_sprop));
strcpy(last_sprop, sprop);
}
}
usleep(100000);
}
/* Free resources */
destroy_pipeline();
close(oob_sock_fd);
return 0;
}

382
sololink/flightcode/video/vid/80211.cpp Normal file
View File

@ -0,0 +1,382 @@
#include <stdio.h>
#include <unistd.h>
#include <net/if.h>
#include <syslog.h>
#include <netlink/netlink.h>
#include <netlink/genl/ctrl.h>
#include <netlink/genl/genl.h>
#include <linux/nl80211.h>
#include <sys/socket.h>
#include <sys/un.h>
/*
* Rate info, contained in station info
*
* Fields commented out are ones where linux/nl80211.h does not say what the
* type is, and we don't need right now, so they are just left out.
*/
typedef struct rate_info {
uint16_t bitrate; /* total bitrate (u16, 100kbit/s) */
uint8_t mcs; /* mcs index for 802.11n (u8) */
/* n40_mhz_width: 40 MHz dualchannel bitrate */
/* short_gi: 400ns guard interval */
uint32_t bitrate32; /* total bitrate (u32, 100kbit/s) */
/* max: highest rate_info number currently defined */
uint8_t vht_mcs; /* MCS index for VHT (u8) */
uint8_t vht_nss; /* number of streams in VHT (u8) */
/* n80_mhz_width: 80 MHz VHT rate */
/* n80p80_mhz_width: 80+80 MHz VHT rate */
/* n160_mhz_width: 160 MHz VHT rate */
} rate_info_t;
/*
* Information retrieved via station info request message
*
* Fields commented out are ones where linux/nl80211.h does not say what the
* type is, and we don't need right now, so they are just left out.
*/
typedef struct station_info {
uint32_t inactive_time; /* time since last activity (u32, msecs) */
uint32_t rx_bytes; /* total received bytes (u32, from this station) */
uint32_t tx_bytes; /* total transmitted bytes (u32, to this station) */
uint64_t rx_bytes64; /* total received bytes (u64, from this station) */
uint64_t tx_bytes64; /* total transmitted bytes (u64, to this station) */
int8_t signal; /* signal strength of last received PPDU (u8, dBm) */
rate_info_t tx_bitrate; /* current unicast tx rate, nested attribute containing
info as possible, see &enum nl80211_rate_info */
uint32_t rx_packets; /* total received packet (u32, from this station) */
uint32_t tx_packets; /* total transmitted packets (u32, to this station) */
uint32_t tx_retries; /* total retries (u32, to this station) */
uint32_t tx_failed; /* total failed packets (u32, to this station) */
int8_t signal_avg; /* signal strength average (u8, dBm) */
/* llid: the station's mesh LLID */
/* plid: the station's mesh PLID */
/* plink_state: peer link state for the station (see %enum nl80211_plink_state) */
rate_info_t rx_bitrate; /* last unicast data frame rx rate, nested attribute,
like NL80211_STA_INFO_TX_BITRATE. */
/* bss_param: current station's view of BSS, nested attribute containing info as
possible, see &enum nl80211_sta_bss_param */
/* connected_time: time since the station is last connected */
/* sta_flags: Contains a struct nl80211_sta_flag_update. */
uint32_t beacon_loss; /* count of times beacon loss was detected (u32) */
int64_t t_offset; /* timing offset with respect to this STA (s64) */
/* local_pm: local mesh STA link-specific power mode */
/* peer_pm: peer mesh STA link-specific power mode */
/* nonpeer_pm: neighbor mesh STA power save mode towards non-peer STA */
} station_info_t;
static void get_station_info(station_info_t *station_info);
static int station_handler(struct nl_msg *msg, void *arg);
static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg);
static int finish_handler(struct nl_msg *msg, void *arg);
static int ack_handler(struct nl_msg *msg, void *arg);
int get_retries(void)
{
station_info_t station_info;
static uint32_t retries_last = 0;
unsigned retries;
get_station_info(&station_info);
/* retries is logged as the difference from last time */
if (retries_last == 0)
retries = 0;
else
retries = station_info.tx_retries - retries_last;
retries_last = station_info.tx_retries;
return retries;
}
/*
* Get station info from interface via netlink
*
* This sends the request to the kernel to send us the station info,
* then waits for the response to come back. The response comes back
* as a callback (station_handler):
*
* Normal operation:
* this function sends request
* this function calls nl_recvmsgs; inside that, station_handler is called
* station_handler fills in station_info
* this function calls nl_recvmsgs; inside that, finish_handler is called
* finish_handler clears 'status'
* this function sees status is cleared and returns
*
* Almost everything returned is parsed out and available. Only a few items
* are logged, and only the signal strength is used in the mavlink message,
* but pulling them all out of the netlink message should make it easier to
* log more (or something different) if we want.
*/
static void get_station_info(station_info_t *station_info)
{
struct nl_sock *nl_sock = NULL;
int nl80211_id = -1;
unsigned int if_index = 0;
uint32_t if_index_uint32;
struct nl_msg *msg = NULL;
struct nl_cb *cmd_cb = NULL;
struct nl_cb *sock_cb = NULL;
void *hdr = NULL;
int num_bytes = -1;
/* This is static because I'm not 100% sure the callbacks that may set it
cannot get called again under some edge condition. Static is safe,
re-entrancy is irrelevant. */
static volatile int status = 0;
memset(station_info, 0, sizeof(*station_info));
nl_sock = nl_socket_alloc();
if (nl_sock == NULL) {
fprintf(stderr, "ERROR allocating netlink socket\n");
status = 1;
goto cleanup;
}
if (genl_connect(nl_sock) != 0) {
fprintf(stderr, "ERROR connecting netlink socket\n");
status = 1;
goto cleanup;
}
nl80211_id = genl_ctrl_resolve(nl_sock, "nl80211");
if (nl80211_id < 0) {
fprintf(stderr, "ERROR resolving netlink socket\n");
status = 1;
goto cleanup;
}
if_index = if_nametoindex("wlan0");
if (if_index == 0) {
fprintf(stderr, "ERROR getting interface index\n");
status = 1;
goto cleanup;
}
msg = nlmsg_alloc();
if (msg == NULL) {
fprintf(stderr, "ERROR allocating netlink message\n");
status = 1;
goto cleanup;
}
cmd_cb = nl_cb_alloc(NL_CB_DEFAULT);
if (cmd_cb == NULL) {
fprintf(stderr, "ERROR allocating netlink command callback\n");
status = 1;
goto cleanup;
}
sock_cb = nl_cb_alloc(NL_CB_DEFAULT);
if (sock_cb == NULL) {
fprintf(stderr, "ERROR allocating netlink socket callback\n");
status = 1;
goto cleanup;
}
hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, nl80211_id, 0, NLM_F_DUMP,
NL80211_CMD_GET_STATION, 0);
if (hdr == NULL) {
fprintf(stderr, "ERROR creating netlink message\n");
status = 1;
goto cleanup;
}
if_index_uint32 = if_index;
if (nla_put(msg, NL80211_ATTR_IFINDEX, sizeof(uint32_t), &if_index_uint32) != 0) {
fprintf(stderr, "ERROR setting message attribute\n");
status = 1;
goto cleanup;
}
if (nl_cb_set(cmd_cb, NL_CB_VALID, NL_CB_CUSTOM, station_handler, station_info) != 0) {
fprintf(stderr, "ERROR setting command callback\n");
status = 1;
goto cleanup;
}
nl_socket_set_cb(nl_sock, sock_cb);
num_bytes = nl_send_auto_complete(nl_sock, msg);
if (num_bytes < 0) {
fprintf(stderr, "ERROR sending netlink message\n");
status = 1;
goto cleanup;
}
status = 1;
nl_cb_err(cmd_cb, NL_CB_CUSTOM, error_handler, (void *)&status);
nl_cb_set(cmd_cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, (void *)&status);
nl_cb_set(cmd_cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, (void *)&status);
/* wait for callback to set station_info and set status=0 */
while (status == 1)
nl_recvmsgs(nl_sock, cmd_cb);
cleanup:
if (sock_cb != NULL)
nl_cb_put(sock_cb);
if (cmd_cb != NULL)
nl_cb_put(cmd_cb);
if (msg != NULL)
nlmsg_free(msg);
if (nl_sock != NULL)
nl_socket_free(nl_sock);
} /* get_station_info */
/*
* Called for each station result message that comes back from the kernel.
* We expect only one before finish_handler is called.
*/
static int station_handler(struct nl_msg *msg, void *arg)
{
station_info_t *station_info = (station_info_t *)arg;
struct nlattr *tb1[NL80211_ATTR_MAX + 1];
struct nlmsghdr *nl_hdr;
struct genlmsghdr *genl_hdr;
struct nlattr *genl_attr_data;
int genl_attr_len;
struct nlattr *tb2[NL80211_STA_INFO_MAX + 1];
static struct nla_policy policy2[NL80211_STA_INFO_MAX + 1] = {{0}};
struct nlattr *tb3[NL80211_RATE_INFO_MAX + 1];
static struct nla_policy policy3[NL80211_RATE_INFO_MAX + 1] = {{0}};
nl_hdr = nlmsg_hdr(msg);
genl_hdr = (struct genlmsghdr *)nlmsg_data(nl_hdr);
genl_attr_data = genlmsg_attrdata(genl_hdr, 0);
genl_attr_len = genlmsg_attrlen(genl_hdr, 0);
if (nla_parse(tb1, NL80211_ATTR_MAX, genl_attr_data, genl_attr_len, NULL) != 0) {
fprintf(stderr, "ERROR parsing netlink message attributes\n");
return NL_SKIP;
}
if (tb1[NL80211_ATTR_STA_INFO] == NULL) {
printf("no data\n");
return NL_SKIP;
}
if (nla_parse_nested(tb2, NL80211_STA_INFO_MAX, tb1[NL80211_ATTR_STA_INFO], policy2) != 0) {
printf("ERROR parsing netlink message nested attributes\n");
return NL_SKIP;
}
/* Description of what attributes there are is in linux/nl80211.h */
/* For each possible attribute, see if it is present in the info we
* got, and if so, copy it to our station_info_t structure. */
if (tb2[NL80211_STA_INFO_INACTIVE_TIME] != NULL)
station_info->inactive_time = nla_get_u32(tb2[NL80211_STA_INFO_INACTIVE_TIME]);
if (tb2[NL80211_STA_INFO_RX_BYTES] != NULL)
station_info->rx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_RX_BYTES]);
if (tb2[NL80211_STA_INFO_TX_BYTES] != NULL)
station_info->tx_bytes = nla_get_u32(tb2[NL80211_STA_INFO_TX_BYTES]);
if (tb2[NL80211_STA_INFO_RX_BYTES64] != NULL)
station_info->rx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_RX_BYTES64]);
if (tb2[NL80211_STA_INFO_TX_BYTES64] != NULL)
station_info->tx_bytes64 = nla_get_u64(tb2[NL80211_STA_INFO_TX_BYTES64]);
/* this appears to be signed dBm, not a u8 */
if (tb2[NL80211_STA_INFO_SIGNAL] != NULL)
station_info->signal = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL]);
/* tx_bitrate is a nested structure; the station_info points to a nested
* attribute thing that needs to be decoded */
if (tb2[NL80211_STA_INFO_TX_BITRATE] != NULL &&
nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_TX_BITRATE], policy3) ==
0) {
rate_info_t *rate_info = &station_info->tx_bitrate;
if (tb3[NL80211_RATE_INFO_BITRATE] != NULL)
rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]);
if (tb3[NL80211_RATE_INFO_MCS] != NULL)
rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]);
if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL)
rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]);
if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL)
rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]);
if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL)
rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]);
}
if (tb2[NL80211_STA_INFO_RX_PACKETS] != NULL)
station_info->rx_packets = nla_get_u32(tb2[NL80211_STA_INFO_RX_PACKETS]);
if (tb2[NL80211_STA_INFO_TX_PACKETS] != NULL)
station_info->tx_packets = nla_get_u32(tb2[NL80211_STA_INFO_TX_PACKETS]);
if (tb2[NL80211_STA_INFO_TX_RETRIES] != NULL)
station_info->tx_retries = nla_get_u32(tb2[NL80211_STA_INFO_TX_RETRIES]);
if (tb2[NL80211_STA_INFO_TX_FAILED] != NULL)
station_info->tx_failed = nla_get_u32(tb2[NL80211_STA_INFO_TX_FAILED]);
/* this appears to be signed dBm, not a u8 */
if (tb2[NL80211_STA_INFO_SIGNAL_AVG] != NULL)
station_info->signal_avg = (int8_t)nla_get_u8(tb2[NL80211_STA_INFO_SIGNAL_AVG]);
/* rx_bitrate is nested, like tx_bitrate */
if (tb2[NL80211_STA_INFO_RX_BITRATE] != NULL &&
nla_parse_nested(tb3, NL80211_RATE_INFO_MAX, tb2[NL80211_STA_INFO_RX_BITRATE], policy3) ==
0) {
rate_info_t *rate_info = &station_info->rx_bitrate;
if (tb3[NL80211_RATE_INFO_BITRATE] != NULL)
rate_info->bitrate = nla_get_u16(tb3[NL80211_RATE_INFO_BITRATE]);
if (tb3[NL80211_RATE_INFO_MCS] != NULL)
rate_info->mcs = nla_get_u8(tb3[NL80211_RATE_INFO_MCS]);
if (tb3[NL80211_RATE_INFO_BITRATE32] != NULL)
rate_info->bitrate32 = nla_get_u32(tb3[NL80211_RATE_INFO_BITRATE32]);
if (tb3[NL80211_RATE_INFO_VHT_MCS] != NULL)
rate_info->vht_mcs = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_MCS]);
if (tb3[NL80211_RATE_INFO_VHT_NSS] != NULL)
rate_info->vht_nss = nla_get_u8(tb3[NL80211_RATE_INFO_VHT_NSS]);
}
if (tb2[NL80211_STA_INFO_BEACON_LOSS] != NULL)
station_info->beacon_loss = nla_get_u32(tb2[NL80211_STA_INFO_BEACON_LOSS]);
if (tb2[NL80211_STA_INFO_T_OFFSET] != NULL)
station_info->t_offset = (int64_t)nla_get_u64(tb2[NL80211_STA_INFO_T_OFFSET]);
return NL_SKIP;
} /* station_handler */
/* Never seen this one called, but the examples have it. */
static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
{
*(int *)arg = err->error;
return NL_STOP;
}
/*
* Called after station info message is sent. The calling of this is how we
* know we're done.
*
* Other commands sent to netlink might get several responses back; this is
* more useful in those cases.
*/
static int finish_handler(struct nl_msg *msg, void *arg)
{
/* arg is a pointer to 'status' in get_station_info() */
*(int *)arg = 0;
return NL_SKIP;
}
/* Never seen this one called, but the examples have it. */
static int ack_handler(struct nl_msg *msg, void *arg)
{
*(int *)arg = 0;
return NL_STOP;
}

6
sololink/flightcode/video/vid/80211.h Normal file
View File

@ -0,0 +1,6 @@
#ifndef __80211_H__
#define __80211_H__
int get_retries(void);
#endif //__80211_H_

48
sololink/flightcode/video/vid/Makefile Normal file
View File

@ -0,0 +1,48 @@
# To cross compile:
#
# Set up as usual for bitbake:
# $ . setup-environment build
#
# In the build directory:
# $ bitbake meta-ide-support
# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
#
# Now a make in this directory should work.
PKGCONFIG = `pkg-config gstreamer-0.10 --cflags --libs`
VPATH = ../../ini ../../ini/cpp
INCS = -I../../ini -I../../ini/cpp -I$(OECORE_TARGET_SYSROOT)/usr/include/libnl3
CFLAGS += -Wall $(INCS)
CXXFLAGS += -Wall $(INCS) $(PKGCONFIG)
LIBS = -lnl-3 -lnl-genl-3
SRCS_CPP = INIReader.cpp
SRCS_CPP += vidlaunch.cpp
SRCS_CPP += 80211.cpp
SRCS_C += ini.c
OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
MAIN = vidlaunch
all: $(MAIN)
$(MAIN): $(OBJS)
$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
clean:
$(RM) *.o *~ $(MAIN) *Test
BASE := ../../..
fmt:
@python $(BASE)/tools/build/clang-format-run.py --apply
fmt-diff:
@python $(BASE)/tools/build/clang-format-run.py
.PHONY: all clean fmt fmt-diff

587
sololink/flightcode/video/vid/vidlaunch.cpp Normal file
View File

@ -0,0 +1,587 @@
#include <gst/gst.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <syslog.h>
#include <stdint.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include "80211.h"
#include "INIReader.h"
/* Default defines */
#define DEFAULT_WIDTH 1280
#define DEFAULT_HEIGHT 720
#define DEFAULT_FRAMERATE 24
#define DEFAULT_BITRATE 4000000 // 4mbps
#define ARTOO_IP "10.1.1.1" // Artoo's IP address.
#define IB_PORT 5550 // The in-band video port
#define OOB_PORT 5551 // The oob data port
#define TOS_VO 0xFF // Max VO bin for the oob port
/* The loop time in us for polling retries, etc */
#define CYCLE_TIME_US 200000
/* Convert seconds to number of loop cycles */
#define SECS_TO_CYCLES(s) (s * 1.e6 / CYCLE_TIME_US)
/* We can currently control one of two things: framerate or bitrate.
* the vary_framerate switch indicates which one to use.
*/
bool vary_framerate; // Whether or not to vary framerate as the control knob
/* The min/max bitrate and framerate values (come from sololink.conf) */
int min_framerate;
int max_framerate;
int min_bitrate;
int max_bitrate;
/* The step at which the bitrate/framerate should be incremented/decremented
* per second based on the retry count */
int framerate_step;
int bitrate_step;
/* Variable stream resolution. This means that the
* video streamed to the Artoo will vary in width/height based
* on the input resolution coming from the HDMI input.
*/
bool var_stream_res;
/* Whether or not to crop the recorded 480p resolution */
bool crop_record_res;
/* Pipeline objects */
GstElement *pipeline;
GstElement *source;
GstElement *converter;
GstElement *convcapsfilter;
GstElement *enccapsfilter;
GstElement *encoder;
GstElement *payloader;
GstElement *sink;
/* RTP timestamping */
uint32_t last_timestamp = 0;
/*Struct containing the width/height/fr/br of the stream */
struct vidresfr {
int width;
int height;
int fr;
int br;
};
/* The input resolution */
struct vidresfr input_res = {DEFAULT_WIDTH, DEFAULT_HEIGHT, DEFAULT_FRAMERATE, DEFAULT_BITRATE};
/* The streaming resolution */
struct vidresfr stream_res = {DEFAULT_WIDTH, DEFAULT_HEIGHT, DEFAULT_FRAMERATE, DEFAULT_BITRATE};
/* VBR quant values for 720p and 480p resolutions.
* This results in roughly 4mbit maxima for both
* 720p and 480p resolutions, with some extreme bitrates
* in the 5mbit range.
*/
#define QUANT_720 35
#define QUANT_480 30 // We use this for 576p (PAL) as well.
/* Looks at the input resolution and determines if the
* stream is NTSC or PAL. Returns 1 for resolutions
* greater than or equal to 720, where both NTSC and PAL
* are the same, other than framerate.
*/
#define IS_NTSC() (input_res.height >= 720 || input_res.height == 480)
/* Looks at the input resolution structure and determines
* if the input is a recording resolution (480p or 576p).
*/
#define IS_RECORD_RES() (input_res.height < 720)
/* Record the last timestamp from the payloader */
inline void record_last_timestamp(void)
{
g_object_get(G_OBJECT(payloader), "timestamp", &last_timestamp, NULL);
syslog(LOG_INFO, "Last timestamp: %u\n", last_timestamp);
}
/*Set the timestamp to last_timestamp in payloader */
inline void set_timestamp(void)
{
g_object_set(G_OBJECT(payloader), "timestamp-offset", last_timestamp, "config-interval", 1,
"pt", 96, NULL);
}
/* Creates a pipeline of the form:
* mfw_v4l2src ! mfw_ipucsc ! vpuenc ! rtph264pay ! udpsink
* with default parameters for each
*/
int create_pipeline(void)
{
/* Build the pipeline */
pipeline = gst_pipeline_new("video_stream");
source = gst_element_factory_make("mfw_v4lsrc", "vidsrc");
convcapsfilter = gst_element_factory_make("capsfilter", NULL);
converter = gst_element_factory_make("mfw_ipucsc", "converter");
enccapsfilter = gst_element_factory_make("capsfilter", NULL);
encoder = gst_element_factory_make("vpuenc", "encoder");
payloader = gst_element_factory_make("rtph264pay", "payloader");
sink = gst_element_factory_make("udpsink", "netsink");
// Make sure they all got created OK
if (!pipeline || !source || !convcapsfilter || !converter || !enccapsfilter || !encoder ||
!payloader || !sink) {
g_printerr("Element(s) could not be created. Exiting.\n");
return -1;
}
/* Set capsfilters for the mfw_ipucsc and vpuenc.
* If we are in 640x480 input mode, assume the video
* is cropped down; the GoPro letterboxes the 480p output
* when recording. Do this by taking 60 pixels from the top/bottom.
* Take 72 pixels from top/bottom if its a PAL input.
* Then, upscale on the output of the ipucsc.
*/
GstCaps *enccaps;
if (IS_RECORD_RES() && crop_record_res) {
enccaps = gst_caps_new_simple("video/x-raw-yuv", "width", G_TYPE_INT, input_res.width,
"height", G_TYPE_INT, input_res.height, "crop-top",
G_TYPE_INT, (IS_NTSC() ? 60 : 72), "crop-bottom", G_TYPE_INT,
(IS_NTSC() ? 60 : 72), NULL);
} else {
enccaps = gst_caps_new_simple("video/x-raw-yuv", "width", G_TYPE_INT, input_res.width,
"height", G_TYPE_INT, input_res.height, NULL);
}
g_object_set(convcapsfilter, "caps", enccaps, NULL);
gst_caps_unref(enccaps);
if (IS_RECORD_RES() && var_stream_res) {
enccaps = gst_caps_new_simple(
"video/x-raw-yuv", "format", GST_TYPE_FOURCC, GST_MAKE_FOURCC('I', '4', '2', '0'),
"width", G_TYPE_INT, stream_res.width,
/*This next line is ugly but, if the user wants a cropped
* recording resolution then we handle the cropped stream
* if it is NTSC or PAL resolution */
"height", G_TYPE_INT,
stream_res.height + (crop_record_res ? (IS_NTSC() ? 120 : 144) : 0), NULL);
} else {
enccaps = gst_caps_new_simple(
"video/x-raw-yuv", "format", GST_TYPE_FOURCC, GST_MAKE_FOURCC('I', '4', '2', '0'),
"width", G_TYPE_INT, stream_res.width, "height", G_TYPE_INT, stream_res.height, NULL);
}
g_object_set(enccapsfilter, "caps", enccaps, NULL);
gst_caps_unref(enccaps);
/* Set encoder parameters */
if (vary_framerate) {
g_object_set(G_OBJECT(encoder), "codec", 6, "framerate-nu", stream_res.fr,
"force-framerate", 1, "seqheader-method", 3, "cbr", 0, "quant",
(IS_RECORD_RES() ? QUANT_480 : QUANT_720), NULL);
} else {
g_object_set(G_OBJECT(encoder), "codec", 6, "framerate-nu", stream_res.fr,
"force-framerate", 1, "seqheader-method", 3, "bitrate", (int64_t)stream_res.br,
NULL);
}
/* Set sink paramters */
g_object_set(G_OBJECT(sink), "host", ARTOO_IP, "port", IB_PORT, "qos-dscp", 32, NULL);
/* Set packet type 96
* NOTE: DO NOT CHANGE PACKET TYPE WITHOUT CONSULTING
* MOBILE GROUP, SPECIFICALLY JON W.
*
* Note: config-interval was previously specified, but it appears
* that the seqheader-method=3 from vpuenc automatically sends out lots
* of sps/pps packets.
*/
g_object_set(G_OBJECT(payloader), "timestamp-offset", last_timestamp, "config-interval", 1,
"pt", 96, NULL);
/* we add all elements into the pipeline */
gst_bin_add_many(GST_BIN(pipeline), source, convcapsfilter, converter, enccapsfilter, encoder,
payloader, sink, NULL);
/* we link the elements together */
gst_element_link_many(source, convcapsfilter, converter, enccapsfilter, encoder, payloader,
sink, NULL);
return 0;
}
/* Sets a new framerate for the encoder (vpuenc) and then
* restarts the pipeline. This happens very quickly (a couple
* of frames)
*/
void set_framerate(int fr)
{
stream_res.fr = fr;
syslog(LOG_INFO, "New framerate: %ifps", fr);
record_last_timestamp();
g_object_set(G_OBJECT(encoder), "framerate-nu", stream_res.fr, "force-framerate", 1, NULL);
// Restart the pipeline
gst_element_set_state(pipeline, GST_STATE_NULL);
gst_element_set_state(pipeline, GST_STATE_READY);
set_timestamp();
gst_element_set_state(pipeline, GST_STATE_PLAYING);
}
/* Sets a new bitrate for the encoder (vpuenc) and then
* restarts the pipeline.
*/
void set_bitrate(int br)
{
stream_res.br = br;
syslog(LOG_INFO, "New bitrate: %ibps", br);
record_last_timestamp();
g_object_set(G_OBJECT(encoder), "bitrate", br, NULL);
// Restart the pipeline
gst_element_set_state(pipeline, GST_STATE_NULL);
gst_element_set_state(pipeline, GST_STATE_READY);
set_timestamp();
gst_element_set_state(pipeline, GST_STATE_PLAYING);
}
/* Shuts down and destroys the pipeline */
int destroy_pipeline(void)
{
/* Pull off the last timestamp in case the pipeline
* is being restarted.
*/
record_last_timestamp();
gst_element_set_state(pipeline, GST_STATE_NULL);
g_object_unref(pipeline);
return 0;
}
/* For catching ctrl+c */
void int_handler(int sig)
{
destroy_pipeline();
exit(0);
}
/* Open the out-of-band socket for sending and
* receiving any out-of-band data, such as a new
* resolution or an error count.
*/
int open_oob_socket(void)
{
int fd;
struct sockaddr_in addr;
int tos = TOS_VO;
/* create a UDP socket */
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
return -1;
/* Bind the socket to any IP, OOB_PORT */
memset((char *)&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons(OOB_PORT);
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
return -1;
/* Set the socket priority */
if (setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) < 0)
return -1;
fcntl(fd, F_SETFL, O_NONBLOCK); // set to non-blocking
return fd;
}
/* Send a packet over UDP with the sprop-parameter-sets as
* its payload.
*/
int send_sprop(int fd)
{
const gchar *gc;
struct sockaddr_in addr;
GstCaps *caps;
GstStructure *str;
GstPad *pad;
int len;
// Always send to ARTOO's IP on the OOB port.
memset((char *)&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
inet_aton(ARTOO_IP, &addr.sin_addr);
addr.sin_port = htons(OOB_PORT);
/*Attempt to pull of the sprop-parameter-sets from the
* payloader. If they're OK, send them along */
if ((pad = gst_element_get_pad(payloader, "src")) != NULL) {
if ((caps = GST_PAD_CAPS(pad)) != NULL) {
if ((str = gst_caps_get_structure(caps, 0)) != NULL) {
gc = gst_structure_get_string(str, "sprop-parameter-sets");
if (gc == NULL) {
syslog(LOG_ERR, "Unable to get sprop from gstreamer");
len = 0;
} else
len = strlen(gc);
if (len > 0) {
if (sendto(fd, (const char *)gc, len, 0, (struct sockaddr *)&addr,
sizeof(addr)) < len) {
syslog(LOG_ERR, "Unable to send data on OOB port\n");
return -1;
} else
return len;
}
}
}
}
/* If we sent -1 bytes its entirely possible that
* the pipeline did not have a prop to get yet */
return -1;
}
/* The main routine. Argumenets are only for gstreamer,
* currently
*/
int main(int argc, char *argv[])
{
int vid_fd;
int oob_sock_fd;
struct v4l2_frmsizeenum sizes;
uint32_t retries_sec = 0;
int good_retries = 0;
int fr;
int br;
/* Start the syslog */
openlog("video", LOG_NDELAY, LOG_LOCAL6);
syslog(LOG_INFO, "main: built " __DATE__ " " __TIME__);
/* Get the min/max framerate/bitrate values from sololink.conf */
INIReader reader("/etc/sololink.conf");
if (reader.ParseError() < 0) {
syslog(LOG_ERR, "can't load /etc/sololink.conf");
return -1;
}
min_framerate = reader.GetInteger("video", "videoMinFR", DEFAULT_FRAMERATE);
max_framerate = reader.GetInteger("video", "videoMaxFR", DEFAULT_FRAMERATE);
min_bitrate = reader.GetInteger("video", "videoMinBR", DEFAULT_BITRATE);
max_bitrate = reader.GetInteger("video", "videoMaxBR", DEFAULT_BITRATE);
framerate_step = reader.GetInteger("video", "videoFRStep", 5);
bitrate_step = reader.GetInteger("video", "videoBRStep", 500000);
var_stream_res = reader.GetBoolean("video", "varStreamRes", true);
crop_record_res = reader.GetBoolean("video", "cropRecordRes", true);
/* Right now we only support varying framerate or bitrate, not both.
* Throw an error if all values are different */
if ((min_framerate != max_framerate) && (min_bitrate != max_bitrate)) {
syslog(LOG_ERR, "framerate min/max and bitrate min/max are all different");
return -1;
}
/* Check the framerate and bitrate values */
if (min_framerate < 1 || min_framerate > 60) {
syslog(LOG_ERR, "minimum framerate is out of bounds (1-60)");
return -1;
}
if (max_framerate < 1 || max_framerate > 60) {
syslog(LOG_ERR, "maximum framerate is out of bounds (1-60)");
return -1;
}
if (max_framerate < min_framerate) {
syslog(LOG_ERR, "maximum framerate < min framerate");
return -1;
}
if (min_bitrate < 800000 || min_bitrate > 6000000) {
syslog(LOG_ERR, "minimum bitrate is out of bounds (800000-6000000)");
return -1;
}
if (max_bitrate < 800000 || max_bitrate > 6000000) {
syslog(LOG_ERR, "maximum bitrate is out of bounds (800000-6000000)");
return -1;
}
if (max_bitrate < min_bitrate) {
syslog(LOG_ERR, "maximum bitrate < min bitrate");
return -1;
}
if (framerate_step < 0 || framerate_step > 30) {
syslog(LOG_ERR, "framerate step is out of bounds (1-30)");
return -1;
}
if (bitrate_step < 100000 || bitrate_step > 2000000) {
syslog(LOG_ERR, "bitrate step is out of bounds (100000 - 2000000)");
return -1;
}
/* Determine which mode we're in (fixed framerate or fixed bitrate) */
if (max_framerate == min_framerate) {
vary_framerate =
false; // Indicate that we should use CBR but change it based on link conditions
syslog(LOG_INFO, "Operating in variable bitrate mode (%i-%i)@%ifps", min_bitrate,
max_bitrate, max_framerate);
} else if (max_bitrate == min_bitrate) {
vary_framerate = true; // Indicate that we should use VBR but vary framerate
syslog(LOG_INFO, "Operating in variable framerate mode (%i-%i)@%ibps", min_framerate,
max_framerate, max_bitrate);
}
/* Mark if we're using a variable stream resolution */
if (var_stream_res)
syslog(LOG_INFO, "Using variable streaming resolution");
/* Only support cropping in variable frame resolution */
if (!var_stream_res && crop_record_res) {
syslog(LOG_INFO,
"WARN: Cropped resolutions only supported in variable streaming resolutions");
crop_record_res = false;
}
/* Set the bitrate/framerates to the set maxima */
stream_res.fr = max_framerate;
stream_res.br = max_bitrate;
fr = max_framerate;
br = max_bitrate;
/* sigint handler to destroy the pipeline */
signal(SIGINT, int_handler);
/* Modprobe the driver */
system("modprobe mxc_v4l2_capture");
/* Initialize GStreamer */
syslog(LOG_INFO, "Initializing");
gst_init(&argc, &argv);
/* Open the oob socket */
oob_sock_fd = open_oob_socket();
if (oob_sock_fd <= 0) {
syslog(LOG_ERR, "Unable to open OOB socket");
return -1;
}
/* Create the pipeline */
syslog(LOG_INFO, "Creating pipeline");
if (create_pipeline() < 0)
return -1;
/* Start playing */
syslog(LOG_INFO, "Starting play");
gst_element_set_state(pipeline, GST_STATE_PLAYING);
/* Open the video fd for the enum_framesizes ioctl call */
vid_fd = open("/dev/video0", O_RDWR | O_NONBLOCK, 0);
if (vid_fd < 0) {
syslog(LOG_ERR, "Unable to open video device for ioctl");
destroy_pipeline();
return -1;
}
/* The main loop */
while (1) {
/* Check the incoming frame size */
sizes.index = 0;
/* Call the ioctl to get the input resolution */
if (ioctl(vid_fd, VIDIOC_ENUM_FRAMESIZES, &sizes) < 0)
syslog(LOG_ERR, "Unable to call VIDIOC_ENUM_FRAMESIZES ioctl");
else {
if (sizes.discrete.width != (unsigned)input_res.width ||
sizes.discrete.height != (unsigned)input_res.height) {
syslog(LOG_INFO, "new size: %ix%i", sizes.discrete.width, sizes.discrete.height);
/* Set the input size */
input_res.width = sizes.discrete.width;
input_res.height = sizes.discrete.height;
if (var_stream_res) {
/* Set the streaming resolution to the incoming size for now */
stream_res.width = input_res.width;
stream_res.height = input_res.height;
}
/* Destroy the pipeline */
syslog(LOG_INFO, "Restarting pipeline");
destroy_pipeline();
/* Re-create the pipeline */
create_pipeline();
/* Start playing */
gst_element_set_state(pipeline, GST_STATE_PLAYING);
}
}
/* Notify the Artoo of the latest resolution. We do this once per cycle */
send_sprop(oob_sock_fd);
/* The framerate/bitrate control is pretty simple. Any time we see
* a retry/sec > 200 we drop the framerate/bitrate by a step. If its
* already at the minimum, we leave it there. If the retries
* are < 200 for 5s we increase the framerate/bitrate by one step up
* to the max.
*/
retries_sec = get_retries() * 1e6 / CYCLE_TIME_US;
if (retries_sec > 200) {
/* Reduce the framerate/bitrate by a step if its not already at the
* minimum value */
if (vary_framerate && stream_res.fr > min_framerate) {
fr = stream_res.fr - framerate_step;
if (fr < min_framerate)
fr = min_framerate;
} else if (!vary_framerate && stream_res.br > min_bitrate) {
br = stream_res.br - bitrate_step;
if (br < min_bitrate)
br = min_bitrate;
}
good_retries = 0;
}
/* If the good_retries is not at the max, increment it */
else if (good_retries < SECS_TO_CYCLES(3)) {
if (++good_retries >= SECS_TO_CYCLES(3)) {
/* Increase the framerate/bitrate by a step if its not already at the
* maximum value */
if (vary_framerate && stream_res.fr < max_framerate) {
fr = stream_res.fr + 2 * framerate_step;
if (fr > max_framerate)
fr = max_framerate;
} else if (!vary_framerate && stream_res.br < max_bitrate) {
br = stream_res.br + 2 * bitrate_step;
if (br > max_bitrate)
br = max_bitrate;
}
/* Reset the good retries so we keep counting up */
good_retries = 0;
}
}
/* Finally, set the actual framerate/bitrate */
if (vary_framerate && stream_res.fr != fr)
set_framerate(fr);
if (!vary_framerate && stream_res.br != br)
set_bitrate(br);
usleep(CYCLE_TIME_US);
}
/* Should never get here. */
/* Free resources */
destroy_pipeline();
return 0;
}