Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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"
79 #include "modules/nav/common_nav.h"
80 #include "modules/gps/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 = radio_control_get(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 }
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:222
Core autopilot interface common to all firmwares.
uint8_t cam_mode
Definition: cam.c:90
bool cam_lock
Definition: cam.c:91
Pan/Tilt camera API.
#define CAM_MODE_XY_TARGET
Definition: cam.h:36
#define CAM_PAN_MIN
Definition: cam.h:47
#define CAM_MODE_RC
Definition: cam.h:40
#define CAM_TILT_MIN
Definition: cam.h:53
#define CAM_MODE_STABILIZED
Definition: cam.h:39
#define CAM_TILT_MAX
Definition: cam.h:50
#define CAM_PAN_MAX
Definition: cam.h:44
#define CAM_MODE_WP_TARGET
Definition: cam.h:37
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
int32_t nav_utm_north0
Definition: common_nav.c:44
float y
Definition: common_nav.h:41
float a
Definition: common_nav.h:42
float x
Definition: common_nav.h:40
#define AP_MODE_AUTO2
Device independent GPS code (interface)
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
@ VECTOR
Definition: guidance_OA.h:52
float fz1
Definition: point.c:93
float fx3
Definition: point.c:91
float fx1
Definition: point.c:91
float fy2
Definition: point.c:92
float fz2
Definition: point.c:93
float fy1
Definition: point.c:92
float fy3
Definition: point.c:92
float fx
Definition: point.c:85
float fz3
Definition: point.c:93
float fy
Definition: point.c:86
float fx2
Definition: point.c:91
float fz
Definition: point.c:87
Definition: point.c:90
Definition: point.c:84
#define MAX_PPRZ
Definition: paparazzi.h:8
#define MIN_PPRZ
Definition: paparazzi.h:9
float memory_z
Definition: point.c:99
float memory_x
Definition: point.c:99
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:116
float cam_phi
Definition: point.c:97
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
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:128
bool heading_positive
Definition: point.c:98
float memory_y
Definition: point.c:99
float cam_theta
Definition: point.c:96
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
Paparazzi floating point math for geodetic calculations.
uint8_t zone
UTM zone number.
float lon
in radians
float east
in meters
float lat
in radians
float north
in meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
Definition: radio_control.h:94
API to get/set the generic vehicle states.
#define FALSE
Definition: std.h:5
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88