Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/gps.h"
58 
59 #include "modules/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 "modules/gps/gps.h"
113 #endif
114 
116 
118 {
124 
125 #ifndef SITL
126  int ret __attribute__((unused));
127  //ret = system("kk"); //No need kill original AP proceess since new AP is already running and that killed original process
128 
129  //TIP: With media-ctl much more can be done for the parameters of the camera only basics are set.
130  ret = system("media-ctl -l \'\"mt9f002 0-0010\":0->\"avicam.0\":0[1]\'");
131  ret = system("media-ctl -l \'\"avicam_dummy_dev.0\":0->\"avicam.0\":0[0]\'");//No bottomcam, used internaly in AP
132 
133  ret = system("prestart dxowrapperd");
134  ret = system("prestart pimp"); // pimp = Parrot IMaging Process
135  //ret = system("pimpctl list-cameras"); //TODO look for 1 or more then define a variable to use or not
136 #if DC_CTRL_PARROT_MYKONOS_STREAM_AT_STARTUP
138 #endif
139 
140 #else
141  //Start your local processes so simulated flight with sim shooting can be perfromed
142  //E.g. using a tile map interface of a pre-recorded stream or a 3D generated image
143  //FIXME: make a complete example, he, it's opensouce and involves your work indeed...
144  //ret = system("whateveryouwantotstartlocallyaddithere");
145 #endif
146 }
147 
149 {
150  //Nice 'n ugly use of True/False
153  } else {
155  }
156  // test log delay if set
158 #ifndef SITL
159  if (get_sys_time_msec() > dc_ctrl_parrot_mykonos.log_delay) { //FIXME: Could also happen in SITL...
160 #endif
161 #if DC_CTRL_PARROT_MYKONOS_LOG
162  dc_ctrl_parrot_mykonos_log_shot_position();
163 #endif
164 
165 #if DC_CTRL_PARROT_MYKONOS_SYNC_SEND
166  dc_ctrl_parrot_mykonos_send_shot_position();
167 #endif
168  // increment photo number
170  // unset log delay
172 #ifndef SITL
173  }
174 #endif
175  }
176 
177  // Common DC Periodic task
178  dc_periodic();
179 }
180 
181 //FIXME muiltiple cams and same DC cam API do no go together,
182 // so for the time being not used here, also one needs to add SITL options
183 
184 /* Command the Camera
185  * Intermidate function so the universal PPRZ Camra API can be used
186  * No need to change flightplan, script or otherwhise
187  * should work on both Fixedwing and Rotorcraft
188 */
189 
190 #ifdef SITL
191 void dc_send_command(uint8_t cmd)
192 {
193  //Nothing yet, empty framework here so sim compiles
194  switch (cmd) {
195  default:
196  break;
197  }
198 
199  // call command send_command function
201 }
202 #endif
203 
204 
205 /* Execute the Shoot, Record and Stream commands */
207 {
208  int ret __attribute__((unused));
209 
211  switch (cmd) {
213 #ifndef SITL
214  ret = system("pimpctl recording-start front");
215 #else
216  //ret = system("addyourlocalsitlcommandshere");
217 #endif
218  break;
221 #ifndef SITL
222  ret = system("pimpctl recording-stop front");
223 #else
224  //ret = system("addyourlocalsitlcommandshere");
225 #endif
226  break;
228 #ifndef SITL
229  ret = system("pimpctl take-picture front");
230 #else
231  //ret = system("addyourlocalsitlcommandshere");
232 #endif
233 
234  break;
236  /*
237  * Note that while .0 as destination IP works multicasting it introduces latency.
238  To avoid this use the real target IP of the video viewer device e.g. the GCS
239  quick and dirty; get ip from latest lease: ./data/lib/misc/dhcp_eth0.leases
240  and look for the name of your host you want to target.
241 
242  TIP: Example to view stream on Host PC:
243 
244  gst-launch-1.0 udpsrc port=55004 ! "application/x-rtp, payload=96" ! rtph264depay ! avdec_h264 ! autovideosink
245 
246  But there are many ways to Rome...
247 
248  */
249 #ifndef SITL
250  ret = system("pimpctl stream-start front 192.168.42.0 55004");//FIXME: Option to target only IP, for less delay
251 #else
252 //ret = system("addyourlocalsitlcommandshere");
253 #endif
254  break;
256 #ifndef SITL
257  ret = system("pimpctl stream-stop front 192.168.42.0 55004");
258 #else
259 //ret = system("addyourlocalsitlcommandshere");
260 #endif
261  break;
264 #ifndef SITL
265  ret = system("pimpctl take-picture front");
266 #else
267 //ret = system("addyourlocalsitlcommandshere");
268 #endif
271  break;
273  //nix
274  break;
275  default:
276  break;
277  }
278 }
279 
281 {
282 // Wait a minimum time between two shots
285  } else {
286  // test distance if needed
287  // or take picture if first of the sequence
288 #ifdef DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST
289  struct EnuCoor_f pos = *stateGetPositionEnu_f();
290  struct FloatVect2 d_pos;
291  d_pos.x = pos.x - dc_ctrl_parrot_mykonos.last_shot_pos.x;
292  d_pos.y = pos.y - dc_ctrl_parrot_mykonos.last_shot_pos.y;
293  if (VECT2_NORM2(d_pos) > (DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST * DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST)
295 #endif
296  // take a picture
298  // reset timer
300 #ifdef DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DIST
301  }
302 #endif
303  }
304 }
305 
307 {
308  // Start taking a picture immediately
311 }
static int16_t course[3]
Definition: airspeed_uADC.c:58
void dc_send_command(uint8_t cmd)
Send Command To Camera.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
void dc_periodic(void)
periodic function
Definition: dc.c:279
void dc_send_command_common(uint8_t cmd)
Command sending function.
Definition: dc.c:192
Standard Digital Camera Control Interface.
void dc_ctrl_parrot_mykonos_autoshoot_start(void)
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY
autoshoot timer delay based on periodic freq from modules.h
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_TIMER_OF_DELAY(_delay)
void dc_ctrl_parrot_mykonos_autoshoot(void)
void dc_ctrl_parrot_mykonos_init(void)
#define DC_CTRL_PARROT_MYKONOS_RECORD_DELAY
NOTES: For now only the front cam is used, feel free to extend.
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
#define DC_CTRL_PARROT_MYKONOS_TIMER_OF_DELAY(_delay)
Get timer from delay based on periodic freq from modules.h.
void dc_ctrl_parrot_mykonos_command(enum dc_ctrl_parrot_mykonos_status cmd)
void dc_ctrl_parrot_mykonos_periodic(void)
Digital video/photo recorder control for Parrot Mykonos Platform, For others that is: control the cam...
enum dc_ctrl_parrot_mykonos_status status
dc_ctrl_parrot_mykonos_status
@ DC_CTRL_PARROT_MYKONOS_SHOOT
@ DC_CTRL_PARROT_MYKONOS_STREAM_START
@ DC_CTRL_PARROT_MYKONOS_RECORD_STOP
@ DC_CTRL_PARROT_MYKONOS_RECORD_START
@ DC_CTRL_PARROT_MYKONOS_STREAM_STOP
@ DC_CTRL_PARROT_MYKONOS_NONE
@ DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_START
@ DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_STOP
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition: gps.h:108
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
API to get/set the generic vehicle states.
Architecture independent timing functions.
Periodic telemetry system header (includes downlink utility and generated code).
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98