Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stereoprotocol.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Kirk + Roland
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
27 #include "stereoprotocol.h"
28 
33 {
34  return (counter + i) % buffer_size;
35 }
36 
37 
42 {
43  return (counter - i + buffer_size) % buffer_size;
44 }
45 
51 {
52 
53  if (stack[i] == 255 && (stack[stereoprot_add(i, 1, buffer_size)] == 0)
54  && (stack[stereoprot_add(i, 2, buffer_size)] == 0) && stack[stereoprot_add(i, 3, buffer_size)] == 171) {
55  return 1;
56  }
57  return 0;
58 }
59 
65 {
66  //printf("Checking start: %d %d %d %d \n",stack[i],stack[stereoprot_add(i, 1,buffer_size)],stack[stereoprot_add(i, 2,buffer_size)],stack[stereoprot_add(i, 3,buffer_size)]);
67  if (stack[i] == 255 && (stack[stereoprot_add(i, 1, buffer_size)] == 0)
68  && (stack[stereoprot_add(i, 2, buffer_size)] == 0) && stack[stereoprot_add(i, 3, buffer_size)] == 175) {
69  return 1;
70  }
71  return 0;
72 }
73 
74 //void stereoprot_get_msg_properties(uint8_t *, MsgProperties *, uint16_t,uint16_t);
75 
76 
77 uint8_t WritePart(struct link_device *dev, uint8_t *code, uint8_t length)
78 {
79  long fd = 0;
80  if (dev->check_free_space(dev->periph, &fd, length)) {
81  for (uint8_t index = 0; index < length; index++) {
82  dev->put_byte(dev->periph, fd, code[index]);
83  }
84  return 1;
85  }
86  return 0;
87 }
88 void stereoprot_sendArray(struct link_device *fd, uint8_t *b, uint8_t array_width, uint8_t array_height)
89 {
90 
91  uint8_t code[4];
92  code[0] = 0xff;
93  code[1] = 0x00;
94  code[2] = 0x00;
95  code[3] = 0xAF; // 175
96  while (WritePart(fd, code, 4) == 0)
97  ;
98 
99 
100  int horizontalLine = 0;
101  for (horizontalLine = 0; horizontalLine < array_height; horizontalLine++) {
102  code[3] = 0x80;//128
103  while (WritePart(fd, code, 4) == 0)
104  ;
105  while (WritePart(fd, b + array_width * horizontalLine, array_width) == 0)
106  ;
107 
108  code[3] = 0xDA;//218
109  while (WritePart(fd, code, 4) == 0)
110  ;
111  }
112 
113  code[3] = 0xAB; //171
114  while (WritePart(fd, code, 4) == 0)
115  ;
116 }
117 
123  uint16_t *msg_start, uint8_t *msg_buf, uint8_t *ser_read_buf, uint8_t *stereocam_datadata_new,
124  uint8_t *stereocam_datalen, uint8_t *stereocam_data_matrix_width, uint8_t *stereocam_data_matrix_height)
125 {
126 
127 
129  // read all data from the stereo com link, check that don't overtake extract
130  if (stereoprot_add(*insert_loc, 1, buffer_size) != *extract_loc) {
131  ser_read_buf[*insert_loc] = newByte;
132  *insert_loc = stereoprot_add(*insert_loc, 1, buffer_size);
133  }
134 
135 
136  // search for complete message in buffer, if found increments read location and returns immediately
137 
138  //while (stereoprot_diff(*insert_loc, stereoprot_add(*extract_loc,3,buffer_size),buffer_size) > 0) {
139  while (stereoprot_diff(*insert_loc, *extract_loc, buffer_size) > 3) {
140  if (stereoprot_isStartOfMsg(ser_read_buf, *extract_loc, buffer_size)) {
141 
142 
143  *msg_start = *extract_loc;
144  } else if (stereoprot_isEndOfMsg(ser_read_buf, *extract_loc, buffer_size)) { // process msg
145 
146 
147  // Find the properties of the image by iterating over the complete image
148  stereoprot_get_msg_properties(ser_read_buf, &msgProperties, *msg_start, buffer_size);
149  // Copy array to circular buffer and remove all bytes that are indications of start and stop lines
150  uint16_t i = stereoprot_add(*msg_start, 8, buffer_size), j = 0, k = 0, index = 0;
151  for (k = 0; k < msgProperties.height; k++) {
152  for (j = 0; j < msgProperties.width; j++) {
153  msg_buf[index++] = ser_read_buf[i];
154  i = stereoprot_add(i, 1, buffer_size);
155  }
156  i = stereoprot_add(i, 8, buffer_size); // step over EOL and SOL
157  } // continue search for new line
158  *stereocam_datalen = msgProperties.width * msgProperties.height;
159  *stereocam_data_matrix_width = msgProperties.width;
160  *stereocam_data_matrix_height = msgProperties.height;
161 
162  *stereocam_datadata_new = 1;
163  *extract_loc = stereoprot_add(*extract_loc, 4, buffer_size); // step over EOM string
164 
165  return 1;
166  }
167  *extract_loc = stereoprot_add(*extract_loc, 1, buffer_size);
168 
169  }
170  return 0;
171 }
172 
173 
177 void stereoprot_get_msg_properties(uint8_t *raw, MsgProperties *properties, uint16_t start, uint16_t buffer_size)
178 {
179  *properties = (MsgProperties) {start, 0, 0};
180  uint16_t i = start, startOfLine = start;
181  while (1) {
182  // Check the first 3 bytes for the pattern 255-0-0, then check what special byte is encoded next
183  if ((raw[i] == 255) && (raw[stereoprot_add(i, 1, buffer_size)] == 0) && (raw[stereoprot_add(i, 2, buffer_size)] == 0)) {
184  if (raw[stereoprot_add(i, 3, buffer_size)] == 171) { // End of image
185  break;
186  }
187  if (raw[stereoprot_add(i, 3, buffer_size)] == 128) { // Start of line
188  startOfLine = i;
189  }
190  if (raw[stereoprot_add(i, 3, buffer_size)] == 218) { // End of line
191  properties->height++;
192  properties->width = stereoprot_diff(i, startOfLine + 4,
193  buffer_size); // removed 4 for the indication bits at the end of line
194  }
195  }
196  i = stereoprot_add(i, 1, buffer_size);
197  }
198 }
void stereoprot_get_msg_properties(uint8_t *raw, MsgProperties *properties, uint16_t start, uint16_t buffer_size)
Retrieve size of image from message.
unsigned short uint16_t
Definition: types.h:16
struct MsgProperties MsgProperties
uint16_t extract_loc
Definition: stereocam.c:55
uint8_t stereoprot_isEndOfMsg(uint8_t *stack, uint16_t i, uint16_t buffer_size)
Checks if the sequence in the array is equal to 255-0-0-171, as this means that this is the end of an...
standard protocol for TUDelft stereocamera data transfer
void stereoprot_sendArray(struct link_device *fd, uint8_t *b, uint8_t array_width, uint8_t array_height)
uint16_t insert_loc
Definition: stereocam.c:55
uint16_t msg_start
Definition: stereocam.c:55
int32_t counter
uint16_t stereoprot_diff(uint16_t counter, uint16_t i, uint16_t buffer_size)
Decrement circular buffer counter by i.
uint8_t stereoprot_isStartOfMsg(uint8_t *stack, uint16_t i, uint16_t buffer_size)
Checks if the sequence in the array is equal to 255-0-0-171, as this means a new image is starting fr...
MsgProperties msgProperties
Definition: stereocam.c:44
uint8_t WritePart(struct link_device *dev, uint8_t *code, uint8_t length)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
unsigned char uint8_t
Definition: types.h:14
int fd
Definition: serial.c:26
uint16_t stereoprot_add(uint16_t counter, uint16_t i, uint16_t buffer_size)
Increment circular buffer counter by i.
uint8_t handleStereoPackage(uint8_t newByte, uint16_t buffer_size, uint16_t *insert_loc, uint16_t *extract_loc, uint16_t *msg_start, uint8_t *msg_buf, uint8_t *ser_read_buf, uint8_t *stereocam_datadata_new, uint8_t *stereocam_datalen, uint8_t *stereocam_data_matrix_width, uint8_t *stereocam_data_matrix_height)
Get all available data from stereo com link and decode any complete messages.
uint8_t ser_read_buf[STEREO_BUF_SIZE]
Definition: stereocam.c:54
uint8_t msg_buf[STEREO_BUF_SIZE]
Definition: stereocam.c:56