Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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"
33 #ifdef POINT_CAM
34 #include "point.h"
35 #endif // POINT_CAM
36 
37 #include "subsystems/datalink/telemetry.h"
38 
39 #ifdef TEST_CAM
40 float test_cam_estimator_x;
41 float test_cam_estimator_y;
42 float test_cam_estimator_z;
43 float test_cam_estimator_phi;
44 float test_cam_estimator_theta;
45 float test_cam_estimator_hspeed_dir;
46 #endif // TEST_CAM
47 
48 //FIXME: use radians
49 #ifdef CAM_PAN_NEUTRAL
50 #if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
51 #error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
52 #endif
53 #if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
54 #error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
55 #endif
56 #endif
57 
58 //FIXME: use radians
59 #ifdef CAM_TILT_NEUTRAL
60 #if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL))
61 #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
62 #endif
63 #if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
64 #error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
65 #endif
66 #endif
67 
68 //FIXME: use radians
69 #ifndef CAM_PAN0
70 #define CAM_PAN0 RadOfDeg(0)
71 #endif
72 float cam_pan_c;
73 
74 //FIXME: use radians
75 #ifndef CAM_TILT0
76 #define CAM_TILT0 RadOfDeg(0)
77 #endif
78 float cam_tilt_c;
79 
80 float cam_phi_c;
81 float cam_theta_c;
82 
83 float cam_target_x, cam_target_y, cam_target_alt;
84 uint8_t cam_target_wp;
85 uint8_t cam_target_ac;
86 
87 #ifndef CAM_MODE0
88 #define CAM_MODE0 CAM_MODE_OFF
89 #endif
90 uint8_t cam_mode;
91 bool cam_lock;
92 
93 int16_t cam_pan_command;
94 int16_t cam_tilt_command;
95 
96 void cam_nadir(void);
97 void cam_angles(void);
98 void cam_target(void);
99 void cam_waypoint_target(void);
100 void cam_ac_target(void);
101 
102 static void send_cam(struct transport_tx *trans, struct link_device *dev)
103 {
104  int16_t x = cam_target_x;
105  int16_t y = cam_target_y;
106  int16_t phi = DegOfRad(cam_phi_c);
107  int16_t theta = DegOfRad(cam_theta_c);
108  pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
109 }
110 
111 #ifdef SHOW_CAM_COORDINATES
112 static void send_cam_point(struct transport_tx *trans, struct link_device *dev)
113 {
114  pprz_msg_send_CAM_POINT(trans, dev, AC_ID,
115  &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
116 }
117 #endif
118 
119 void cam_init(void)
120 {
121  cam_mode = CAM_MODE0;
122 
123  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM, send_cam);
124 #ifdef SHOW_CAM_COORDINATES
125  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM_POINT, send_cam_point);
126 #endif
127 }
128 
129 void cam_periodic(void)
130 {
131 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
132  //Position the camera for straight view.
133  if (autopilot_get_mode() == AP_MODE_AUTO2) {
134 #endif
135  switch (cam_mode) {
136  case CAM_MODE_OFF:
137  cam_pan_c = RadOfDeg(CAM_PAN0);
138  cam_tilt_c = RadOfDeg(CAM_TILT0);
139  cam_angles();
140  break;
141  case CAM_MODE_ANGLES:
142  cam_angles();
143  break;
144  case CAM_MODE_NADIR:
145  cam_nadir();
146  break;
147  case CAM_MODE_XY_TARGET:
148  cam_target();
149  break;
150  case CAM_MODE_WP_TARGET:
151  cam_waypoint_target();
152  break;
153  case CAM_MODE_AC_TARGET:
154  cam_ac_target();
155  break;
156  // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
157  // The "TARGET" waypoint coordinates are not used.
158  // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
159  case CAM_MODE_STABILIZED:
160  cam_waypoint_target();
161  break;
162  // In this mode the angles come from the pan and tilt radio channels.
163  // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
164  // in order to calculate the coordinates of where the camera is looking.
165  case CAM_MODE_RC:
166  cam_waypoint_target();
167  break;
168  default:
169  break;
170  }
171 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
172  } else if (autopilot_get_mode() == AP_MODE_AUTO1) {
173  //Position the camera for straight view.
174 
175 #if defined(CAM_TILT_POSITION_FOR_FPV)
176  cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
177 #else
178  cam_tilt_c = RadOfDeg(90);
179 #endif
180 #if defined(CAM_PAN_POSITION_FOR_FPV)
181  cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
182 #else
183  cam_pan_c = RadOfDeg(0);
184 #endif
185  cam_angles();
186 #ifdef SHOW_CAM_COORDINATES
187  cam_point_lon = 0;
188  cam_point_lat = 0;
189  cam_point_distance_from_home = 0;
190 #endif
191  }
192 #endif
193 
194 
195 #if defined(COMMAND_CAM_PWR_SW)
196  if (video_tx_state) { imcu_set_command(COMMAND_CAM_PWR_SW, MAX_PPRZ); } else { imcu_set_command(COMMAND_CAM_PWR_SW, MIN_PPRZ); }
197 #elif defined(VIDEO_TX_SWITCH)
198  if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); }
199 #endif
200 }
201 
203 void cam_angles(void)
204 {
205  float cam_pan = 0;
206  float cam_tilt = 0;
207  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
208  cam_pan_c = RadOfDeg(CAM_PAN_MAX);
209  } else {
210  if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) {
211  cam_pan_c = RadOfDeg(CAM_PAN_MIN);
212  }
213  }
214 
215  if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)) {
216  cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
217  } else {
218  if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) {
219  cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
220  }
221  }
222 
223 #ifdef CAM_PAN_NEUTRAL
224  float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
225  if (pan_diff > 0) {
226  cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
227  } else {
228  cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
229  }
230 #else
231  cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN));
232 #endif
233 
234 #ifdef CAM_TILT_NEUTRAL
235  float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
236  if (tilt_diff > 0) {
237  cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
238  } else {
239  cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
240  }
241 #else
242  cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(
243  CAM_TILT_MAX - CAM_TILT_MIN));
244 #endif
245 
246  cam_pan = TRIM_PPRZ(cam_pan);
247  cam_tilt = TRIM_PPRZ(cam_tilt);
248 
249  cam_phi_c = cam_pan_c;
250  cam_theta_c = cam_tilt_c;
251 
252 #ifdef COMMAND_CAM_PAN
253  imcu_set_command(COMMAND_CAM_PAN, cam_pan);
254 #endif
255 #ifdef COMMAND_CAM_TILT
256  imcu_set_command(COMMAND_CAM_TILT, cam_tilt);
257 #endif
258 }
259 
261 void cam_target(void)
262 {
263 #ifdef TEST_CAM
264  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
265  test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
266  cam_target_x, cam_target_y, cam_target_alt,
267  &cam_pan_c, &cam_tilt_c);
268 #else
269  struct EnuCoor_f *pos = stateGetPositionEnu_f();
270  struct FloatEulers *att = stateGetNedToBodyEulers_f();
271  vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
272  att->phi, att->theta, stateGetHorizontalSpeedDir_f(),
273  cam_target_x, cam_target_y, cam_target_alt,
274  &cam_pan_c, &cam_tilt_c);
275 #endif
276  cam_angles();
277 }
278 
280 void cam_nadir(void)
281 {
282  struct EnuCoor_f *pos = stateGetPositionEnu_f();
283 #ifdef TEST_CAM
284  cam_target_x = test_cam_estimator_x;
285  cam_target_y = test_cam_estimator_y;
286 #else
287  cam_target_x = pos->x;
288  cam_target_y = pos->y;
289 #endif
290  cam_target_alt = -10;
291  cam_target();
292 }
293 
294 
295 void cam_waypoint_target(void)
296 {
297  if (cam_target_wp < nb_waypoint) {
298  cam_target_x = WaypointX(cam_target_wp);
299  cam_target_y = WaypointY(cam_target_wp);
300  }
301  cam_target_alt = ground_alt;
302  cam_target();
303 }
304 
305 #ifdef TRAFFIC_INFO
306 #include "modules/multi/traffic_info.h"
307 
308 void cam_ac_target(void)
309 {
310  struct EnuCoor_f ac_pos *ac = acInfoGetPositionEnu_f(cam_target_ac);
311  cam_target_x = ac->x;
312  cam_target_y = ac->y;
313  cam_target_alt = acInfoGetPositionUtm_f()->alt;
314  cam_target();
315 }
316 #else
317 void cam_ac_target(void) {}
318 #endif // TRAFFIC_INFO
traffic_info.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
cam_lock
bool cam_lock
Definition: cam.c:91
point.h
CAM_TILT0
#define CAM_TILT0
Definition: cam.c:76
CAM_PAN0
#define CAM_PAN0
Definition: cam.c:70
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
AP_MODE_AUTO2
#define AP_MODE_AUTO2
Definition: autopilot_static.h:38
cam_pan_command
int16_t cam_pan_command
Definition: cam.c:93
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
cam_target_y
float cam_target_y
Definition: cam.c:83
cam_periodic
void cam_periodic(void)
For CAM_MODE_AC_TARGET mode.
Definition: cam.c:129
cam_target
void cam_target(void)
Computes the right angles from target_x, target_y, target_alt.
Definition: cam.c:261
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
cam_init
void cam_init(void)
Definition: cam.c:119
common_nav.h
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
cam_target_alt
float cam_target_alt
Definition: cam.c:83
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
CAM_MODE_ANGLES
#define CAM_MODE_ANGLES
Definition: cam.h:34
CAM_MODE_RC
#define CAM_MODE_RC
Definition: cam.h:40
CAM_MODE_NADIR
#define CAM_MODE_NADIR
Definition: cam.h:35
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
CAM_PAN_MIN
#define CAM_PAN_MIN
Definition: cam.h:47
cam_angles
void cam_angles(void)
Computes the servo values from cam_pan_c and cam_tilt_c.
Definition: cam.c:203
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
CAM_MODE_WP_TARGET
#define CAM_MODE_WP_TARGET
Definition: cam.h:37
acInfoGetPositionEnu_f
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
telemetry.h
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
CAM_MODE_AC_TARGET
#define CAM_MODE_AC_TARGET
Definition: cam.h:38
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:37
send_cam
static void send_cam(struct transport_tx *trans, struct link_device *dev)
Definition: cam.c:102
cam_nadir
void cam_nadir(void)
Point straight down.
Definition: cam.c:280
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int16_t
signed short int16_t
Definition: types.h:17
uint8_t
unsigned char uint8_t
Definition: types.h:14
MIN_PPRZ
#define MIN_PPRZ
Definition: paparazzi.h:9
cam_theta_c
float cam_theta_c
Definition: cam.c:81
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
cam_ac_target
void cam_ac_target(void)
Definition: cam.c:317
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
autopilot.h
CAM_TILT_MIN
#define CAM_TILT_MIN
Definition: cam.h:53
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
cam_mode
uint8_t cam_mode
Definition: cam.c:90
CAM_MODE_OFF
#define CAM_MODE_OFF
Definition: cam.h:33
cam_target_ac
uint8_t cam_target_ac
For CAM_MODE_WP_TARGET mode.
Definition: cam.c:85
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
cam.h
Pan/Tilt camera API.
cam_phi_c
float cam_phi_c
Definition: cam.c:80
vPoint
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:164
cam_target_x
float cam_target_x
Radians, for CAM_MODE_ANGLES mode.
Definition: cam.c:83
cam_pan_c
float cam_pan_c
Definition: cam.c:72
CAM_TILT_MAX
#define CAM_TILT_MAX
Definition: cam.h:50
CAM_MODE0
#define CAM_MODE0
Definition: cam.c:88
cam_target_wp
uint8_t cam_target_wp
For CAM_MODE_XY_TARGET mode.
Definition: cam.c:84
CAM_MODE_STABILIZED
#define CAM_MODE_STABILIZED
Definition: cam.h:39
cam_waypoint_target
void cam_waypoint_target(void)
Definition: cam.c:295
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
TRIM_PPRZ
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
state.h
CAM_MODE_XY_TARGET
#define CAM_MODE_XY_TARGET
Definition: cam.h:36
cam_tilt_command
int16_t cam_tilt_command
Definition: cam.c:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
CAM_PAN_MAX
#define CAM_PAN_MAX
Definition: cam.h:44
acInfoGetPositionUtm_f
static struct UtmCoor_f * acInfoGetPositionUtm_f(uint8_t ac_id)
Get position from UTM coordinates (float).
Definition: traffic_info.h:361
AP_MODE_AUTO1
#define AP_MODE_AUTO1
Definition: autopilot_static.h:37
cam_tilt_c
float cam_tilt_c
Definition: cam.c:78