Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
cam.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
29 #include <math.h>
30 #include "cam.h"
31 #include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 #include "estimator.h"
36 #ifdef POINT_CAM
37 #include "point.h"
38 #endif // POINT_CAM
39 
40 #ifdef TEST_CAM
41 float test_cam_estimator_x;
42 float test_cam_estimator_y;
43 float test_cam_estimator_z;
44 float test_cam_estimator_phi;
45 float test_cam_estimator_theta;
46 float test_cam_estimator_hspeed_dir;
47 #endif // TEST_CAM
48 
49 //FIXME: use radians
50 #ifdef CAM_PAN_NEUTRAL
51 #if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
52 #error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
53 #endif
54 #if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
55 #error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
56 #endif
57 #endif
58 
59 //FIXME: use radians
60 #ifdef CAM_TILT_NEUTRAL
61 #if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL))
62 #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
63 #endif
64 #if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
65 #error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
66 #endif
67 #endif
68 
69 //FIXME: use radians
70 #ifndef CAM_PAN0
71 #define CAM_PAN0 RadOfDeg(0)
72 #endif
73 float cam_pan_c;
74 
75 //FIXME: use radians
76 #ifndef CAM_TILT0
77 #define CAM_TILT0 RadOfDeg(0)
78 #endif
79 float cam_tilt_c;
80 
81 float cam_phi_c;
83 
87 
88 #ifndef CAM_MODE0
89 #define CAM_MODE0 CAM_MODE_OFF
90 #endif
92 bool_t cam_lock;
93 
96 
97 void cam_nadir(void);
98 void cam_angles(void);
99 void cam_target(void);
100 void cam_waypoint_target(void);
101 void cam_ac_target(void);
102 
103 void cam_init( void ) {
105 }
106 
107 void cam_periodic( void ) {
108 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
109  //Position the camera for straight view.
110  if (pprz_mode == PPRZ_MODE_AUTO2){
111 #endif
112  switch (cam_mode) {
113  case CAM_MODE_OFF:
114  cam_pan_c = RadOfDeg(CAM_PAN0);
115  cam_tilt_c = RadOfDeg(CAM_TILT0);
116  cam_angles();
117  break;
118  case CAM_MODE_ANGLES:
119  cam_angles();
120  break;
121  case CAM_MODE_NADIR:
122  cam_nadir();
123  break;
124  case CAM_MODE_XY_TARGET:
125  cam_target();
126  break;
127  case CAM_MODE_WP_TARGET:
129  break;
130  case CAM_MODE_AC_TARGET:
131  cam_ac_target();
132  break;
133  // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
134  // The "TARGET" waypoint coordinates are not used.
135  // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
136  case CAM_MODE_STABILIZED:
138  break;
139  // In this mode the angles come from the pan and tilt radio channels.
140  // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
141  // in order to calculate the coordinates of where the camera is looking.
142  case CAM_MODE_RC:
144  break;
145  }
146 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
147  }else if (pprz_mode == PPRZ_MODE_AUTO1){
148  //Position the camera for straight view.
149 
150 #if defined(CAM_TILT_POSITION_FOR_FPV)
151  cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
152 #else
153  cam_tilt_c = RadOfDeg(90);
154 #endif
155 #if defined(CAM_PAN_POSITION_FOR_FPV)
156  cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
157 #else
158  cam_pan_c = RadOfDeg(0);
159 #endif
160  cam_angles();
161 #ifdef SHOW_CAM_COORDINATES
162  cam_point_lon = 0;
163  cam_point_lat = 0;
164  cam_point_distance_from_home = 0;
165 #endif
166  }
167 #endif
168 
169 
170 #if defined(COMMAND_CAM_PWR_SW)
171  if(video_tx_state){ ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; }else{ ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
172 #elif defined(VIDEO_TX_SWITCH)
173  if(video_tx_state){ LED_OFF(VIDEO_TX_SWITCH); }else{ LED_ON(VIDEO_TX_SWITCH); }
174 #endif
175 }
176 
178 void cam_angles( void ) {
179  float cam_pan = 0;
180  float cam_tilt = 0;
181  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
182  cam_pan_c = RadOfDeg(CAM_PAN_MAX);
183  } else {
184  if (cam_pan_c < RadOfDeg(CAM_PAN_MIN))
185  cam_pan_c = RadOfDeg(CAM_PAN_MIN);
186  }
187 
188  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){
189  cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
190  } else {
191  if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN))
192  cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
193  }
194 
195 #ifdef CAM_PAN_NEUTRAL
196  float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
197  if (pan_diff > 0)
198  cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
199  else
200  cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
201 #else
202  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX-CAM_PAN_MIN) );
203 #endif
204 
205 #ifdef CAM_TILT_NEUTRAL
206  float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
207  if (tilt_diff > 0)
208  cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
209  else
210  cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
211 #else
212  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_TILT_MAX-CAM_TILT_MIN) );
213 #endif
214 
215  cam_pan = TRIM_PPRZ(cam_pan);
216  cam_tilt = TRIM_PPRZ(cam_tilt);
217 
220 
221 #ifdef COMMAND_CAM_PAN
222  ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
223 #endif
224 #ifdef COMMAND_CAM_TILT
225  ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
226 #endif
227 }
228 
230 void cam_target( void ) {
231 #ifdef TEST_CAM
232  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
233  test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
235  &cam_pan_c, &cam_tilt_c);
236 #else
240  &cam_pan_c, &cam_tilt_c);
241 #endif
242  cam_angles();
243 }
244 
246 void cam_nadir( void ) {
247 #ifdef TEST_CAM
248  cam_target_x = test_cam_estimator_x;
249  cam_target_y = test_cam_estimator_y;
250 #else
253 #endif
254  cam_target_alt = -10;
255  cam_target();
256 }
257 
258 
259 void cam_waypoint_target( void ) {
260  if (cam_target_wp < nb_waypoint) {
263  }
265  cam_target();
266 }
267 
268 void cam_ac_target( void ) {
269 #ifdef TRAFFIC_INFO
270  struct ac_info_ * ac = get_ac_info(cam_target_ac);
271  cam_target_x = ac->east;
272  cam_target_y = ac->north;
273  cam_target_alt = ac->alt;
274  cam_target();
275 #endif // TRAFFIC_INFO
276 }
float cam_target_y
Definition: cam.c:84
int16_t cam_pan_command
Definition: cam.c:94
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
void cam_waypoint_target(void)
Definition: cam.c:259
float cam_phi_c
Definition: cam.c:81
uint8_t pprz_mode
Definition: main_ap.c:111
#define MIN_PPRZ
Definition: paparazzi.h:9
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float cam_pan_c
Definition: cam.c:73
float cam_target_x
Radians, for CAM_MODE_ANGLES mode.
Definition: cam.c:84
#define CAM_MODE_STABILIZED
Definition: cam.h:41
float north
Definition: traffic_info.h:38
#define CAM_MODE_XY_TARGET
Definition: cam.h:38
float cam_target_alt
Definition: cam.c:84
#define LED_ON(i)
Definition: led_hw.h:28
float estimator_y
north position in meters
Definition: estimator.c:43
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:46
void cam_ac_target(void)
Definition: cam.c:268
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define CAM_MODE_RC
Definition: cam.h:42
float east
Definition: traffic_info.h:37
struct ap_state * ap_state
Definition: inter_mcu.c:35
float alt
Definition: traffic_info.h:40
#define CAM_PAN_MIN
Definition: cam.h:49
#define CAM_MODE0
Definition: cam.c:89
float cam_tilt_c
Definition: cam.c:79
#define CAM_TILT0
Definition: cam.c:77
void cam_periodic(void)
For CAM_MODE_AC_TARGET mode.
Definition: cam.c:107
signed short int16_t
Definition: types.h:17
#define CAM_MODE_OFF
Definition: cam.h:35
#define PPRZ_MODE_AUTO1
Definition: autopilot.h:45
float estimator_x
east position in meters
Definition: estimator.c:42
void cam_target(void)
Computes the right angles from target_x, target_y, target_alt.
Definition: cam.c:230
uint8_t cam_target_wp
For CAM_MODE_XY_TARGET mode.
Definition: cam.c:85
#define CAM_PAN_MAX
Definition: cam.h:46
void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, float fRollAngle, float fPitchAngle, float fYawAngle, float fObjectEast, float fObjectNorth, float fAltitude, float *fPan, float *fTilt)
Definition: point.c:163
#define CAM_TILT_MAX
Definition: cam.h:52
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
void cam_init(void)
Definition: cam.c:103
float cam_theta_c
Definition: cam.c:82
int16_t cam_tilt_command
Definition: cam.c:95
uint8_t cam_target_ac
For CAM_MODE_WP_TARGET mode.
Definition: cam.c:86
State estimation, fusioning sensors.
#define CAM_MODE_WP_TARGET
Definition: cam.h:39
Pan/Tilt camera API.
#define LED_OFF(i)
Definition: led_hw.h:29
uint8_t cam_mode
Definition: cam.c:91
void cam_angles(void)
Computes the servo values from cam_pan_c and cam_tilt_c.
Definition: cam.c:178
#define MAX_PPRZ
Definition: paparazzi.h:8
#define CAM_PAN0
Definition: cam.c:71
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define CAM_TILT_MIN
Definition: cam.h:55
#define CAM_MODE_AC_TARGET
Definition: cam.h:40
bool_t cam_lock
Definition: cam.c:92
Informations relative to the other aircrafts.
#define CAM_MODE_ANGLES
Definition: cam.h:36
#define CAM_MODE_NADIR
Definition: cam.h:37
void cam_nadir(void)
Point straight down.
Definition: cam.c:246