Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cam.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
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  */
27 #include <math.h>
28 #include "cam.h"
29 #include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt
30 #include "autopilot.h"
31 #include "generated/flight_plan.h"
32 #include "state.h"
34 #ifdef POINT_CAM
35 #include "point.h"
36 #endif // POINT_CAM
37 
38 #ifdef TEST_CAM
39 float test_cam_estimator_x;
40 float test_cam_estimator_y;
41 float test_cam_estimator_z;
42 float test_cam_estimator_phi;
43 float test_cam_estimator_theta;
44 float test_cam_estimator_hspeed_dir;
45 #endif // TEST_CAM
46 
47 //FIXME: use radians
48 #ifdef CAM_PAN_NEUTRAL
49 #if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
50 #error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
51 #endif
52 #if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
53 #error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
54 #endif
55 #endif
56 
57 //FIXME: use radians
58 #ifdef CAM_TILT_NEUTRAL
59 #if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL))
60 #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
61 #endif
62 #if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
63 #error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
64 #endif
65 #endif
66 
67 //FIXME: use radians
68 #ifndef CAM_PAN0
69 #define CAM_PAN0 RadOfDeg(0)
70 #endif
71 float cam_pan_c;
72 
73 //FIXME: use radians
74 #ifndef CAM_TILT0
75 #define CAM_TILT0 RadOfDeg(0)
76 #endif
77 float cam_tilt_c;
78 
79 float cam_phi_c;
81 
85 
86 #ifndef CAM_MODE0
87 #define CAM_MODE0 CAM_MODE_OFF
88 #endif
90 bool_t cam_lock;
91 
94 
95 void cam_nadir(void);
96 void cam_angles(void);
97 void cam_target(void);
98 void cam_waypoint_target(void);
99 void cam_ac_target(void);
100 
101 void cam_init( void ) {
103 }
104 
105 void cam_periodic( void ) {
106 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
107  //Position the camera for straight view.
108  if (pprz_mode == PPRZ_MODE_AUTO2){
109 #endif
110  switch (cam_mode) {
111  case CAM_MODE_OFF:
112  cam_pan_c = RadOfDeg(CAM_PAN0);
113  cam_tilt_c = RadOfDeg(CAM_TILT0);
114  cam_angles();
115  break;
116  case CAM_MODE_ANGLES:
117  cam_angles();
118  break;
119  case CAM_MODE_NADIR:
120  cam_nadir();
121  break;
122  case CAM_MODE_XY_TARGET:
123  cam_target();
124  break;
125  case CAM_MODE_WP_TARGET:
127  break;
128  case CAM_MODE_AC_TARGET:
129  cam_ac_target();
130  break;
131  // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
132  // The "TARGET" waypoint coordinates are not used.
133  // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
134  case CAM_MODE_STABILIZED:
136  break;
137  // In this mode the angles come from the pan and tilt radio channels.
138  // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
139  // in order to calculate the coordinates of where the camera is looking.
140  case CAM_MODE_RC:
142  break;
143  }
144 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
145  }else if (pprz_mode == PPRZ_MODE_AUTO1){
146  //Position the camera for straight view.
147 
148 #if defined(CAM_TILT_POSITION_FOR_FPV)
149  cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
150 #else
151  cam_tilt_c = RadOfDeg(90);
152 #endif
153 #if defined(CAM_PAN_POSITION_FOR_FPV)
154  cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
155 #else
156  cam_pan_c = RadOfDeg(0);
157 #endif
158  cam_angles();
159 #ifdef SHOW_CAM_COORDINATES
160  cam_point_lon = 0;
161  cam_point_lat = 0;
162  cam_point_distance_from_home = 0;
163 #endif
164  }
165 #endif
166 
167 
168 #if defined(COMMAND_CAM_PWR_SW)
169  if(video_tx_state){ ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; }else{ ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
170 #elif defined(VIDEO_TX_SWITCH)
171  if(video_tx_state){ LED_OFF(VIDEO_TX_SWITCH); }else{ LED_ON(VIDEO_TX_SWITCH); }
172 #endif
173 }
174 
176 void cam_angles( void ) {
177  float cam_pan = 0;
178  float cam_tilt = 0;
179  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
180  cam_pan_c = RadOfDeg(CAM_PAN_MAX);
181  } else {
182  if (cam_pan_c < RadOfDeg(CAM_PAN_MIN))
183  cam_pan_c = RadOfDeg(CAM_PAN_MIN);
184  }
185 
186  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){
187  cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
188  } else {
189  if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN))
190  cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
191  }
192 
193 #ifdef CAM_PAN_NEUTRAL
194  float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
195  if (pan_diff > 0)
196  cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
197  else
198  cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
199 #else
200  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX-CAM_PAN_MIN) );
201 #endif
202 
203 #ifdef CAM_TILT_NEUTRAL
204  float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
205  if (tilt_diff > 0)
206  cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
207  else
208  cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
209 #else
210  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_TILT_MAX-CAM_TILT_MIN) );
211 #endif
212 
213  cam_pan = TRIM_PPRZ(cam_pan);
214  cam_tilt = TRIM_PPRZ(cam_tilt);
215 
218 
219 #ifdef COMMAND_CAM_PAN
220  ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
221 #endif
222 #ifdef COMMAND_CAM_TILT
223  ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
224 #endif
225 }
226 
228 void cam_target( void ) {
229 #ifdef TEST_CAM
230  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
231  test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
233  &cam_pan_c, &cam_tilt_c);
234 #else
235  struct EnuCoor_f* pos = stateGetPositionEnu_f();
236  struct FloatEulers* att = stateGetNedToBodyEulers_f();
237  vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
238  att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
240  &cam_pan_c, &cam_tilt_c);
241 #endif
242  cam_angles();
243 }
244 
246 void cam_nadir( void ) {
247  struct EnuCoor_f* pos = stateGetPositionEnu_f();
248 #ifdef TEST_CAM
249  cam_target_x = test_cam_estimator_x;
250  cam_target_y = test_cam_estimator_y;
251 #else
252  cam_target_x = pos->x;
253  cam_target_y = pos->y;
254 #endif
255  cam_target_alt = -10;
256  cam_target();
257 }
258 
259 
260 void cam_waypoint_target( void ) {
261  if (cam_target_wp < nb_waypoint) {
264  }
266  cam_target();
267 }
268 
269 void cam_ac_target( void ) {
270 #ifdef TRAFFIC_INFO
271  struct ac_info_ * ac = get_ac_info(cam_target_ac);
272  cam_target_x = ac->east;
273  cam_target_y = ac->north;
274  cam_target_alt = ac->alt;
275  cam_target();
276 #endif // TRAFFIC_INFO
277 }
float cam_target_y
Definition: cam.c:82
int16_t cam_pan_command
Definition: cam.c:92
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
void cam_waypoint_target(void)
Definition: cam.c:260
float cam_phi_c
Definition: cam.c:79
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
#define MIN_PPRZ
Definition: paparazzi.h:9
float cam_pan_c
Definition: cam.c:71
uint8_t pprz_mode
Definition: autopilot.c:31
float cam_target_x
Radians, for CAM_MODE_ANGLES mode.
Definition: cam.c:82
#define CAM_MODE_STABILIZED
Definition: cam.h:39
float north
Definition: traffic_info.h:38
#define CAM_MODE_XY_TARGET
Definition: cam.h:36
float cam_target_alt
Definition: cam.c:82
#define LED_ON(i)
Definition: led_hw.h:28
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:53
void cam_ac_target(void)
Definition: cam.c:269
float theta
in radians
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
euler angles
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float y
in meters
#define CAM_MODE_RC
Definition: cam.h:40
float east
Definition: traffic_info.h:37
struct ap_state * ap_state
Definition: inter_mcu.c:33
float alt
Definition: traffic_info.h:40
#define CAM_PAN_MIN
Definition: cam.h:47
#define CAM_MODE0
Definition: cam.c:87
float cam_tilt_c
Definition: cam.c:77
#define CAM_TILT0
Definition: cam.c:75
float phi
in radians
void cam_periodic(void)
For CAM_MODE_AC_TARGET mode.
Definition: cam.c:105
signed short int16_t
Definition: types.h:17
float x
in meters
#define CAM_MODE_OFF
Definition: cam.h:33
#define PPRZ_MODE_AUTO1
Definition: autopilot.h:52
void cam_target(void)
Computes the right angles from target_x, target_y, target_alt.
Definition: cam.c:228
uint8_t cam_target_wp
For CAM_MODE_XY_TARGET mode.
Definition: cam.c:83
#define CAM_PAN_MAX
Definition: cam.h:44
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:161
#define CAM_TILT_MAX
Definition: cam.h:50
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void cam_init(void)
Definition: cam.c:101
float cam_theta_c
Definition: cam.c:80
int16_t cam_tilt_command
Definition: cam.c:93
uint8_t cam_target_ac
For CAM_MODE_WP_TARGET mode.
Definition: cam.c:84
#define CAM_MODE_WP_TARGET
Definition: cam.h:37
Pan/Tilt camera API.
#define LED_OFF(i)
Definition: led_hw.h:29
uint8_t cam_mode
Definition: cam.c:89
vector in East North Up coordinates Units: meters
void cam_angles(void)
Computes the servo values from cam_pan_c and cam_tilt_c.
Definition: cam.c:176
#define MAX_PPRZ
Definition: paparazzi.h:8
#define CAM_PAN0
Definition: cam.c:69
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define CAM_TILT_MIN
Definition: cam.h:53
#define CAM_MODE_AC_TARGET
Definition: cam.h:38
bool_t cam_lock
Definition: cam.c:90
Information relative to the other aircrafts.
#define CAM_MODE_ANGLES
Definition: cam.h:34
#define CAM_MODE_NADIR
Definition: cam.h:35
void cam_nadir(void)
Point straight down.
Definition: cam.c:246