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
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 
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 static void send_cam(void) {
105 }
106 
107 #ifdef SHOW_CAM_COORDINATES
108 static void send_cam_point(void) {
109  DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice,
110  &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
111 }
112 #endif
113 
114 void cam_init( void ) {
116 
117  register_periodic_telemetry(DefaultPeriodic, "CAM", send_cam);
118 #ifdef SHOW_CAM_COORDINATES
119  register_periodic_telemetry(DefaultPeriodic, "CAM_POINT", send_cam_point);
120 #endif
121 }
122 
123 void cam_periodic( void ) {
124 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
125  //Position the camera for straight view.
126  if (pprz_mode == PPRZ_MODE_AUTO2){
127 #endif
128  switch (cam_mode) {
129  case CAM_MODE_OFF:
130  cam_pan_c = RadOfDeg(CAM_PAN0);
131  cam_tilt_c = RadOfDeg(CAM_TILT0);
132  cam_angles();
133  break;
134  case CAM_MODE_ANGLES:
135  cam_angles();
136  break;
137  case CAM_MODE_NADIR:
138  cam_nadir();
139  break;
140  case CAM_MODE_XY_TARGET:
141  cam_target();
142  break;
143  case CAM_MODE_WP_TARGET:
145  break;
146  case CAM_MODE_AC_TARGET:
147  cam_ac_target();
148  break;
149  // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
150  // The "TARGET" waypoint coordinates are not used.
151  // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
152  case CAM_MODE_STABILIZED:
154  break;
155  // In this mode the angles come from the pan and tilt radio channels.
156  // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
157  // in order to calculate the coordinates of where the camera is looking.
158  case CAM_MODE_RC:
160  break;
161  }
162 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
163  }else if (pprz_mode == PPRZ_MODE_AUTO1){
164  //Position the camera for straight view.
165 
166 #if defined(CAM_TILT_POSITION_FOR_FPV)
167  cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
168 #else
169  cam_tilt_c = RadOfDeg(90);
170 #endif
171 #if defined(CAM_PAN_POSITION_FOR_FPV)
172  cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
173 #else
174  cam_pan_c = RadOfDeg(0);
175 #endif
176  cam_angles();
177 #ifdef SHOW_CAM_COORDINATES
178  cam_point_lon = 0;
179  cam_point_lat = 0;
180  cam_point_distance_from_home = 0;
181 #endif
182  }
183 #endif
184 
185 
186 #if defined(COMMAND_CAM_PWR_SW)
187  if(video_tx_state){ ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; }else{ ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
188 #elif defined(VIDEO_TX_SWITCH)
189  if(video_tx_state){ LED_OFF(VIDEO_TX_SWITCH); }else{ LED_ON(VIDEO_TX_SWITCH); }
190 #endif
191 }
192 
194 void cam_angles( void ) {
195  float cam_pan = 0;
196  float cam_tilt = 0;
197  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
198  cam_pan_c = RadOfDeg(CAM_PAN_MAX);
199  } else {
200  if (cam_pan_c < RadOfDeg(CAM_PAN_MIN))
201  cam_pan_c = RadOfDeg(CAM_PAN_MIN);
202  }
203 
204  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){
205  cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
206  } else {
207  if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN))
208  cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
209  }
210 
211 #ifdef CAM_PAN_NEUTRAL
212  float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
213  if (pan_diff > 0)
214  cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
215  else
216  cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
217 #else
218  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX-CAM_PAN_MIN) );
219 #endif
220 
221 #ifdef CAM_TILT_NEUTRAL
222  float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
223  if (tilt_diff > 0)
224  cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
225  else
226  cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
227 #else
228  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_TILT_MAX-CAM_TILT_MIN) );
229 #endif
230 
231  cam_pan = TRIM_PPRZ(cam_pan);
232  cam_tilt = TRIM_PPRZ(cam_tilt);
233 
236 
237 #ifdef COMMAND_CAM_PAN
238  ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
239 #endif
240 #ifdef COMMAND_CAM_TILT
241  ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
242 #endif
243 }
244 
246 void cam_target( void ) {
247 #ifdef TEST_CAM
248  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
249  test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
251  &cam_pan_c, &cam_tilt_c);
252 #else
253  struct EnuCoor_f* pos = stateGetPositionEnu_f();
254  struct FloatEulers* att = stateGetNedToBodyEulers_f();
255  vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
256  att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
258  &cam_pan_c, &cam_tilt_c);
259 #endif
260  cam_angles();
261 }
262 
264 void cam_nadir( void ) {
265  struct EnuCoor_f* pos = stateGetPositionEnu_f();
266 #ifdef TEST_CAM
267  cam_target_x = test_cam_estimator_x;
268  cam_target_y = test_cam_estimator_y;
269 #else
270  cam_target_x = pos->x;
271  cam_target_y = pos->y;
272 #endif
273  cam_target_alt = -10;
274  cam_target();
275 }
276 
277 
278 void cam_waypoint_target( void ) {
279  if (cam_target_wp < nb_waypoint) {
282  }
284  cam_target();
285 }
286 
287 void cam_ac_target( void ) {
288 #ifdef TRAFFIC_INFO
289  struct ac_info_ * ac = get_ac_info(cam_target_ac);
290  cam_target_x = ac->east;
291  cam_target_y = ac->north;
292  cam_target_alt = ac->alt;
293  cam_target();
294 #endif // TRAFFIC_INFO
295 }
float cam_target_y
Definition: cam.c:84
int16_t cam_pan_command
Definition: cam.c:94
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
void cam_waypoint_target(void)
Definition: cam.c:278
float cam_phi_c
Definition: cam.c:81
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 ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
Periodic telemetry system header (includes downlink utility and generated code).
float cam_pan_c
Definition: cam.c:73
uint8_t pprz_mode
Definition: autopilot.c:40
float cam_target_x
Radians, for CAM_MODE_ANGLES mode.
Definition: cam.c:84
#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:84
#define LED_ON(i)
Definition: led_hw.h:28
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:52
void cam_ac_target(void)
Definition: cam.c:287
float theta
in radians
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
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:89
float cam_tilt_c
Definition: cam.c:79
#define CAM_TILT0
Definition: cam.c:77
float phi
in radians
void cam_periodic(void)
For CAM_MODE_AC_TARGET mode.
Definition: cam.c:123
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:51
void cam_target(void)
Computes the right angles from target_x, target_y, target_alt.
Definition: cam.c:246
uint8_t cam_target_wp
For CAM_MODE_XY_TARGET mode.
Definition: cam.c:85
#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:114
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
#define CAM_MODE_WP_TARGET
Definition: cam.h:37
Pan/Tilt camera API.
static void send_cam(void)
Definition: cam.c:103
#define LED_OFF(i)
Definition: led_hw.h:29
uint8_t cam_mode
Definition: cam.c:91
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:194
#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:53
#define CAM_MODE_AC_TARGET
Definition: cam.h:38
#define SEND_CAM(_trans, _dev)
Definition: cam.h:99
bool_t cam_lock
Definition: cam.c:92
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:264