/home/usappz/DeadalusSim/source/quadrotor_dynamics/quadrotor_dynamics.hh

/* -----------------------------------------------------------------------------
 *
 *
 *  ____                   _____     _
 * / ___|  __ _ _ __  _ __|__  /  __| | ___
 * \___ \ / _` | '_ \| '_ \ / /  / _` |/ _ \
 *  ___) | (_| | |_) | |_) / /_ | (_| |  __/
 * |____/ \__,_| .__/| .__/____(_)__,_|\___|
 *             |_|   |_|
 *
 *
 *
 * Author: Andreas Gschossmann
 * Email:  andreas.gschossmann@hs-regensburg.de
 *
 * quadrotor_dynamics.hh - Relationship between rotor-speed or alternatively
 * digital value for brushless drivers and resulting thrust of the motor.  *
 * -----------------------------------------------------------------------------*/
#ifndef _GAZEBO_DYNAMICS_HH_ 
#define _GAZEBO_DYNAMICS_HH_

#include <string>

#include <math.h>
#include <gazebo/math/Vector3.hh>

#include "../dependencies/resources.hh"
#include "../dependencies/constants.hh"


class QuadRotorDynamics 
{ 
    public: QuadRotorDynamics(
                 double k_t = 1.0,              
                 double k_m = 1.0,              
                 double moment_arm = 0.220,     
                 int error_type = NO_ERROR,     
                 double K = 1.0,                
                 double C = 0.0,                
                 double T = 0.0,                
                 double dead_time = 0.0         
                 ); 

    public: virtual ~QuadRotorDynamics();

    public: Mechanics update(ActuationVal actuation_value, double deltaT);
    //public: Mechanics update(AngularVel rotor_speed, double deltaT);
  
    private: void check_limits();
    
    private: void aerodynamics();

    private: void roll_pitch_torque();

    private: void disturbance();

    private:
      AngularVel        _rotor_speed;           
      AngularVel        _rotor_speed_prev;      
      AngularVel        _rotor_acceleration;    
      ActuationVal      _actuation_value;       
      Mechanics         _mechanics;             
      double    _dT;                            

    // aerodynamics constants
    private:
      double _k_t;              
      double _k_m;              
      
    private:
      double _moment_arm;       

    private:
      int _error_type;          
      double _sine_index;       
  
    // For PT1 K*u + C = T*(d/dt)omega + omega 
    private:
      double _K;                
      double _C;                
      double _T;                
      double _dead_time;        
    
    private: void pt1_element(); // K*u + C = T*(d/dt)omega + omega
   
    // values for limit check 
    private:
      double _rotor_speed_max;          
      double _rotor_acceleration_max;   
      bool   _limit_const_act;          

      bool   _limit_transient_act;      


    private:
      bool   _gyroscopic_torque_active; 


    // for check_limits()
    public: void activate_limit_constant( bool active );                
    public: void activate_limit_transient( bool active );               
    public: void set_limit_const(double rotor_speed_max);               
    public: void set_limit_transient(double rotor_acceleration_max);    

    public: void activate_gyroscopic_torque( bool active );             
   
    // set functions for pt1 element
    public: void set_k_pt1(double K);                   
    public: void set_t_pt1(double T);                   
    public: void set_c_pt1(double C);                   
    public: void set_dead_time_pt1(double dead_time);   
     
    public:
      Mechanics getMechanics(); 
      Thrust    getThrust();    
      Torque    getTorque();    

    public: void setErrorType(int error_type); 

};
#endif