HummingBirdUav.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 __HUMMING_BIRD_UAV_H__
#define __HUMMING_BIRD_UAV_H__

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

#ifdef USE_ROS
 #ifdef FOUND_ROS_LIBS
  #include <ros/ros.h>
  #include <std_msgs/String.h>
  #include <std_msgs/Float32.h>
  #include <std_msgs/Float32MultiArray.h>
  #include <ros/callback_queue.h>
 #endif
#endif

#include "HummingBirdUavComm.h"
#include "HBstructs.h"

#include <Uav.h>
 
namespace MipResources {
 /* @{ */
 
 class HummingBirdUavOptions : public Options {
 public:
 StringOption *port_name;
 IntOption  *max_input;
 IntOption  *fdata_send_rate;
 IntOption  *read_rate;
 IntOption  *send_rate;
 
 IntOption   *ctrlin_send_rate;
 IntOption   *ctrlfl_send_rate;
 
 IntOption   *h_sonar_rate;
 IntOption   *h_sonar_p_gain;
 IntOption   *h_sonar_i_gain;
 IntOption   *h_sonar_d_gain;
 DecimalOption *height_ref;

 StringOption *log_path;
 StringOption *log_name;
 BoolOption   *make_log;
  BoolOption   *use_ros;
  BoolOption   *use_test;
 
 StringOption *ros_gains_sender_name;

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

 HummingBirdUavOptions() {
  MIP_DEBUG(5,"HummingBird Options constructor");

  port_name  = createStringOption("hbUavPortName" ,
   "Name of com port, default \"/dev/ttyUSB0\".",
   "/dev/ttyUSB0");

  max_input  = createIntOption("hbUavMaxInput",
   "Maximum remote input in [-2047,+2047], default 500.",
   500);

  fdata_send_rate  = createIntOption("hbUavFdataSendRate",
   "Rate of data transmission (1000/sendRate Hz) from quadrotor to pc, default 20.",20);
  read_rate  = createIntOption("hbUavReadRate",
   "Rate of data reading loop (1000/readRate Hz), default 20.",20);
  send_rate  = createIntOption("hbUavSendRate",
   "Rate of data sending to the quadrotor (1000/sendRate Hz), default 30.",30);

  height_ref = createDecimalOption("hbUavHReference",
   "Initial height reference for controller, default 1.5.",1.5);

  h_sonar_rate = createIntOption("hbUavhSonarRate",
   "Rate for sonar height controller (1000/rate Hz), default 40.", 40);
  h_sonar_p_gain = createIntOption("hbUavhSonarPGain",
   "Proportional gain in pid height controller", 200);
  h_sonar_i_gain = createIntOption("hbUavhSonarIGain",
   "Integral gain in pid height controller",0);
  h_sonar_d_gain = createIntOption("hbUavhSonarDGain", 
   "Derivative gain in pid height controller", 220);

  log_path = createStringOption("hbUavlogPath",
   "Path to save log. Default: current directory.", "./");

  ros_gains_sender_name = createStringOption("hbGainsRosTopicName",
   "Name of ros topic for sending gains.", "quad_sendGains");

  time_t rawtime;
  struct tm * timeinfo;
  char buffer [80];
  time ( &rawtime );
  timeinfo = localtime ( &rawtime );
  strftime(buffer,80,"%Y-%m-%d_%H-%M_HBdata",timeinfo);

  log_name = createStringOption("hbUavlogName",
   "Name for log file. Default: date_hour_HBdata.", buffer);

  make_log  = createBoolOption("hbUavmakeLog",
   "Whether on not register log. Deafult: true.", false);
    use_ros   = createBoolOption("hbUavUseRos",
   "Whether on not use ROS. Deafult: false." , false);
    use_test  = createBoolOption("hbUavUseTest",
   "Whether on not use test version. Deafult: true.", false);

  updateValues();
 }
};
 
 class HummingBirdUavPar : public UavPar{
  private:
  public:
 };
 
 class HummingBirdUavVar : public UavVar{
  private:
  public:
 };
 
 class HummingBirdUav : public Uav{
  private:
   static const ResourcePlate _plate = UAV_HUBI_RES;   
   HummingBirdUavComm* _com;
   HummingBirdUavVar * _HBvar;

   Thread    _dataThread;
   Thread    _ctrlThread;

   EnhancedMutEx _comMutex;  
   EnhancedMutEx _ctrlMutex;  

   Time mutexTimeoutWrite;
   Time mutexTimeoutRead;

   HummingBirdUavOptions options;

   LABROB_CTRL_INPUT ctrl_input;
   bool ctrl_input_update;

   static const Decimal  angle2hubi;   
   static const Decimal  hubi2angle;   
   static const Decimal  hubi2angleVel;  
      
   static const int  thrust2hubi = 4095; 
   static const Decimal  hubi2thrust;  
   ofstream    _logFile;
   int         _numrow;
   
   //ROS FOR HEIGHT
   #ifdef USE_ROS
    #ifdef FOUND_ROS_LIBS
     std_msgs::Float32MultiArray *posDataRos;
     ros::NodeHandle *handleRos;
     ros::Publisher *chatter_pub;
     
     //ROS FOR CTRL GAINS
     std_msgs::Float32MultiArray *posGainsRos;
     ros::NodeHandle *handleGainsRos;
     ros::Publisher *chatterGainsPub;
     
     ros::NodeHandle *handleGainsRos_subscriber;
     ros::CallbackQueue queueCallbackGainsRos;
     ros::Subscriber subsGainsRos;   // maybe removed (?)
     
     void receiveNewGains(const std_msgs::Float32MultiArray::ConstPtr& msg);
    #endif
   #endif

  public:
   void dataLoop();
   void ctrlLoop();
   void doClean(){}

   void startCtrlLoop(){ _ctrlThread.startWork();  }
   void stopCtrlLoop() { _ctrlThread.stopWork();  }
   void startDataLoop(){ _dataThread.startWork();  }
   void stopDataLoop() {_dataThread.stopWork();   }
   
   /* two mandatory wrappers for mutex */
   bool askExclusiveAccess(Time timeout){
    return _comMutex.askExclusiveAccess(timeout);
   }
   void leaveExclusiveAccess(){
    _comMutex.leaveExclusiveAccess();
   }

   /* two mandatory wrappers for mutex */
   bool ctrlAskExclusiveAccess(Time timeout){
    return _ctrlMutex.askExclusiveAccess(timeout);
   }
   void ctrlLeaveExclusiveAccess(){
    _ctrlMutex.leaveExclusiveAccess();
   }

   HummingBirdUav(int argc, const char** argv);

   ~HummingBirdUav();

   ResourcePlate getPlate() const {
    return _plate;
   }

   Position3D getPosition(Time& t) {
    t = _HBvar->time();
    return _HBvar->position();
   }

   Orientation3D getAttitude(Time& t) {
    t = _HBvar->time();
    return _HBvar->attitude();
   }

   HummingBirdUavComm* getCom() {
    return _com;
   }

   bool setStatus(Orientation3D& pDes, Decimal& tDes);

   bool setThrust(Decimal tDes);
      
   bool setControllersGains(Decimal height_pid_des[3]);

   bool setFiltersGains(Decimal sonar_h_filt_des[2]);

   bool setYaw(Angle yaw){}

   bool setPose(Pose3D& pDes, short ctrlIn);

   bool setAttitude(Orientation3D& oDes);

   bool setPosition( Position3D p, 
            Velocity3D v    = Velocity3D(0.0,0.0,0.0),
            Acceleration3D a  = Acceleration3D(0.0,0.0,0.0));

   bool setVelocity(Velocity3D p,Acceleration3D a){}

   bool setAcceleration(Acceleration3D p){}

   bool readPosition(){}

   bool readVelocity(){};

   bool readAttitude(){}

   // Luca Ricci mod. //
   bool readAllData(){};
   // ************** //
 };
}; // end of namespace MipResources

#endif

/* @} */