Paparazzi UAS  v6.2_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 {
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 }
Dc_Ctrl_Parrot_Mykonos::timer
uint32_t timer
Definition: dc_ctrl_parrot_mykonos.h:62
dc_send_command_common
void dc_send_command_common(uint8_t cmd)
Command sending function.
Definition: dc.c:189
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
DC_CTRL_PARROT_MYKONOS_RECORD_STOP
@ DC_CTRL_PARROT_MYKONOS_RECORD_STOP
Definition: dc_ctrl_parrot_mykonos.h:52
DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_DELAY
autoshoot timer delay based on periodic freq from modules.h
Definition: dc_ctrl_parrot_mykonos.c:47
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
dc_ctrl_parrot_mykonos_autoshoot
void dc_ctrl_parrot_mykonos_autoshoot(void)
Definition: dc_ctrl_parrot_mykonos.c:283
Dc_Ctrl_Parrot_Mykonos::status
enum dc_ctrl_parrot_mykonos_status status
Definition: dc_ctrl_parrot_mykonos.h:61
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
dc_ctrl_parrot_mykonos
struct Dc_Ctrl_Parrot_Mykonos dc_ctrl_parrot_mykonos
Send report.
Definition: dc_ctrl_parrot_mykonos.c:115
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetPositionLla_i
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
DC_CTRL_PARROT_MYKONOS_LOG_DELAY
#define DC_CTRL_PARROT_MYKONOS_LOG_DELAY
delay in milli-seconds before logging after a shot in ms
Definition: dc_ctrl_parrot_mykonos.c:40
dc_ctrl_parrot_mykonos_periodic
void dc_ctrl_parrot_mykonos_periodic(void)
Definition: dc_ctrl_parrot_mykonos.c:151
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
dc.h
DC_CTRL_PARROT_MYKONOS_NONE
@ DC_CTRL_PARROT_MYKONOS_NONE
Definition: dc_ctrl_parrot_mykonos.h:50
FloatVect2
Definition: pprz_algebra_float.h:49
telemetry.h
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
dc_ctrl_parrot_mykonos_status
dc_ctrl_parrot_mykonos_status
Definition: dc_ctrl_parrot_mykonos.h:49
gps.h
Device independent GPS code (interface)
Dc_Ctrl_Parrot_Mykonos::photo_nr
int16_t photo_nr
Definition: dc_ctrl_parrot_mykonos.h:63
dc_send_command
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Definition: atmega_i2c_cam_ctrl.c:41
DC_CTRL_PARROT_MYKONOS_STREAM_START
@ DC_CTRL_PARROT_MYKONOS_STREAM_START
Definition: dc_ctrl_parrot_mykonos.h:54
sys_time.h
Architecture independent timing functions.
get_sys_time_msec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:86
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
dc_ctrl_parrot_mykonos.h
Digital video/photo recorder control for Parrot Mykonos Platform, For others that is: control the cam...
Dc_Ctrl_Parrot_Mykonos::autoshoot
uint32_t autoshoot
Definition: dc_ctrl_parrot_mykonos.h:64
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_TIMER_OF_DELAY
#define DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_TIMER_OF_DELAY(_delay)
Definition: dc_ctrl_parrot_mykonos.c:49
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
DC_CTRL_PARROT_MYKONOS_TIMER_OF_DELAY
#define DC_CTRL_PARROT_MYKONOS_TIMER_OF_DELAY(_delay)
Get timer from delay based on periodic freq from modules.h.
Definition: dc_ctrl_parrot_mykonos.c:43
DC_CTRL_PARROT_MYKONOS_SHOOT
@ DC_CTRL_PARROT_MYKONOS_SHOOT
Definition: dc_ctrl_parrot_mykonos.h:53
DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_START
@ DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_START
Definition: dc_ctrl_parrot_mykonos.h:56
DC_CTRL_PARROT_MYKONOS_RECORD_DELAY
#define DC_CTRL_PARROT_MYKONOS_RECORD_DELAY
NOTES: For now only the front cam is used, feel free to extend.
Definition: dc_ctrl_parrot_mykonos.c:37
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_STOP
@ DC_CTRL_PARROT_MYKONOS_AUTOSHOOT_STOP
Definition: dc_ctrl_parrot_mykonos.h:57
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
dc_ctrl_parrot_mykonos_autoshoot_start
void dc_ctrl_parrot_mykonos_autoshoot_start(void)
Definition: dc_ctrl_parrot_mykonos.c:309
Dc_Ctrl_Parrot_Mykonos::last_shot_pos
struct EnuCoor_f last_shot_pos
Definition: dc_ctrl_parrot_mykonos.h:65
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
dc_ctrl_parrot_mykonos_init
void dc_ctrl_parrot_mykonos_init(void)
Definition: dc_ctrl_parrot_mykonos.c:117
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
DC_CTRL_PARROT_MYKONOS_STREAM_STOP
@ DC_CTRL_PARROT_MYKONOS_STREAM_STOP
Definition: dc_ctrl_parrot_mykonos.h:55
dc_ctrl_parrot_mykonos_command
void dc_ctrl_parrot_mykonos_command(enum dc_ctrl_parrot_mykonos_status cmd)
Definition: dc_ctrl_parrot_mykonos.c:209
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
DC_CTRL_PARROT_MYKONOS_RECORD_START
@ DC_CTRL_PARROT_MYKONOS_RECORD_START
Definition: dc_ctrl_parrot_mykonos.h:51
state.h
Dc_Ctrl_Parrot_Mykonos::log_delay
uint32_t log_delay
Definition: dc_ctrl_parrot_mykonos.h:66
dc_init
void dc_init(void)
initialize settings
Definition: dc.c:156
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
dc_periodic
void dc_periodic(void)
periodic function
Definition: dc.c:273
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
Dc_Ctrl_Parrot_Mykonos
Definition: dc_ctrl_parrot_mykonos.h:60