Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 #include "mcu_periph/uart.h"
53 #include "messages.h"
55 #include "state.h"
56 #include "subsystems/gps.h"
57 
58 void dc_send_shot_position(void)
59 {
60  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
61  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
62  float gps_z = ((float)gps.hmsl) / 1000.0f;
63  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
64  int16_t photo_nr = -1;
65 
66  if (dc_buffer < DC_IMAGE_BUFFER) {
67  dc_buffer++;
68  dc_photo_nr++;
69  photo_nr = dc_photo_nr;
70  }
71 
72  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
73  &photo_nr,
74  &gps.utm_pos.east,
75  &gps.utm_pos.north,
76  &gps_z,
77  &gps.utm_pos.zone,
78  &phi,
79  &theta,
80  &course,
81  &gps.gspeed,
82  &gps.tow);
83 }
84 #endif /* SENSOR_SYNC_SEND */
85 
87 #ifdef DOWNLINK_SEND_DC_INFO
88  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
89  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
90  &dc_autoshoot,
93  &course,
94  &dc_buffer,
95  &dc_gps_dist,
97  &dc_gps_x,
98  &dc_gps_y,
102  &dc_gps_count,
104 #endif
105  return 0;
106 }
107 
108 /* shoot on circle */
109 uint8_t dc_circle(float interval, float start) {
111  dc_gps_count = 0;
112  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
113 
114  if(start == DC_IGNORE) {
115  start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
116  }
117 
118  dc_circle_start_angle = fmodf(start, 360.);
119  if (start < 0.)
120  start += 360.;
121  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
124  dc_probing = TRUE;
125  dc_info();
126  return 0;
127 }
128 
129 /* shoot on survey */
130 uint8_t dc_survey(float interval, float x, float y) {
132  dc_gps_count = 0;
133  dc_gps_dist = interval;
134 
135  if (x == DC_IGNORE && y == DC_IGNORE) {
138  } else if (y == DC_IGNORE) {
139  dc_gps_x = waypoints[(uint8_t)x].x;
140  dc_gps_y = waypoints[(uint8_t)x].y;
141  } else {
142  dc_gps_x = x;
143  dc_gps_y = y;
144  }
145  dc_gps_next_dist = 0;
146  dc_info();
147  return 0;
148 }
149 
152  dc_info();
153  return 0;
154 }
155 
156 
157 /*
158 #ifndef DC_GPS_TRIGGER_START
159 #define DC_GPS_TRIGGER_START 1
160 #endif
161 #ifndef DC_GPS_TRIGGER_STOP
162 #define DC_GPS_TRIGGER_STOP 3
163 #endif
164 
165 
166 static inline void dc_shoot_on_gps( void ) {
167  static uint8_t gps_msg_counter = 0;
168 
169  if (dc_shoot > 0)
170  {
171 
172  if (gps_msg_counter == 0)
173  {
174  DC_PUSH(DC_SHUTTER_LED);
175 
176  dc_send_shot_position();
177  }
178  else if (gps_msg_counter == DC_GPS_TRIGGER_START)
179  {
180  DC_RELEASE(DC_SHUTTER_LED);
181  }
182 
183  gps_msg_counter++;
184  if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
185  gps_msg_counter = 0;
186  }
187 }
188 */
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
dc_autoshoot_type
Definition: dc.h:106
#define dc_send_shot_position()
Definition: dc.h:126
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
uint8_t dc_info(void)
Send an info message containing information about position, course, buffer and all other internal var...
Definition: dc.c:86
uint8_t zone
UTM zone number.
float dc_gps_y
Definition: dc.c:42
#define DC_IMAGE_BUFFER
Definition: dc.h:136
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float dc_circle_last_block
Definition: dc.c:36
float y
in meters
#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:109
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition: dc.c:130
Standard Digital Camera Control Interface.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
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 x
in meters
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
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
API to get/set the generic vehicle states.
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:150
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
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:41
#define DC_IGNORE
Definition: dc.h:131
float dc_cam_angle
Definition: dc.c:32