Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
point.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2005-2008 Arnold Schroeter
5  * Modified and expanded to show the coordinates of where the camera is looking at
6  * by Chris Efstathiou 23-Jan-2011 AD.
7  *
8  * This file is part of paparazzi.
9  *
10  * paparazzi is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2, or (at your option)
13  * any later version.
14  *
15  * paparazzi is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with paparazzi; see the file COPYING. If not, write to
22  * the Free Software Foundation, 59 Temple Place - Suite 330,
23  * Boston, MA 02111-1307, USA.
24  *
25  */
26 
76 #include <math.h>
77 #include "cam.h"
78 #include "point.h"
79 #include "autopilot.h"
80 #include "generated/flight_plan.h"
82 #include "subsystems/gps.h"
84 
85 typedef struct {
86  float fx;
87  float fy;
88  float fz;} 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;} 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 unsigned int 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 ***/
183 
185 
186 /*######################################## TILT CONTROL INPUT #############################################*/
187 #ifdef CAM_TILT_NEUTRAL
188 
189 #if defined(RADIO_TILT)
190  if ((*fbw_state).channels[RADIO_TILT] >= 0){
191  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ) *
192  (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
193 
194  }else{
195  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MIN_PPRZ) *
196  (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
197  }
198 #elif defined(RADIO_PITCH)
199  if ((*fbw_state).channels[RADIO_PITCH] >= 0){
200  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ) *
201  (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
202 
203  }else{
204  cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MIN_PPRZ) *
205  (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
206  }
207 #else
208 #error RADIO_TILT or RADIO_PITCH not defined.
209 #endif
210 
211 #else //#ifdef CAM_TILT_NEUTRAL
212 
213 #if defined(RADIO_TILT)
214  cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ));
215 #elif defined(RADIO_PITCH)
216  cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ));
217 #else
218 #error RADIO_TILT or RADIO_PITCH not defined.
219 #endif
220 
221 #endif //#ifdef CAM_TILT_NEUTRAL
222 /*######################################## END OF TILT CONTROL INPUT ########################################*/
223 
224 /*########################################### PAN CONTROL INPUT #############################################*/
225 #ifdef CAM_PAN_NEUTRAL
226 
227 #if defined(RADIO_PAN)
228  if ((*fbw_state).channels[RADIO_PAN] >= 0){
229  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ) *
230  RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
231 
232  }else{
233  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MIN_PPRZ) *
234  RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
235  }
236 #elif defined(RADIO_ROLL)
237  if ((*fbw_state).channels[RADIO_ROLL] >= 0){
238  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ) *
239  RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
240 
241  }else{
242  cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MIN_PPRZ) *
243  RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
244  }
245 #else
246 #error RADIO_PAN or RADIO_ROLL not defined.
247 #endif
248 
249 #else //#ifdef CAM_PAN_NEUTRAL
250 
251 #if defined(RADIO_PAN)
252  cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ));
253 #elif defined(RADIO_ROLL)
254  cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ));
255 #else
256 #error RADIO_PAN or RADIO_ROLL not defined.
257 #endif
258 
259 #endif //#ifdef CAM_PAN_NEUTRAL
260 /*######################################## END OF PAN CONTROL INPUT #############################################*/
261 
262  // Bound Pan and Tilt angles.
263  if (cam_theta > RadOfDeg(CAM_TILT_MAX)){
264  cam_theta = RadOfDeg(CAM_TILT_MAX);
265 
266  }else if(cam_theta < RadOfDeg(CAM_TILT_MIN)){ cam_theta = RadOfDeg(CAM_TILT_MIN); }
267 
268  if (cam_phi > RadOfDeg(CAM_PAN_MAX)){
269  cam_phi = RadOfDeg(CAM_PAN_MAX);
270 
271  }else if(cam_phi < RadOfDeg(CAM_PAN_MIN)){ cam_phi = RadOfDeg(CAM_PAN_MIN); }
272 
273 
274  svPlanePosition.fx = fPlaneEast;
275  svPlanePosition.fy = fPlaneNorth;
276  svPlanePosition.fz = fPlaneAltitude;
277 
278 // FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
279 //cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
280 //cam_theta = RadOfDeg(70); // LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
281 
282  if ( cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC){ // Not much to see after 80 degrees of tilt so stop tracking.
283  *fPan = cam_phi;
284  *fTilt = cam_theta;
285 #ifdef SHOW_CAM_COORDINATES
286  cam_point_distance_from_home = 0;
287  cam_point_lon = 0;
288  cam_point_lat = 0;
289 #endif
290  return;
291 
292  }else{
293  sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta)*(fPlaneAltitude-ground_alt));
294  sv_cam_projection_buf.fy = svPlanePosition.fy;
295  }
296 
297 #if defined(WP_CAM_POINT)
298  sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
299 #else
300  sv_cam_projection_buf.fz = ground_alt;
301 #endif
302 
303  /* distance between plane and camera projection */
304  vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
305 
306  float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
307  if(heading_radians > RadOfDeg(180)){ heading_radians -= RadOfDeg(360); }
308  if(heading_radians < RadOfDeg(-180)){ heading_radians += RadOfDeg(360); }
309  //heading_radians += cam_theta;
310 
311  /* camera pan angle correction, using a clockwise rotation */
312  smRotation.fx1 = (float)(cos(cam_phi));
313  smRotation.fx2 = (float)(sin(cam_phi));
314  smRotation.fx3 = 0.;
315  smRotation.fy1 = -smRotation.fx2;
316  smRotation.fy2 = smRotation.fx1;
317  smRotation.fy3 = 0.;
318  smRotation.fz1 = 0.;
319  smRotation.fz2 = 0.;
320  smRotation.fz3 = 1.;
321 
322  vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
323 
324  /* yaw correction using a counter clockwise rotation*/
325  smRotation.fx1 = (float)(cos(heading_radians));
326  smRotation.fx2 = -(float)(sin(heading_radians));
327  smRotation.fx3 = 0.;
328  smRotation.fy1 = -smRotation.fx2;
329  smRotation.fy2 = smRotation.fx1;
330  smRotation.fy3 = 0.;
331  smRotation.fz1 = 0.;
332  smRotation.fz2 = 0.;
333  smRotation.fz3 = 1.;
334 
335  vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
336 
337 #if defined(RADIO_CAM_LOCK)
338  if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ/2)) && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = TRUE; }
339  if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ/2 && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = FALSE; }
340 #endif
341  // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
342  if (cam_lock == FALSE){
343  fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
344  fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
345  fAltitude = ground_alt;
346  memory_x = fObjectEast;
347  memory_y = fObjectNorth;
348  memory_z = fAltitude;
349 #if defined(WP_CAM_POINT)
350  waypoints[WP_CAM_POINT].x = fObjectEast;
351  waypoints[WP_CAM_POINT].y = fObjectNorth;
352  waypoints[WP_CAM_POINT].a = ground_alt;
353 #endif
354 #if defined(SHOW_CAM_COORDINATES)
355  cam_point_x = fObjectEast;
356  cam_point_y = fObjectNorth;
357 
358  cam_point_distance_from_home = distance_correction * (uint16_t) (sqrt((cam_point_x*cam_point_x) + (cam_point_y*cam_point_y) ));
359 
360  struct UtmCoor_f utm;
361  utm.east = gps.utm_pos.east/100. + sv_cam_projection.fx;
362  utm.north = gps.utm_pos.north/100. + sv_cam_projection.fy;
363  utm.zone = gps.utm_pos.zone;
364  struct LlaCoor_f lla;
365  lla_of_utm_f(&lla, &utm);
366  cam_point_lon = lla.lon*(180/M_PI);
367  cam_point_lat = lla.lat*(180/M_PI);
368 #endif
369 
370  }else{
371  fObjectEast = memory_x;
372  fObjectNorth = memory_y;
373  fAltitude = memory_z;
374 #if defined(WP_CAM_POINT)
375  waypoints[WP_CAM_POINT].x = fObjectEast;
376  waypoints[WP_CAM_POINT].y = fObjectNorth;
377  waypoints[WP_CAM_POINT].a = fAltitude;
378 #endif
379  }
380 
381 if (cam_mode == CAM_MODE_RC && cam_lock == 0 ){
382  *fPan = cam_phi;
383  *fTilt = cam_theta;
384  return;
385 }
386 
387 #if defined(WP_CAM_POINT)
388  else{
389  waypoints[WP_CAM_POINT].x = fObjectEast;
390  waypoints[WP_CAM_POINT].y = fObjectNorth;
391  waypoints[WP_CAM_POINT].a = fAltitude;
392  }
393 #endif
394 /*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/
395 }else{
396 /*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/
397 #ifdef SHOW_CAM_COORDINATES
398  cam_point_distance_from_home = distance_correction * (uint16_t) fabs( ((uint16_t) (sqrt((fObjectNorth*fObjectNorth) + (fObjectEast*fObjectEast) ))) -
399  ((uint16_t) (sqrt((fPlaneNorth*fPlaneNorth) + (fPlaneEast*fPlaneEast) ))) );
400 
401  struct UtmCoor_f utm;
402  utm.east = nav_utm_east0 + fObjectEast;
403  utm.north = nav_utm_north0 + fObjectNorth;
404  utm.zone = gps.utm_pos.zone;
405  struct LlaCoor_f lla;
406  lla_of_utm_f(&lla, &utm);
407  cam_point_lon = lla.lon*(180/M_PI);
408  cam_point_lat = lla.lat*(180/M_PI);
409 #endif
410 
411 
412 #if defined(WP_CAM_POINT)
413  waypoints[WP_CAM_POINT].x = fObjectEast;
414  waypoints[WP_CAM_POINT].y = fObjectNorth;
415  waypoints[WP_CAM_POINT].a = fAltitude;
416 #endif
417 
418  }
419 
420 }
421 
422 //************************************************************************************************
423 //************************************************************************************************
424 //************************************************************************************************
425 //************************************************************************************************
426 
427 /*
428 By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
429 to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
430 applied to the rotation matrices (in radians).
431 In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
432 and +90 is to the top (counterclockwise rotation).
433 When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x
434 represented on a circle in standard mathematical notation.
435 */
436  svPlanePosition.fx = fPlaneNorth;
437  svPlanePosition.fy = fPlaneEast;
438  svPlanePosition.fz = fPlaneAltitude;
439 
440  svObjectPosition.fx = fObjectNorth;
441  svObjectPosition.fy = fObjectEast;
442  svObjectPosition.fz = fAltitude;
443 
444  /* distance between plane and object */
445  vSubtractVectors(&svObjectPositionForPlane, svObjectPosition, svPlanePosition);
446 
447  /* yaw */
448  smRotation.fx1 = (float)(cos(fYawAngle));
449  smRotation.fx2 = (float)(sin(fYawAngle));
450  smRotation.fx3 = 0.;
451  smRotation.fy1 = -smRotation.fx2;
452  smRotation.fy2 = smRotation.fx1;
453  smRotation.fy3 = 0.;
454  smRotation.fz1 = 0.;
455  smRotation.fz2 = 0.;
456  smRotation.fz3 = 1.;
457 
458  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
459 
460  /* pitch */
461  smRotation.fx1 = (float)(cos(fPitchAngle));
462  smRotation.fx2 = 0.;
463  smRotation.fx3 = (float)(sin(fPitchAngle));
464  smRotation.fy1 = 0.;
465  smRotation.fy2 = 1.;
466  smRotation.fy3 = 0.;
467  smRotation.fz1 = -smRotation.fx3;
468  smRotation.fz2 = 0.;
469  smRotation.fz3 = smRotation.fx1;
470 
471  vMultiplyMatrixByVector(&svObjectPositionForPlane, smRotation, svObjectPositionForPlane2);
472 
473  /* roll */
474  smRotation.fx1 = 1.;
475  smRotation.fx2 = 0.;
476  smRotation.fx3 = 0.;
477  smRotation.fy1 = 0.;
478  smRotation.fy2 = (float)(cos(fRollAngle));
479  smRotation.fy3 = (float)(-sin(fRollAngle));
480  smRotation.fz1 = 0.;
481  smRotation.fz2 = -smRotation.fy3;
482  smRotation.fz3 = smRotation.fy2;
483 
484  vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation, svObjectPositionForPlane);
485 
486 #ifdef POINT_CAM_PITCH
487 
488  /*
489  * This is for one axis pitch camera mechanisms. The pitch servo neutral
490  * makes the camera look down, 90 is to the front and -90 is to the
491  * back. The pitch value is given through the tilt parameter.
492  * The camera picture is upright when looking in flight direction.
493  *
494  * tilt servo, looking from left:
495  *
496  * plane front <-------------- plane back
497  * / I \
498  * / I \
499  * 45 I -45
500  * 0
501  *
502  * (should be hyperbolic, we use lines to make it better, the plane rolls
503  * away from the object while flying towards it!)
504  *
505  */
506 
507  /* fTilt = 0 -> camera looks down
508  90 -> camera looks forward
509  -90 -> camera looks backward
510  */
511 #if 0 //we roll away anyways
512  *fTilt = (float)(atan2( svObjectPositionForPlane2.fx,
513  sqrt( svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy
514  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
515  ));
516 #else
517  *fTilt = (float)(atan2( svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz ));
518 #endif
519 
520  /* fPan is deactivated
521  */
522  *fPan = 0;
523 #else
524 #ifdef POINT_CAM_ROLL
525 
526  /*
527  * This is for single axis roll camera mechanisms. The tilt servo neutral
528  * makes the camera look down, -90 is to the right and 90 is to the
529  * left.
530  * The camera picture is upright when looking to the right.
531  *
532  *
533  * tilt servo, looking from behind:
534  *
535  * plane left --------------- plane right
536  * / I \
537  * / I \
538  * 45 I -45
539  * 0
540  *
541  */
542 #if 1 // have to check if it helps
543  *fTilt = (float)(atan2( svObjectPositionForPlane2.fy,
544  sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
545  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
546  ));
547 #else
548  *fTilt = (float)(atan2( svObjectPositionForPlane2.fy, -svObjectPositionForPlane2.fz));
549 #endif
550 
551  /* fPan is deactivated
552  */
553  *fPan = 0;
554 #else
555 #ifdef POINT_CAM_YAW_PITCH_NOSE
556 
557 /*
558  -45 0 45
559  \ | /
560  \ | /
561  \|/
562  ##
563  ##
564  _____________##______________
565 left tip|_____________________________|right wing tip
566  ##
567  ##
568  ##
569  ##
570  ______##______
571  |_____|_|_____|
572  |
573 */
574 
575 #if defined(CAM_PAN_MODE) && CAM_PAN_MODE == 360
576  /* fixed to the plane*/
577  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, (svObjectPositionForPlane2.fx)));
578 
579  *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
580  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ),
581  -svObjectPositionForPlane2.fz
582  ));
583 
584  // I need to avoid oscillations around the 180 degree mark.
585 /*
586  if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; }
587  if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; }
588 
589  if (*fPan > RadOfDeg(175) && heading_positive == 0){
590  *fPan = RadOfDeg(-180);
591 
592  }else if (*fPan < RadOfDeg(-175) && heading_positive){
593  *fPan = RadOfDeg(180);
594  heading_positive = 0;
595  }
596 */
597 #else
598  *fPan = (float)(atan2(svObjectPositionForPlane2.fy, fabs(svObjectPositionForPlane2.fx)));
599 
600  *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
601  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ),
602  -svObjectPositionForPlane2.fz
603  ));
604 
605  if (svObjectPositionForPlane2.fx < 0)
606  {
607  *fPan = -*fPan;
608  *fTilt = -*fTilt;
609  }
610 
611  // I need to avoid oscillations around the 180 degree mark.
612 /*
613  if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; }
614  if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; }
615 
616  if (*fPan > RadOfDeg(85) && heading_positive == 0){
617  *fPan = RadOfDeg(-90);
618 
619  }else if (*fPan < RadOfDeg(-85) && heading_positive){
620  *fPan = RadOfDeg(90);
621  heading_positive = 0;
622  }
623 */
624 #endif
625 
626 #else
627 #ifdef POINT_CAM_YAW_PITCH
628 
629 /*
630  * This is for two axes pan/tilt camera mechanisms. The default is to
631  * circle clockwise so view is right. The pan servo neutral makes
632  * the camera look to the right with 0 given, 90 is to the back and
633  * -90 is to the front. The tilt servo neutral makes the camera look
634  * down with 0 given, 90 is to the right and -90 is to the left (all
635  * values are used in radian in the software). If the camera looks to
636  * the right side of the plane, the picture is upright. It is upside
637  * down when looking to the left. That is corrected with the MPEG
638  * decoding software on the laptop by mirroring. The pan servo is fixed
639  * in the plane and the tilt servo is moved by the pan servo and moves
640  * the camera.
641  *
642  *
643  * pan servo, tilt set to 90, looking from top:
644  *
645  * plane front
646  *
647  * ^
648  * I
649  * I 45
650  * I /
651  * I/
652  * I------- 0
653  * I\
654  * I \
655  * I -45
656  * I
657  *
658  * plane back
659  *
660  *
661  * tilt servo, pan set to 0, looking from back:
662  *
663  * plane left --------------- plane right
664  * / I \
665  * / I \
666  * -45 I 45
667  * 0
668  *
669  */
670 
671  /* fPan = 0 -> camera looks along the wing
672  90 -> camera looks in flight direction
673  -90 -> camera looks backwards
674  */
675  /* fixed to the plane*/
676  *fPan = (float)(atan2(svObjectPositionForPlane2.fx, fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF)
677  // (CEF) It turned out that Object_North is loaded with x and Object_East with y (reverse). See line #155
678  // this means that:
679  // *fPan = (float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fy)); // makes the camera 0 to look to the nose?
680 
681  /* fTilt = 0 -> camera looks down
682  90 -> camera looks into right hemisphere
683  -90 -> camera looks into left hemispere
684  actually the camera always looks more or less downwards, but never upwards
685  */
686  *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
687  + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ),
688  -svObjectPositionForPlane2.fz
689  ));
690  if (svObjectPositionForPlane2.fy < 0)
691  {
692  *fPan = -*fPan;
693  *fTilt = -*fTilt;
694  }
695 
696 #else
697 #ifdef POINT_CAM_PITCH_ROLL
698 
699 /*
700  * This is for another two axes camera mechanisms. The tilt servo is fixed to
701  * the fuselage and moves the pan servo.
702  *
703  * tilt servo, looking from left:
704  *
705  * plane front <--------------- plane back
706  * / I \
707  * / I \
708  * 45 I -45
709  * 0
710  *
711  *
712  * pan servo, looking from back:
713  *
714  * plane left --------------- plane right
715  * / I \
716  * / I \
717  * 45 I -45
718  * 0
719  *
720  */
721 
722  *fTilt = (float)(atan2( svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz));
723 
724  *fPan = (float)(atan2(-svObjectPositionForPlane2.fy,
725  sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
726  + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
727  ));
728 
729 #else
730 #error at least one POINT_CAM_* camera mount has to be defined!
731 #endif
732 #endif
733 #endif
734 #endif
735 #endif
736 }
unsigned short uint16_t
Definition: types.h:16
float fz
Definition: point.c:88
float fz1
Definition: point.c:93
float cam_phi
Definition: point.c:96
float fx
Definition: point.c:86
uint8_t pprz_mode
Definition: main_ap.c:111
#define MIN_PPRZ
Definition: paparazzi.h:9
uint8_t zone
UTM zone number.
float fy
Definition: point.c:87
float fx3
Definition: point.c:91
#define CAM_MODE_STABILIZED
Definition: cam.h:41
float memory_z
Definition: point.c:98
float cam_theta
Definition: point.c:95
float fz2
Definition: point.c:93
#define CAM_MODE_XY_TARGET
Definition: cam.h:38
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:115
float lat
in radians
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:46
vector in Latitude, Longitude and Altitude
#define CAM_MODE_RC
Definition: cam.h:42
#define FALSE
Definition: imu_chimu.h:141
#define RADIO_PITCH
Definition: spektrum_arch.h:42
#define CAM_PAN_MIN
Definition: cam.h:49
Paparazzi floating point math for geodetic calculations.
float fz3
Definition: point.c:93
uint8_t zone
UTM zone number.
int32_t nav_utm_north0
Definition: common_nav.c:42
float north
in meters
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)
position in UTM coordinates Units: meters
float fy3
Definition: point.c:92
float memory_x
Definition: point.c:98
Definition: point.c:90
float fy1
Definition: point.c:92
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:127
int32_t north
in centimeters
float fx2
Definition: point.c:91
#define TRUE
Definition: imu_chimu.h:144
#define CAM_PAN_MAX
Definition: cam.h:46
float fy2
Definition: point.c:92
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
float fx1
Definition: point.c:91
int32_t east
in centimeters
#define CAM_TILT_MAX
Definition: cam.h:52
int32_t nav_utm_east0
Definition: common_nav.c:41
#define CAM_MODE_WP_TARGET
Definition: cam.h:39
Pan/Tilt camera API.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
uint8_t cam_mode
Definition: cam.c:91
#define MAX_PPRZ
Definition: paparazzi.h:8
#define RADIO_ROLL
Definition: spektrum_arch.h:41
float lon
in radians
Definition: point.c:85
float east
in meters
#define CAM_TILT_MIN
Definition: cam.h:55
struct GpsState gps
global GPS state
Definition: gps.c:31
bool_t cam_lock
Definition: cam.c:92