Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 AP
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_SURVEY_INTERVAL
58 #define DC_AUTOSHOOT_SURVEY_INTERVAL 50
59 #endif
60 
61 // Variables with boot defaults
65 float dc_cam_angle = 0;
66 
71 
73 float dc_gps_next_dist = 0;
74 float dc_gps_x = 0;
75 float dc_gps_y = 0;
76 
77 static struct FloatVect2 last_shot_pos = {0.0, 0.0};
80 
82 #ifndef DC_SHOT_SYNC_SEND
83 #define DC_SHOT_SYNC_SEND 1
84 #endif
85 
86 #if DC_SHOT_SYNC_SEND
87 
89 
90 #include "mcu_periph/uart.h"
91 #include "pprzlink/messages.h"
93 #include "state.h"
94 #include "subsystems/gps.h"
95 
97 {
98  // angles in decideg
99  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
100  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
101  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
102  // course in decideg
103  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
104  // ground speed in cm/s
106  int16_t photo_nr = -1;
107 
109  dc_photo_nr++;
110  photo_nr = dc_photo_nr;
111  }
112 
113  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
114  &photo_nr,
115  &stateGetPositionLla_i()->lat,
116  &stateGetPositionLla_i()->lon,
117  &stateGetPositionLla_i()->alt,
118  &gps.hmsl,
119  &phi,
120  &theta,
121  &psi,
122  &course,
123  &speed,
124  &gps.tow);
125 }
126 #else
127 void dc_send_shot_position(void)
128 {
129 }
130 #endif /* DC_SHOT_SYNC_SEND */
131 
132 void dc_init(void)
133 {
137 }
138 
140 {
141 #ifdef DOWNLINK_SEND_DC_INFO
142  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
144  uint8_t shutter = dc_autoshoot_period * 10;
145  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
146  &mode,
147  &stateGetPositionLla_i()->lat,
148  &stateGetPositionLla_i()->lon,
149  &stateGetPositionLla_i()->alt,
150  &course,
151  &dc_photo_nr,
154  &dc_gps_x,
155  &dc_gps_y,
159  &dc_gps_count,
160  &shutter);
161 #endif
162  return 0;
163 }
164 
165 /* shoot on distance */
166 uint8_t dc_distance(float interval)
167 {
169  dc_gps_count = 0;
170  dc_distance_interval = interval;
171  last_shot_pos.x = 0;
172  last_shot_pos.y = 0;
173 
174  dc_info();
175  return 0;
176 }
177 
178 /* shoot on circle */
179 uint8_t dc_circle(float interval, float start)
180 {
182  dc_gps_count = 0;
183  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
184 
185  if (start == DC_IGNORE) {
186  start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
187  }
188 
189  dc_circle_start_angle = fmodf(start, 360.);
190  if (start < 0.) {
191  start += 360.;
192  }
193  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
195  dc_circle_max_blocks = floorf(360. / dc_circle_interval);
196  dc_info();
197  return 0;
198 }
199 
200 /* shoot on survey */
201 uint8_t dc_survey(float interval, float x, float y)
202 {
204  dc_gps_count = 0;
205  dc_survey_interval = interval;
206 
207  if (x == DC_IGNORE && y == DC_IGNORE) {
210  } else if (y == DC_IGNORE) {
211  uint8_t wp = (uint8_t)x;
212  dc_gps_x = WaypointX(wp);
213  dc_gps_y = WaypointY(wp);
214  } else {
215  dc_gps_x = x;
216  dc_gps_y = y;
217  }
218  dc_gps_next_dist = 0;
219  dc_info();
220  return 0;
221 }
222 
224 {
226  dc_info();
227  return 0;
228 }
229 
230 static float dim_mod(float a, float b, float m)
231 {
232  if (a < b) {
233  float tmp = a;
234  a = b;
235  b = tmp;
236  }
237  return fminf(a - b, b + m - a);
238 }
239 
240 void dc_periodic(void)
241 {
242  static float last_shot_time = 0.;
243 
244  switch (dc_autoshoot) {
245 
246  case DC_AUTOSHOOT_PERIODIC: {
247  float now = get_sys_time_float();
248  if (now - last_shot_time > dc_autoshoot_period) {
249  last_shot_time = now;
251  }
252  }
253  break;
254 
255  case DC_AUTOSHOOT_DISTANCE: {
256  struct FloatVect2 cur_pos;
257  cur_pos.x = stateGetPositionEnu_f()->x;
258  cur_pos.y = stateGetPositionEnu_f()->y;
259  struct FloatVect2 delta_pos;
260  VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
261  float dist_to_last_shot = float_vect2_norm(&delta_pos);
262  if (dist_to_last_shot > dc_distance_interval) {
263  dc_gps_count++;
265  VECT2_COPY(last_shot_pos, cur_pos);
266  }
267  }
268  break;
269 
270  case DC_AUTOSHOOT_CIRCLE: {
271  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
272  if (course < 0.) {
273  course += 360.;
274  }
275  float current_block = floorf(course / dc_circle_interval);
276 
277  if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
278  dc_gps_count++;
279  dc_circle_last_block = current_block;
281  }
282  }
283  break;
284 
285  case DC_AUTOSHOOT_SURVEY: {
286  float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
287  float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
288 
289  if (dist_x * dist_x + dist_y * dist_y >= dc_gps_next_dist * dc_gps_next_dist) {
290  dc_gps_next_dist += dc_survey_interval;
291  dc_gps_count++;
293  }
294  }
295  break;
296 
297  default:
299  }
300 }
unsigned short uint16_t
Definition: types.h:16
#define DC_AUTOSHOOT_SURVEY_INTERVAL
default distance interval for survey mode: 50m
Definition: dc.c:58
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
dc_autoshoot_type
Auotmatic Digital Camera Photo Triggering modes.
Definition: dc.h:124
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint8_t dc_info(void)
Send an info message.
Definition: dc.c:139
float dc_gps_y
Definition: dc.c:75
#define DC_IMAGE_BUFFER
Definition: dc.h:144
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:67
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
uint16_t dc_photo_nr
export the number of the last photo
Definition: dc.c:88
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
float dc_circle_last_block
Definition: dc.c:69
float dc_circle_interval
angle between dc shots in degree
Definition: dc.c:67
uint8_t dc_circle(float interval, float start)
Sets the dc control in circle mode.
Definition: dc.c:179
void dc_init(void)
initialize settings
Definition: dc.c:132
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:201
Definition: dc.h:100
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
Standard Digital Camera Control Interface.
float dc_circle_max_blocks
Definition: dc.c:70
#define WaypointX(_wp)
Definition: common_nav.h:45
static float float_vect2_norm(struct FloatVect2 *v)
float x
in meters
Architecture independent timing functions.
static float dim_mod(float a, float b, float m)
Definition: dc.c:230
uint32_t tow
GPS time of week in ms.
Definition: gps.h:101
Device independent GPS code (interface)
dc_autoshoot_type dc_autoshoot
Definition: dc.c:62
uint8_t dc_distance(float interval)
Sets the dc control in distance mode.
Definition: dc.c:166
float dc_gps_x
point of reference for the survey mode
Definition: dc.c:74
signed short int16_t
Definition: types.h:17
float dc_gps_next_dist
Definition: dc.c:73
#define DC_AUTOSHOOT_PERIOD
default time interval for periodic mode: 1sec
Definition: dc.c:48
static struct FloatVect2 last_shot_pos
Definition: dc.c:77
#define WaypointY(_wp)
Definition: common_nav.h:46
float dc_circle_start_angle
angle a where first image will be taken at a + delta
Definition: dc.c:68
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
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:240
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:66
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:223
static int16_t course[3]
Definition: airspeed_uADC.c:57
float dc_survey_interval
distance between dc shots in meters
Definition: dc.c:72
uint8_t dc_cam_tracing
Definition: dc.c:64
float dc_autoshoot_period
AutoShoot photos every X seconds.
Definition: dc.c:79
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:78
uint16_t dc_gps_count
number of images taken since the last change of dc_mode
Definition: dc.c:63
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:75
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
#define DC_AUTOSHOOT_DISTANCE_INTERVAL
default distance interval for distance mode: 50m
Definition: dc.c:53
#define DC_IGNORE
Definition: dc.h:139
void dc_send_shot_position(void)
Send Down the coordinates of where the photo was taken.
Definition: dc.c:96
float dc_cam_angle
camera angle
Definition: dc.c:65