Paparazzi UAS  v5.18.0_stable
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 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_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 
76 
78 float dc_gps_next_dist = 0;
79 float dc_gps_x = 0;
80 float dc_gps_y = 0;
81 
82 static struct FloatVect2 last_shot_pos = {0.0, 0.0};
85 
87 #ifndef DC_SHOT_SYNC_SEND
88 #define DC_SHOT_SYNC_SEND 1
89 #endif
90 
91 #if DC_SHOT_SYNC_SEND
92 
94 
95 #include "mcu_periph/uart.h"
96 #include "pprzlink/messages.h"
98 #include "state.h"
99 #include "subsystems/gps.h"
100 #if DC_SHOT_EXTRA_DL
102 #endif
103 
105 {
106  // angles in decideg
107  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
108  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
109  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
110  // course in decideg
111  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
112  // ground speed in cm/s
114  int16_t photo_nr = -1;
115 
117  dc_photo_nr++;
118  photo_nr = dc_photo_nr;
119  }
120 
121 #if DC_SHOT_EXTRA_DL
122  // send a message on second datalink first
123  // (for instance an embedded CPU)
124  DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
125  &photo_nr,
126  &stateGetPositionLla_i()->lat,
127  &stateGetPositionLla_i()->lon,
128  &stateGetPositionLla_i()->alt,
129  &gps.hmsl,
130  &phi,
131  &theta,
132  &psi,
133  &course,
134  &speed,
135  &gps.tow);
136 #endif
137  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
138  &photo_nr,
139  &stateGetPositionLla_i()->lat,
140  &stateGetPositionLla_i()->lon,
141  &stateGetPositionLla_i()->alt,
142  &gps.hmsl,
143  &phi,
144  &theta,
145  &psi,
146  &course,
147  &speed,
148  &gps.tow);
149 }
150 #else
151 void dc_send_shot_position(void)
152 {
153 }
154 #endif /* DC_SHOT_SYNC_SEND */
155 
156 void dc_init(void)
157 {
161 }
162 
164 {
165 #ifdef DOWNLINK_SEND_DC_INFO
166  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
168  uint8_t shutter = dc_autoshoot_period * 10;
169  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
170  &mode,
171  &stateGetPositionLla_i()->lat,
172  &stateGetPositionLla_i()->lon,
173  &stateGetPositionLla_i()->alt,
174  &course,
175  &dc_photo_nr,
178  &dc_gps_x,
179  &dc_gps_y,
183  &dc_gps_count,
184  &shutter);
185 #endif
186  return 0;
187 }
188 
189 void dc_send_command_common(uint8_t cmd __attribute__((unused)))
190 {
191 #if DC_SHOT_EXTRA_DL
192  uint8_t tab[] = { cmd };
193  DOWNLINK_SEND_PAYLOAD_COMMAND(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, 0, 1, tab);
194 #endif
195 }
196 
197 
198 /* shoot on distance */
199 uint8_t dc_distance(float interval)
200 {
202  dc_gps_count = 0;
203  dc_distance_interval = interval;
204  last_shot_pos.x = 0;
205  last_shot_pos.y = 0;
206 
207  dc_info();
208  return 0;
209 }
210 
211 /* shoot on circle */
212 uint8_t dc_circle(float interval, float start)
213 {
215  dc_gps_count = 0;
216  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
217 
218  if (start == DC_IGNORE) {
219  start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
220  }
221 
222  dc_circle_start_angle = fmodf(start, 360.);
223  if (start < 0.) {
224  start += 360.;
225  }
226  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
228  dc_circle_max_blocks = floorf(360. / dc_circle_interval);
229  dc_info();
230  return 0;
231 }
232 
233 /* shoot on survey */
234 uint8_t dc_survey(float interval, float x, float y)
235 {
237  dc_gps_count = 0;
238  dc_survey_interval = interval;
239 
240  if (x == DC_IGNORE && y == DC_IGNORE) {
243  } else if (y == DC_IGNORE) {
244  uint8_t wp = (uint8_t)x;
245  dc_gps_x = WaypointX(wp);
246  dc_gps_y = WaypointY(wp);
247  } else {
248  dc_gps_x = x;
249  dc_gps_y = y;
250  }
252  dc_info();
253  return 0;
254 }
255 
257 {
259  dc_info();
260  return 0;
261 }
262 
263 static float dim_mod(float a, float b, float m)
264 {
265  if (a < b) {
266  float tmp = a;
267  a = b;
268  b = tmp;
269  }
270  return fminf(a - b, b + m - a);
271 }
272 
273 void dc_periodic(void)
274 {
275  static float last_shot_time = 0.;
276 
277  switch (dc_autoshoot) {
278 
279  case DC_AUTOSHOOT_PERIODIC: {
280  float now = get_sys_time_float();
281  if (now - last_shot_time > dc_autoshoot_period) {
282  last_shot_time = now;
284  }
285  }
286  break;
287 
288  case DC_AUTOSHOOT_DISTANCE: {
289  struct FloatVect2 cur_pos;
290  cur_pos.x = stateGetPositionEnu_f()->x;
291  cur_pos.y = stateGetPositionEnu_f()->y;
292  struct FloatVect2 delta_pos;
293  VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
294  float dist_to_last_shot = float_vect2_norm(&delta_pos);
295  if (dist_to_last_shot > dc_distance_interval) {
296  dc_gps_count++;
298  VECT2_COPY(last_shot_pos, cur_pos);
299  }
300  }
301  break;
302 
303  case DC_AUTOSHOOT_CIRCLE: {
304  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
305  if (course < 0.) {
306  course += 360.;
307  }
308  float current_block = floorf(course / dc_circle_interval);
309 
310  if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
311  dc_gps_count++;
312  dc_circle_last_block = current_block;
314  }
315  }
316  break;
317 
318  case DC_AUTOSHOOT_SURVEY: {
319  float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
320  float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
321 
322  if (dist_x * dist_x + dist_y * dist_y >= dc_gps_next_dist * dc_gps_next_dist) {
324  dc_gps_count++;
326  }
327  }
328  break;
329 
330  default:
332  }
333 }
dc_send_command_common
void dc_send_command_common(uint8_t cmd)
Command sending function.
Definition: dc.c:189
dc_autoshoot
dc_autoshoot_type dc_autoshoot
Definition: dc.c:67
uint16_t
unsigned short uint16_t
Definition: types.h:16
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
dc_cam_tracing
uint8_t dc_cam_tracing
Definition: dc.c:69
dc_circle_start_angle
float dc_circle_start_angle
angle a where first image will be taken at a + delta
Definition: dc.c:73
dc_photo_nr
uint16_t dc_photo_nr
export the number of the last photo
Definition: dc.c:93
b
float b
Definition: wedgebug.c:202
last_shot_pos
static struct FloatVect2 last_shot_pos
Definition: dc.c:82
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
DC_AUTOSHOOT_PERIOD
#define DC_AUTOSHOOT_PERIOD
default time interval for periodic mode: 1sec
Definition: dc.c:48
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
DC_AUTOSHOOT_CIRCLE
@ DC_AUTOSHOOT_CIRCLE
Definition: dc.h:133
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
DC_IGNORE
#define DC_IGNORE
Definition: dc.h:142
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
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_stop
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:256
common_nav.h
dc_circle_max_blocks
float dc_circle_max_blocks
Definition: dc.c:75
extra_pprz_dl.h
dc_autoshoot_type
dc_autoshoot_type
Auotmatic Digital Camera Photo Triggering modes.
Definition: dc.h:127
DC_AUTOSHOOT_SURVEY
@ DC_AUTOSHOOT_SURVEY
Definition: dc.h:132
DC_AUTOSHOOT_PERIODIC
@ DC_AUTOSHOOT_PERIODIC
Definition: dc.h:129
dc_distance_interval
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition: dc.c:83
dim_mod
static float dim_mod(float a, float b, float m)
Definition: dc.c:263
DC_AUTOSHOOT_DISTANCE_INIT
#define DC_AUTOSHOOT_DISTANCE_INIT
default distance of the first shoot: 0m (start immediately)
Definition: dc.c:58
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
dc.h
dc_autoshoot_period
float dc_autoshoot_period
AutoShoot photos every X seconds.
Definition: dc.c:84
DC_IMAGE_BUFFER
#define DC_IMAGE_BUFFER
Definition: dc.h:147
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
dc_distance
uint8_t dc_distance(float interval)
Sets the dc control in distance mode.
Definition: dc.c:199
FloatVect2
Definition: pprz_algebra_float.h:49
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
dc_gps_count
uint16_t dc_gps_count
number of images taken since the last change of dc_mode
Definition: dc.c:68
gps.h
Device independent GPS code (interface)
dc_send_command
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Definition: atmega_i2c_cam_ctrl.c:41
dc_circle
uint8_t dc_circle(float interval, float start)
Sets the dc control in circle mode.
Definition: dc.c:212
int16_t
signed short int16_t
Definition: types.h:17
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
dc_send_shot_position
void dc_send_shot_position(void)
Send Down the coordinates of where the photo was taken.
Definition: dc.c:104
DC_AUTOSHOOT_SURVEY_INTERVAL
#define DC_AUTOSHOOT_SURVEY_INTERVAL
default distance interval for survey mode: 50m
Definition: dc.c:63
extra_pprz_tp
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:57
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
dc_info
uint8_t dc_info(void)
Send an info message.
Definition: dc.c:163
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
dc_gps_y
float dc_gps_y
Definition: dc.c:80
DC_AUTOSHOOT_DISTANCE
@ DC_AUTOSHOOT_DISTANCE
Definition: dc.h:130
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
dc_gps_next_dist
float dc_gps_next_dist
Definition: dc.c:78
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
DC_SHOOT
@ DC_SHOOT
Definition: dc.h:100
DC_AUTOSHOOT_DISTANCE_INTERVAL
#define DC_AUTOSHOOT_DISTANCE_INTERVAL
default distance interval for distance mode: 50m
Definition: dc.c:53
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
navigation.h
dc_cam_angle
float dc_cam_angle
camera angle
Definition: dc.c:70
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
mode
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
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
dc_circle_interval
float dc_circle_interval
angle between dc shots in degree
Definition: dc.c:72
dc_init
void dc_init(void)
initialize settings
Definition: dc.c:156
DC_AUTOSHOOT_STOP
@ DC_AUTOSHOOT_STOP
Definition: dc.h:128
dc_circle_last_block
float dc_circle_last_block
Definition: dc.c:74
dc_survey
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:234
dc_periodic
void dc_periodic(void)
periodic function
Definition: dc.c:273
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
dc_gps_x
float dc_gps_x
point of reference for the survey mode
Definition: dc.c:79
dc_survey_interval
float dc_survey_interval
distance between dc shots in meters
Definition: dc.c:77