Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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(struct transport_tx *trans, struct link_device *dev)
104 {
105  int16_t x = cam_target_x;
106  int16_t y = cam_target_y;
107  int16_t phi = DegOfRad(cam_phi_c);
108  int16_t theta = DegOfRad(cam_theta_c);
109  pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
110 }
111 
112 #ifdef SHOW_CAM_COORDINATES
113 static void send_cam_point(struct transport_tx *trans, struct link_device *dev)
114 {
115  pprz_msg_send_CAM_POINT(trans, dev, AC_ID,
116  &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
117 }
118 #endif
119 
120 void cam_init(void)
121 {
123 
125 #ifdef SHOW_CAM_COORDINATES
126  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM_POINT, send_cam_point);
127 #endif
128 }
129 
130 void cam_periodic(void)
131 {
132 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
133  //Position the camera for straight view.
134  if (pprz_mode == PPRZ_MODE_AUTO2) {
135 #endif
136  switch (cam_mode) {
137  case CAM_MODE_OFF:
138  cam_pan_c = RadOfDeg(CAM_PAN0);
139  cam_tilt_c = RadOfDeg(CAM_TILT0);
140  cam_angles();
141  break;
142  case CAM_MODE_ANGLES:
143  cam_angles();
144  break;
145  case CAM_MODE_NADIR:
146  cam_nadir();
147  break;
148  case CAM_MODE_XY_TARGET:
149  cam_target();
150  break;
151  case CAM_MODE_WP_TARGET:
153  break;
154  case CAM_MODE_AC_TARGET:
155  cam_ac_target();
156  break;
157  // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
158  // The "TARGET" waypoint coordinates are not used.
159  // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
160  case CAM_MODE_STABILIZED:
162  break;
163  // In this mode the angles come from the pan and tilt radio channels.
164  // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
165  // in order to calculate the coordinates of where the camera is looking.
166  case CAM_MODE_RC:
168  break;
169  default:
170  break;
171  }
172 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
173  } else if (pprz_mode == PPRZ_MODE_AUTO1) {
174  //Position the camera for straight view.
175 
176 #if defined(CAM_TILT_POSITION_FOR_FPV)
177  cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
178 #else
179  cam_tilt_c = RadOfDeg(90);
180 #endif
181 #if defined(CAM_PAN_POSITION_FOR_FPV)
182  cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
183 #else
184  cam_pan_c = RadOfDeg(0);
185 #endif
186  cam_angles();
187 #ifdef SHOW_CAM_COORDINATES
188  cam_point_lon = 0;
189  cam_point_lat = 0;
190  cam_point_distance_from_home = 0;
191 #endif
192  }
193 #endif
194 
195 
196 #if defined(COMMAND_CAM_PWR_SW)
197  if (video_tx_state) { ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; } else { ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
198 #elif defined(VIDEO_TX_SWITCH)
199  if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); }
200 #endif
201 }
202 
204 void cam_angles(void)
205 {
206  float cam_pan = 0;
207  float cam_tilt = 0;
208  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
209  cam_pan_c = RadOfDeg(CAM_PAN_MAX);
210  } else {
211  if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) {
212  cam_pan_c = RadOfDeg(CAM_PAN_MIN);
213  }
214  }
215 
216  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)) {
217  cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
218  } else {
219  if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) {
220  cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
221  }
222  }
223 
224 #ifdef CAM_PAN_NEUTRAL
225  float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
226  if (pan_diff > 0) {
227  cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
228  } else {
229  cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
230  }
231 #else
232  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN));
233 #endif
234 
235 #ifdef CAM_TILT_NEUTRAL
236  float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
237  if (tilt_diff > 0) {
238  cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
239  } else {
240  cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
241  }
242 #else
243  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(
245 #endif
246 
247  cam_pan = TRIM_PPRZ(cam_pan);
248  cam_tilt = TRIM_PPRZ(cam_tilt);
249 
252 
253 #ifdef COMMAND_CAM_PAN
254  ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
255 #endif
256 #ifdef COMMAND_CAM_TILT
257  ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
258 #endif
259 }
260 
262 void cam_target(void)
263 {
264 #ifdef TEST_CAM
265  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
266  test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
268  &cam_pan_c, &cam_tilt_c);
269 #else
270  struct EnuCoor_f *pos = stateGetPositionEnu_f();
271  struct FloatEulers *att = stateGetNedToBodyEulers_f();
272  vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
273  att->phi, att->theta, stateGetHorizontalSpeedDir_f(),
275  &cam_pan_c, &cam_tilt_c);
276 #endif
277  cam_angles();
278 }
279 
281 void cam_nadir(void)
282 {
283  struct EnuCoor_f *pos = stateGetPositionEnu_f();
284 #ifdef TEST_CAM
285  cam_target_x = test_cam_estimator_x;
286  cam_target_y = test_cam_estimator_y;
287 #else
288  cam_target_x = pos->x;
289  cam_target_y = pos->y;
290 #endif
291  cam_target_alt = -10;
292  cam_target();
293 }
294 
295 
297 {
298  if (cam_target_wp < nb_waypoint) {
301  }
303  cam_target();
304 }
305 
306 void cam_ac_target(void)
307 {
308 #ifdef TRAFFIC_INFO
309  struct ac_info_ * ac = get_ac_info(cam_target_ac);
310  cam_target_x = ac->east;
311  cam_target_y = ac->north;
312  cam_target_alt = ac->alt;
313  cam_target();
314 #endif // TRAFFIC_INFO
315 }
float cam_target_y
Definition: cam.c:84
int16_t cam_pan_command
Definition: cam.c:94
void cam_waypoint_target(void)
Definition: cam.c:296
float phi
in radians
Generic transmission transport header.
Definition: transport.h:89
float cam_phi_c
Definition: cam.c:81
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
#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
vector in East North Up coordinates Units: meters
uint8_t pprz_mode
Definition: autopilot.c:41
float cam_target_x
Radians, for CAM_MODE_ANGLES mode.
Definition: cam.c:84
#define CAM_MODE_STABILIZED
Definition: cam.h:39
#define CAM_MODE_XY_TARGET
Definition: cam.h:36
float cam_target_alt
Definition: cam.c:84
float north
Definition: traffic_info.h:38
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:52
void cam_ac_target(void)
Definition: cam.c:306
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:45
#define CAM_MODE_RC
Definition: cam.h:40
struct ap_state * ap_state
Definition: inter_mcu.c:37
euler angles
float alt
Definition: traffic_info.h:40
#define CAM_PAN_MIN
Definition: cam.h:47
float theta
in radians
#define CAM_MODE0
Definition: cam.c:89
float cam_tilt_c
Definition: cam.c:79
float east
Definition: traffic_info.h:37
#define CAM_TILT0
Definition: cam.c:77
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
void cam_periodic(void)
For CAM_MODE_AC_TARGET mode.
Definition: cam.c:130
signed short int16_t
Definition: types.h:17
#define CAM_MODE_OFF
Definition: cam.h:33
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#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:262
#define WaypointY(_wp)
Definition: common_nav.h:46
const uint8_t nb_waypoint
Definition: common_nav.c:37
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
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:163
#define LED_ON(i)
Definition: led_hw.h:42
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
#define CAM_TILT_MAX
Definition: cam.h:50
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:675
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void cam_init(void)
Definition: cam.c:120
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.
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:204
static void send_cam(struct transport_tx *trans, struct link_device *dev)
Definition: cam.c:103
#define MAX_PPRZ
Definition: paparazzi.h:8
#define CAM_PAN0
Definition: cam.c:71
#define LED_OFF(i)
Definition: led_hw.h:43
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float y
in meters
#define CAM_TILT_MIN
Definition: cam.h:53
#define CAM_MODE_AC_TARGET
Definition: cam.h:38
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
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:281