Пример #1
0
// Calculate distance and bearing using Geodetic Datums
void au_uav_ros::CSimulation::GeodeticCalculation(double lat1,double lat2,
						  double long1,double long2,
						  double& newLat,double& newLong,
						  double& actualBearing,
						  double& bearing,
						  double& distanceToDestination,
			       			  double& groundSpeed)
{ 
  GeographicLib::Math::real azi1, azi2, s12, m12, a12, M12, M21, S12;
  // Using WGS84 a,f (radius and flattening values)
  GeographicLib::Math::real
    a = GeographicLib::Constants::WGS84_a<GeographicLib::Math::real>(),
    f = GeographicLib::Constants::WGS84_f<GeographicLib::Math::real>();
  const GeographicLib::Geodesic geod(a, f);

  // All calculation done in degrees
  double lat1_deg,lat2_deg,long1_deg,long2_deg;

  lat1_deg = lat1*RADIANS_TO_DEGREES;
  lat2_deg = lat2*RADIANS_TO_DEGREES;
  long1_deg = long1*RADIANS_TO_DEGREES;
  long2_deg = long2*RADIANS_TO_DEGREES;

  // Calculate Bearing 
  a12 = geod.Inverse(lat1_deg, long1_deg, 
		     lat2_deg, long2_deg, 
		     s12, azi1, azi2,
		     m12, M12, M21, S12);

  bearing = azi1*DEGREES_TO_RADIANS;

  // Distance to Destination Returned to the caller
  distanceToDestination = s12;

  // DIfference between course and bearing
  actualBearing = CheckTurningRadius(actualBearing*RADIANS_TO_DEGREES,bearing*RADIANS_TO_DEGREES);

  double distance = MPS_SPEED;

  // Apply Environment and GPS effects
  au_uav_ros::CSimulation::GetInstance().UpdateSimulatedValues(actualBearing,distance, groundSpeed);

  // Geodesic Line along which the UAV is moving
  GeographicLib::GeodesicLine l;
  double temp_actualBearing = actualBearing*RADIANS_TO_DEGREES;
  l = geod.Line(lat1_deg, long1_deg,temp_actualBearing );
  
  // Obtain next location along geodesic line
  a12 = l.Position(distance, newLat, newLong, temp_actualBearing, m12, M12, M21, S12);       
 
  newLat = newLat*DEGREES_TO_RADIANS;
  newLong = newLong*DEGREES_TO_RADIANS;

}
Пример #2
0
void Trajectory::calcLengthPath(void){

    try {
        const Geodesic& geod = Geodesic::WGS84();

        double a12 = geod.Inverse(this->startPoint.getDecLatitude(), this->startPoint.getDecLongitude(),this->endPoint.getDecLatitude(), this->endPoint.getDecLongitude(), this->distanceLengthStartToEnd, this->azimuthStartEndPointFromStart,this->azimuthStartEndPointFormEnd);

        const GeographicLib::GeodesicLine line(geod,this->startPoint.getDecLatitude(),this->startPoint.getDecLongitude(), this->azimuthStartEndPointFromStart);
        this->numberOfInervals = int(ceil(this->distanceLengthStartToEnd / this->quantumStep));
        this->distanceStartToNthPoint=this->distanceLengthStartToEnd/this->numberOfInervals;
        for (int i = 0; i <=this->numberOfInervals; ++i) {
            double lat,lon;
            line.Position(i*this->distanceStartToNthPoint,lat,lon);
            this->nThPoint.setDecLatitude(lat);
            this->nThPoint.setDecLongitude(lon);
            this->pathLength.push_back(this->nThPoint);
        }
        vypis(this->pathLength);

    }    catch (const exception& e) {
        cerr << "Caught exception: " << e.what() << "\n";
        //return 1;
    }
}