Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/gps/gps.h"
82#include "state.h"
83
84typedef struct {
85 float fx;
86 float fy;
87 float fz;
88} VECTOR;
89
90typedef 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
97float cam_phi;
100#if defined(SHOW_CAM_COORDINATES)
101float cam_point_x;
102float cam_point_y;
105float distance_correction = 1;
106#endif
107
110
111/*******************************************************************
112; function name: vSubtractVectors
113; description: subtracts two vectors a = b - c
114; parameters:
115;*******************************************************************/
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;*******************************************************************/
165 float fRollAngle, float fPitchAngle, float fYawAngle,
166 float fObjectEast, float fObjectNorth, float fAltitude,
167 float *fPan, float *fTilt)
168{
169 static VECTOR svPlanePosition,
173
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
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) *
197
198 } else {
199 cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MIN_PPRZ) *
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) *
206
207 } else {
208 cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MIN_PPRZ) *
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)
219 *fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ));
220#elif defined(RADIO_PITCH)
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) *
237
238 } else {
239 cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MIN_PPRZ) *
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) *
246
247 } else {
248 cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MIN_PPRZ) *
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 #############################################*/
270
271 // Bound Pan and Tilt angles.
274
276
279
281
282
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
296 cam_point_lon = 0;
297 cam_point_lat = 0;
298#endif
299 return;
300
301 } else {
304 }
305
306#if defined(WP_CAM_POINT)
308#else
310#endif
311
312 /* distance between plane and camera projection */
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
332
333 /* yaw correction using a counter clockwise rotation*/
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
345
346#if defined(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) {
359#if defined(WP_CAM_POINT)
363#endif
364#if defined(SHOW_CAM_COORDINATES)
367
370
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 {
384#if defined(WP_CAM_POINT)
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 {
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
409 (fObjectEast * fObjectEast)))) -
411
412 struct UtmCoor_f utm;
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)
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 */
450
454
455 /* distance between plane and object */
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
470
471 /* pitch */
473 smRotation.fx2 = 0.;
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
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
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
526 ));
527#else
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
557 ));
558#else
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*/
589
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
610
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*/
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 */
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
733
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
int pprz_mtx_unlock(pprz_mutex_t *mtx)
int pprz_mtx_lock(pprz_mutex_t *mtx)
int32_t nav_utm_east0
Definition common_nav.c:48
uint8_t nav_utm_zone0
Definition common_nav.c:50
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:46
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
int32_t nav_utm_north0
Definition common_nav.c:49
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:821
uint16_t foo
Definition main_demo5.c:58
float fz1
Definition point.c:93
float fx1
Definition point.c:91
float fy1
Definition point.c:92
float fx
Definition point.c:85
float fy
Definition point.c:86
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.
float lon
in radians
float east
in meters
float lat
in radians
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
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.