Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[vision] add computer vision library in standard modules + 2 modules

Browse Source 
- video RTP stream
- image download

for ARDrone2
This commit is contained in:
Gautier Hattenberger
2014-10-11 23:19:30 +02:00
parent a31a006333
commit 9ce18f8146
29 changed files with 3203 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/computer_vision/cv/color.h
+93
View File
@@ -0,0 +1,93 @@
/*
* Copyright (C) 2012-2013
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdint.h>
#include "image.h"
inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output);
inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
source++;
for (int y=0;y<output->h;y++)
{
for (int x=0;x<output->w;x++)
{
// UYVY
*dest++ = 127; // U
*dest++ = *source; // Y
source+=2;
}
}
}
inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
{
int cnt = 0;
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
for (int y=0;y<output->h;y++)
{
for (int x=0;x<output->w;x+=2)
{
// Color Check:
if (
// Light
(dest[1] >= y_m)
&& (dest[1] <= y_M)
&& (dest[0] >= u_m)
&& (dest[0] <= u_M)
&& (dest[2] >= v_m)
&& (dest[2] <= v_M)
)// && (dest[2] > 128))
{
cnt ++;
// UYVY
dest[0] = 64; // U
dest[1] = source[1]; // Y
dest[2] = 255; // V
dest[3] = source[3]; // Y
}
else
{
// UYVY
char u = source[0]-127;
u/=4;
dest[0] = 127; // U
dest[1] = source[1]; // Y
u = source[2]-127;
u/=4;
dest[2] = 127; // V
dest[3] = source[3]; // Y
}
dest+=4;
source+=4;
}
}
return cnt;
}
sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
+1102
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/cv/encoding/jpeg.h
+45
View File
@@ -0,0 +1,45 @@
#ifndef __MY_JPEG_HEADER__
#define __MY_JPEG_HEADER__
#include <stdint.h>
#define FOUR_ZERO_ZERO 0
#define FOUR_TWO_ZERO 1
#define FOUR_TWO_TWO 2
#define FOUR_FOUR_FOUR 3
#define RGB 4
unsigned char *encode_image (
uint8_t* in,
uint8_t* out,
uint32_t q, // image quality 1-8
uint32_t fmt, // image format code
uint32_t width, // image width
uint32_t height, // image height
uint8_t add_dri_header // data only or full jpeg file
);
unsigned char *encode_image_rtp (
uint8_t* in,
uint8_t* out,
uint32_t q, // image quality 1-8
uint32_t fmt, // image format code
uint32_t width, // image width
uint32_t height, // image height
uint8_t add_dri_header // data only or full jpeg file
);
unsigned char *encode_image_std_qt (
uint8_t* in,
uint8_t* out,
uint32_t q, // image quality 0 to 99
uint32_t fmt, // image format code
uint32_t width, // image width
uint32_t height, // image height
uint8_t add_dri_header // data only or full jpeg file
);
int create_svs_jpeg_header(unsigned char* buf, int32_t size, int w);
#endif
sw/airborne/modules/computer_vision/cv/encoding/rtp.c
+196
View File
@@ -0,0 +1,196 @@
#include <stdint.h>
#include <string.h>
#include <sys/time.h>
#include "udp/socket.h"
#include "rtp.h"
void send_rtp_packet(struct UdpSocket *sock, uint8_t* Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header);
// http://www.ietf.org/rfc/rfc3550.txt
#define KJpegCh1ScanDataLen 32
#define KJpegCh2ScanDataLen 56
// RGB JPEG images as RTP payload - 64x48 pixel
uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen] =
{
0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd,
0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
};
uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] =
{
0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc,
0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
};
void test_rtp_frame(struct UdpSocket *sock)
{
static uint32_t framecounter = 0;
static uint32_t timecounter = 0;
static uint8_t toggle = 0;
toggle = ! toggle;
uint8_t format_code = 0x01;
uint8_t quality_code = 0x54;
if (toggle)
{
send_rtp_packet(sock, JpegScanDataCh2A,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
}
else
{
send_rtp_packet(sock, JpegScanDataCh2B,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
}
framecounter++;
timecounter+=3600;
}
void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
{
static uint32_t packetcounter = 0;
static uint32_t timecounter = 0;
uint32_t offset = 0;
#define MAX_PACKET_SIZE 1400
if (delta_t <= 0)
{
struct timeval tv;
gettimeofday(&tv, 0);
uint32_t t = (tv.tv_sec % (256*256)) * 90000 + tv.tv_usec * 9 / 100;
timecounter = t;
}
// Split frame into packets
for (;JpegLen > 0;)
{
uint32_t len = MAX_PACKET_SIZE;
uint8_t lastpacket = 0;
if (JpegLen <= len)
{
lastpacket = 1;
len = JpegLen;
}
send_rtp_packet(sock, Jpeg,len,packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
JpegLen -= len;
Jpeg += len;
offset += len;
packetcounter++;
}
if (delta_t > 0)
{
// timestamp = 1 / 90 000 seconds
timecounter+=delta_t;
}
}
/*
* The RTP timestamp is in units of 90000Hz. The same timestamp MUST
appear in each fragment of a given frame. The RTP marker bit MUST be
set in the last packet of a frame.
*
*/
void send_rtp_packet(
struct UdpSocket *sock,
uint8_t * Jpeg, int JpegLen,
uint32_t m_SequenceNumber, uint32_t m_Timestamp,
uint32_t m_offset, uint8_t marker_bit,
int w, int h,
uint8_t format_code, uint8_t quality_code,
uint8_t has_dri_header)
{
#define KRtpHeaderSize 12 // size of the RTP header
#define KJpegHeaderSize 8 // size of the special JPEG payload header
uint8_t RtpBuf[2048];
int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
memset(RtpBuf,0x00,sizeof(RtpBuf));
/*
The RTP header has the following format:
0 1 2 3
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|V=2|P|X| CC |M| PT | sequence number |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| timestamp |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| synchronization source (SSRC) identifier |
+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
| contributing source (CSRC) identifiers |
| .... |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* */
// Prepare the 12 byte RTP header
RtpBuf[0] = 0x80; // RTP version
RtpBuf[1] = 0x1a + (marker_bit<<7); // JPEG payload (26) and marker bit
RtpBuf[2] = m_SequenceNumber >> 8;
RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
RtpBuf[7] = (m_Timestamp & 0x000000FF);
RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
RtpBuf[10] = 0x7e;
RtpBuf[11] = 0x67;
/* JPEG header", are as follows:
*
* http://tools.ietf.org/html/rfc2435
0 1 2 3
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type-specific | Fragment Offset |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Q | Width | Height |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*/
// Prepare the 8 byte payload JPEG header
RtpBuf[12] = 0x00; // type specific
RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
RtpBuf[15] = (m_offset & 0x000000FF);
RtpBuf[16] = 0x00; // type: 0 422 or 1 421
RtpBuf[17] = 60; // quality scale factor
RtpBuf[16] = format_code; // type: 0 422 or 1 421
if (has_dri_header)
RtpBuf[16] |= 0x40; // DRI flag
RtpBuf[17] = quality_code; // quality scale factor
RtpBuf[18] = w/8; // width / 8 -> 48 pixel
RtpBuf[19] = h/8; // height / 8 -> 32 pixel
// append the JPEG scan data to the RTP buffer
memcpy(&RtpBuf[20],Jpeg,JpegLen);
udp_write(sock,RtpBuf,RtpPacketSize);
};
sw/airborne/modules/computer_vision/cv/encoding/rtp.h
+17
View File
@@ -0,0 +1,17 @@
#include <stdint.h>
#include "udp/socket.h"
void send_rtp_frame(
struct UdpSocket *sock, // socket
uint8_t * Jpeg, uint32_t JpegLen, // jpeg data
int w, int h, // width and height
uint8_t format_code, // 0=422, 1=421
uint8_t quality_code, // 0-99 of 128 for custom (include
uint8_t has_dri_header, // Does Jpeg data include Header Info?
uint32_t delta_t // time step 90kHz
);
void test_rtp_frame(struct UdpSocket *sock);
sw/airborne/modules/computer_vision/cv/image.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2012-2013
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef _MY_IMAGE_HEADER_
#define _MY_IMAGE_HEADER_
struct img_struct {
int seq;
double timestamp;
unsigned char *buf;
int w;
int h;
};
#endif
sw/airborne/modules/computer_vision/cv/resize.h
+50
View File
@@ -0,0 +1,50 @@
/*
* Copyright (C) 2012-2013
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <stdint.h>
#include "image.h"
inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample);
inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample)
{
uint8_t *source = input->buf;
uint8_t *dest = output->buf;
int pixelskip = downsample-1;
for (int y=0;y<output->h;y++)
{
for (int x=0;x<output->w;x+=2)
{
// YUYV
*dest++ = *source++; // U
*dest++ = *source++; // Y
// now skip 3 pixels
if (pixelskip) source+=(pixelskip+1)*2;
*dest++ = *source++; // U
*dest++ = *source++; // V
if (pixelskip) source+=(pixelskip-1)*2;
}
// skip 3 rows
if (pixelskip) source += pixelskip * input->w * 2;
}
}
sw/airborne/modules/computer_vision/image_nc_send.c
+208
View File
@@ -0,0 +1,208 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/image_nc_send.c
*
* Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
*/
// Own header
#include "modules/computer_vision/image_nc_send.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/wait.h>
// Video
#include "modules/computer_vision/lib/v4l/video.h"
#include "modules/computer_vision/cv/resize.h"
#include "modules/computer_vision/cv/encoding/jpeg.h"
// Threaded computer vision
#include <pthread.h>
// Default netcat server IP (destination)
#ifndef IMAGE_SERVER_IP
#define IMAGE_SERVER_IP "192.168.1.2"
#endif
// Default netcat port
#ifndef IMAGE_SERVER_PORT
#define IMAGE_SERVER_PORT 4900
#endif
// Default downsize factor
#ifndef IMAGE_DOWNSIZE_FACTOR
#define IMAGE_DOWNSIZE_FACTOR 1
#endif
// JPEG compression quality factor from 0 to 99 (99=high)
#ifndef IMAGE_QUALITY_FACTOR
#define IMAGE_QUALITY_FACTOR 99
#endif
// Frame Per Second
#ifndef IMAGE_FPS
#define IMAGE_FPS (1./6.)
#endif
// Save images by default
#ifndef IMAGE_SAVE
#define IMAGE_SAVE 1
#endif
void image_nc_send_run(void) {}
/////////////////////////////////////////////////////////////////////////
// COMPUTER VISION THREAD
pthread_t computervision_thread;
volatile uint8_t computervision_thread_status = 0;
volatile uint8_t computer_vision_thread_command = 0;
void *computervision_thread_main(void* data);
void *computervision_thread_main(void* data)
{
// Video Input
struct vid_struct vid;
vid.device = (char*)"/dev/video1";
vid.w=1280;
vid.h=720;
vid.n_buffers = 4;
if (video_init(&vid)<0) {
printf("Error initialising video\n");
computervision_thread_status = -1;
return 0;
}
// Frame Grabbing
struct img_struct* img_new = video_create_image(&vid);
// Frame Resizing
uint8_t quality_factor = IMAGE_QUALITY_FACTOR;
uint8_t dri_jpeg_header = 1;
struct img_struct small;
small.w = vid.w / IMAGE_DOWNSIZE_FACTOR;
small.h = vid.h / IMAGE_DOWNSIZE_FACTOR;
small.buf = (uint8_t*)malloc(small.w*small.h*2);
// Commpressed image buffer
uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
// file index (search from 0)
int file_index = 0;
int microsleep = (int)(1000000. / IMAGE_FPS);
while (computer_vision_thread_command > 0)
{
usleep(microsleep);
video_grab_image(&vid, img_new);
// Resize
resize_uyuv(img_new, &small, IMAGE_DOWNSIZE_FACTOR);
// JPEG encode the image:
uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header);
uint32_t size = end-(jpegbuf);
#if IMAGE_SAVE
FILE* save;
char save_name[128];
if (system("mkdir -p /data/video/images") == 0) {
// search available index (max is 99)
for ( ; file_index < 99; file_index++) {
printf("search %d\n",file_index);
sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
// test if file exists or not
if (access(save_name, F_OK) == -1) {
printf("access\n");
save = fopen(save_name, "w");
if (save != NULL) {
fwrite(jpegbuf, sizeof(uint8_t), size, save);
fclose(save);
}
else {
printf("Error when opening file %s\n", save_name);
}
// leave for loop
break;
}
else {printf("file exists\n");}
}
}
#endif
// Fork process
int status;
pid_t pid = fork();
if (pid == 0) {
// Open process to send using netcat in child process
char nc_cmd[64];
sprintf(nc_cmd, "nc %s %d", IMAGE_SERVER_IP, IMAGE_SERVER_PORT);
FILE* netcat;
netcat = popen(nc_cmd, "w");
if (netcat != NULL) {
fwrite(jpegbuf, sizeof(uint8_t), size, netcat);
if (pclose(netcat) == 0) {
printf("Sending image succesfully\n");
}
}
else {
printf("Fail sending image\n");
}
exit(0);
}
else if (pid < 0) {
printf("Fork failed\n");
}
else {
// Parent is waiting for child to terminate
wait(&status);
}
}
printf("Thread Closed\n");
video_close(&vid);
computervision_thread_status = -100;
return 0;
}
void image_nc_send_start(void)
{
computer_vision_thread_command = 1;
int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
}
}
void image_nc_send_stop(void)
{
computer_vision_thread_command = 0;
}
sw/airborne/modules/computer_vision/image_nc_send.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/image_nc_send.h
*
* Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
*/
#ifndef IMAGE_NC_SEND_H
#define IMAGE_NC_SEND_H
// Module functions
extern void image_nc_send_run(void);
extern void image_nc_send_start(void);
extern void image_nc_send_stop(void);
#endif /* IMAGE_NC_SEND_H */
sw/airborne/modules/computer_vision/image_nc_send_nps.c
+31
View File
@@ -0,0 +1,31 @@
/*
* Copyright (C) 2014 Gautier Hattenberger
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
// Own header
#include "image_capture.h"
void image_capture_run(void) {}
void image_capture_start(void) {}
void image_capture_stop(void) {}
sw/airborne/modules/computer_vision/lib/udp/socket.c
+103
View File
@@ -0,0 +1,103 @@
#include "socket.h"
#include <netdb.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <unistd.h>
#include <netdb.h>
#include <errno.h>
#include <netinet/in.h>
# define ADDR_SIZE_TYPE socklen_t
# define SOCKET_ERROR -1
# define IO_SOCKET ioctl
struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast)
{
struct UdpSocket* me = malloc(sizeof(struct UdpSocket));
int so_reuseaddr = 1;
me->socket_out = socket( AF_INET, SOCK_DGRAM, 0);
setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
/* only set broadcast option if explicitly enabled */
if (broadcast)
setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST,
&broadcast, sizeof(broadcast));
me->addr_out.sin_family = AF_INET;
me->addr_out.sin_port = htons(port_out);
me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out);
me->socket_in = socket( AF_INET, SOCK_DGRAM, 0);
setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
me->addr_in.sin_family = AF_INET;
me->addr_in.sin_port = htons(port_in);
me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in));
return me;
}
#include <stdio.h>
//#define UDP_MODE MSG_DONTWAIT
#define UDP_MODE 0
int udp_write(struct UdpSocket* me, unsigned char* buf, int len) {
sendto(me->socket_out, buf, len, UDP_MODE,
(struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
//printf("sendto ret=%d\n",ret);
return len;
}
unsigned long MIN(unsigned long a, unsigned long b);
unsigned long MIN(unsigned long a, unsigned long b)
{
if (a<b) return a;
return b;
}
int udp_read(struct UdpSocket* me, unsigned char* buf, int len)
{
unsigned long toread = 0;
int btr = 1; // set to >0 in order to start the reading loop
int newbytes = 0;
int status;
// if socket is connected
for (;btr>0;)
{
// Check Status
status = IO_SOCKET(me->socket_in, FIONREAD, &toread);
if(status == SOCKET_ERROR) {
printf("problem receiving from socket\n");
break;
}
//printf("UDP has %d bytes\n", toread);
if (toread <= 0)
break;
// If status: ok and new data: read it
btr = MIN(toread,(unsigned long)len);
recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr*)&me->addr_in, (socklen_t *) sizeof(me->addr_in) );
newbytes += btr;
}
return newbytes;
}
sw/airborne/modules/computer_vision/lib/udp/socket.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef SOCKET_H
#define SOCKET_H
#include <sys/socket.h>
#include <arpa/inet.h>
#define FMS_UNICAST 0
#define FMS_BROADCAST 1
struct UdpSocket {
int socket_in;
int socket_out;
struct sockaddr_in addr_in;
struct sockaddr_in addr_out;
};
extern struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast);
extern int udp_write(struct UdpSocket* me, unsigned char* buf, int len);
extern int udp_read(struct UdpSocket* me, unsigned char* buf, int len);
#endif /* SOCKET_H */
sw/airborne/modules/computer_vision/lib/v4l/video.c
+233
View File
@@ -0,0 +1,233 @@
/*
video.c - video driver
Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
MA 02110-1301 USA.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <asm/types.h>
#include <linux/videodev2.h>
#include <pthread.h>
#include "video.h"
#define CLEAR(x) memset (&(x), 0, sizeof (x))
pthread_t video_thread;
void *video_thread_main(void* data);
void *video_thread_main(void* data)
{
struct vid_struct* vid = (struct vid_struct*)data;
printf("video_thread_main started\n");
while (1) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(vid->fd, &fds);
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno) continue;
printf("select err\n");
}
if (0 == r) {
fprintf(stderr, "select timeout\n");
return 0;
}
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(vid->fd, VIDIOC_DQBUF, &buf) < 0) {
printf("ioctl() VIDIOC_DQBUF failed.\n");
break;
}
assert(buf.index < vid->n_buffers);
vid->seq++;
if(vid->trigger) {
// todo add timestamp again
//vid->img->timestamp = util_timestamp();
vid->img->seq = vid->seq;
memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2);
vid->trigger=0;
}
if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
printf("ioctl() VIDIOC_QBUF failed.\n");
break;
}
}
return 0;
}
int video_init(struct vid_struct *vid)
{
struct v4l2_capability cap;
struct v4l2_format fmt;
unsigned int i;
enum v4l2_buf_type type;
vid->seq=0;
vid->trigger=0;
if(vid->n_buffers==0) vid->n_buffers=4;
vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0);
if (ioctl(vid->fd, VIDIOC_QUERYCAP, &cap) < 0) {
printf("ioctl() VIDIOC_QUERYCAP failed.\n");
return -1;
}
//printf("2 driver = %s, card = %s, version = %d, capabilities = 0x%x\n", cap.driver, cap.card, cap.version, cap.capabilities);
CLEAR(fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = vid->w;
fmt.fmt.pix.height = vid->h;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
if (ioctl(vid->fd, VIDIOC_S_FMT, &fmt) < 0) {
printf("ioctl() VIDIOC_S_FMT failed.\n");
return -1;
}
//image_size = fmt.fmt.pix.width * fmt.fmt.pix.height *3/2;
struct v4l2_requestbuffers req;
CLEAR(req);
req.count = vid->n_buffers;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (ioctl(vid->fd, VIDIOC_REQBUFS, &req) < 0) {
printf("ioctl() VIDIOC_REQBUFS failed.\n");
return -1;
}
printf("Buffer count = %d\n", vid->n_buffers);
vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
for (i = 0; i < vid->n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (ioctl(vid->fd, VIDIOC_QUERYBUF, &buf) < 0) {
printf("ioctl() VIDIOC_QUERYBUF failed.\n");
return -1;
}
vid->buffers[i].length = buf.length;
printf("buffer%d.length=%d\n",i,buf.length);
vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
if (MAP_FAILED == vid->buffers[i].buf) {
printf ("mmap() failed.\n");
return -1;
}
}
for (i = 0; i < vid->n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
printf("ioctl() VIDIOC_QBUF failed.\n");
return -1;
}
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
printf("ioctl() VIDIOC_STREAMON failed.\n");
return -1;
}
//start video thread
int rc = pthread_create(&video_thread, NULL, video_thread_main, vid);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
return 202;
}
return 0;
}
void video_close(struct vid_struct *vid)
{
int i;
for (i = 0; i < (int)vid->n_buffers; ++i) {
if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n");
}
close(vid->fd);
}
struct img_struct *video_create_image(struct vid_struct *vid)
{
struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct));
img->w=vid->w;
img->h=vid->h;
img->buf = (unsigned char*)malloc(vid->h*vid->w*2);
return img;
}
pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER;
void video_grab_image(struct vid_struct *vid, struct img_struct *img) {
pthread_mutex_lock(&video_grab_mutex);
vid->img = img;
vid->trigger=1;
// while(vid->trigger) pthread_yield();
while(vid->trigger) usleep(1);
pthread_mutex_unlock(&video_grab_mutex);
}
sw/airborne/modules/computer_vision/lib/v4l/video.h
+53
View File
@@ -0,0 +1,53 @@
/*
video.h - video driver
Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
MA 02110-1301 USA.
*/
#ifndef _VIDEO_H
#define _VIDEO_H
#include "../../cv/image.h"
struct buffer_struct {
void * buf;
size_t length;
};
struct vid_struct {
char *device;
int w;
int h;
int seq;
unsigned int n_buffers;
//private members
int trigger;
struct img_struct *img;
struct buffer_struct * buffers;
int fd;
};
int video_init(struct vid_struct *vid);
struct img_struct *video_create_image(struct vid_struct *vid);
void video_grab_image(struct vid_struct *vid, struct img_struct *img);
void video_close(struct vid_struct *vid);
#endif
sw/airborne/modules/computer_vision/viewvideo.c
+251
View File
@@ -0,0 +1,251 @@
/*
* Copyright (C) 2012-2014 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/viewvideo.c
*
* Get live images from a RTP/UDP stream
* and save pictures on internal memory
*
* Works on Linux platforms
*/
// Own header
#include "modules/computer_vision/viewvideo.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/time.h>
// UDP RTP Images
#include "modules/computer_vision/lib/udp/socket.h"
// Video
#include "modules/computer_vision/lib/v4l/video.h"
#include "modules/computer_vision/cv/resize.h"
#include "modules/computer_vision/cv/encoding/jpeg.h"
#include "modules/computer_vision/cv/encoding/rtp.h"
// Threaded computer vision
#include <pthread.h>
// Default broadcast IP
#ifndef VIDEO_SOCK_IP
#define VIDEO_SOCK_IP "192.168.1.255"
#endif
// Output socket can be defined from an offset
#ifdef VIDEO_SOCK_OUT_OFFSET
#define VIDEO_SOCK_OUT (5000+VIDEO_SOCK_OUT_OFFSET)
#endif
#ifndef VIDEO_SOCK_OUT
#define VIDEO_SOCK_OUT 5000
#endif
#ifndef VIDEO_SOCK_IN
#define VIDEO_SOCK_IN 4999
#endif
// Downsize factor for video stream
#ifndef VIDEO_DOWNSIZE_FACTOR
#define VIDEO_DOWNSIZE_FACTOR 4
#endif
// From 0 to 99 (99=high)
#ifndef VIDEO_QUALITY_FACTOR
#define VIDEO_QUALITY_FACTOR 50
#endif
// Frame Per Seconds
#ifndef VIDEO_FPS
#define VIDEO_FPS 4.
#endif
void viewvideo_run(void) {}
// take shot flag
int viewvideo_shot = 0;
/////////////////////////////////////////////////////////////////////////
// COMPUTER VISION THREAD
pthread_t computervision_thread;
volatile uint8_t computervision_thread_status = 0;
volatile uint8_t computer_vision_thread_command = 0;
void *computervision_thread_main(void* data);
void *computervision_thread_main(void* data)
{
// Video Input
struct vid_struct vid;
vid.device = (char*)"/dev/video1";
vid.w=1280;
vid.h=720;
vid.n_buffers = 4;
if (video_init(&vid)<0) {
printf("Error initialising video\n");
computervision_thread_status = -1;
return 0;
}
// Video Grabbing
struct img_struct* img_new = video_create_image(&vid);
// Video Resizing
uint8_t quality_factor = VIDEO_QUALITY_FACTOR;
uint8_t dri_jpeg_header = 0;
int microsleep = (int)(1000000. / VIDEO_FPS);
struct img_struct small;
small.w = vid.w / VIDEO_DOWNSIZE_FACTOR;
small.h = vid.h / VIDEO_DOWNSIZE_FACTOR;
small.buf = (uint8_t*)malloc(small.w*small.h*2);
// Video Compression
uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
// Network Transmit
struct UdpSocket* vsock;
vsock = udp_socket(VIDEO_SOCK_IP, VIDEO_SOCK_OUT, VIDEO_SOCK_IN, FMS_BROADCAST);
// Create SPD file and make folder if necessary
FILE* sdp;
if (system("mkdir -p /data/video/sdp") == 0) {
sdp = fopen("/data/video/sdp/x86_config-mjpeg.sdp","w");
if (sdp != NULL) {
fprintf(sdp, "v=0\n");
fprintf(sdp, "m=video %d RTP/AVP 26\n", (int)(VIDEO_SOCK_OUT));
fprintf(sdp, "c=IN IP4 0.0.0.0");
fclose(sdp);
}
}
// file index (search from 0)
int file_index = 0;
// time
struct timeval last_time;
gettimeofday(&last_time, NULL);
while (computer_vision_thread_command > 0)
{
// compute usleep to have a more stable frame rate
struct timeval time;
gettimeofday(&time, NULL);
int dt = (int)(time.tv_sec - last_time.tv_sec)*1000000 + (int)(time.tv_usec - last_time.tv_usec);
if (dt < microsleep) usleep(microsleep - dt);
last_time = time;
// Grab new frame
video_grab_image(&vid, img_new);
// Save picture on disk
if (computer_vision_thread_command == 2)
{
uint8_t* end = encode_image (img_new->buf, jpegbuf, 99, FOUR_TWO_TWO, vid.w, vid.h, 1);
uint32_t size = end-(jpegbuf);
FILE* save;
char save_name[128];
if (system("mkdir -p /data/video/images") == 0) {
// search available index (max is 99)
for ( ; file_index < 99; file_index++) {
printf("search %d\n",file_index);
sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
// test if file exists or not
if (access(save_name, F_OK) == -1) {
printf("access\n");
save = fopen(save_name, "w");
if (save != NULL) {
fwrite(jpegbuf, sizeof(uint8_t), size, save);
fclose(save);
}
else {
printf("Error when opening file %s\n", save_name);
}
// leave for loop
break;
} else {
printf("file exists\n");
}
}
}
computer_vision_thread_command = 1;
viewvideo_shot = 0;
}
// Resize
resize_uyuv(img_new, &small, VIDEO_DOWNSIZE_FACTOR);
// JPEG encode the image:
uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header);
uint32_t size = end - (jpegbuf);
// Send image with RTP
printf("Sending an image ...%u\n",size);
send_rtp_frame(
vsock, // UDP
jpegbuf,size, // JPEG
small.w, small.h, // Img Size
0, // Format 422
quality_factor, // Jpeg-Quality
dri_jpeg_header, // DRI Header
1 // 90kHz time increment
);
// Extra note: when the time increment is set to 0,
// it is automaticaly calculated by the send_rtp_frame function
// based on gettimeofday value. This seems to introduce some lag or jitter.
// An other way is to compute the time increment and set the correct value.
// It seems that a lower value is also working (when the frame is received
// the timestamp is always "late" so the frame is displayed immediately).
// Here, we set the time increment to the lowest possible value
// (1 = 1/90000 s) which is probably stupid but is actually working.
}
printf("Thread Closed\n");
video_close(&vid);
computervision_thread_status = -100;
return 0;
}
void viewvideo_start(void)
{
computer_vision_thread_command = 1;
int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
}
}
void viewvideo_stop(void)
{
computer_vision_thread_command = 0;
}
int viewvideo_save_shot(void)
{
if (computer_vision_thread_command > 0) {
computer_vision_thread_command = 2;
}
return 0;
}
sw/airborne/modules/computer_vision/viewvideo.h
+50
View File
@@ -0,0 +1,50 @@
/*
* Copyright (C) 2012-2014 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/viewvideo.h
*
* Get live images from a RTP/UDP stream
* and save pictures on internal memory
*
* Works on Linux platforms
*/
#ifndef VIEW_VIDEO_H
#define VIEW_VIDEO_H
// Module functions
extern void viewvideo_run(void);
extern void viewvideo_start(void);
extern void viewvideo_stop(void);
// Save picture on disk at full resolution
// can be called from flight plan
extern int viewvideo_save_shot(void);
extern int viewvideo_shot;
#define viewvideo_SaveShot(_v) { \
viewvideo_shot = 1; \
viewvideo_save_shot(); \
}
#endif /* VIEW_VIDEO_H */
sw/airborne/modules/computer_vision/viewvideo_nps.c
+39
View File
@@ -0,0 +1,39 @@
/*
* Copyright (C) 2012-2013
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** Dummy C implementation for simulation
*/
// Own header
#include "viewvideo.h"
int viewvideo_shot = 0;
void viewvideo_run(void) {}
void viewvideo_start(void) {}
void viewvideo_stop(void) {}
int viewvideo_save_shot(void)
{
return 0;
}
sw/tools/parrot/ardrone2/dump_images.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/bin/bash
# base address for ARDrone2
ADDR_BASE=192.168.1.
# tmp folder for images files (relative path by default)
IMAGES_DIR=${PAPARAZZI_HOME=../../../..}/var/images_tmp
# test if a complete IP address is passed as first argument or just the last digit
if [ `grep -c '\.' <<< $1` == 1 ]
then
ADDR=$1
else
ADDR=$ADDR_BASE$1
fi
# download images folder from ARDrone
mkdir -p $IMAGES_DIR/$ADDR
../ardrone2.py --host=$ADDR download_dir $IMAGES_DIR/$ADDR images
sw/tools/parrot/ardrone2/start_avplay.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/bin/bash
# base address for ARDrone2
ADDR_BASE=192.168.1.
# test if a complete IP address is passed as first argument or just the last digit
if [ `grep -c '\.' <<< $1` == 1 ]
then
ADDR=$1
PORT=$2
else
ADDR=$ADDR_BASE$1
PORT=$((5000+$1))
fi
pid=0
echo "Start video for $ADDR on port $PORT"
/usr/bin/avplay -loglevel quiet -max_delay 50 -fflags nobuffer rtp://$ADDR:$PORT
sw/tools/parrot/ardrone2/start_mplayer.sh
Executable
+50
View File
@@ -0,0 +1,50 @@
#!/bin/bash
# base address for ARDrone2
ADDR_BASE=192.168.1.
# tmp folder for sdp files (relative path by default)
SDP_DIR=${PAPARAZZI_HOME=../../../..}/var/sdp_tmp
# test if a complete IP address is passed as first argument or just the last digit
if [ `grep -c '\.' <<< $1` == 1 ]
then
ADDR=$1
else
ADDR=$ADDR_BASE$1
fi
pid=0
function quit {
echo "Stop video"
if [ $pid > 0 ]
then
kill -9 $pid
fi
exit 0
}
# trap control+c to stop mplayer
trap quit SIGINT
# fetch sdp file on the ARDrone
mkdir -p $SDP_DIR/$ADDR
../ardrone2.py --host=$ADDR download_file $SDP_DIR/$ADDR/x86_config-mjpeg.sdp sdp
if [ ! -f $SDP_DIR/$ADDR/x86_config-mjpeg.sdp ];
then
echo "Unable to download sdp file from $ADDR"
exit 0
fi
# start mplayer and respawn if needed
echo "Start video"
while [ 1 ]
do
/usr/bin/mplayer -really-quiet $SDP_DIR/$ADDR/x86_config-mjpeg.sdp&
pid=$!
wait $pid
echo "Restart video"
done