Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
catia.c
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <pthread.h>
4 #include <errno.h>
5 
6 #include "serial.h"
7 #include "chdk_pipe.h"
8 #include "protocol.h"
9 
10 #define MAX_FILENAME 512
11 #define MAX_PROCESSING_THREADS 8
12 #define MAX_IMAGE_BUFFERS 25
13 #define IMAGE_SIZE 70
14 // Search&Rescue Onboard Detection Application
15 #define SODA "/root/develop/allthings_obc2014/src/soda/soda"
16 
17 static void *handle_msg_shoot(void *ptr);
18 static inline void send_msg_image_buffer(void);
19 static inline void send_msg_status(void);
20 
23 static pthread_mutex_t mut = PTHREAD_MUTEX_INITIALIZER;
24 
25 int main(int argc, char *argv[])
26 {
27  pthread_t shooting_threads[MAX_PROCESSING_THREADS];
28  char c;
29  int i;
30 
31  // Initialization
32  printf("CATIA:\tStarting Camera Application Triggering Image Analysis\n");
34  int ret = serial_init("/dev/ttySAC0");
35  if (ret < 0) {
36  printf("CATIA:\tfailed to open /dev/ttySAC0\n");
37  return -1;
38  }
39  pthread_mutex_init(&mut, NULL);
40  socket_init(1);
41 
42  // Initial settings
43  is_shooting = 0;
45  image_idx = 0;
46  image_count = 0;
47  shooting_idx = 0;
48  shooting_count = 0;
50 
51  // MAIN loop
52  while (1) {
53 
54  // Read the serial
55  if (read(fd, &c, 1) > 0) {
57  } else if (errno != 11) {
58  printf("CATIA:\nSerial error: %d\n" , errno);
59  }
60 
61  // Parse serial commands
63  // Process Only Once
65 
66  // Shoot an image if not busy
68  // Parse the shoot message
69  union dc_shot_union *shoot = (union dc_shot_union *) malloc(sizeof(union dc_shot_union));
70  for (i = 0; i < MORA_SHOOT_MSG_SIZE; i++) {
71  shoot->bin[i] = mora_protocol.payload[i];
72  }
73  printf("CATIA:\tSHOT %d,%d\n", shoot->data.nr, shoot->data.phi);
74 
75  pthread_create(&shooting_threads[(shooting_idx++ % MAX_PROCESSING_THREADS)], NULL, handle_msg_shoot, (void *)shoot);
77  }
78 
79  // Fill the image buffer (happens busy because needs fd anyway)
82  }
83  }
84 
85  // Read the socket
88 
90  image_count++;
91  }
92  }
93 
94  }
95 
96  // Close
97  close(fd);
99 
100  printf("CATIA:\tShutdown\n");
101  return 0;
102 }
103 
104 static void *handle_msg_shoot(void *ptr)
105 {
106  char filename[MAX_FILENAME], soda_call[512];
107  union dc_shot_union *shoot = (union dc_shot_union *) ptr;
108 
109  // Test if can shoot
110  pthread_mutex_lock(&mut);
111  if (is_shooting) {
112  pthread_mutex_unlock(&mut);
113  printf("CATIA-%d:\tShooting: too fast\n", shoot->data.nr);
114 
115  free(shoot);
116  return NULL;
117  }
118 
119  is_shooting = 1;
120  shooting_count++;
122  pthread_mutex_unlock(&mut);
123 
124  printf("CATIA-%d:\tShooting: start\n", shoot->data.nr);
125  chdk_pipe_shoot(filename);
126  printf("CATIA-%d:\tShooting: got image %s\n", shoot->data.nr, filename);
127 
128  pthread_mutex_lock(&mut);
129  is_shooting = 0;
130  pthread_mutex_unlock(&mut);
131 
132  //Parse the image
133  sprintf(soda_call, "%s %s %d %d %d %d %d %d %d %d %d %d", SODA, filename,
134  shoot->data.nr, shoot->data.lat, shoot->data.lon, shoot->data.alt, shoot->data.phi, shoot->data.theta, shoot->data.psi,
135  shoot->data.vground, shoot->data.course, shoot->data.groundalt);
136  printf("CATIA-%d:\tCalling '%s'\n", shoot->data.nr, soda_call);
137  short int ret = system(soda_call);
138  printf("CATIA-%d:\tShooting: soda return %d of image %s\n", shoot->data.nr, ret, filename);
139 
140  pthread_mutex_lock(&mut);
142  pthread_mutex_unlock(&mut);
143 
144  free(shoot);
145 }
146 
147 static inline void send_msg_image_buffer(void)
148 {
149  int i;
150 
151  // Check if image is available
152  if (image_count > 0) {
153  printf("CATIA:\thandle_msg_buffer: Send %d\n", image_idx);
154  // Send the image
156  image_count--;
157 
159  for (i = 0; i < IMAGE_SIZE; i++) {
161  }
162  MoraTrailer();
163  }
164 }
165 
166 static inline void send_msg_status(void)
167 {
168  int i;
169  struct mora_status_struct status_msg;
170  char *buffer = (char *) &status_msg;
171 
172  pthread_mutex_lock(&mut);
173  status_msg.cpu = 0;
174  status_msg.threads = shooting_thread_count;
175  status_msg.shots = shooting_count;
176  status_msg.extra = 0;
177  pthread_mutex_unlock(&mut);
178 
180  for (i = 0; i < MORA_STATUS_MSG_SIZE; i++) {
181  MoraPutUint8(buffer[i]);
182  }
183  MoraTrailer();
184 }
#define IMAGE_SIZE
Definition: catia.c:13
int main(int argc, char *argv[])
Definition: catia.c:25
static volatile int shooting_idx
Definition: catia.c:21
static volatile int image_idx
Definition: catia.c:21
static pthread_mutex_t mut
Definition: catia.c:23
static char image_buffer[MAX_IMAGE_BUFFERS][IMAGE_SIZE]
Definition: catia.c:22
static volatile int shooting_thread_count
Definition: catia.c:21
#define MAX_IMAGE_BUFFERS
Definition: catia.c:12
static void * handle_msg_shoot(void *ptr)
Definition: catia.c:104
static void send_msg_image_buffer(void)
Definition: catia.c:147
static volatile int image_count
Definition: catia.c:21
static volatile int is_shooting
Definition: catia.c:21
#define MAX_FILENAME
Definition: catia.c:10
static void send_msg_status(void)
Definition: catia.c:166
#define MAX_PROCESSING_THREADS
Definition: catia.c:11
#define SODA
Definition: catia.c:15
static volatile int shooting_count
Definition: catia.c:21
void chdk_pipe_init(void)
Initialize the CHDK pipe.
Definition: chdk_pipe.c:50
void chdk_pipe_shoot(char *filename)
Shoot an image.
Definition: chdk_pipe.c:92
void chdk_pipe_deinit(void)
Deinitialize CHDK pipe.
Definition: chdk_pipe.c:79
#define MORA_PAYLOAD_MSG_SIZE
Definition: protocol.h:75
struct dc_shot_union::@285 data
#define MORA_STATUS
Definition: protocol.h:80
#define MORA_STATUS_MSG_SIZE
Definition: protocol.h:81
#define MoraTrailer()
Definition: protocol.h:117
#define MoraHeader(msg_id, payload_len)
Definition: protocol.h:109
#define MoraPutUint8(_byte)
Definition: protocol.h:103
uint8_t payload[256]
Definition: protocol.h:132
#define MORA_BUFFER_EMPTY
Definition: protocol.h:70
#define MORA_SHOOT_MSG_SIZE
Definition: protocol.h:48
uint8_t msg_id
Definition: protocol.h:134
uint8_t bin[MORA_SHOOT_MSG_SIZE]
Definition: protocol.h:66
int32_t i[10]
Definition: protocol.h:67
bool msg_received
Definition: protocol.h:135
#define MORA_PAYLOAD
Definition: protocol.h:74
#define MORA_SHOOT
Definition: protocol.h:47
uint8_t status
Definition: protocol.h:138
void parse_mora(struct mora_transport *t, uint8_t c)
Definition: protocol.c:20
struct mora_transport mora_protocol
Definition: protocol.c:18
PX4IO interface protocol.
int fd
Definition: serial.c:26
int serial_init(char *port_name)
Definition: serial.c:28
void socket_init(int is_server)
Definition: socket.c:19
int socket_recv(char *buffer, int len)
Definition: socket.c:41