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