UavControl.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_CONTROL_H
#define __UAV_CONTROL_H

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

#include <baselib.h>

using namespace MipBaselib;


enum UavTypes{
 UAV_TYPES_SWARMSIM,
 UAV_TYPES_MIKROKOP,
 UAV_TYPES_HUMBIRD,
 UAV_TYPES_NUM
};

static const char* UavTypesNames[UAV_TYPES_NUM] = {
 "SwarmSim quadcopter",
 "Mikrokopter",
 "HummingBird"
};

enum UavControlTypes{
 UAV_CTRL_TYPE_POS,
 UAV_CTRL_TYPE_VEL,
 UAV_CTRL_TYPE_ACC,
 UAV_CTRL_TYPE_NUM
};

static const char* UavControlTypesNames[UAV_CTRL_TYPE_NUM] = {
 "Uav Position Control",
 "Uav Velocity Control",
 "Uav Acceleration Control"
};

class UavControlOptions : public Options {
 public:
  DecimalOption *propGain;
  DecimalOption *derivGain;
  DecimalOption *gravity;
  DecimalOption *maxRollSin;
  DecimalOption *maxPitchSin;
  DecimalOption *minThrust;
  
  UavControlOptions();

  string getObjectName() const {
   return "UavControlOptions";
  }
};

class UavPosiControlIn {
 private:
  
 public:
  UavPosiControlIn();

  UavPosiControlIn(Position3D worldPosi,Orientation3D worldAtt,
     Position3D worldVel, Position3D    worldDesPosi,
   Velocity3D worldDesVel = Velocity3D(0.0,0.0,0.0), Acceleration3D worldDesAcc = Acceleration3D(0.0,0.0,0.0));
  
  UavPosiControlIn(Position3D worldPosi,Orientation3D worldAtt,
         Position3D worldVel, Velocity3D worldDesVel, Acceleration3D worldDesAcc = Acceleration3D(0.0,0.0,0.0));
  
  UavPosiControlIn(Position3D worldPosi,Orientation3D worldAtt,
         Position3D worldVel, Acceleration3D worldDesAcc);
   
  UavPosiControlIn(const UavPosiControlIn& p);
  
  UavPosiControlIn& operator=(const UavPosiControlIn& p);
    
  UavControlTypes controlType;
  Position3D   worldPosition;
  Orientation3D  worldAttitude;
  Position3D   worldVelocity;
  Position3D   worldDesiredPosi;
  Velocity3D   worldDesiredVel;
  Acceleration3D worldDesiredAcc;
};

class UavYawControlIn {
 private:
  
 public:
  UavYawControlIn();

  UavYawControlIn(Angle currentYaw, Angle desiredYaw,Decimal desiredYawRate=0.0);
  
  UavYawControlIn(Angle currentYaw, Decimal desiredYawRate);
   
//   UavYawControlIn(const UavYawControlIn& p);
  
//   UavYawControlIn& operator=(const UavYawControlIn& p);
    
  UavControlTypes controlType;
  Decimal currentYaw;
  Decimal desiredYaw;
  Decimal desiredYawRate;
};


class UavControlOut {
 private:
  
 public:
  vector<Decimal> commands; 
};


class UavControlPar : public MIPObject {
 private:
  UavTypes _uavType;
  Decimal _propGain;
  Decimal _derivGain;
  Decimal _mass;
  Decimal _gravity;
  Decimal _maxRollSin;
  Decimal _maxPitchSin;
  Decimal _minThrust;
  
  stringstream _ssfatal;
  
 public:
  UavControlPar(UavTypes uavType=UAV_TYPES_SWARMSIM,Decimal propGain=1.0,Decimal derivGain=2.0,Decimal mass=1.5,Decimal gravity=9.81,
         Decimal maxRollSin_ = sin(20.0*M_PI/180.0),Decimal maxPitchSin_ = sin(20.0*M_PI/180.0), Decimal minThrust=5.0);
  
  UavControlPar(const UavControlPar& p);
  
  UavControlPar& operator=(const UavControlPar& rhs);
  
  UavTypes uavType();
  
  Decimal propGain();
  
  Decimal derivGain();
  
  Decimal mass();
  
  Decimal gravity();
  
  Decimal maxRollSin();
  
  Decimal maxPitchSin();
  
  Decimal minThrust();
  
  string getObjectName() const {
   return "UavControlPar";
  }
};

class UavControlVar {
 private:
  
 public:
  
};


class UavControl : public MIPObject{
 private:
  UavControlVar *_var;
  UavControlPar *_par;
  
  stringstream _ssfatal;
  
  void _runYawSwarmSim(UavYawControlIn *in, UavControlOut *out);
  void _runSwarmSim(UavPosiControlIn *in, UavControlOut *out);
 public:
  
  UavControl(UavControlPar &par);
  
  ~UavControl();
  
  void run(UavPosiControlIn *input,UavControlOut *output);
  
  void run(UavYawControlIn *input,UavControlOut *output);
  
  string getObjectName() const {
   return "UavControl";
  }
};


#endif


/* @} */