Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
dc_ctrl_parrot_mykonos.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 OpenUAS
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 
30 #include "generated/modules.h"
31 #include "generated/airframe.h"
32 #include "mcu_periph/sys_time.h"
33 // Include Standard Camera Control Interface
34 #include "modules/digital_cam/dc.h"
35 
37 #define DC_CTRL_PARROT_MYKONOS_RECORD_DELAY 0.05
38 
40 #define DC_CTRL_PARROT_MYKONOS_LOG_DELAY 50
41 
43 #define DC_CTRL_PARROT_MYKONOS_TIMER_OF_DELAY(_delay) ((uint32_t)(_delay * DC_CTRL_PARROT_MYKONOS_PERIODIC_FREQ))
44 
46 #ifndef DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY
47 #define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY 5.0
48 #endif
49 #define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_TIMER_OF_DELAY(_delay) ((uint32_t)(_delay * DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_FREQ))
50 
52 #if DC_CTRL_PARROT_MYKONOS_SYNC_SEND
53 
54 #include "pprzlink/messages.h"
56 #include "state.h"
57 #include "subsystems/gps.h"
58 
59 #include "subsystems/datalink/telemetry.h" //? or just downlink.h should be enough
60 
61 static inline void dc_ctrl_parrot_mykonos_send_shot_position(void)
62 {
63  // angles in decideg
64  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
65  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
66  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
67  // course in decideg
68  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
69  // ground speed in cm/s
71 
72  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
74  &stateGetPositionLla_i()->lat,
75  &stateGetPositionLla_i()->lon,
76  &stateGetPositionLla_i()->alt,
77  &gps.hmsl,
78  &phi,
79  &theta,
80  &psi,
81  &course,
82  &speed,
83  &gps.tow);
84 }
85 #endif
86 
87 
88 /*
89  The pimpctl command is available per default on a Disco and can be used for
90  triggering various camera related tasks:
91 
92  Possible arguments for pimpctl command
93  list-cameras = print the list of available cameras
94  stream-start front <address> <port> = start streaming video
95  stream-stop front = stop streaming video
96  take-picture <camera-name> = ehh, take a photo indeed
97  recording-start <camera-name> = start recording video
98  recording-stop <camera-name> = stop recording video
99  set-controller <camera-name> <controller_id> = set a new camera controller
100  0 -> USER FLAT (yaw axis is free)
101  1 -> USER ABSOLUTE (fully stabilized)
102  2 -> USER FPV (fixed relatively to the drone))
103  3 -> AUTOPILOT
104  cam-orientation <camera-name> <y> <p> <r> = set camera orientation (in degree CCW)
105  y -> yaw pitch rotatio (psi euler angle)
106  p -> pitch rotation (theta euler angle)
107  r -> roll rotation (phi euler angle)
108 */
109 
110 #if DC_CTRL_PARROT_MYKONOS_LOG
111 #include "state.h"
112 #include "subsystems/gps.h"
113 #endif
114 
116 
118 {
119  // Call common DC init
120  dc_init();
121 
127 
128 #ifndef SITL
129  int ret __attribute__((unused));
130  //ret = system("kk"); //No need kill original AP proceess since new AP is already running and that killed original process
131 
132  //TIP: With media-ctl much more can be done for the parameters of the camera only basics are set.
133  ret = system("media-ctl -l \'\"mt9f002 0-0010\":0->\"avicam.0\":0[1]\'");
134  ret = system("media-ctl -l \'\"avicam_dummy_dev.0\":0->\"avicam.0\":0[0]\'");//No bottomcam, used internaly in AP
135 
136  ret = system("prestart dxowrapperd");
137  ret = system("prestart pimp"); // pimp = Parrot IMaging Process
138  //ret = system("pimpctl list-cameras"); //TODO look for 1 or more then define a variable to use or not
139 #if DC_CTRL_PARROT_MYKONOS_STREAM_AT_STARTUP
141 #endif
142 
143 #else
144  //Start your local processes so simulated flight with sim shooting can be perfromed
145  //E.g. using a tile map interface of a pre-recorded stream or a 3D generated image
146  //FIXME: make a complete example, he, it's opensouce and involves your work indeed...
147  //ret = system("whateveryouwantotstartlocallyaddithere");
148 #endif
149 }
150 
152 {
153  //Nice 'n ugly use of True/False
156  } else {
158  }
159  // test log delay if set
161 #ifndef SITL
162  if (get_sys_time_msec() > dc_ctrl_parrot_mykonos.log_delay) { //FIXME: Could also happen in SITL...
163 #endif
164 #if DC_CTRL_PARROT_MYKONOS_LOG
165  dc_ctrl_parrot_mykonos_log_shot_position();
166 #endif
167 
168 #if DC_CTRL_PARROT_MYKONOS_SYNC_SEND
169  dc_ctrl_parrot_mykonos_send_shot_position();
170 #endif
171  // increment photo number
173  // unset log delay
175 #ifndef SITL
176  }
177 #endif
178  }
179 
180  // Common DC Periodic task
181  dc_periodic();
182 }
183 
184 //FIXME muiltiple cams and same DC cam API do no go together,
185 // so for the time being not used here, also one needs to add SITL options
186 
187 /* Command the Camera
188  * Intermidate function so the universal PPRZ Camra API can be used
189  * No need to change flightplan, script or otherwhise
190  * should work on both Fixedwing and Rotorcraft
191 */
192 
193 #ifdef SITL
194 void dc_send_command(uint8_t cmd)
195 {
196  //Nothing yet, empty framework here so sim compiles
197  switch (cmd) {
198  default:
199  break;
200  }
201 
202  // call command send_command function
204 }
205 #endif
206 
207 
208 /* Execute the Shoot, Record and Stream commands */
210 {
211  int ret __attribute__((unused));
212 
214  switch (cmd) {
216 #ifndef SITL
217  ret = system("pimpctl recording-start front");
218 #else
219  //ret = system("addyourlocalsitlcommandshere");
220 #endif
221  break;
224 #ifndef SITL
225  ret = system("pimpctl recording-stop front");
226 #else
227  //ret = system("addyourlocalsitlcommandshere");
228 #endif
229  break;
231 #ifndef SITL
232  ret = system("pimpctl take-picture front");
233 #else
234  //ret = system("addyourlocalsitlcommandshere");
235 #endif
236 
237  break;
239  /*
240  * Note that while .0 as destination IP works multicasting it introduces latency.
241  To avoid this use the real target IP of the video viewer device e.g. the GCS
242  quick and dirty; get ip from latest lease: ./data/lib/misc/dhcp_eth0.leases
243  and look for the name of your host you want to target.
244 
245  TIP: Example to view stream on Host PC:
246 
247  gst-launch-1.0 udpsrc port=55004 ! "application/x-rtp, payload=96" ! rtph264depay ! avdec_h264 ! autovideosink
248 
249  But there are many ways to Rome...
250 
251  */
252 #ifndef SITL
253  ret = system("pimpctl stream-start front 192.168.42.0 55004");//FIXME: Option to target only IP, for less delay
254 #else
255 //ret = system("addyourlocalsitlcommandshere");
256 #endif
257  break;
259 #ifndef SITL
260  ret = system("pimpctl stream-stop front 192.168.42.0 55004");
261 #else
262 //ret = system("addyourlocalsitlcommandshere");
263 #endif
264  break;
267 #ifndef SITL
268  ret = system("pimpctl take-picture front");
269 #else
270 //ret = system("addyourlocalsitlcommandshere");
271 #endif
274  break;
276  //nix
277  break;
278  default:
279  break;
280  }
281 }
282 
284 {
285 // Wait a minimum time between two shots
288  } else {
289  // test distance if needed
290  // or take picture if first of the sequence
291 #ifdef DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST
292  struct EnuCoor_f pos = *stateGetPositionEnu_f();
293  struct FloatVect2 d_pos;
294  d_pos.x = pos.x - dc_ctrl_parrot_mykonos.last_shot_pos.x;
295  d_pos.y = pos.y - dc_ctrl_parrot_mykonos.last_shot_pos.y;
296  if (VECT2_NORM2(d_pos) > (DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST * DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST)
298 #endif
299  // take a picture
301  // reset timer
303 #ifdef DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST
304  }
305 #endif
306  }
307 }
308 
310 {
311  // Start taking a picture immediately
314 }
unsigned short uint16_t
Definition: types.h:16
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY
autoshoot timer delay based on periodic freq from modules.h
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
void dc_ctrl_parrot_mykonos_autoshoot(void)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Periodic telemetry system header (includes downlink utility and generated code).
enum dc_ctrl_parrot_mykonos_status status
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
vector in East North Up coordinates Units: meters
struct Dc_Ctrl_Parrot_Mykonos dc_ctrl_parrot_mykonos
Send report.
#define DC_CTRL_PARROT_MYKONOS_LOG_DELAY
delay in milli-seconds before logging after a shot in ms
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
void dc_ctrl_parrot_mykonos_periodic(void)
void dc_init(void)
initialize settings
Definition: dc.c:156
Digital video/photo recorder control for Parrot Mykonos Platform, For others that is: control the cam...
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
Standard Digital Camera Control Interface.
float x
in meters
dc_ctrl_parrot_mykonos_status
Architecture independent timing functions.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
Device independent GPS code (interface)
void dc_send_command_common(uint8_t cmd)
Command sending function.
Definition: dc.c:189
signed short int16_t
Definition: types.h:17
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_TIMER_OF_DELAY(_delay)
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
void dc_ctrl_parrot_mykonos_autoshoot_start(void)
void dc_send_command(uint8_t cmd)
Send Command To Camera.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
#define DC_CTRL_PARROT_MYKONOS_TIMER_OF_DELAY(_delay)
Get timer from delay based on periodic freq from modules.h.
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void dc_periodic(void)
periodic function
Definition: dc.c:273
#define DC_CTRL_PARROT_MYKONOS_RECORD_DELAY
NOTES: For now only the front cam is used, feel free to extend.
static int16_t course[3]
Definition: airspeed_uADC.c:57
void dc_ctrl_parrot_mykonos_init(void)
void dc_ctrl_parrot_mykonos_command(enum dc_ctrl_parrot_mykonos_status cmd)
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:69
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683