UavHeightControl.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_HEIGHTCONTROL_TASK_H_
#define UAV_HEIGHTCONTROL_TASK_H_

#include <Task.h>
#include <Resource.h>
#include <Uav.h>
#include <CommonOptions.h>
#include <Keyboard.h>

#include <iostream>

#ifndef MIP_PLATFORM_ARM
#include <DraWin.h>
#endif

#define M2INCH 0.0254

namespace MipTasks{
 
 /* @{ */
 
 enum UavHeightControlStates{
  UAV_HEIGHT_CONTROL_FIRST_RUN,
  UAV_HEIGHT_CONTROL_NORMAL_RUN_BKS,
  UAV_HEIGHT_CONTROL_NORMAL_RUN_PID,
  UAV_HEIGHT_CONTROL_ONOFF_MOTORS,
  UAV_HEIGHT_CONTROL_STATES_NUM
 };
 
 static const char* UavHeightControlStatesNames[UAV_HEIGHT_CONTROL_STATES_NUM] = {
  "FirstRun",
  "NormalRun_bks",
  "NormalRun_pid",
  "OnOffMotors"
 };
 
 
 class UavHeightControlOptions : public Options {
  public:
   IntOption* controller_type;
   
   DecimalOption* gain1;   
   DecimalOption* gain2;   
   DecimalOption* gain3;   

   DecimalOption* Kp;   
   DecimalOption* Kd;   
   DecimalOption* Ki;   

   DecimalOption* thrust_scale_bks;   
   DecimalOption* thrust_scale_pid;   
   
   BoolOption*   direct_control;   
   DecimalOption* height_ref;     
   
   UavHeightControlOptions();

   string getObjectName() const {
    return "UavHeightControlOptions";
   }
 };
 
 
 class UavHeightControl : public Task{
  private:
   
   static const TaskPlate _plate = UHC_TASK;
   
   static const long int  _maxDurSec  = 0,      _maxDurUsec = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 50000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 40000;
   
   Uav*  _uav;
   
   UavHeightControlOptions _options;
   
   int   _exampleOfIntVariable;
   double _exampleOfDecimalVariable;
   UavHeightControlStates _state;
   
   void _firstRun();
   void _normalRun_pid();
   void _normalRun_bks();
   void _onoff_motors();
   
   void readInput();

   Decimal posZ_des;

   Time t_motor;
   
   Decimal s_mes[4];
   Decimal v_est[4];
   Decimal t_mes[4];
   Decimal last_t_sonar;
   Decimal last_quad_time;
   Decimal sonar_h, sonar_dh, sonar_h_last;
   Decimal sonar_ddh, sonar_dh_last;
   Decimal g_est;
   Decimal thrust_control;
     
   void dirtyderivative();
   
   Decimal c1;
   Decimal c2;
   Decimal c3;
   Decimal thrust_scale_bks;
   Decimal backstepping();
   
   Decimal Kp;
   Decimal Kd;
   Decimal Ki;
      
   Decimal integral;
      
   Decimal thrust_scale_pid;
   Decimal pid();
   Decimal discrete_pid();
   
   Decimal error_k_1, error_k_2, u_k_1;
   
   Keyboard*   _keyboard;    
   
   ofstream logfile;
  public:
   UavHeightControl(ResourcePointers resources,int argc, const char* argv[]);
   
   ~UavHeightControl();
   
   TaskPlate getPlate() const {
    return _plate;
   }
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   
   TaskOutputs run();
 };
 
 
 /* @} */
 
};// end namespace MipTasks

#endif