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
point.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2008 Arnold Schroeter
3  * Modified and expanded to show the coordinates of where the camera is looking at
4  * by Chris Efstathiou 23-Jan-2011 AD.
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  */
24 
74 #include <math.h>
75 #include "cam.h"
76 #include "point.h"
77 #include "autopilot.h"
78 #include "generated/flight_plan.h"
80 #include "subsystems/gps.h"
82 
83 typedef struct {
84  float fx;
85  float fy;
86  float fz;
87 } VECTOR;
88 
89 typedef struct {
90  float fx1; float fx2; float fx3;
91  float fy1; float fy2; float fy3;
92  float fz1; float fz2; float fz3;
93 } MATRIX;
94 
95 float cam_theta;
96 float cam_phi;
97 bool_t heading_positive = 0;
99 #if defined(SHOW_CAM_COORDINATES)
100 float cam_point_x;
101 float cam_point_y;
102 uint16_t cam_point_distance_from_home;
103 float cam_point_lon, cam_point_lat;
104 float distance_correction = 1;
105 #endif
106 
107 void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
108 void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
109 
110 /*******************************************************************
111 ; function name: vSubtractVectors
112 ; description: subtracts two vectors a = b - c
113 ; parameters:
114 ;*******************************************************************/
115 void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
116 {
117  svA->fx = svB.fx - svC.fx;
118  svA->fy = svB.fy - svC.fy;
119  svA->fz = svB.fz - svC.fz;
120 }
121 
122 /*******************************************************************
123 ; function name: vMultiplyMatrixByVector
124 ; description: multiplies matrix by vector svA = smB * svC
125 ; parameters:
126 ;*******************************************************************/
128 {
129  svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
130  svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
131  svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
132 }
133 
134 /*******************************************************************
135 ; function name: vPoint
136 ; description: Transforms ground coordinate system into
137 ; plane's coordinate system via three rotations
138 ; and determines positions of camera servos.
139 ; parameters: fPlaneNorth, fPlaneEast, fPlaneAltitude plane's
140 ; position with respect to ground
141 ; in m (actually the units do not matter as
142 ; long as they are the same as for the object's
143 ; position)
144 ; fRollAngle level=0; right wing down = positive values
145 ; fPitchAngle level=0; nose up = positive values
146 ; plane's pitch and roll angles
147 ; with respect to ground in radians
148 ; fYawAngle north=0; right= positive values in radians
149 ; plane's yaw angle with respect to north
150 ; fObjectNorth, fObjectEast, fAltitude object's
151 ; position with respect to ground
152 ; in m (actually the units do not matter as
153 ; long as they are the same for the plane's
154 ; position)
155 ; fPan, fTilt angles for camera servos in radians,
156 ; pan is turn/left-right and tilt is down-up
157 ; in reference to the camera picture
158 ; camera mount: The way the camera is mounted is given through a
159 ; define POINT_CAM_a_[_b] where a gives the mount
160 ; angle within the aircraft and b the angle when
161 ; viewing the direction of the first servo.
162 ;*******************************************************************/
163 void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
164  float fRollAngle, float fPitchAngle, float fYawAngle,
165  float fObjectEast, float fObjectNorth, float fAltitude,
166  float *fPan, float *fTilt)
167 {
168  static VECTOR svPlanePosition,
169  svObjectPosition,
170  svObjectPositionForPlane,
171  svObjectPositionForPlane2;
172 
173  static VECTOR sv_cam_projection,
174  sv_cam_projection_buf;
175 
176  static MATRIX smRotation;
177 
178  /*** BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS) ***/
179  /*** IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT ***/
180  /*** THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC. ***/
181  /*** IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED ***/
184 
186 
187  /*######################################## TILT CONTROL INPUT #############################################*/
188 #ifdef CAM_TILT_NEUTRAL
189 
190 #if defined(RADIO_TILT)
191  if ((*fbw_state).channels[RADIO_TILT] >= 0) {
192  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ) *
193  (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
194 
195  } else {
196  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MIN_PPRZ) *
197  (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
198  }
199 #elif defined(RADIO_PITCH)
200  if ((*fbw_state).channels[RADIO_PITCH] >= 0) {
201  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ) *
202  (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
203 
204  } else {
205  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MIN_PPRZ) *
206  (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
207  }
208 #else
209 #error RADIO_TILT or RADIO_PITCH not defined.
210 #endif
211 
212 #else //#ifdef CAM_TILT_NEUTRAL
213 
214 #if defined(RADIO_TILT)
215  cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(
216  *fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ));
217 #elif defined(RADIO_PITCH)
218  cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(
219  *fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ));
220 #else
221 #error RADIO_TILT or RADIO_PITCH not defined.
222 #endif
223 
224 #endif //#ifdef CAM_TILT_NEUTRAL
225  /*######################################## END OF TILT CONTROL INPUT ########################################*/
226 
227  /*########################################### PAN CONTROL INPUT #############################################*/
228 #ifdef CAM_PAN_NEUTRAL
229 
230 #if defined(RADIO_PAN)
231  if ((*fbw_state).channels[RADIO_PAN] >= 0) {
232  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MAX_PPRZ) *
233  RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL));
234 
235  } else {
236  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MIN_PPRZ) *
237  RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL));
238  }
239 #elif defined(RADIO_ROLL)
240  if ((*fbw_state).channels[RADIO_ROLL] >= 0) {
241  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MAX_PPRZ) *
242  RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL));
243 
244  } else {
245  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MIN_PPRZ) *
246  RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL));
247  }
248 #else
249 #error RADIO_PAN or RADIO_ROLL not defined.
250 #endif
251 
252 #else //#ifdef CAM_PAN_NEUTRAL
253 
254 #if defined(RADIO_PAN)
255  cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /
256  (float)MAX_PPRZ));
257 #elif defined(RADIO_ROLL)
258  cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /
259  (float)MAX_PPRZ));
260 #else
261 #error RADIO_PAN or RADIO_ROLL not defined.
262 #endif
263 
264 #endif //#ifdef CAM_PAN_NEUTRAL
265  /*######################################## END OF PAN CONTROL INPUT #############################################*/
266 
267  // Bound Pan and Tilt angles.
268  if (cam_theta > RadOfDeg(CAM_TILT_MAX)) {
269  cam_theta = RadOfDeg(CAM_TILT_MAX);
270 
271  } else if (cam_theta < RadOfDeg(CAM_TILT_MIN)) { cam_theta = RadOfDeg(CAM_TILT_MIN); }
272 
273  if (cam_phi > RadOfDeg(CAM_PAN_MAX)) {
274  cam_phi = RadOfDeg(CAM_PAN_MAX);
275 
276  } else if (cam_phi < RadOfDeg(CAM_PAN_MIN)) { cam_phi = RadOfDeg(CAM_PAN_MIN); }
277 
278 
279  svPlanePosition.fx = fPlaneEast;
280  svPlanePosition.fy = fPlaneNorth;
281  svPlanePosition.fz = fPlaneAltitude;
282 
283 // FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
284 //cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
285 //cam_theta = RadOfDeg(70); // LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
286 
287  if (cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC) { // Not much to see after 80 degrees of tilt so stop tracking.
288  *fPan = cam_phi;
289  *fTilt = cam_theta;
290 #ifdef SHOW_CAM_COORDINATES
291  cam_point_distance_from_home = 0;
292  cam_point_lon = 0;
293  cam_point_lat = 0;
294 #endif
295  return;
296 
297  } else {
298  sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta) * (fPlaneAltitude - ground_alt));
299  sv_cam_projection_buf.fy = svPlanePosition.fy;
300  }
301 
302 #if defined(WP_CAM_POINT)
303  sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
304 #else
305  sv_cam_projection_buf.fz = ground_alt;
306 #endif
307 
308  /* distance between plane and camera projection */
309  vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
310 
311  float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
312  if (heading_radians > RadOfDeg(180)) { heading_radians -= RadOfDeg(360); }
313  if (heading_radians < RadOfDeg(-180)) { heading_radians += RadOfDeg(360); }
314  //heading_radians += cam_theta;
315 
316  /* camera pan angle correction, using a clockwise rotation */
317  smRotation.fx1 = (float)(cos(cam_phi));
318  smRotation.fx2 = (float)(sin(cam_phi));
319  smRotation.fx3 = 0.;
320  smRotation.fy1 = -smRotation.fx2;
321  smRotation.fy2 = smRotation.fx1;
322  smRotation.fy3 = 0.;
323  smRotation.fz1 = 0.;
324  smRotation.fz2 = 0.;
325  smRotation.fz3 = 1.;
326 
327  vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
328 
329  /* yaw correction using a counter clockwise rotation*/
330  smRotation.fx1 = (float)(cos(heading_radians));
331  smRotation.fx2 = -(float)(sin(heading_radians));
332  smRotation.fx3 = 0.;
333  smRotation.fy1 = -smRotation.fx2;
334  smRotation.fy2 = smRotation.fx1;
335  smRotation.fy3 = 0.;
336  smRotation.fz1 = 0.;
337  smRotation.fz2 = 0.;
338  smRotation.fz3 = 1.;
339 
340  vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
341 
342 #if defined(RADIO_CAM_LOCK)
343  if ((float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ / 2)) && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = TRUE; }
344  if ((float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ / 2 && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = FALSE; }
345 #endif
346  // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
347  if (cam_lock == FALSE) {
348  fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
349  fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
350  fAltitude = ground_alt;
351  memory_x = fObjectEast;
352  memory_y = fObjectNorth;
353  memory_z = fAltitude;
354 #if defined(WP_CAM_POINT)
355  waypoints[WP_CAM_POINT].x = fObjectEast;
356  waypoints[WP_CAM_POINT].y = fObjectNorth;
357  waypoints[WP_CAM_POINT].a = ground_alt;
358 #endif
359 #if defined(SHOW_CAM_COORDINATES)
360  cam_point_x = fObjectEast;
361  cam_point_y = fObjectNorth;
362 
363  cam_point_distance_from_home = distance_correction * (uint16_t)(sqrt((cam_point_x * cam_point_x) +
364  (cam_point_y * cam_point_y)));
365 
366  struct UtmCoor_f utm;
367  utm.east = gps.utm_pos.east / 100. + sv_cam_projection.fx;
368  utm.north = gps.utm_pos.north / 100. + sv_cam_projection.fy;
369  utm.zone = gps.utm_pos.zone;
370  struct LlaCoor_f lla;
371  lla_of_utm_f(&lla, &utm);
372  cam_point_lon = lla.lon * (180 / M_PI);
373  cam_point_lat = lla.lat * (180 / M_PI);
374 #endif
375 
376  } else {
377  fObjectEast = memory_x;
378  fObjectNorth = memory_y;
379  fAltitude = memory_z;
380 #if defined(WP_CAM_POINT)
381  waypoints[WP_CAM_POINT].x = fObjectEast;
382  waypoints[WP_CAM_POINT].y = fObjectNorth;
383  waypoints[WP_CAM_POINT].a = fAltitude;
384 #endif
385  }
386 
387  if (cam_mode == CAM_MODE_RC && cam_lock == 0) {
388  *fPan = cam_phi;
389  *fTilt = cam_theta;
390  return;
391  }
392 
393 #if defined(WP_CAM_POINT)
394  else {
395  waypoints[WP_CAM_POINT].x = fObjectEast;
396  waypoints[WP_CAM_POINT].y = fObjectNorth;
397  waypoints[WP_CAM_POINT].a = fAltitude;
398  }
399 #endif
400  /*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/
401  } else {
402  /*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/
403 #ifdef SHOW_CAM_COORDINATES
404  cam_point_distance_from_home = distance_correction * (uint16_t) fabs(((uint16_t)(sqrt((fObjectNorth * fObjectNorth) +
405  (fObjectEast * fObjectEast)))) -
406  ((uint16_t)(sqrt((fPlaneNorth * fPlaneNorth) + (fPlaneEast * fPlaneEast)))));
407 
408  struct UtmCoor_f utm;
409  utm.east = nav_utm_east0 + fObjectEast;
410  utm.north = nav_utm_north0 + fObjectNorth;
411  utm.zone = gps.utm_pos.zone;
412  struct LlaCoor_f lla;
413  lla_of_utm_f(&lla, &utm);
414  cam_point_lon = lla.lon * (180 / M_PI);
415  cam_point_lat = lla.lat * (180 / M_PI);
416 #endif
417 
418 
419 #if defined(WP_CAM_POINT)
420  waypoints[WP_CAM_POINT].x = fObjectEast;
421  waypoints[WP_CAM_POINT].y = fObjectNorth;
422  waypoints[WP_CAM_POINT].a = fAltitude;
423 #endif
424 
425  }
426 
427  }
428 
429 //************************************************************************************************
430 //************************************************************************************************
431 //************************************************************************************************
432 //************************************************************************************************
433 
434  /*
435  By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
436  to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
437  applied to the rotation matrices (in radians).
438  In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
439  and +90 is to the top (counterclockwise rotation).
440  When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x
441  represented on a circle in standard mathematical notation.
442  */
443  svPlanePosition.fx = fPlaneNorth;
444  svPlanePosition.fy = fPlaneEast;
445  svPlanePosition.fz = fPlaneAltitude;
446 
447  svObjectPosition.fx = fObjectNorth;
448  svObjectPosition.fy = fObjectEast;
449  svObjectPosition.fz = fAltitude;
450 
451  /* distance between plane and object */
452  vSubtractVectors(&svObjectPositionForPlane, svObjectPosition, svPlanePosition);
453 
454  /* yaw */
455  smRotation.fx1 = (float)(cos(fYawAngle));
456  smRotation.fx2 = (float)(sin(fYawAngle));
457  smRotation.fx3 = 0.;
458  smRotation.fy1 = -smRotation.fx2;
459  smRotation.fy2 = smRotation.fx1;
460  smRotation.fy3 = 0.;
461  smRotation.fz1 = 0.;
462  smRotation.fz2 = 0.;
463  smRotation.fz3 = 1.;
464 
465  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
466 
467  /* pitch */
468  smRotation.fx1 = (float)(cos(fPitchAngle));
469  smRotation.fx2 = 0.;
470  smRotation.fx3 = (float)(sin(fPitchAngle));
471  smRotation.fy1 = 0.;
472  smRotation.fy2 = 1.;
473  smRotation.fy3 = 0.;
474  smRotation.fz1 = -smRotation.fx3;
475  smRotation.fz2 = 0.;
476  smRotation.fz3 = smRotation.fx1;
477 
478  vMultiplyMatrixByVector(&svObjectPositionForPlane, smRotation, svObjectPositionForPlane2);
479 
480  /* roll */
481  smRotation.fx1 = 1.;
482  smRotation.fx2 = 0.;
483  smRotation.fx3 = 0.;
484  smRotation.fy1 = 0.;
485  smRotation.fy2 = (float)(cos(fRollAngle));
486  smRotation.fy3 = (float)(-sin(fRollAngle));
487  smRotation.fz1 = 0.;
488  smRotation.fz2 = -smRotation.fy3;
489  smRotation.fz3 = smRotation.fy2;
490 
491  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
492 
493 #ifdef POINT_CAM_PITCH
494 
495  /*
496  * This is for one axis pitch camera mechanisms. The pitch servo neutral
497  * makes the camera look down, 90 is to the front and -90 is to the
498  * back. The pitch value is given through the tilt parameter.
499  * The camera picture is upright when looking in flight direction.
500  *
501  * tilt servo, looking from left:
502  *
503  * plane front <-------------- plane back
504  * / I \
505  * / I \
506  * 45 I -45
507  * 0
508  *
509  * (should be hyperbolic, we use lines to make it better, the plane rolls
510  * away from the object while flying towards it!)
511  *
512  */
513 
514  /* fTilt = 0 -> camera looks down
515  90 -> camera looks forward
516  -90 -> camera looks backward
517  */
518 #if 0 //we roll away anyways
519  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx,
520  sqrt(svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy
521  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
522  ));
523 #else
524  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz));
525 #endif
526 
527  /* fPan is deactivated
528  */
529  *fPan = 0;
530 #else
531 #ifdef POINT_CAM_ROLL
532 
533  /*
534  * This is for single axis roll camera mechanisms. The tilt servo neutral
535  * makes the camera look down, -90 is to the right and 90 is to the
536  * left.
537  * The camera picture is upright when looking to the right.
538  *
539  *
540  * tilt servo, looking from behind:
541  *
542  * plane left --------------- plane right
543  * / I \
544  * / I \
545  * 45 I -45
546  * 0
547  *
548  */
549 #if 1 // have to check if it helps
550  *fTilt = (float)(atan2(svObjectPositionForPlane2.fy,
551  sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
552  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
553  ));
554 #else
555  *fTilt = (float)(atan2(svObjectPositionForPlane2.fy, -svObjectPositionForPlane2.fz));
556 #endif
557 
558  /* fPan is deactivated
559  */
560  *fPan = 0;
561 #else
562 #ifdef POINT_CAM_YAW_PITCH_NOSE
563 
564  /*
565  -45 0 45
566  \ | /
567  \ | /
568  \|/
569  ##
570  ##
571  _____________##______________
572  left tip|_____________________________|right wing tip
573  ##
574  ##
575  ##
576  ##
577  ______##______
578  |_____|_|_____|
579  |
580  */
581 
582 #if defined(CAM_PAN_MODE) && CAM_PAN_MODE == 360
583  /* fixed to the plane*/
584  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, (svObjectPositionForPlane2.fx)));
585 
586  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
587  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
588  -svObjectPositionForPlane2.fz
589  ));
590 
591  // I need to avoid oscillations around the 180 degree mark.
592  /*
593  if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; }
594  if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; }
595 
596  if (*fPan > RadOfDeg(175) && heading_positive == 0){
597  *fPan = RadOfDeg(-180);
598 
599  }else if (*fPan < RadOfDeg(-175) && heading_positive){
600  *fPan = RadOfDeg(180);
601  heading_positive = 0;
602  }
603  */
604 #else
605  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, fabs(svObjectPositionForPlane2.fx)));
606 
607  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
608  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
609  -svObjectPositionForPlane2.fz
610  ));
611 
612  if (svObjectPositionForPlane2.fx < 0) {
613  *fPan = -*fPan;
614  *fTilt = -*fTilt;
615  }
616 
617  // I need to avoid oscillations around the 180 degree mark.
618  /*
619  if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; }
620  if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; }
621 
622  if (*fPan > RadOfDeg(85) && heading_positive == 0){
623  *fPan = RadOfDeg(-90);
624 
625  }else if (*fPan < RadOfDeg(-85) && heading_positive){
626  *fPan = RadOfDeg(90);
627  heading_positive = 0;
628  }
629  */
630 #endif
631 
632 #else
633 #ifdef POINT_CAM_YAW_PITCH
634 
635  /*
636  * This is for two axes pan/tilt camera mechanisms. The default is to
637  * circle clockwise so view is right. The pan servo neutral makes
638  * the camera look to the right with 0 given, 90 is to the back and
639  * -90 is to the front. The tilt servo neutral makes the camera look
640  * down with 0 given, 90 is to the right and -90 is to the left (all
641  * values are used in radian in the software). If the camera looks to
642  * the right side of the plane, the picture is upright. It is upside
643  * down when looking to the left. That is corrected with the MPEG
644  * decoding software on the laptop by mirroring. The pan servo is fixed
645  * in the plane and the tilt servo is moved by the pan servo and moves
646  * the camera.
647  *
648  *
649  * pan servo, tilt set to 90, looking from top:
650  *
651  * plane front
652  *
653  * ^
654  * I
655  * I 45
656  * I /
657  * I/
658  * I------- 0
659  * I\
660  * I \
661  * I -45
662  * I
663  *
664  * plane back
665  *
666  *
667  * tilt servo, pan set to 0, looking from back:
668  *
669  * plane left --------------- plane right
670  * / I \
671  * / I \
672  * -45 I 45
673  * 0
674  *
675  */
676 
677  /* fPan = 0 -> camera looks along the wing
678  90 -> camera looks in flight direction
679  -90 -> camera looks backwards
680  */
681  /* fixed to the plane*/
682  *fPan = (float)(atan2(svObjectPositionForPlane2.fx,
683  fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF)
684  // (CEF) It turned out that Object_North is loaded with x and Object_East with y (reverse). See line #155
685  // this means that:
686  // *fPan = (float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fy)); // makes the camera 0 to look to the nose?
687 
688  /* fTilt = 0 -> camera looks down
689  90 -> camera looks into right hemisphere
690  -90 -> camera looks into left hemispere
691  actually the camera always looks more or less downwards, but never upwards
692  */
693  *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
694  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy),
695  -svObjectPositionForPlane2.fz
696  ));
697  if (svObjectPositionForPlane2.fy < 0) {
698  *fPan = -*fPan;
699  *fTilt = -*fTilt;
700  }
701 
702 #else
703 #ifdef POINT_CAM_PITCH_ROLL
704 
705  /*
706  * This is for another two axes camera mechanisms. The tilt servo is fixed to
707  * the fuselage and moves the pan servo.
708  *
709  * tilt servo, looking from left:
710  *
711  * plane front <--------------- plane back
712  * / I \
713  * / I \
714  * 45 I -45
715  * 0
716  *
717  *
718  * pan servo, looking from back:
719  *
720  * plane left --------------- plane right
721  * / I \
722  * / I \
723  * 45 I -45
724  * 0
725  *
726  */
727 
728  *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz));
729 
730  *fPan = (float)(atan2(-svObjectPositionForPlane2.fy,
731  sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
732  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz)
733  ));
734 
735 #else
736 #error at least one POINT_CAM_* camera mount has to be defined!
737 #endif
738 #endif
739 #endif
740 #endif
741 #endif
742 }
unsigned short uint16_t
Definition: types.h:16
int32_t north
in centimeters
float fx2
Definition: point.c:90
float cam_phi
Definition: point.c:96
float x
Definition: common_nav.h:40
float east
in meters
float north
in meters
#define RADIO_ROLL
Definition: spektrum_arch.h:43
#define MIN_PPRZ
Definition: paparazzi.h:9
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
float fz
Definition: point.c:86
uint8_t pprz_mode
Definition: autopilot.c:41
#define CAM_MODE_STABILIZED
Definition: cam.h:39
float memory_z
Definition: point.c:98
float cam_theta
Definition: point.c:95
#define CAM_MODE_XY_TARGET
Definition: cam.h:36
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:115
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:52
position in UTM coordinates Units: meters
int32_t east
in centimeters
float fz1
Definition: point.c:92
float fy1
Definition: point.c:91
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:70
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
#define CAM_MODE_RC
Definition: cam.h:40
#define FALSE
Definition: std.h:5
#define CAM_PAN_MIN
Definition: cam.h:47
uint8_t zone
UTM zone number.
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
#define TRUE
Definition: std.h:4
float y
Definition: common_nav.h:41
float fz2
Definition: point.c:92
int32_t nav_utm_north0
Definition: common_nav.c:43
Definition: point.c:83
#define RADIO_PITCH
Definition: spektrum_arch.h:44
bool_t heading_positive
Definition: point.c:97
Device independent GPS code (interface)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
float memory_x
Definition: point.c:98
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:127
uint8_t zone
UTM zone number.
Definition: point.c:89
float fy2
Definition: point.c:91
#define CAM_PAN_MAX
Definition: cam.h:44
float fx1
Definition: point.c:90
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
float memory_y
Definition: point.c:98
#define CAM_TILT_MAX
Definition: cam.h:50
int32_t nav_utm_east0
Definition: common_nav.c:42
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
#define CAM_MODE_WP_TARGET
Definition: cam.h:37
Pan/Tilt camera API.
float lon
in radians
uint8_t cam_mode
Definition: cam.c:91
float lat
in radians
float a
Definition: common_nav.h:42
float fx3
Definition: point.c:90
float fy
Definition: point.c:85
#define MAX_PPRZ
Definition: paparazzi.h:8
#define CAM_TILT_MIN
Definition: cam.h:53
struct GpsState gps
global GPS state
Definition: gps.c:41
float fy3
Definition: point.c:91
float fz3
Definition: point.c:92
float fx
Definition: point.c:84
bool_t cam_lock
Definition: cam.c:92