Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
dc.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2014 The Paparazzi Team
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 
35 #include "dc.h"
36 
37 // for waypoints, include correct header until we have unified API
38 #ifdef FIXEDWING_FIRMWARE
39 #include "modules/nav/common_nav.h"
40 #else
42 #endif
43 
44 #include "mcu_periph/sys_time.h"
45 
47 #ifndef DC_AUTOSHOOT_PERIOD
48 #define DC_AUTOSHOOT_PERIOD 1.0
49 #endif
50 
52 #ifndef DC_AUTOSHOOT_DISTANCE_INTERVAL
53 #define DC_AUTOSHOOT_DISTANCE_INTERVAL 50
54 #endif
55 
57 #ifndef DC_AUTOSHOOT_DISTANCE_INIT
58 #define DC_AUTOSHOOT_DISTANCE_INIT 0
59 #endif
60 
62 #ifndef DC_AUTOSHOOT_SURVEY_INTERVAL
63 #define DC_AUTOSHOOT_SURVEY_INTERVAL 50
64 #endif
65 
66 // Variables with boot defaults
70 float dc_cam_angle = 0;
71 
73 
78 
80 float dc_gps_next_dist = 0;
81 float dc_gps_x = 0;
82 float dc_gps_y = 0;
83 
84 static struct FloatVect2 last_shot_pos = {0.0, 0.0};
87 
89 #ifndef DC_SHOT_SYNC_SEND
90 #define DC_SHOT_SYNC_SEND 1
91 #endif
92 
93 #if DC_SHOT_SYNC_SEND
94 
96 
97 #include "mcu_periph/uart.h"
98 #include "pprzlink/messages.h"
100 #include "state.h"
101 #include "modules/gps/gps.h"
102 #if DC_SHOT_EXTRA_DL
104 #endif
105 
107 {
108  // angles in decideg
109  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
110  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
111  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
112  // course in decideg
113  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
114  // ground speed in cm/s
116  int16_t photo_nr = -1;
117 
119  dc_photo_nr++;
120  photo_nr = dc_photo_nr;
121  }
122 
123 #if DC_SHOT_EXTRA_DL
124  // send a message on second datalink first
125  // (for instance an embedded CPU)
126  DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
127  &photo_nr,
128  &stateGetPositionLla_i()->lat,
129  &stateGetPositionLla_i()->lon,
130  &stateGetPositionLla_i()->alt,
131  &gps.hmsl,
132  &phi,
133  &theta,
134  &psi,
135  &course,
136  &speed,
137  &gps.tow);
138 #endif
139  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
140  &photo_nr,
141  &stateGetPositionLla_i()->lat,
142  &stateGetPositionLla_i()->lon,
143  &stateGetPositionLla_i()->alt,
144  &gps.hmsl,
145  &phi,
146  &theta,
147  &psi,
148  &course,
149  &speed,
150  &gps.tow);
151 }
152 #else
153 void dc_send_shot_position(void)
154 {
155 }
156 #endif /* DC_SHOT_SYNC_SEND */
157 
158 void dc_init(void)
159 {
160  dc_exposure = 0.f;
164 }
165 
167 {
168 #ifdef DOWNLINK_SEND_DC_INFO
169  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
171  uint8_t shutter = dc_autoshoot_period * 10;
172  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
173  &mode,
174  &stateGetPositionLla_i()->lat,
175  &stateGetPositionLla_i()->lon,
176  &stateGetPositionLla_i()->alt,
177  &course,
178  &dc_photo_nr,
181  &dc_gps_x,
182  &dc_gps_y,
186  &dc_gps_count,
187  &shutter);
188 #endif
189  return 0;
190 }
191 
192 void dc_send_command_common(uint8_t cmd __attribute__((unused)))
193 {
194 #if DC_SHOT_EXTRA_DL
195  uint8_t tab[] = { cmd };
196  uint8_t dst_id = 0;
197  DOWNLINK_SEND_PAYLOAD_COMMAND(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, &dst_id, 1, tab);
198 #endif
199 }
200 
201 // weak function for set_expo, to be implemented by drivers if available
202 void WEAK dc_set_expo(float expo UNUSED) {}
203 
204 /* shoot on distance */
205 uint8_t dc_distance(float interval)
206 {
208  dc_gps_count = 0;
209  dc_distance_interval = interval;
210  last_shot_pos.x = 0;
211  last_shot_pos.y = 0;
212 
213  dc_info();
214  return 0;
215 }
216 
217 /* shoot on circle */
218 uint8_t dc_circle(float interval, float start)
219 {
221  dc_gps_count = 0;
222  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
223 
224  if (start == DC_IGNORE) {
225  start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
226  }
227 
228  dc_circle_start_angle = fmodf(start, 360.);
229  if (start < 0.) {
230  start += 360.;
231  }
232  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
234  dc_circle_max_blocks = floorf(360. / dc_circle_interval);
235  dc_info();
236  return 0;
237 }
238 
239 /* shoot on survey */
240 uint8_t dc_survey(float interval, float x, float y)
241 {
243  dc_gps_count = 0;
244  dc_survey_interval = interval;
245 
246  if (x == DC_IGNORE && y == DC_IGNORE) {
249  } else if (y == DC_IGNORE) {
250  uint8_t wp = (uint8_t)x;
251  dc_gps_x = WaypointX(wp);
252  dc_gps_y = WaypointY(wp);
253  } else {
254  dc_gps_x = x;
255  dc_gps_y = y;
256  }
258  dc_info();
259  return 0;
260 }
261 
263 {
265  dc_info();
266  return 0;
267 }
268 
269 static float dim_mod(float a, float b, float m)
270 {
271  if (a < b) {
272  float tmp = a;
273  a = b;
274  b = tmp;
275  }
276  return fminf(a - b, b + m - a);
277 }
278 
279 void dc_periodic(void)
280 {
281  static float last_shot_time = 0.;
282 
283  switch (dc_autoshoot) {
284 
285  case DC_AUTOSHOOT_PERIODIC: {
286  float now = get_sys_time_float();
287  if (now - last_shot_time > dc_autoshoot_period) {
288  last_shot_time = now;
290  }
291  }
292  break;
293 
294  case DC_AUTOSHOOT_DISTANCE: {
295  struct FloatVect2 cur_pos;
296  cur_pos.x = stateGetPositionEnu_f()->x;
297  cur_pos.y = stateGetPositionEnu_f()->y;
298  struct FloatVect2 delta_pos;
299  VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
300  float dist_to_last_shot = float_vect2_norm(&delta_pos);
301  if (dist_to_last_shot > dc_distance_interval) {
302  dc_gps_count++;
304  VECT2_COPY(last_shot_pos, cur_pos);
305  }
306  }
307  break;
308 
309  case DC_AUTOSHOOT_CIRCLE: {
310  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
311  if (course < 0.) {
312  course += 360.;
313  }
314  float current_block = floorf(course / dc_circle_interval);
315 
316  if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
317  dc_gps_count++;
318  dc_circle_last_block = current_block;
320  }
321  }
322  break;
323 
324  case DC_AUTOSHOOT_SURVEY: {
325  float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
326  float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
327 
328  if (dist_x * dist_x + dist_y * dist_y >= dc_gps_next_dist * dc_gps_next_dist) {
330  dc_gps_count++;
332  }
333  }
334  break;
335 
336  default:
338  }
339 }
static int16_t course[3]
Definition: airspeed_uADC.c:58
void dc_send_command(uint8_t cmd)
Send Command To Camera.
uint8_t last_wp UNUSED
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointY(_wp)
Definition: common_nav.h:46
#define DC_AUTOSHOOT_PERIOD
default time interval for periodic mode: 1sec
Definition: dc.c:48
float dc_circle_start_angle
angle a where first image will be taken at a + delta
Definition: dc.c:75
#define DC_AUTOSHOOT_DISTANCE_INIT
default distance of the first shoot: 0m (start immediately)
Definition: dc.c:58
#define DC_AUTOSHOOT_SURVEY_INTERVAL
default distance interval for survey mode: 50m
Definition: dc.c:63
float dc_survey_interval
distance between dc shots in meters
Definition: dc.c:79
void dc_periodic(void)
periodic function
Definition: dc.c:279
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:240
void dc_init(void)
initialize settings
Definition: dc.c:158
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:85
dc_autoshoot_type dc_autoshoot
Definition: dc.c:67
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:262
void dc_send_command_common(uint8_t cmd)
Command sending function.
Definition: dc.c:192
float dc_circle_last_block
Definition: dc.c:76
#define DC_AUTOSHOOT_DISTANCE_INTERVAL
default distance interval for distance mode: 50m
Definition: dc.c:53
static float dim_mod(float a, float b, float m)
Definition: dc.c:269
float dc_gps_next_dist
Definition: dc.c:80
uint16_t dc_photo_nr
export the number of the last photo
Definition: dc.c:95
float dc_gps_x
point of reference for the survey mode
Definition: dc.c:81
uint8_t dc_distance(float interval)
Sets the dc control in distance mode.
Definition: dc.c:205
uint8_t dc_cam_tracing
Definition: dc.c:69
float dc_autoshoot_period
AutoShoot photos every X seconds.
Definition: dc.c:86
uint16_t dc_gps_count
number of images taken since the last change of dc_mode
Definition: dc.c:68
void dc_send_shot_position(void)
Send Down the coordinates of where the photo was taken.
Definition: dc.c:106
float dc_exposure
camera exposure
Definition: dc.c:72
uint8_t dc_info(void)
Send an info message.
Definition: dc.c:166
float dc_gps_y
Definition: dc.c:82
float dc_circle_interval
angle between dc shots in degree
Definition: dc.c:74
uint8_t dc_circle(float interval, float start)
Sets the dc control in circle mode.
Definition: dc.c:218
float dc_cam_angle
camera angle
Definition: dc.c:70
float dc_circle_max_blocks
Definition: dc.c:77
static struct FloatVect2 last_shot_pos
Definition: dc.c:84
void WEAK dc_set_expo(float expo UNUSED)
Set camera exposure.
Definition: dc.c:202
Standard Digital Camera Control Interface.
#define DC_IMAGE_BUFFER
Definition: dc.h:152
@ DC_SHOOT
Definition: dc.h:102
#define DC_IGNORE
Definition: dc.h:147
dc_autoshoot_type
Auotmatic Digital Camera Photo Triggering modes.
Definition: dc.h:132
@ DC_AUTOSHOOT_DISTANCE
Definition: dc.h:135
@ DC_AUTOSHOOT_PERIODIC
Definition: dc.h:134
@ DC_AUTOSHOOT_SURVEY
Definition: dc.h:137
@ DC_AUTOSHOOT_CIRCLE
Definition: dc.h:138
@ DC_AUTOSHOOT_STOP
Definition: dc.h:133
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:57
Extra datalink and telemetry using PPRZ protocol.
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
static float float_vect2_norm(struct FloatVect2 *v)
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
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
Rotorcraft navigation functions.
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
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
float b
Definition: wedgebug.c:202