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