void airborne_ant_point_periodic(void)
{
float airborne_ant_pan_servo = 0;
static VECTOR svPlanePosition,
Home_Position,
Home_PositionForPlane,
Home_PositionForPlane2;
static MATRIX smRotation;
svPlanePosition.fx = stateGetPositionEnu_f()->y;
svPlanePosition.fy = stateGetPositionEnu_f()->x;
svPlanePosition.fz = stateGetPositionUtm_f()->alt;
Home_Position.fx = WaypointY(WP_HOME);
Home_Position.fy = WaypointX(WP_HOME);
Home_Position.fz = waypoints[WP_HOME].a;
/* distance between plane and object */
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
/*
* This is for one axis pan antenna mechanisms. The default is to
* circle clockwise so view is right. The pan servo neutral makes
* the antenna look to the right with 0� given, 90� is to the back and
* -90� is to the front.
*
*
*
* plane front
*
* 90
^
* I
* 135 I 45�
* \ I /
* \I/
* 180-------I------- 0�
* /I\
* / I \
* -135 I -45�
* I
* -90
* plane back
*
*
*/
/* fPan = 0 -> antenna looks along the wing
90 -> antenna looks in flight direction
-90 -> antenna looks backwards
*/
/* fixed to the plane*/
airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
// I need to avoid oscillations around the 180 degree mark.
if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
airborne_ant_pan = RadOfDeg(-180);
} else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
airborne_ant_pan = RadOfDeg(180);
ant_pan_positive = 0;
}
#ifdef ANT_PAN_NEUTRAL
airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
if (airborne_ant_pan > 0) {
airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
} else {
airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
}
#endif
airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
#ifdef COMMAND_ANT_PAN
ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
#endif
return;
}
/*******************************************************************
; function name: vPoint
; description: Transforms ground coordinate system into
; plane's coordinate system via three rotations
; and determines positions of camera servos.
; parameters: fPlaneNorth, fPlaneEast, fPlaneAltitude plane's
; position with respect to ground
; in m (actually the units do not matter as
; long as they are the same as for the object's
; position)
; 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)
; 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 fRollAngle, float fPitchAngle, float fYawAngle,
float fObjectEast, float fObjectNorth, float fAltitude,
float *fPan, float *fTilt)
{
static VECTOR svPlanePosition,
svObjectPosition,
svObjectPositionForPlane,
svObjectPositionForPlane2;
static VECTOR sv_cam_projection,
sv_cam_projection_buf;
static MATRIX smRotation;
/*** BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS) ***/
/*** IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT ***/
/*** THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC. ***/
/*** IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED ***/
if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET || cam_mode == CAM_MODE_XY_TARGET ){
if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){
/*######################################## TILT CONTROL INPUT #############################################*/
#ifdef CAM_TILT_NEUTRAL
#if defined(RADIO_TILT)
if ((*fbw_state).channels[RADIO_TILT] >= 0){
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
}else{
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MIN_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
}
#elif defined(RADIO_PITCH)
if ((*fbw_state).channels[RADIO_PITCH] >= 0){
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) );
}else{
cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MIN_PPRZ) *
(float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) );
}
#else
#error RADIO_TILT or RADIO_PITCH not defined.
#endif
#else //#ifdef CAM_TILT_NEUTRAL
#if defined(RADIO_TILT)
cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ));
#elif defined(RADIO_PITCH)
cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ));
#else
#error RADIO_TILT or RADIO_PITCH not defined.
#endif
#endif //#ifdef CAM_TILT_NEUTRAL
/*######################################## END OF TILT CONTROL INPUT ########################################*/
/*########################################### PAN CONTROL INPUT #############################################*/
#ifdef CAM_PAN_NEUTRAL
#if defined(RADIO_PAN)
if ((*fbw_state).channels[RADIO_PAN] >= 0){
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ) *
RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
}else{
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MIN_PPRZ) *
RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
}
#elif defined(RADIO_ROLL)
if ((*fbw_state).channels[RADIO_ROLL] >= 0){
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ) *
RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) );
}else{
cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MIN_PPRZ) *
RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) );
}
#else
#error RADIO_PAN or RADIO_ROLL not defined.
#endif
#else //#ifdef CAM_PAN_NEUTRAL
#if defined(RADIO_PAN)
cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ));
#elif defined(RADIO_ROLL)
cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ));
#else
#error RADIO_PAN or RADIO_ROLL not defined.
#endif
#endif //#ifdef CAM_PAN_NEUTRAL
/*######################################## END OF PAN CONTROL INPUT #############################################*/
// Bound Pan and Tilt angles.
if (cam_theta > RadOfDeg(CAM_TILT_MAX)){
cam_theta = RadOfDeg(CAM_TILT_MAX);
}else if(cam_theta < RadOfDeg(CAM_TILT_MIN)){ cam_theta = RadOfDeg(CAM_TILT_MIN); }
if (cam_phi > RadOfDeg(CAM_PAN_MAX)){
cam_phi = RadOfDeg(CAM_PAN_MAX);
}else if(cam_phi < RadOfDeg(CAM_PAN_MIN)){ cam_phi = RadOfDeg(CAM_PAN_MIN); }
svPlanePosition.fx = fPlaneEast;
svPlanePosition.fy = fPlaneNorth;
svPlanePosition.fz = fPlaneAltitude;
// FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES
//cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT
//cam_theta = RadOfDeg(70); // LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT
if ( cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC){ // Not much to see after 80 degrees of tilt so stop tracking.
*fPan = cam_phi;
*fTilt = cam_theta;
#ifdef SHOW_CAM_COORDINATES
cam_point_distance_from_home = 0;
cam_point_lon = 0;
cam_point_lat = 0;
#endif
return;
}else{
sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta)*(fPlaneAltitude-ground_alt));
sv_cam_projection_buf.fy = svPlanePosition.fy;
}
#if defined(WP_CAM_POINT)
sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a;
#else
sv_cam_projection_buf.fz = ground_alt;
#endif
/* distance between plane and camera projection */
vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition);
float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation.
if(heading_radians > RadOfDeg(180)){ heading_radians -= RadOfDeg(360); }
if(heading_radians < RadOfDeg(-180)){ heading_radians += RadOfDeg(360); }
//heading_radians += cam_theta;
/* camera pan angle correction, using a clockwise rotation */
smRotation.fx1 = (float)(cos(cam_phi));
smRotation.fx2 = (float)(sin(cam_phi));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection);
/* yaw correction using a counter clockwise rotation*/
smRotation.fx1 = (float)(cos(heading_radians));
smRotation.fx2 = -(float)(sin(heading_radians));
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf);
#if defined(RADIO_CAM_LOCK)
if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ/2)) && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = TRUE; }
if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ/2 && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = FALSE; }
#endif
// When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
if (cam_lock == FALSE){
fObjectEast = (fPlaneEast + sv_cam_projection.fx) ;
fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ;
fAltitude = ground_alt;
memory_x = fObjectEast;
memory_y = fObjectNorth;
memory_z = fAltitude;
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = ground_alt;
#endif
#if defined(SHOW_CAM_COORDINATES)
cam_point_x = fObjectEast;
cam_point_y = fObjectNorth;
cam_point_distance_from_home = distance_correction * (uint16_t) (sqrt((cam_point_x*cam_point_x) + (cam_point_y*cam_point_y) ));
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east/100. + sv_cam_projection.fx;
utm.north = gps.utm_pos.north/100. + sv_cam_projection.fy;
utm.zone = gps.utm_pos.zone;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon*(180/M_PI);
cam_point_lat = lla.lat*(180/M_PI);
#endif
}else{
fObjectEast = memory_x;
fObjectNorth = memory_y;
fAltitude = memory_z;
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
#endif
}
if (cam_mode == CAM_MODE_RC && cam_lock == 0 ){
*fPan = cam_phi;
*fTilt = cam_theta;
return;
}
#if defined(WP_CAM_POINT)
else{
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
}
#endif
/*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/
}else{
/*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/
#ifdef SHOW_CAM_COORDINATES
cam_point_distance_from_home = distance_correction * (uint16_t) fabs( ((uint16_t) (sqrt((fObjectNorth*fObjectNorth) + (fObjectEast*fObjectEast) ))) -
((uint16_t) (sqrt((fPlaneNorth*fPlaneNorth) + (fPlaneEast*fPlaneEast) ))) );
struct UtmCoor_f utm;
utm.east = nav_utm_east0 + fObjectEast;
utm.north = nav_utm_north0 + fObjectNorth;
utm.zone = gps.utm_pos.zone;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon*(180/M_PI);
cam_point_lat = lla.lat*(180/M_PI);
#endif
#if defined(WP_CAM_POINT)
waypoints[WP_CAM_POINT].x = fObjectEast;
waypoints[WP_CAM_POINT].y = fObjectNorth;
waypoints[WP_CAM_POINT].a = fAltitude;
#endif
}
}