Commit 161fbd08 authored by Timothy Stack's avatar Timothy Stack

Keep a circular buffer of the last 30 frames from mezzanine to assist

in debugging.  The frames are dumped to disk on SIGHUP or if an
MTP_SNAPSHOT packet is received.  Fortunately, we can do this since
there is just barely enough CPU and RAM to do this for all four
cameras on a machine.
parent b0df3886
......@@ -8,6 +8,7 @@
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <assert.h>
#include <string.h>
#include <stdlib.h>
......@@ -15,15 +16,20 @@
#include <signal.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <zlib.h>
#include "log.h"
#include "mtp.h"
#include <math.h>
#define RECORD_FRAME_COUNT 30
#define MAX_CLIENT_COUNT 10
#if defined(HAVE_MEZZANINE)
......@@ -51,6 +57,7 @@ typedef struct {
typedef struct {
double time;
int calibrate;
mezz_objectlist_t objectlist;
} mezz_mmap_t;
......@@ -76,6 +83,10 @@ static int debug = 0;
*/
static volatile int looping = 1;
static volatile int do_snapshot = 0;
static int snapshot_start;
static int snapshot_frame = 0;
/**
* Counter used to track when a new frame was received from mezzanine.
*/
......@@ -89,6 +100,10 @@ static struct robot_position offsets;
static double z_offset = 0.0;
static mezz_mmap_t *recorded_frames[RECORD_FRAME_COUNT];
static char *recordpath = "/var/log/mezzframe";
static XDR xdr;
static char packet_buffer[2048];
static char *cursor;
......@@ -192,6 +207,23 @@ void local2global_posit_trans(struct robot_position *p_inout)
*p_inout = rp;
}
static void record_frame(mezz_mmap_t *mm)
{
int frame_number;
assert(mm != NULL);
frame_number = (mezz_frame_count - 1) % RECORD_FRAME_COUNT;
memcpy(recorded_frames[frame_number], mm, sizeof(mezz_mmap_t));
recorded_frames[frame_number]->calibrate = -1;
}
static void sighup(int sig)
{
snapshot_start = mezz_frame_count;
do_snapshot = 1;
}
/**
* Encode any object identified by mezzanine as mtp packets in the given
* buffer.
......@@ -344,7 +376,7 @@ int main(int argc, char *argv[])
xdrrec_create(&xdr, 0, 0, NULL, NULL, mem_write);
while ((c = getopt(argc, argv, "hdp:l:i:f:x:y:z:o:")) != -1) {
while ((c = getopt(argc, argv, "hdr:p:l:i:f:x:y:z:o:")) != -1) {
switch (c) {
case 'h':
usage();
......@@ -359,6 +391,9 @@ int main(int argc, char *argv[])
case 'i':
pidfile = optarg;
break;
case 'r':
recordpath = optarg;
break;
case 'p':
if (sscanf(optarg, "%d", &port) != 1) {
error("error: -p option is not a number: %s\n", optarg);
......@@ -420,6 +455,8 @@ int main(int argc, char *argv[])
signal(SIGQUIT, sigquit);
signal(SIGTERM, sigquit);
signal(SIGINT, sigquit);
signal(SIGHUP, sighup);
signal(SIGPIPE, SIG_IGN);
......@@ -445,6 +482,8 @@ int main(int argc, char *argv[])
exit(2);
}
mezzmap = mezz_mmap();
// We want mezzanine to copy the image data over.
mezzmap->calibrate += 1;
}
#else
{
......@@ -510,14 +549,21 @@ int main(int argc, char *argv[])
}
else {
mtp_handle_t mh[MAX_CLIENT_COUNT];
int lpc, last_mezz_frame = 0;
fd_set readfds, clientfds;
int last_mezz_frame = 0;
FD_ZERO(&readfds);
FD_ZERO(&clientfds); /* We'll track clients using this fd_set. */
FD_SET(serv_sock, &readfds);
for (lpc = 0; lpc < RECORD_FRAME_COUNT; lpc++) {
if ((recorded_frames[lpc] = malloc(sizeof(mezz_mmap_t))) == NULL) {
fatal("cannot allocate space for recorded frames\n");
}
mlock(recorded_frames[lpc], sizeof(mezz_mmap_t));
}
while (looping) { /* The main loop. */
fd_set rreadyfds;
int rc;
......@@ -592,11 +638,20 @@ int main(int argc, char *argv[])
error("invalid client %d %p\n", rc, mp);
}
else {
if (mp.data.opcode == MTP_CONFIG_VMC_CLIENT) {
switch (mp.data.opcode) {
case MTP_CONFIG_VMC_CLIENT:
offsets.x = mp.data.mtp_payload_u.
config_vmc_client.fixed_x;
offsets.y = mp.data.mtp_payload_u.
config_vmc_client.fixed_y;
break;
case MTP_SNAPSHOT:
info("doing snapshot\n");
snapshot_start = mezz_frame_count;
do_snapshot = 1;
break;
default:
break;
}
good = 1;
}
......@@ -616,32 +671,69 @@ int main(int argc, char *argv[])
* Check the current frame count against the last one so we
* can tell if there really is a new frame ready.
*/
if (mezz_frame_count != last_mezz_frame) {
if (debug > 1) {
info("vmc-client: new frame\n");
}
do {
if (mezz_frame_count != last_mezz_frame) {
if (debug > 1) {
info("vmc-client: new frame\n");
}
record_frame(mezzmap);
if ((rc = encode_packets(mezzmap)) == -1) {
errorc("error: unable to encode packets");
}
else if (rc == 0) {
if (debug) {
info("vmc-client: nothing to send %d\n",
mezz_frame_count);
if ((rc = encode_packets(mezzmap)) == -1) {
errorc("error: unable to encode packets");
}
else if (rc == 0) {
if (debug) {
info("vmc-client: nothing to send %d\n",
mezz_frame_count);
}
}
else {
int lpc;
for (lpc = 0; lpc < FD_SETSIZE; lpc++) {
if (FD_ISSET(lpc, &clientfds)) {
write(lpc, packet_buffer, rc);
}
}
}
last_mezz_frame = mezz_frame_count;
}
else {
int lpc;
for (lpc = 0; lpc < FD_SETSIZE; lpc++) {
if (FD_ISSET(lpc, &clientfds)) {
write(lpc, packet_buffer, rc);
}
}
}
last_mezz_frame = mezz_frame_count;
}
if (snapshot_frame >= RECORD_FRAME_COUNT) {
snapshot_frame = 0;
do_snapshot = 0;
}
else if (do_snapshot) {
char filename[128];
int fd;
snprintf(filename, sizeof(filename),
"%s.%03d",
recordpath, snapshot_frame);
if ((fd = open(filename,
O_WRONLY|O_CREAT|O_TRUNC,
0644)) == -1) {
error("cannot open snapshot file %s\n",
filename);
}
else {
int frame_number;
frame_number =
(snapshot_start + snapshot_frame) %
RECORD_FRAME_COUNT;
write(fd,
recorded_frames[frame_number],
sizeof(mezz_mmap_t));
close(fd);
fd = -1;
}
snapshot_frame += 1;
}
} while (do_snapshot);
break;
default:
errorc("unhandled select error\n");
......@@ -652,6 +744,7 @@ int main(int argc, char *argv[])
}
#if defined(HAVE_MEZZANINE)
mezzmap->calibrate -= 1;
mezz_term(0);
#endif
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment