Exemplo n.º 1
0
double ODSelect::vGetLengthOfNormal( pODVector2D a, pODVector2D b, pODVector2D n )
{
    ODvector2D c, vNormal;
    vNormal.x = 0;
    vNormal.y = 0;
    //
    //Obtain projection vector.
    //
    //c = ((a * b)/(|b|^2))*b
    //
    c.x = b->x * ( vDotProduct( a, b ) / vDotProduct( b, b ) );
    c.y = b->y * ( vDotProduct( a, b ) / vDotProduct( b, b ) );
//
    //Obtain perpendicular projection : e = a - c
    //
    vSubtractVectors( a, &c, &vNormal );
    //
    //Fill PROJECTION structure with appropriate values.
    //
    *n = vNormal;

    return ( vVectorMagnitude( &vNormal ) );
}
Exemplo n.º 2
0
bool Routeman::UpdateProgress()
{
    bool bret_val = false;

    if( pActiveRoute ) {
//      Update bearing, range, and crosstrack error

//  Bearing is calculated as Mercator Sailing, i.e. a  cartographic "bearing"
        double north, east;
        toSM( pActivePoint->m_lat, pActivePoint->m_lon, gLat, gLon, &east, &north );
        double a = atan( north / east );
        if( fabs( pActivePoint->m_lon - gLon ) < 180. ) {
            if( pActivePoint->m_lon > gLon ) CurrentBrgToActivePoint = 90. - ( a * 180 / PI );
            else
                CurrentBrgToActivePoint = 270. - ( a * 180 / PI );
        } else {
            if( pActivePoint->m_lon > gLon ) CurrentBrgToActivePoint = 270. - ( a * 180 / PI );
            else
                CurrentBrgToActivePoint = 90. - ( a * 180 / PI );
        }

//      Calculate range using Great Circle Formula

        double d5 = DistGreatCircle( gLat, gLon, pActivePoint->m_lat, pActivePoint->m_lon );
        CurrentRngToActivePoint = d5;

//      Get the XTE vector, normal to current segment
        vector2D va, vb, vn;

        double brg1, dist1, brg2, dist2;
        DistanceBearingMercator( pActivePoint->m_lat, pActivePoint->m_lon,
                                 pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon, &brg1,
                                 &dist1 );
        vb.x = dist1 * sin( brg1 * PI / 180. );
        vb.y = dist1 * cos( brg1 * PI / 180. );

        DistanceBearingMercator( pActivePoint->m_lat, pActivePoint->m_lon, gLat, gLon, &brg2,
                                 &dist2 );
        va.x = dist2 * sin( brg2 * PI / 180. );
        va.y = dist2 * cos( brg2 * PI / 180. );

        double sdelta = vGetLengthOfNormal( &va, &vb, &vn );             // NM
        CurrentXTEToActivePoint = sdelta;

//    Calculate the distance to the arrival line, which is perpendicular to the current route segment
//    Taking advantage of the calculated normal from current position to route segment vn
        vector2D vToArriveNormal;
        vSubtractVectors( &va, &vn, &vToArriveNormal );

        CurrentRangeToActiveNormalCrossing = vVectorMagnitude( &vToArriveNormal );

//          Compute current segment course
//          Using simple Mercater projection
        double x1, y1, x2, y2;
        toSM( pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon,
              pActiveRouteSegmentBeginPoint->m_lat, pActiveRouteSegmentBeginPoint->m_lon, &x1,
              &y1 );

        toSM( pActivePoint->m_lat, pActivePoint->m_lon, pActiveRouteSegmentBeginPoint->m_lat,
              pActiveRouteSegmentBeginPoint->m_lon, &x2, &y2 );

        double e1 = atan2( ( x2 - x1 ), ( y2 - y1 ) );
        CurrentSegmentCourse = e1 * 180 / PI;
        if( CurrentSegmentCourse < 0 ) CurrentSegmentCourse += 360;

        //      Compute XTE direction
        double h = atan( vn.y / vn.x );
        if( vn.x > 0 ) CourseToRouteSegment = 90. - ( h * 180 / PI );
        else
            CourseToRouteSegment = 270. - ( h * 180 / PI );

        h = CurrentBrgToActivePoint - CourseToRouteSegment;
        if( h < 0 ) h = h + 360;

        if( h > 180 ) XTEDir = 1;
        else
            XTEDir = -1;

//      Determine Arrival

        bool bDidArrival = false;

        if( CurrentRangeToActiveNormalCrossing <= pActiveRoute->GetRouteArrivalRadius() ) {
            m_bArrival = true;
            UpdateAutopilot();

            bDidArrival = true;

            if( !ActivateNextPoint( pActiveRoute, false ) )            // at the end?
            {
                Route *pthis_route = pActiveRoute;
                DeactivateRoute( true );                  // this is an arrival
                if( pthis_route->m_bDeleteOnArrival ) {
                    pConfig->DeleteConfigRoute( pthis_route );
                    DeleteRoute( pthis_route );
                    if( pRoutePropDialog ) {
                        pRoutePropDialog->SetRouteAndUpdate( NULL );
                        pRoutePropDialog->UpdateProperties();
                    }
                    if( pRouteManagerDialog ) pRouteManagerDialog->UpdateRouteListCtrl();

                }
            }

        }

        if( !bDidArrival )                                        // Only once on arrival
            UpdateAutopilot();

        bret_val = true;                                        // a route is active
    }

    m_bDataValid = true;

    return bret_val;
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
/*******************************************************************
; 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

     }

}