Uav.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP 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.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



#ifndef __UAV_H_
#define __UAV_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <stdlib.h>
#include <string>
#include <sstream>
#include <vector>
#include <assert.h>

#include <CommonOptions.h>

#include <baselib.h>
#include <RangeSens.h>
#include <MotionModule3D.h>
#include <PositionDetector3D.h>

#include <Resource.h>

namespace MipResources {
 

 /* @{ */

 enum UavModes{
  UAV_FREE_MODE,
  UAV_POSITION_MODE,
  UAV_VELOCITY_MODE,
  UAV_ACCELERATION_MODE,
  UAV_MODE_NUM,
 };

 static const char* UavModesNames[UAV_MODE_NUM] = {
  "FreeMode",
  "PositionMode",
  "VelocityMode",
  "AccelerationMode"
 };
 
 class UavOptions : public Options {
  public:
   IntOption* gps;
   UavOptions(){
    gps  = createIntOption("uavGps",  "whether (1) or not (0) the gps is onboard, default 0", 0);
    updateValues();
   }

   string getObjectName() const {
    return "UavOptions";
   }
 };
 
 
 class UavPar{
  private:
   int  _id;  
   Decimal _lenght; 
   Decimal _width; 
   Decimal _height; 
   string _name; 
  protected:
   void setId(int i){
    assert(i>0);
    _id = i;
   }
   
   void setLength(Decimal r){
    assert(r > 0.0);
    _lenght = r;
   }
   
   void setWidth(Decimal r){
    assert(r>0.0);
    _width=r;
   }
   
   void setHeight(Decimal r){
    assert(r>0.0);
    _height=r;
   }
 
   void setName(string n){
    _name=n;
   }
  
  public:
   UavPar (){
    _id = 0;
    _lenght = 0.0;
    _width = 0.0;
    _height = 0.0;
    _name  = string();
   }
   
   UavPar(const UavPar &r){
    _id  = r._id;
    _lenght = r._lenght;
    _width = r._width;
    _height = r._height;
    _name  = r._name;
   }
   
   UavPar& operator=(const UavPar& r){
    if (this != &r){
     _id  = r._id;
     _lenght = r._lenght;
     _width = r._width;
     _height = r._height;
     _name  = r._name;
    }
    return *this;
   }
   int id(){
    return _id;
   }
   Decimal lenght(){
    return _lenght;
   }
   Decimal width(){
    return _width;
   }
   Decimal height(){
    return _height;
   }
   string name(){
    return _name;
   }
   string print(){
    stringstream s;
    s << "name " << name() << endl
     << "id " << _id << endl 
     << "lenght " << _lenght << endl
     << "width " << _width << endl
     << "height " << _height << endl;
    return s.str();
   }
 };
 
 
 class UavVar{

  protected:
   Position3D  _position;  
   Orientation3D           _attitude;  
   Velocity3D  _velocity;  
   Position3D  _omega;   
   Position3D  _localVelocity; 
   Position3D  _localOmega; 
   Time   _lastTime;  
   Time   _lastTimeQuad; 

                        Acceleration3D      _accel; //< Actual acceleration of the robot in global frame.
                        bool ptammInitProxy; //< proxy used for PTAMM's map initialization (Gazebo only)
                        //Time   _prevLastTime;
                        /* ---------------- */
   
   pthread_mutex_t _varMutex;

                        int remCtrl;                            
  public:
   Position3D  desPosi; 
   Velocity3D  desVel;  
   Acceleration3D desAcc; 
   Orientation3D desAtt;  
   Decimal    desThr;  
   Angle    desYaw;   
   
   UavModes controlMode;  

   UavVar ();
   
   Time time();
   
                        
                        // Luca Ricci mod. //
                        //Time prevTime();
                        inline void setPtammInitProxy(bool value){ptammInitProxy = value;};
                        inline bool getPtammInitProxy(){return ptammInitProxy;};
                        // *************** //
   Time quadTime();
   
   Position3D position();
   
   Orientation3D attitude();
   
   Velocity3D velocity();



   Acceleration3D acceleration();
                        /* ---------------- */
                        
   Position3D omega();
   
   Position3D localOmega();
   
   Position3D localVelocity();
   
   void setPositionVal(Decimal x, Decimal y, Decimal z);
   
   void setPositionVal(Position3D &p);
   
   void setAttitudeVal(Decimal roll, Decimal pitch, Decimal yaw);
   
   void setVelocityVal(Decimal x, Decimal y, Decimal z);
   
   void setOmegaVal(Decimal x, Decimal y, Decimal z);
   
   void setLocalVelocityVal(Decimal x, Decimal y, Decimal z);
   
   void setLocalOmegaVal(Decimal x, Decimal y, Decimal z);
   
   void setTime(Time newTime);
   
                        // Luca Ricci mod. //
                        //void setPrevTime(Time newTime);
                        // *************** //

   void setTimeQuad(Time newTime);

   void setRemoteControlled(int newStatus);

   int getRemoteControlled();

 };
 
 
 class Uav : public Resource{
  protected:
   UavOptions options;   
   UavPar     *_par;   
   UavVar     *_var; 
   PositionDetector3D  *_gps; 
   bool gpsActive;

  public:
   Uav(){
   }
   
   Uav(int argc, const char* argv[]){
    gpsActive = false;
   }
   
   /*virtual TODO logging, anzi il logger è un task */
   
   virtual UavPar* getPar() {
    return _par;
   }
   
   virtual UavVar* getVar(){
    return _var;
   }
   
   virtual bool setStatus(Orientation3D& oDes, Decimal& tdes) = 0;
   
   virtual bool setThrust(Decimal tdes) = 0;
   
   virtual bool setYaw(Angle yaw) = 0;
   
   virtual bool setPose(Pose3D& pDes, short ctrlIn = 0) = 0;
   
   virtual bool setAttitude(Orientation3D& oDes) = 0;
   
   virtual bool setPosition(Position3D p,Velocity3D v = Velocity3D(0.0,0.0,0.0),Acceleration3D a = Acceleration3D(0.0,0.0,0.0)) = 0;
   
   virtual bool setVelocity(Velocity3D p,Acceleration3D a = Acceleration3D(0.0,0.0,0.0)) = 0;
   
   virtual bool setAcceleration(Acceleration3D p) = 0;

   virtual bool readPosition() = 0;

   virtual bool readVelocity() = 0;
   
   virtual bool readAttitude() = 0;

                        // Luca Ricci mod. //
   virtual bool readAllData() = 0;
                        // *************** //
 };

}; // end namespace

#endif

/* @} */