asctecUav.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: franchi@diag.uniroma1.it stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------
*/

#ifndef ASCTEC_UAV_H
#define ASCTEC_UAV_H

#include "HummingBirdUavComm.h"
#include "HBstructs.h"
#include <Uav.h>

namespace MipResources {
 /* @{ */

 class asctecUavOptions : 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 "asctecUavOptions";
 }

 asctecUavOptions() {
  MIP_DEBUG(5,"asctec Options constructor");

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

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

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

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

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

  log_path = createStringOption("asctecUavlogPath",
   "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("asctecUavlogName",
   "Name for log file. Default: date_hour_HBdata.", buffer);

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

  updateValues();
 }
};

 class asctecUavPar : public UavPar{
  private:
  public:
 };

 class asctecUavVar : public UavVar{
  private:
  public:
 };

 class asctecUav : public Uav{
  private:
   static const ResourcePlate _plate = UAV_HUBI_RES;   
   HummingBirdUavComm* _com;
   asctecUavVar * _HBvar;

   Thread    _dataThread;
   Thread    _ctrlThread;

   EnhancedMutEx _comMutex;  
   EnhancedMutEx _ctrlMutex;  

   Time mutexTimeoutWrite;
   Time mutexTimeoutRead;

   asctecUavOptions 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;

  public:
   void dataLoop();
   void ctrlLoop();

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

   ~asctecUav();

   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(){}

  private:
   // Needed to manage thread
   pthread_t    thread_data;  // Getting thread
   pthread_attr_t  thread_attr;  // Getting thread attributes
   pthread_mutex_t mutex_run;    // Getting thread mutex (to stop the getting loop)

   pthread_mutex_t  mutex_list;  // Data list mutex (to avoid concurrent read/write)

   pthread_t        thread_data_write; // Writing thread
   pthread_attr_t   thread_attr_write;  // Writing thread attributes
   pthread_mutex_t  mutex_run_write;    // Writing thread mutex (to stop the writing loop)

   int stack_size_max;
   int stack_size;
   bool run_write_loop;
   bool run_data_loop;

   void stopLoop();
 };
}; // end of namespace MipResources

#endif // ASCTEC_UAV_H