Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
dc.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 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  */
22 
23 
24 #include "dc.h"
25 
26 // Variables with boot defaults
32 float dc_cam_angle = 0;
33 
38 
39 float dc_gps_dist = 50;
40 float dc_gps_next_dist = 0;
41 float dc_gps_x = 0;
42 float dc_gps_y = 0;
43 
44 bool_t dc_probing = FALSE;
45 
46 
47 #ifdef SENSOR_SYNC_SEND
48 
49 uint16_t dc_photo_nr = 0;
50 uint16_t dc_buffer = 0;
51 
52 #ifndef DOWNLINK_DEVICE
53 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
54 #endif
55 #include "mcu_periph/uart.h"
56 #include "messages.h"
58 #include "estimator.h"
59 #include "subsystems/gps.h"
60 
61 void dc_send_shot_position(void)
62 {
63  int16_t phi = DegOfRad(estimator_phi*10.0f);
64  int16_t theta = DegOfRad(estimator_theta*10.0f);
65  float gps_z = ((float)gps.hmsl) / 1000.0f;
66  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
67  int16_t photo_nr = -1;
68 
69  if (dc_buffer < DC_IMAGE_BUFFER) {
70  dc_buffer++;
71  dc_photo_nr++;
72  photo_nr = dc_photo_nr;
73  }
74 
75  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
76  &photo_nr,
77  &gps.utm_pos.east,
78  &gps.utm_pos.north,
79  &gps_z,
80  &gps.utm_pos.zone,
81  &phi,
82  &theta,
83  &course,
84  &gps.gspeed,
85  &gps.tow);
86 }
87 #endif /* SENSOR_SYNC_SEND */
88 
90 #ifdef DOWNLINK_SEND_DC_INFO
91  float course = DegOfRad(estimator_psi);
92  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
93  &dc_autoshoot,
94  &estimator_x,
95  &estimator_y,
96  &course,
97  &dc_buffer,
98  &dc_gps_dist,
100  &dc_gps_x,
101  &dc_gps_y,
105  &dc_gps_count,
107 #endif
108  return 0;
109 }
110 
111 /* shoot on circle */
112 uint8_t dc_circle(float interval, float start) {
114  dc_gps_count = 0;
115  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
116 
117  if(start == DC_IGNORE) {
118  start = DegOfRad(estimator_psi);
119  }
120 
121  dc_circle_start_angle = fmodf(start, 360.);
122  if (start < 0.)
123  start += 360.;
124  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
127  dc_probing = TRUE;
128  dc_info();
129  return 0;
130 }
131 
132 /* shoot on survey */
133 uint8_t dc_survey(float interval, float x, float y) {
135  dc_gps_count = 0;
136  dc_gps_dist = interval;
137 
138  if (x == DC_IGNORE && y == DC_IGNORE) {
141  } else if (y == DC_IGNORE) {
142  dc_gps_x = waypoints[(uint8_t)x].x;
143  dc_gps_y = waypoints[(uint8_t)x].y;
144  } else {
145  dc_gps_x = x;
146  dc_gps_y = y;
147  }
148  dc_gps_next_dist = 0;
149  dc_info();
150  return 0;
151 }
152 
155  dc_info();
156  return 0;
157 }
158 
159 
160 /*
161 #ifndef DC_GPS_TRIGGER_START
162 #define DC_GPS_TRIGGER_START 1
163 #endif
164 #ifndef DC_GPS_TRIGGER_STOP
165 #define DC_GPS_TRIGGER_STOP 3
166 #endif
167 
168 
169 static inline void dc_shoot_on_gps( void ) {
170  static uint8_t gps_msg_counter = 0;
171 
172  if (dc_shoot > 0)
173  {
174 
175  if (gps_msg_counter == 0)
176  {
177  DC_PUSH(DC_SHUTTER_LED);
178 
179  dc_send_shot_position();
180  }
181  else if (gps_msg_counter == DC_GPS_TRIGGER_START)
182  {
183  DC_RELEASE(DC_SHUTTER_LED);
184  }
185 
186  gps_msg_counter++;
187  if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
188  gps_msg_counter = 0;
189  }
190 }
191 */
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
dc_autoshoot_type
Definition: dc.h:106
#define dc_send_shot_position()
Definition: dc.h:126
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
uint8_t dc_info(void)
Send an info message containing information about position, course, buffer and all other internal var...
Definition: dc.c:89
uint8_t zone
UTM zone number.
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float dc_gps_y
Definition: dc.c:42
#define DC_IMAGE_BUFFER
Definition: dc.h:136
float estimator_y
north position in meters
Definition: estimator.c:43
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float dc_circle_last_block
Definition: dc.c:36
#define FALSE
Definition: imu_chimu.h:141
float dc_circle_interval
Definition: dc.c:34
uint8_t dc_circle(float interval, float start)
Sets the dc control in circle mode.
Definition: dc.c:112
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:133
Standard Digital Camera Control Interface.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float dc_circle_max_blocks
Definition: dc.c:37
Device independent GPS code (interface)
dc_autoshoot_type dc_autoshoot
Definition: dc.c:29
uint8_t dc_autoshoot_meter_grid
Definition: dc.c:27
float dc_gps_x
Definition: dc.c:41
signed short int16_t
Definition: types.h:17
float dc_gps_next_dist
Definition: dc.c:40
float dc_gps_dist
Definition: dc.c:39
bool_t dc_probing
Definition: dc.c:44
float estimator_x
east position in meters
Definition: estimator.c:42
int32_t north
in centimeters
uint8_t dc_autoshoot_quartersec_period
Definition: dc.c:28
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
float dc_circle_start_angle
Definition: dc.c:35
int32_t east
in centimeters
unsigned char uint8_t
Definition: types.h:14
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:153
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
State estimation, fusioning sensors.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
uint8_t dc_cam_tracing
Definition: dc.c:31
uint16_t dc_gps_count
Definition: dc.c:30
struct GpsState gps
global GPS state
Definition: gps.c:31
#define DC_IGNORE
Definition: dc.h:131
float dc_cam_angle
Definition: dc.c:32