/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/guidance/Guide.cc

Go to the documentation of this file.

/*
 * Guide.cc
 * Copyright (C) Eduardo Wiputra 2009 <ewiputra@users.sourceforge.net>
 *
 * Guide.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Guide.cc is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <string>
#include <fstream>
#include <vector>
#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "uvsim/guidance/Guide.h"
#include "uvsim/guidance/FlightPlan.h"
#include "uvsim/guidance/GeoCoord.h"

namespace uvsim
{
Guide::Guide()
{
}

Guide::Guide(GeoCoord & pos)
{
    setPosition(pos); //assign the current position to m_currentPosition
}

Guide::~Guide()
{
}

FlightPlan* Guide::getFlightPlan()
{
    return &flightplan; //returns flightplan data
}

void Guide::computeCrossTrack()
{
    GeoCoord tempWaypoint; //temporary waypoint
    GeoCoord currentWaypoint; //current waypoint
    vector<double> previousWaypoint (3); //vector that contains the first waypoint
    vector<double> nextWaypoint (3); //vector that contains the second waypoint
    vector<double> currentPosition (3); //vector that contains the current position
    vector<double> tempPosition (3); //vector that will contain the temporary position
    vector<double> deltaXHat (3); //unit vector from temporary to second waypoint
    GeoCoord m_currentPosition(40.430925,-86.9163194,190); //initializes the current position
    int counter = 0; //initializes counter for loop
    double deltaX; //distance between temporary waypoint and next waypoint
    double deltaY; //distance between temporary waypoint and current position
    double delta = 0.1; //error permitted
//      do
//      {

    //Assuming Current Position is inputted as Latitude, Longitude, and Altitude

    //Assigns the value in m_currentPosition to vector of currentPosition
    currentPosition[0] = m_currentPosition.m_Latitude;
    currentPosition[1] = m_currentPosition.m_Longitude;
    currentPosition[2] = m_currentPosition.m_Altitude;

    //Assigns the values in currentWaypoint from currentPosition
    currentWaypoint.setGeoCoord(currentPosition[0], currentPosition[1], currentPosition[2]);

    //Converts current position from geodetic to Earth Center Earth Fixed (ECEF)
    currentPosition = currentWaypoint.getCartesian();

    //Converts previous and next waypoints to cartesian coordinate
    previousWaypoint = flightplan[counter].getCartesian();
    nextWaypoint = flightplan[counter + 1].getCartesian();

    std::cout << "Previous Waypoint in Cartesian: (" <<previousWaypoint[0]
              << ", " << previousWaypoint[1] << ", " << previousWaypoint[2] << ")\n";
    std::cout << "Next Waypoint in Cartesian: (" <<nextWaypoint[0]
              << ", " << nextWaypoint[1] << ", " << nextWaypoint[2] << ")\n";

    //Compute c1 (distance from next waypoint to temporary waypoint)
    deltaXHat = (previousWaypoint - nextWaypoint) / norm_2(previousWaypoint - nextWaypoint);
    deltaX = inner_prod((currentPosition - nextWaypoint), deltaXHat);

    //Compute D (location of tempWaypoint)
    tempPosition = nextWaypoint + deltaX * deltaXHat;
    tempWaypoint.getGeoCoord(tempPosition[0], tempPosition[1], tempPosition[2]);

    //Compute c2 (distance between temporary waypoint and the current waypoint)
    deltaY = norm_2(currentPosition - tempPosition);

    std::cout << "Current Position in Cartesian: (" << currentPosition[0] << ", "
         << currentPosition[1] << ", " << currentPosition[2] << ")\n";
    std::cout << "Temporary Waypoint Position in Cartesian: (" << tempPosition[0]
         << ", " << tempPosition[1] << ", " << tempPosition[2] << ")\n";
    std::cout << "DeltaX (c1) = " << deltaX << "\tDeltaY (c2) = " << deltaY << "\n";

    if (fabs(deltaX) < delta)
    {
        //If deltaX is less than the permitted error, continue checking with the next waypoint
        counter++;
    }
    else if (fabs(deltaY) > delta)
    {
        //If deltaY is more than the permitted error,
        //the current position amd temporary waypoint are added to the waypoint list.
        flightplan.insert(flightplan.begin() + counter + 1, m_currentPosition);
        flightplan.insert(flightplan.begin() + counter + 2, tempWaypoint);
        counter++;
    }
//      }while(counter <= myGeoCoord.size() - 2);
}

void Guide::setPosition(GeoCoord & pos)
{
    m_currentPosition = pos; //assigns position to the m_currentPosition
}
}
// vim:ts=2:sw=2

---