diff --git a/sw/airborne/cam.c b/sw/airborne/cam.c index a4bb5f35d7..f0334a30ca 100644 --- a/sw/airborne/cam.c +++ b/sw/airborne/cam.c @@ -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 ) { diff --git a/sw/airborne/cam.h b/sw/airborne/cam.h index 5f99c8470a..e439858e60 100644 --- a/sw/airborne/cam.h +++ b/sw/airborne/cam.h @@ -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); diff --git a/sw/airborne/point.c b/sw/airborne/point.c index ff70c04932..aaef50f4fd 100644 --- a/sw/airborne/point.c +++ b/sw/airborne/point.c @@ -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 } diff --git a/sw/airborne/point.h b/sw/airborne/point.h index b89b648587..73ca8ff6b8 100644 --- a/sw/airborne/point.h +++ b/sw/airborne/point.h @@ -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 */