mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
Code style (new)
This commit is contained in:
dewagter
@@ -40,8 +40,10 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output)
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
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;
|
||||
|
||||
@@ -433,7 +433,8 @@ void MakeTables(int q)
|
||||
|
||||
|
||||
|
||||
uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
|
||||
uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format,
|
||||
uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
|
||||
{
|
||||
uint16_t i, j;
|
||||
|
||||
|
||||
@@ -7,7 +7,9 @@
|
||||
|
||||
#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);
|
||||
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
|
||||
|
||||
@@ -49,16 +51,19 @@ void test_rtp_frame(struct UdpSocket *sock)
|
||||
uint8_t quality_code = 0x54;
|
||||
|
||||
if (toggle) {
|
||||
send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
|
||||
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);
|
||||
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)
|
||||
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;
|
||||
@@ -83,7 +88,8 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int
|
||||
len = JpegLen;
|
||||
}
|
||||
|
||||
send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
|
||||
send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code,
|
||||
has_dri_header);
|
||||
|
||||
JpegLen -= len;
|
||||
Jpeg += len;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
Copyright (c) 2006, 2008 Edward Rosten
|
||||
Copyright(c) 2006, 2008 Edward Rosten
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
@@ -6,25 +6,25 @@ modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
|
||||
*Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
*Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
*Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
*Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and / or other materials provided with the distribution.
|
||||
|
||||
*Neither the name of the University of Cambridge nor the names of
|
||||
its contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
*Neither the name of the University of Cambridge nor the names of
|
||||
its contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
||||
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
||||
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
||||
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
EXEMPLARY, OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
|
||||
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
||||
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT(INCLUDING
|
||||
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -7,16 +7,16 @@ modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
|
||||
*Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
*Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
*Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
*Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
|
||||
*Neither the name of the University of Cambridge nor the names of
|
||||
its contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
*Neither the name of the University of Cambridge nor the names of
|
||||
its contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
@@ -34,18 +34,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#ifndef FAST_H
|
||||
#define FAST_H
|
||||
|
||||
typedef struct { int x, y; } xyFAST;
|
||||
typedef struct { int x, y; } xyFAST;
|
||||
typedef unsigned char byte;
|
||||
|
||||
int fast9_corner_score(const byte* p, const int pixel[], int bstart);
|
||||
int fast9_corner_score(const byte *p, const int pixel[], int bstart);
|
||||
|
||||
xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
|
||||
xyFAST *fast9_detect(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners);
|
||||
|
||||
int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b);
|
||||
int *fast9_score(const byte *i, int stride, xyFAST *corners, int num_corners, int b);
|
||||
|
||||
xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
|
||||
xyFAST *fast9_detect_nonmax(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners);
|
||||
|
||||
xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax);
|
||||
xyFAST *nonmax_suppression(const xyFAST *corners, const int *scores, int num_corners, int *ret_num_nonmax);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -80,7 +80,8 @@ void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height)
|
||||
|
||||
}
|
||||
|
||||
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor)
|
||||
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size,
|
||||
int subpixel_factor)
|
||||
{
|
||||
int x, y, x_0, y_0, x_0_or, y_0_or, i, j, window_size, alpha_x, alpha_y, max_x, max_y;
|
||||
//int printed, limit;
|
||||
@@ -305,7 +306,8 @@ int calculateError(int *ImC, int width, int height)
|
||||
return error;
|
||||
}
|
||||
|
||||
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
|
||||
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
|
||||
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
|
||||
{
|
||||
// A straightforward one-level implementation of Lucas-Kanade.
|
||||
// For all points:
|
||||
@@ -363,7 +365,8 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
|
||||
//printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]);
|
||||
|
||||
// if the pixel is outside the ROI in the image, do not track it:
|
||||
if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
|
||||
if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor
|
||||
&& p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
|
||||
// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
|
||||
status[p] = 0;
|
||||
}
|
||||
@@ -415,7 +418,10 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
|
||||
//printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y);
|
||||
//printf("it = %d;", it);
|
||||
// if the pixel goes outside the ROI in the image, stop tracking:
|
||||
if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor) && p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor) && p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
|
||||
if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor)
|
||||
&& p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor
|
||||
&& p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor)
|
||||
&& p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
|
||||
// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
|
||||
status[p] = 0;
|
||||
break;
|
||||
|
||||
@@ -30,12 +30,14 @@
|
||||
#define OPTIC
|
||||
void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height);
|
||||
void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height);
|
||||
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor);
|
||||
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size,
|
||||
int subpixel_factor);
|
||||
void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size);
|
||||
int getSumPatch(int *Patch, int size);
|
||||
int calculateG(int *G, int *DX, int *DY, int half_window_size);
|
||||
int calculateError(int *ImC, int width, int height);
|
||||
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
|
||||
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
|
||||
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
|
||||
void quick_sort(float *a, int n);
|
||||
void quick_sort_int(int *a, int n);
|
||||
void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH);
|
||||
|
||||
@@ -136,7 +136,8 @@ int atan_zelf(int y, int x)
|
||||
|
||||
if (x < 0) { x = -x; }
|
||||
if (y < 0) { y = -y; }
|
||||
flip = 0; if (x < y) { flip = 1; t = x; x = y; y = t; }
|
||||
flip = 0;
|
||||
if (x < y) { flip = 1; t = x; x = y; y = t; }
|
||||
if (x == 0) { return 90; }
|
||||
|
||||
xy = (y * 1000) / x;
|
||||
|
||||
@@ -26,7 +26,7 @@ inline void paparazzi_message_send(void)
|
||||
{
|
||||
udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz));
|
||||
int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst));
|
||||
printf("read: %d \n",ret);
|
||||
printf("read: %d \n", ret);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -22,7 +22,8 @@
|
||||
# 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 *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));
|
||||
|
||||
|
||||
@@ -43,10 +43,10 @@
|
||||
|
||||
|
||||
pthread_t video_thread;
|
||||
void *video_thread_main(void* data);
|
||||
void *video_thread_main(void* data)
|
||||
void *video_thread_main(void *data);
|
||||
void *video_thread_main(void *data)
|
||||
{
|
||||
struct vid_struct* vid = (struct vid_struct*)data;
|
||||
struct vid_struct *vid = (struct vid_struct *)data;
|
||||
printf("video_thread_main started\n");
|
||||
while (1) {
|
||||
fd_set fds;
|
||||
@@ -61,7 +61,7 @@ void *video_thread_main(void* data)
|
||||
r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
|
||||
|
||||
if (-1 == r) {
|
||||
if (EINTR == errno) continue;
|
||||
if (EINTR == errno) { continue; }
|
||||
printf("select err\n");
|
||||
}
|
||||
|
||||
@@ -84,13 +84,13 @@ void *video_thread_main(void* data)
|
||||
|
||||
vid->seq++;
|
||||
|
||||
if(vid->trigger) {
|
||||
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;
|
||||
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) {
|
||||
@@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid)
|
||||
unsigned int i;
|
||||
enum v4l2_buf_type type;
|
||||
|
||||
vid->seq=0;
|
||||
vid->trigger=0;
|
||||
if(vid->n_buffers==0) vid->n_buffers=4;
|
||||
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);
|
||||
|
||||
@@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid)
|
||||
|
||||
printf("Buffer count = %d\n", vid->n_buffers);
|
||||
|
||||
vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
|
||||
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;
|
||||
@@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid)
|
||||
}
|
||||
|
||||
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);
|
||||
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");
|
||||
printf("mmap() failed.\n");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
@@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid)
|
||||
}
|
||||
|
||||
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||
if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
|
||||
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) {
|
||||
if (rc) {
|
||||
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
|
||||
return 202;
|
||||
}
|
||||
@@ -207,27 +207,28 @@ 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");
|
||||
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);
|
||||
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) {
|
||||
void video_grab_image(struct vid_struct *vid, struct img_struct *img)
|
||||
{
|
||||
pthread_mutex_lock(&video_grab_mutex);
|
||||
vid->img = img;
|
||||
vid->trigger=1;
|
||||
vid->trigger = 1;
|
||||
// while(vid->trigger) pthread_yield();
|
||||
while(vid->trigger) usleep(1);
|
||||
while (vid->trigger) { usleep(1); }
|
||||
pthread_mutex_unlock(&video_grab_mutex);
|
||||
}
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
#include "../../cv/image.h"
|
||||
|
||||
struct buffer_struct {
|
||||
void * buf;
|
||||
void *buf;
|
||||
size_t length;
|
||||
};
|
||||
|
||||
@@ -39,7 +39,7 @@ struct vid_struct {
|
||||
//private members
|
||||
int trigger;
|
||||
struct img_struct *img;
|
||||
struct buffer_struct * buffers;
|
||||
struct buffer_struct *buffers;
|
||||
int fd;
|
||||
};
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
void dummyFunction(void) {
|
||||
return;
|
||||
void dummyFunction(void)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -65,102 +65,85 @@ float Vely_Int;
|
||||
float Error_Velx;
|
||||
float Error_Vely;
|
||||
|
||||
#define CMD_OF_SAT 1500 // 40 deg = 2859.1851
|
||||
#define CMD_OF_SAT 1500 // 40 deg = 2859.1851
|
||||
unsigned char saturateX = 0, saturateY = 0;
|
||||
unsigned int set_heading;
|
||||
|
||||
void init_hover_stabilization_onvision()
|
||||
{
|
||||
INT_EULERS_ZERO(cmd_euler);
|
||||
INT_EULERS_ZERO(cmd_euler);
|
||||
|
||||
activate_opticflow_hover = VISION_HOVER;
|
||||
vision_phi_pgain = VISION_PHI_PGAIN;
|
||||
vision_phi_igain = VISION_PHI_IGAIN;
|
||||
vision_theta_pgain = VISION_THETA_PGAIN;
|
||||
vision_theta_igain = VISION_THETA_IGAIN;
|
||||
vision_desired_vx = VISION_DESIRED_VX;
|
||||
vision_desired_vy = VISION_DESIRED_VY;
|
||||
activate_opticflow_hover = VISION_HOVER;
|
||||
vision_phi_pgain = VISION_PHI_PGAIN;
|
||||
vision_phi_igain = VISION_PHI_IGAIN;
|
||||
vision_theta_pgain = VISION_THETA_PGAIN;
|
||||
vision_theta_igain = VISION_THETA_IGAIN;
|
||||
vision_desired_vx = VISION_DESIRED_VX;
|
||||
vision_desired_vy = VISION_DESIRED_VY;
|
||||
|
||||
set_heading = 1;
|
||||
set_heading = 1;
|
||||
|
||||
Error_Velx = 0;
|
||||
Error_Vely = 0;
|
||||
Velx_Int = 0;
|
||||
Vely_Int = 0;
|
||||
Error_Velx = 0;
|
||||
Error_Vely = 0;
|
||||
Velx_Int = 0;
|
||||
Vely_Int = 0;
|
||||
}
|
||||
|
||||
void run_hover_stabilization_onvision(void)
|
||||
{
|
||||
if(autopilot_mode == AP_MODE_VISION_HOVER)
|
||||
{
|
||||
run_opticflow_hover();
|
||||
}
|
||||
else
|
||||
{
|
||||
Velx_Int = 0;
|
||||
Vely_Int = 0;
|
||||
}
|
||||
if (autopilot_mode == AP_MODE_VISION_HOVER) {
|
||||
run_opticflow_hover();
|
||||
} else {
|
||||
Velx_Int = 0;
|
||||
Vely_Int = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void run_opticflow_hover(void)
|
||||
{
|
||||
if(flow_count)
|
||||
{
|
||||
Error_Velx = Velx - vision_desired_vx;
|
||||
Error_Vely = Vely - vision_desired_vy;
|
||||
}
|
||||
else
|
||||
{
|
||||
Error_Velx = 0;
|
||||
Error_Vely = 0;
|
||||
}
|
||||
if (flow_count) {
|
||||
Error_Velx = Velx - vision_desired_vx;
|
||||
Error_Vely = Vely - vision_desired_vy;
|
||||
} else {
|
||||
Error_Velx = 0;
|
||||
Error_Vely = 0;
|
||||
}
|
||||
|
||||
if(saturateX==0)
|
||||
{
|
||||
if(activate_opticflow_hover==TRUE)
|
||||
{
|
||||
Velx_Int += vision_theta_igain*Error_Velx;
|
||||
}
|
||||
else
|
||||
{
|
||||
Velx_Int += vision_theta_igain*V_body.x;
|
||||
}
|
||||
}
|
||||
if(saturateY==0)
|
||||
{
|
||||
if(activate_opticflow_hover==TRUE)
|
||||
{
|
||||
Vely_Int += vision_phi_igain*Error_Vely;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vely_Int += vision_phi_igain*V_body.y;
|
||||
}
|
||||
}
|
||||
if (saturateX == 0) {
|
||||
if (activate_opticflow_hover == TRUE) {
|
||||
Velx_Int += vision_theta_igain * Error_Velx;
|
||||
} else {
|
||||
Velx_Int += vision_theta_igain * V_body.x;
|
||||
}
|
||||
}
|
||||
if (saturateY == 0) {
|
||||
if (activate_opticflow_hover == TRUE) {
|
||||
Vely_Int += vision_phi_igain * Error_Vely;
|
||||
} else {
|
||||
Vely_Int += vision_phi_igain * V_body.y;
|
||||
}
|
||||
}
|
||||
|
||||
if(set_heading)
|
||||
{
|
||||
cmd_euler.psi = stateGetNedToBodyEulers_i()->psi;
|
||||
set_heading = 0;
|
||||
}
|
||||
if (set_heading) {
|
||||
cmd_euler.psi = stateGetNedToBodyEulers_i()->psi;
|
||||
set_heading = 0;
|
||||
}
|
||||
|
||||
if(activate_opticflow_hover==TRUE)
|
||||
{
|
||||
cmd_euler.phi = - (vision_phi_pgain*Error_Vely + Vely_Int);
|
||||
cmd_euler.theta = (vision_theta_pgain*Error_Velx + Velx_Int);
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd_euler.phi = - (vision_phi_pgain*V_body.y + Vely_Int);
|
||||
cmd_euler.theta = (vision_theta_pgain*V_body.x + Velx_Int);
|
||||
}
|
||||
if (activate_opticflow_hover == TRUE) {
|
||||
cmd_euler.phi = - (vision_phi_pgain * Error_Vely + Vely_Int);
|
||||
cmd_euler.theta = (vision_theta_pgain * Error_Velx + Velx_Int);
|
||||
} else {
|
||||
cmd_euler.phi = - (vision_phi_pgain * V_body.y + Vely_Int);
|
||||
cmd_euler.theta = (vision_theta_pgain * V_body.x + Velx_Int);
|
||||
}
|
||||
|
||||
saturateX = 0; saturateY = 0;
|
||||
if(cmd_euler.phi<-CMD_OF_SAT){cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;}
|
||||
else if(cmd_euler.phi>CMD_OF_SAT){cmd_euler.phi = CMD_OF_SAT; saturateX = 1;}
|
||||
if(cmd_euler.theta<-CMD_OF_SAT){cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;}
|
||||
else if(cmd_euler.theta>CMD_OF_SAT){cmd_euler.theta = CMD_OF_SAT;saturateY = 1;}
|
||||
saturateX = 0; saturateY = 0;
|
||||
if (cmd_euler.phi < -CMD_OF_SAT) {cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;}
|
||||
else if (cmd_euler.phi > CMD_OF_SAT) {cmd_euler.phi = CMD_OF_SAT; saturateX = 1;}
|
||||
if (cmd_euler.theta < -CMD_OF_SAT) {cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;}
|
||||
else if (cmd_euler.theta > CMD_OF_SAT) {cmd_euler.theta = CMD_OF_SAT; saturateY = 1;}
|
||||
|
||||
stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
|
||||
DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
|
||||
stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
|
||||
DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi,
|
||||
&cmd_euler.theta);
|
||||
}
|
||||
|
||||
@@ -95,238 +95,216 @@ struct FloatVect3 V_body;
|
||||
// Called by plugin
|
||||
void my_plugin_init(void)
|
||||
{
|
||||
// Initialize variables
|
||||
gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
|
||||
prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char));
|
||||
prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
|
||||
x = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
new_x = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
y = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
new_y = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
status = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
dx = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
dy = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
old_img_init = 1;
|
||||
OFx = 0.0;
|
||||
OFy = 0.0;
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
diff_roll = 0.0;
|
||||
diff_pitch = 0.0;
|
||||
cam_h = 0.0;
|
||||
prev_pitch = 0.0;
|
||||
prev_roll = 0.0;
|
||||
curr_pitch = 0.0;
|
||||
curr_roll = 0.0;
|
||||
curr_yaw = 0.0;
|
||||
OFx_trans = 0.0;
|
||||
OFy_trans = 0.0;
|
||||
Velx = 0.0;
|
||||
Vely = 0.0;
|
||||
// Initialize variables
|
||||
gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char));
|
||||
prev_frame = (unsigned char *) calloc(imgWidth * imgHeight * 2, sizeof(unsigned char));
|
||||
prev_gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char));
|
||||
x = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
new_x = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
y = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
new_y = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
status = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
dx = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
dy = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
old_img_init = 1;
|
||||
OFx = 0.0;
|
||||
OFy = 0.0;
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
diff_roll = 0.0;
|
||||
diff_pitch = 0.0;
|
||||
cam_h = 0.0;
|
||||
prev_pitch = 0.0;
|
||||
prev_roll = 0.0;
|
||||
curr_pitch = 0.0;
|
||||
curr_roll = 0.0;
|
||||
curr_yaw = 0.0;
|
||||
OFx_trans = 0.0;
|
||||
OFy_trans = 0.0;
|
||||
Velx = 0.0;
|
||||
Vely = 0.0;
|
||||
}
|
||||
|
||||
void my_plugin_run(unsigned char *frame)
|
||||
{
|
||||
if(old_img_init == 1)
|
||||
{
|
||||
memcpy(prev_frame,frame,imgHeight*imgWidth*2);
|
||||
CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight);
|
||||
old_img_init = 0;
|
||||
}
|
||||
if (old_img_init == 1) {
|
||||
memcpy(prev_frame, frame, imgHeight * imgWidth * 2);
|
||||
CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight);
|
||||
old_img_init = 0;
|
||||
}
|
||||
|
||||
// ***********************************************************************************************************************
|
||||
// Additional information from other sensors
|
||||
// ***********************************************************************************************************************
|
||||
// ***********************************************************************************************************************
|
||||
// Additional information from other sensors
|
||||
// ***********************************************************************************************************************
|
||||
|
||||
// Compute body velocities from ENU
|
||||
V_Ned.x = stateGetSpeedNed_f()->x;
|
||||
V_Ned.y = stateGetSpeedNed_f()->y;
|
||||
V_Ned.z = stateGetSpeedNed_f()->z;
|
||||
// Compute body velocities from ENU
|
||||
V_Ned.x = stateGetSpeedNed_f()->x;
|
||||
V_Ned.y = stateGetSpeedNed_f()->y;
|
||||
V_Ned.z = stateGetSpeedNed_f()->z;
|
||||
|
||||
struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f();
|
||||
FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions);
|
||||
RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned);
|
||||
struct FloatQuat *BodyQuaternions = stateGetNedToBodyQuat_f();
|
||||
FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body, *BodyQuaternions);
|
||||
RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned);
|
||||
|
||||
// ***********************************************************************************************************************
|
||||
// Corner detection
|
||||
// ***********************************************************************************************************************
|
||||
// ***********************************************************************************************************************
|
||||
// Corner detection
|
||||
// ***********************************************************************************************************************
|
||||
|
||||
// FAST corner detection
|
||||
int fast_threshold = 20;
|
||||
xyFAST* pnts_fast;
|
||||
pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count);
|
||||
// FAST corner detection
|
||||
int fast_threshold = 20;
|
||||
xyFAST *pnts_fast;
|
||||
pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count);
|
||||
|
||||
if(count > MAX_COUNT) count = MAX_COUNT;
|
||||
for(int i = 0; i < count; i++)
|
||||
{
|
||||
x[i] = pnts_fast[i].x;
|
||||
y[i] = pnts_fast[i].y;
|
||||
}
|
||||
free(pnts_fast);
|
||||
if (count > MAX_COUNT) { count = MAX_COUNT; }
|
||||
for (int i = 0; i < count; i++) {
|
||||
x[i] = pnts_fast[i].x;
|
||||
y[i] = pnts_fast[i].y;
|
||||
}
|
||||
free(pnts_fast);
|
||||
|
||||
// Remove neighbouring corners
|
||||
min_distance = 3;
|
||||
min_distance2 = min_distance*min_distance;
|
||||
int *labelmin;
|
||||
labelmin = (int *) calloc(MAX_COUNT,sizeof(int));
|
||||
for(int i = 0; i < count; i++)
|
||||
{
|
||||
for(int j = i+1; j < count; j++)
|
||||
{
|
||||
// distance squared:
|
||||
distance2 = (x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]);
|
||||
if(distance2 < min_distance2)
|
||||
{
|
||||
labelmin[i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Remove neighbouring corners
|
||||
min_distance = 3;
|
||||
min_distance2 = min_distance * min_distance;
|
||||
int *labelmin;
|
||||
labelmin = (int *) calloc(MAX_COUNT, sizeof(int));
|
||||
for (int i = 0; i < count; i++) {
|
||||
for (int j = i + 1; j < count; j++) {
|
||||
// distance squared:
|
||||
distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
|
||||
if (distance2 < min_distance2) {
|
||||
labelmin[i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int count_fil = count;
|
||||
for(int i = count-1; i >= 0; i-- )
|
||||
{
|
||||
remove_point = 0;
|
||||
int count_fil = count;
|
||||
for (int i = count - 1; i >= 0; i--) {
|
||||
remove_point = 0;
|
||||
|
||||
if(labelmin[i])
|
||||
{
|
||||
remove_point = 1;
|
||||
}
|
||||
if (labelmin[i]) {
|
||||
remove_point = 1;
|
||||
}
|
||||
|
||||
if(remove_point)
|
||||
{
|
||||
for(c = i; c <count_fil-1; c++)
|
||||
{
|
||||
x[c] = x[c+1];
|
||||
y[c] = y[c+1];
|
||||
}
|
||||
count_fil--;
|
||||
}
|
||||
}
|
||||
if (remove_point) {
|
||||
for (c = i; c < count_fil - 1; c++) {
|
||||
x[c] = x[c + 1];
|
||||
y[c] = y[c + 1];
|
||||
}
|
||||
count_fil--;
|
||||
}
|
||||
}
|
||||
|
||||
if(count_fil>max_count) count_fil = max_count;
|
||||
count = count_fil;
|
||||
free(labelmin);
|
||||
if (count_fil > max_count) { count_fil = max_count; }
|
||||
count = count_fil;
|
||||
free(labelmin);
|
||||
|
||||
// **********************************************************************************************************************
|
||||
// Corner Tracking
|
||||
// **********************************************************************************************************************
|
||||
CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight);
|
||||
// **********************************************************************************************************************
|
||||
// Corner Tracking
|
||||
// **********************************************************************************************************************
|
||||
CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight);
|
||||
|
||||
error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100);
|
||||
error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status,
|
||||
5, 100);
|
||||
|
||||
flow_count = count_fil;
|
||||
for(int i=count_fil-1; i>=0; i--)
|
||||
{
|
||||
remove_point = 1;
|
||||
flow_count = count_fil;
|
||||
for (int i = count_fil - 1; i >= 0; i--) {
|
||||
remove_point = 1;
|
||||
|
||||
if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) ||
|
||||
new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery)))
|
||||
{
|
||||
remove_point = 0;
|
||||
}
|
||||
if (status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth - 1 - borderx) ||
|
||||
new_y[i] < bordery || new_y[i] > (imgHeight - 1 - bordery))) {
|
||||
remove_point = 0;
|
||||
}
|
||||
|
||||
if(remove_point)
|
||||
{
|
||||
for(c = i; c <flow_count-1; c++)
|
||||
{
|
||||
x[c] = x[c+1];
|
||||
y[c] = y[c+1];
|
||||
new_x[c] = new_x[c+1];
|
||||
new_y[c] = new_y[c+1];
|
||||
}
|
||||
flow_count--;
|
||||
}
|
||||
}
|
||||
if (remove_point) {
|
||||
for (c = i; c < flow_count - 1; c++) {
|
||||
x[c] = x[c + 1];
|
||||
y[c] = y[c + 1];
|
||||
new_x[c] = new_x[c + 1];
|
||||
new_y[c] = new_y[c + 1];
|
||||
}
|
||||
flow_count--;
|
||||
}
|
||||
}
|
||||
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
|
||||
// Optical Flow Computation
|
||||
for(int i=0; i<flow_count; i++)
|
||||
{
|
||||
dx[i] = new_x[i] - x[i];
|
||||
dy[i] = new_y[i] - y[i];
|
||||
}
|
||||
// Optical Flow Computation
|
||||
for (int i = 0; i < flow_count; i++) {
|
||||
dx[i] = new_x[i] - x[i];
|
||||
dy[i] = new_y[i] - y[i];
|
||||
}
|
||||
|
||||
// Median Filter
|
||||
if(flow_count)
|
||||
{
|
||||
quick_sort_int(dx,flow_count); // 11
|
||||
quick_sort_int(dy,flow_count); // 11
|
||||
// Median Filter
|
||||
if (flow_count) {
|
||||
quick_sort_int(dx, flow_count); // 11
|
||||
quick_sort_int(dy, flow_count); // 11
|
||||
|
||||
dx_sum = (float) dx[flow_count/2];
|
||||
dy_sum = (float) dy[flow_count/2];
|
||||
}
|
||||
else
|
||||
{
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
}
|
||||
dx_sum = (float) dx[flow_count / 2];
|
||||
dy_sum = (float) dy[flow_count / 2];
|
||||
} else {
|
||||
dx_sum = 0.0;
|
||||
dy_sum = 0.0;
|
||||
}
|
||||
|
||||
// Flow Derotation
|
||||
curr_pitch = stateGetNedToBodyEulers_f()->theta;
|
||||
curr_roll = stateGetNedToBodyEulers_f()->phi;
|
||||
curr_yaw = stateGetNedToBodyEulers_f()->psi;
|
||||
// Flow Derotation
|
||||
curr_pitch = stateGetNedToBodyEulers_f()->theta;
|
||||
curr_roll = stateGetNedToBodyEulers_f()->phi;
|
||||
curr_yaw = stateGetNedToBodyEulers_f()->psi;
|
||||
|
||||
diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H;
|
||||
diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W;
|
||||
diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H;
|
||||
diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W;
|
||||
|
||||
prev_pitch = curr_pitch;
|
||||
prev_roll = curr_roll;
|
||||
prev_pitch = curr_pitch;
|
||||
prev_roll = curr_roll;
|
||||
|
||||
#ifdef FLOW_DEROTATION
|
||||
if(flow_count)
|
||||
{
|
||||
OFx_trans = dx_sum - diff_roll;
|
||||
OFy_trans = dy_sum - diff_pitch;
|
||||
if (flow_count) {
|
||||
OFx_trans = dx_sum - diff_roll;
|
||||
OFy_trans = dy_sum - diff_pitch;
|
||||
|
||||
if((OFx_trans<=0) != (dx_sum<=0))
|
||||
{
|
||||
OFx_trans = 0;
|
||||
OFy_trans = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OFx_trans = dx_sum;
|
||||
OFy_trans = dy_sum;
|
||||
}
|
||||
if ((OFx_trans <= 0) != (dx_sum <= 0)) {
|
||||
OFx_trans = 0;
|
||||
OFy_trans = 0;
|
||||
}
|
||||
} else {
|
||||
OFx_trans = dx_sum;
|
||||
OFy_trans = dy_sum;
|
||||
}
|
||||
#else
|
||||
OFx_trans = dx_sum;
|
||||
OFy_trans = dy_sum;
|
||||
OFx_trans = dx_sum;
|
||||
OFy_trans = dy_sum;
|
||||
#endif
|
||||
|
||||
// Average Filter
|
||||
OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1);
|
||||
// Average Filter
|
||||
OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1);
|
||||
|
||||
// Velocity Computation
|
||||
#ifdef USE_SONAR
|
||||
cam_h = 1; //ins_impl.sonar_z;
|
||||
#else
|
||||
cam_h = 1;
|
||||
#endif
|
||||
// Velocity Computation
|
||||
#ifdef USE_SONAR
|
||||
cam_h = 1; //ins_impl.sonar_z;
|
||||
#else
|
||||
cam_h = 1;
|
||||
#endif
|
||||
|
||||
if(flow_count)
|
||||
{
|
||||
Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05;
|
||||
Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1;
|
||||
}
|
||||
else
|
||||
{
|
||||
Velx = 0.0;
|
||||
Vely = 0.0;
|
||||
}
|
||||
if (flow_count) {
|
||||
Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05;
|
||||
Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1;
|
||||
} else {
|
||||
Velx = 0.0;
|
||||
Vely = 0.0;
|
||||
}
|
||||
|
||||
// **********************************************************************************************************************
|
||||
// Next Loop Preparation
|
||||
// **********************************************************************************************************************
|
||||
// **********************************************************************************************************************
|
||||
// Next Loop Preparation
|
||||
// **********************************************************************************************************************
|
||||
|
||||
memcpy(prev_frame,frame,imgHeight*imgWidth*2);
|
||||
memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth);
|
||||
memcpy(prev_frame, frame, imgHeight * imgWidth * 2);
|
||||
memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth);
|
||||
|
||||
// **********************************************************************************************************************
|
||||
// Downlink Message
|
||||
// **********************************************************************************************************************
|
||||
DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count);
|
||||
// **********************************************************************************************************************
|
||||
// Downlink Message
|
||||
// **********************************************************************************************************************
|
||||
DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch,
|
||||
&Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count);
|
||||
}
|
||||
|
||||
|
||||
@@ -8,19 +8,17 @@
|
||||
|
||||
#define N_BINS 10
|
||||
|
||||
struct gst2ppz_message_struct
|
||||
{
|
||||
struct gst2ppz_message_struct {
|
||||
unsigned int ID; // Keep different modules for using each others data
|
||||
unsigned int counter; // counter to keep track of data
|
||||
unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction
|
||||
unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction
|
||||
unsigned int counter; // counter to keep track of data
|
||||
unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction
|
||||
unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction
|
||||
};
|
||||
extern struct gst2ppz_message_struct gst2ppz;
|
||||
|
||||
struct ppz2gst_message_struct
|
||||
{
|
||||
struct ppz2gst_message_struct {
|
||||
unsigned int ID; // Keep different modules for using each others data
|
||||
unsigned int counter; //counter to keep track of data
|
||||
unsigned int counter; //counter to keep track of data
|
||||
int pitch;
|
||||
int roll;
|
||||
int alt;
|
||||
|
||||
@@ -45,42 +45,43 @@
|
||||
float FPS;
|
||||
volatile long timestamp;
|
||||
#define USEC_PER_SEC 1000000L
|
||||
long time_elapsed (struct timeval *t1, struct timeval *t2)
|
||||
long time_elapsed(struct timeval *t1, struct timeval *t2)
|
||||
{
|
||||
long sec, usec;
|
||||
sec = t2->tv_sec - t1->tv_sec;
|
||||
usec = t2->tv_usec - t1->tv_usec;
|
||||
if (usec < 0)
|
||||
{
|
||||
--sec;
|
||||
usec = usec + USEC_PER_SEC;
|
||||
}
|
||||
return sec*USEC_PER_SEC + usec;
|
||||
long sec, usec;
|
||||
sec = t2->tv_sec - t1->tv_sec;
|
||||
usec = t2->tv_usec - t1->tv_usec;
|
||||
if (usec < 0) {
|
||||
--sec;
|
||||
usec = usec + USEC_PER_SEC;
|
||||
}
|
||||
return sec * USEC_PER_SEC + usec;
|
||||
}
|
||||
|
||||
struct timeval start_time;
|
||||
struct timeval end_time;
|
||||
|
||||
void start_timer() {
|
||||
gettimeofday (&start_time, NULL);
|
||||
void start_timer()
|
||||
{
|
||||
gettimeofday(&start_time, NULL);
|
||||
}
|
||||
long end_timer() {
|
||||
gettimeofday (&end_time, NULL);
|
||||
return time_elapsed(&start_time, &end_time);
|
||||
long end_timer()
|
||||
{
|
||||
gettimeofday(&end_time, NULL);
|
||||
return time_elapsed(&start_time, &end_time);
|
||||
}
|
||||
|
||||
void opticflow_module_init(void)
|
||||
{
|
||||
// Immediately start the vision thread when the module initialized
|
||||
opticflow_module_start();
|
||||
// Immediately start the vision thread when the module initialized
|
||||
opticflow_module_start();
|
||||
|
||||
// Stabilization Code Initialization
|
||||
init_hover_stabilization_onvision();
|
||||
// Stabilization Code Initialization
|
||||
init_hover_stabilization_onvision();
|
||||
|
||||
// Frame Rate Initialization
|
||||
FPS = 0.0;
|
||||
timestamp=0;
|
||||
start_timer();
|
||||
// Frame Rate Initialization
|
||||
FPS = 0.0;
|
||||
timestamp = 0;
|
||||
start_timer();
|
||||
}
|
||||
|
||||
|
||||
@@ -90,8 +91,7 @@ void opticflow_module_run(void)
|
||||
{
|
||||
|
||||
// Read Latest Vision Module Results
|
||||
if (computervision_thread_has_results)
|
||||
{
|
||||
if (computervision_thread_has_results) {
|
||||
computervision_thread_has_results = 0;
|
||||
run_hover_stabilization_onvision();
|
||||
}
|
||||
@@ -120,98 +120,97 @@ void opticflow_module_run(void)
|
||||
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)
|
||||
void *computervision_thread_main(void *data);
|
||||
void *computervision_thread_main(void *data)
|
||||
{
|
||||
// Video Input
|
||||
struct vid_struct vid;
|
||||
vid.device = (char*)"/dev/video2"; // video1 = front camera; video2 = bottom camera
|
||||
vid.w=320;
|
||||
vid.h=240;
|
||||
vid.n_buffers = 4;
|
||||
// Video Input
|
||||
struct vid_struct vid;
|
||||
vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera
|
||||
vid.w = 320;
|
||||
vid.h = 240;
|
||||
vid.n_buffers = 4;
|
||||
|
||||
if (video_init(&vid)<0) {
|
||||
printf("Error initialising video\n");
|
||||
computervision_thread_status = -1;
|
||||
return 0;
|
||||
}
|
||||
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 Grabbing
|
||||
struct img_struct *img_new = video_create_image(&vid);
|
||||
|
||||
// Video Resizing
|
||||
#define DOWNSIZE_FACTOR 1
|
||||
struct img_struct small;
|
||||
small.w = vid.w / DOWNSIZE_FACTOR;
|
||||
small.h = vid.h / DOWNSIZE_FACTOR;
|
||||
small.buf = (uint8_t*)malloc(small.w*small.h*2);
|
||||
// Video Resizing
|
||||
#define DOWNSIZE_FACTOR 1
|
||||
struct img_struct small;
|
||||
small.w = vid.w / DOWNSIZE_FACTOR;
|
||||
small.h = vid.h / DOWNSIZE_FACTOR;
|
||||
small.buf = (uint8_t *)malloc(small.w * small.h * 2);
|
||||
|
||||
#ifdef DOWNLINK_VIDEO
|
||||
// Video Compression
|
||||
uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
|
||||
// Video Compression
|
||||
uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2);
|
||||
|
||||
// Network Transmit
|
||||
struct UdpSocket* vsock;
|
||||
//#define FMS_UNICAST 0
|
||||
//#define FMS_BROADCAST 1
|
||||
vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
|
||||
// Network Transmit
|
||||
struct UdpSocket *vsock;
|
||||
//#define FMS_UNICAST 0
|
||||
//#define FMS_BROADCAST 1
|
||||
vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
|
||||
#endif
|
||||
|
||||
// First Apply Settings before init
|
||||
imgWidth = small.w;
|
||||
imgHeight = small.h;
|
||||
verbose = 2;
|
||||
my_plugin_init();
|
||||
// First Apply Settings before init
|
||||
imgWidth = small.w;
|
||||
imgHeight = small.h;
|
||||
verbose = 2;
|
||||
my_plugin_init();
|
||||
|
||||
while (computer_vision_thread_command > 0)
|
||||
{
|
||||
video_grab_image(&vid, img_new);
|
||||
while (computer_vision_thread_command > 0) {
|
||||
video_grab_image(&vid, img_new);
|
||||
|
||||
// FPS
|
||||
timestamp = end_timer();
|
||||
FPS = (float) 1000000/(float)timestamp;
|
||||
// printf("dt = %d, FPS = %f\n",timestamp, FPS);
|
||||
start_timer();
|
||||
// FPS
|
||||
timestamp = end_timer();
|
||||
FPS = (float) 1000000 / (float)timestamp;
|
||||
// printf("dt = %d, FPS = %f\n",timestamp, FPS);
|
||||
start_timer();
|
||||
|
||||
// Resize
|
||||
//resize_uyuv(img_new, &small, DOWNSIZE_FACTOR);
|
||||
// Resize
|
||||
//resize_uyuv(img_new, &small, DOWNSIZE_FACTOR);
|
||||
|
||||
// Run Image Processing
|
||||
my_plugin_run(img_new->buf);
|
||||
// my_plugin_run(small.buf);
|
||||
// Run Image Processing
|
||||
my_plugin_run(img_new->buf);
|
||||
// my_plugin_run(small.buf);
|
||||
|
||||
#ifdef DOWNLINK_VIDEO
|
||||
// JPEG encode the image:
|
||||
uint32_t quality_factor = 10; //20 if no resize,
|
||||
uint8_t dri_header = 0;
|
||||
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_header);
|
||||
uint32_t size = end-(jpegbuf);
|
||||
#ifdef DOWNLINK_VIDEO
|
||||
// JPEG encode the image:
|
||||
uint32_t quality_factor = 10; //20 if no resize,
|
||||
uint8_t dri_header = 0;
|
||||
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_header);
|
||||
uint32_t size = end - (jpegbuf);
|
||||
|
||||
printf("Sending an image ...%u\n",size);
|
||||
uint32_t delta_t_per_frame = 0; // 0 = use drone clock
|
||||
send_rtp_frame(vsock, jpegbuf,size, small.w, small.h,0, quality_factor, dri_header, delta_t_per_frame);
|
||||
#endif
|
||||
computervision_thread_has_results++;
|
||||
}
|
||||
printf("Thread Closed\n");
|
||||
video_close(&vid);
|
||||
computervision_thread_status = -100;
|
||||
return 0;
|
||||
printf("Sending an image ...%u\n", size);
|
||||
uint32_t delta_t_per_frame = 0; // 0 = use drone clock
|
||||
send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame);
|
||||
#endif
|
||||
computervision_thread_has_results++;
|
||||
}
|
||||
printf("Thread Closed\n");
|
||||
video_close(&vid);
|
||||
computervision_thread_status = -100;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void opticflow_module_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);
|
||||
}
|
||||
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 opticflow_module_stop(void)
|
||||
{
|
||||
computer_vision_thread_command = 0;
|
||||
computer_vision_thread_command = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user