Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Martin Mueller
2008-02-07 21:35:08 +00:00
parent 5b6e13dc62
commit 79921eb405
4 changed files with 162 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/cam.c
+87 -92
View File
@@ -38,32 +38,41 @@
#include "point.h"
#endif // POINT_CAM
#ifdef CAM_PITCH_NEUTRAL
#if (CAM_PITCH_MAX == CAM_PITCH_NEUTRAL)
#error CAM_PITCH_MAX has to be different from CAM_PITCH_NEUTRAL
#ifdef CAM_PAN_NEUTRAL
#if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
#error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
#endif
#if (CAM_PITCH_NEUTRAL == CAM_PITCH_MIN)
#error CAM_PITCH_MIN has to be different from CAM_PITCH_NEUTRAL
#if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
#error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
#endif
#endif
#ifdef CAM_TILT_NEUTRAL
#if (CAM_TILT_MAX == CAM_TILT_NEUTRAL)
#error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
#endif
#if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
#error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
#endif
#endif
#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
#define DELTA_ALPHA 0.2
#define MAX_CAM_ROLL M_PI/2
#define MAX_CAM_PITCH M_PI/3
#ifdef CAM_PHI0
float phi_c = RadOfDeg(CAM_PHI0);
#ifdef CAM_PAN0
float pan_c = RadOfDeg(CAM_PAN0);
#else
float pan_c;
#endif
#ifdef CAM_TILT0
float tilt_c = RadOfDeg(CAM_TILT0);
#else
float tilt_c;
#endif
float phi_c;
#endif
#ifdef CAM_THETA0
float theta_c = RadOfDeg(CAM_THETA0);
#else
float theta_c;
#endif
float target_x, target_y, target_alt;
@@ -74,112 +83,98 @@ int cam_mode_auto = 1;
void cam_manual( void ) {
if (pprz_mode == PPRZ_MODE_AUTO2) {
static pprz_t cam_roll, cam_pitch;
static pprz_t cam_pan, cam_tilt;
int16_t yaw = fbw_state->channels[RADIO_YAW];
if (yaw > MIN_PPRZ_CAM || yaw < -MIN_PPRZ_CAM) {
cam_roll += FLOAT_OF_PPRZ(yaw, 0, 300.);
cam_roll = TRIM_PPRZ(cam_roll);
cam_pan += FLOAT_OF_PPRZ(yaw, 0, 300.);
cam_pan = TRIM_PPRZ(cam_pan);
}
int16_t pitch = fbw_state->channels[RADIO_PITCH];
if (pitch > MIN_PPRZ_CAM || pitch < -MIN_PPRZ_CAM) {
cam_pitch += FLOAT_OF_PPRZ(pitch, 0, 300.);
cam_pitch = TRIM_PPRZ(cam_pitch);
cam_tilt += FLOAT_OF_PPRZ(pitch, 0, 300.);
cam_tilt = TRIM_PPRZ(cam_tilt);
}
ap_state->commands[COMMAND_CAM_ROLL] = cam_roll;
ap_state->commands[COMMAND_CAM_PITCH] = cam_pitch;
#ifdef COMMAND_CAM_PAN
ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
#endif
#ifdef COMMAND_CAM_TILT
ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
#endif
}
}
void cam_nadir( void ) {
phi_c = -estimator_phi;
theta_c = -estimator_theta;
pan_c = -estimator_phi;
tilt_c = -estimator_theta;
}
#if 0
void cam_target_trigo( void ) {
/** Relative height of the target */
float h = estimator_z - target_alt;
/** Relative heading of the target (trigo) */
float dy = target_y-estimator_y;
float dx = target_x-estimator_x;
float alpha = atan2(dy, dx);
/** Projection in a horizontal plane of the distance ac-target */
float d = sqrt (dy*dy + dx*dx);
float d_h = d / h;
float pseudo_psi = M_PI/2 - estimator_hspeed_dir; /** Trigo */
float psi_alpha = - (pseudo_psi - M_PI/2 - alpha);
phi_c = atan(d_h * cos (psi_alpha)) + estimator_phi;
theta_c = atan(d_h * sin (psi_alpha)) - estimator_theta;
}
#endif
void cam_target( void ) {
#ifdef POINT_CAM
/* phi fRoll */
/* theta fTilt */
float cam_pitch;
float cam_pan = 0;
float cam_tilt = 0;
if (cam_mode_auto != 0)
{
vPoint(estimator_x, estimator_y, estimator_z,
estimator_hspeed_dir, estimator_theta, estimator_phi,
estimator_phi, estimator_theta, estimator_hspeed_dir,
target_x, target_y, target_alt,
&phi_c, &theta_c);
&pan_c, &tilt_c);
}
if (theta_c < RadOfDeg(CAM_PITCH_MIN - 30)) theta_c = RadOfDeg(CAM_PITCH_MAX);
if (theta_c > RadOfDeg(CAM_PITCH_NEUTRAL))
{
// cam_pitch = (theta_c - CAM_PITCH_NEUTRAL) * (MAX_PPRZ / (CAM_PITCH_MAX - CAM_PITCH_NEUTRAL));
cam_pitch = MAX_PPRZ * (theta_c / (RadOfDeg(CAM_PITCH_MAX - CAM_PITCH_NEUTRAL)) - 1.);
}
#ifdef CAM_PAN_NEUTRAL
if (theta_c > RadOfDeg(CAM_PAN_NEUTRAL))
cam_pan = MAX_PPRZ * (theta_c / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)) - 1.);
else
{
// cam_pitch = (theta_c - CAM_PITCH_NEUTRAL) * MIN_PPRZ / (CAM_PITCH_MAX - CAM_PITCH_NEUTRAL);
cam_pitch = MIN_PPRZ * (1. - (theta_c / (RadOfDeg(CAM_PITCH_NEUTRAL - CAM_PITCH_MIN))));
}
cam_pan = MIN_PPRZ * (1. - (theta_c / (RadOfDeg(CAM_PAN_NEUTRAL - CAM_PAN_MIN))));
#endif
#ifdef CAM_TILT_NEUTRAL
if (theta_c > RadOfDeg(CAM_TILT_NEUTRAL))
cam_tilt = MAX_PPRZ * (theta_c / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) - 1.);
else
cam_tilt = MIN_PPRZ * (1. - (theta_c / (RadOfDeg(CAM_TILT_NEUTRAL - CAM_TILT_MIN))));
#endif
cam_pitch = TRIM_PPRZ(cam_pitch);
cam_pan = TRIM_PPRZ(cam_pan);
cam_tilt = TRIM_PPRZ(cam_tilt);
#ifdef COMMAND_HATCH
// if (0 != use_hatch_cam)
ap_state->commands[COMMAND_HATCH] = cam_pitch;
ap_state->commands[COMMAND_HATCH] = cam_tilt;
#endif
#ifdef COMMAND_CAM_PAN
ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
#endif
#ifdef COMMAND_CAM_TILT
ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
#endif
#if 0
printf("pitch %f, roll %f, cam_tilt %f\n",
(estimator_theta/M_PI)*180., (estimator_phi/M_PI)*180., (theta_c/M_PI)*180.);
printf("alt = %f\n", estimator_z - target_alt);
printf("y = %f\n", target_y-estimator_y);
printf("x = %f\n", target_x-estimator_x);
printf("servo = %f\n", cam_pitch);
#endif
#if 0
/** Relative height of the target */
float h = estimator_z - target_alt;
/** Relative heading of the target (trigo) */
float dy = target_y-estimator_y;
float dx = target_x-estimator_x;
float alpha = atan2(dy, dx);
/** Projection in a horizontal plane of the distance ac-target */
float d = sqrt (dy*dy + dx*dx);
float d_h = d / h;
float pseudo_psi = M_PI/2 - estimator_hspeed_dir; /** Trigo */
float psi_alpha = - (pseudo_psi - M_PI/2 - alpha);
phi_c = atan(d_h * cos (psi_alpha)) + estimator_phi;
theta_c = atan(d_h * sin (psi_alpha)) - estimator_theta;
#endif
#else
/** Relative height of the target */
float h = estimator_z - target_alt;
/** Relative heading of the target (trigo) */
float dy = target_y-estimator_y;
float dx = target_x-estimator_x;
float alpha = atan2(dy, dx);
/** Projection in a horizontal plane of the distance ac-target */
float d = sqrt (dy*dy + dx*dx);
float d_h = d / h;
float pseudo_psi = M_PI/2 - estimator_hspeed_dir; /** Trigo */
float psi_alpha = - (pseudo_psi - M_PI/2 - alpha);
phi_c = atan(d_h * cos (psi_alpha)) + estimator_phi;
theta_c = atan(d_h * sin (psi_alpha)) - estimator_theta;
#endif // POINT_CAM
/* not true in all camera mount cases */
phi_c = pan_c;
theta_c = tilt_c;
}
#define MAX_DIST_TARGET 500.
void cam_manual_target( void ) {
sw/airborne/cam.h
+2 -1
View File
@@ -33,12 +33,13 @@
extern float phi_c, theta_c;
extern float pan_c, tilt_c;
extern float target_x, target_y;
extern int use_hatch_cam;
extern int cam_mode_auto;
#define CamNull() { phi_c = 0; theta_c = 0; }
#define CamNull() { phi_c = 0; theta_c = 0; pan_c = 0; tilt_c = 0; }
#define CamFix() {}
void cam_nadir(void);
sw/airborne/point.c
+70 -26
View File
@@ -1,7 +1,7 @@
/*
* $Id$
*
* Copyright (C) 2005-2007 Arnold Schroeter
* Copyright (C) 2005-2008 Arnold Schroeter
*
* This file is part of paparazzi.
*
@@ -23,10 +23,10 @@
*/
/** \file point.c
* \brief Determines camera turn and tilt angles.
* \brief Determines camera pan and tilt angles.
*
* project: Depronazzi MAV
* description: Determines camera turn and tilt angles from
* project: Paparazzi
* description: Determines camera pan and tilt angles from
* plane's and object's positions and plane's
* pitch and roll angles. Software might be optimized
* by removing multiplications with 0, it is left this
@@ -121,23 +121,29 @@ void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC)
; in m (actually the units do not matter as
; long as they are the same as for the object's
; position)
; fTurnAngle north=0; right= positive values in radians
; fPitchAngle level=0; nose up = positive values
; fRollAngle level=0; right wing down = positive values
; fPitchAngle level=0; nose up = positive values
; plane's pitch and roll angles
; with respect to ground in radians
; fYawAngle north=0; right= positive values in radians
; plane's yaw angle with respect to north
; fObjectNorth, fObjectEast, fAltitude object's
; position with respect to ground
; in m (actually the units do not matter as
; long as they are the same for the plane's
; position)
; turn, tilt turn and tilt angles for camera servos
; in radians
; fPan, fTilt angles for camera servos in radians,
; pan is turn/left-right and tilt is down-up
; in reference to the camera picture
; camera mount: The way the camera is mounted is given through a
; define POINT_CAM_a_[_b] where a gives the mount
; angle within the aircraft and b the angle when
; viewing the direction of the first servo.
;*******************************************************************/
void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
float fTurnAngle, float fPitchAngle, float fRollAngle,
float fRollAngle, float fPitchAngle, float fYawAngle,
float fObjectEast, float fObjectNorth, float fAltitude,
float *fTurn, float *fTilt)
float *fPan, float *fTilt)
{
static VECTOR svPlanePosition,
svObjectPosition,
@@ -157,9 +163,9 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
/* distance between plane and object */
vSubtractVectors(&svObjectPositionForPlane, svObjectPosition, svPlanePosition);
/* turn */
smRotation.fx1 = (float)(cos(fTurnAngle));
smRotation.fx2 = (float)(sin(fTurnAngle));
/* yaw */
smRotation.fx1 = (float)(cos(fYawAngle));
smRotation.fx2 = (float)(sin(fYawAngle));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
@@ -204,35 +210,36 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
* back. The pitch value is given through the tilt parameter.
*
*
* pitch servo, looking from right:
* tilt servo, looking from left:
*
* plane back ---------------> plane front
* / I \
* / I \
* -45° I 45°
* 0°
* plane front <-------------- plane back
* / I \
* / I \
* 45° I -45°
* 0°
*
* (should be hyperbolic, we use lines to make it better, the plane rolls
* away from the object while flying towards it!)
*
*/
/* fTurn is deactivated
*/
*fTurn = 0;
/* fTilt = 0 -> camera looks down
90 -> camera looks forward
-90 -> camera looks backward
*/
*fTilt = (float)(atan2( svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz ));
#if 0
#if 0 //we roll away anyways
*fTilt = (float)(atan2( svObjectPositionForPlane2.fx,
sqrt( svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy
+ svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
));
#endif
/* fPan is deactivated
*/
*fPan = 0;
#else
#ifdef POINT_CAM_YAW_PITCH
/*
* This is for two axes pan/tilt camera mechanisms. The default is to
@@ -276,11 +283,11 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
*
*/
/* fTurn = 0 -> camera looks along the wing
/* fPan = 0 -> camera looks along the wing
-90 -> camera looks in flight direction
90 -> camera looks backwards
*/
*fTurn = -(float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fz));
*fPan = -(float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fz));
/* fTilt = 0 -> camera looks down
90 -> camera looks into right hemisphere
@@ -291,5 +298,42 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
sqrt( svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy
+ svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
));
#else
#ifdef POINT_CAM_PITCH_ROLL
/*
* This is for another two axes camera mechanisms. The tilt servo is fixed to
* the fuselage and moves the pan servo.
*
* tilt servo, looking from left:
*
* plane front <--------------- plane back
* / I \
* / I \
* 45° I -45°
* 0°
*
*
* pan servo, looking from back:
*
* plane left --------------- plane right
* / I \
* / I \
* 45° I -45°
* 0°
*
*/
*fTilt = (float)(atan2( svObjectPositionForPlane2.fx, svObjectPositionForPlane2.fz));
*fPan = (float)(atan2(-svObjectPositionForPlane2.fy,
sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx
+ svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz )
));
#else
#error at least one CAM_POINT_* camera mount has to be defined!
#endif
#endif
#endif
}
sw/airborne/point.h
+3 -3
View File
@@ -1,7 +1,7 @@
/*
* $Id$
*
* Copyright (C) 2005 Arnold Schroeter
* Copyright (C) 2005-2008 Arnold Schroeter
*
* This file is part of paparazzi.
*
@@ -27,9 +27,9 @@
#define POINT_H
void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
float fTurnAngle, float fPitchAngle, float fRollAngle,
float fRollAngle, float fPitchAngle, float fYawAngle,
float fObjectEast, float fObjectNorth, float fAltitude,
float *fTurn, float *fTilt);
float *fPan, float *fTilt);
#endif /* POINT_H */