git.maemo.org Git - mardrone/blob - mardrone/dronelib/dronecontrol.h

   1 #ifndef DRONECONTROL_H
   2 #define DRONECONTROL_H
   3
   4 #include <QGraphicsWidget>
   5 #include <QGraphicsItem>
   6 #include <QPainter>
   7 #include <QUdpSocket>
   8 #include <QTcpSocket>
   9 #include <QThread>
  10 #include <QTimer>
  11 #include <QSettings>
  12 #ifdef HAVE_SDL
  13 #include <SDL/SDL.h>
  14 #endif
  15 #ifdef QT_SENSORS
  16 #include <QRotationSensor>
  17 #endif
  18 #undef _GNU_SOURCE // just get rid of error message double definition
  19 #include <navdata.h>
  20
  21 class DroneThread;
  22
  23 #ifdef QT_SENSORS
  24 QTM_USE_NAMESPACE
  25 #endif
  26
  27 class DroneControl : public QObject
  28 {
  29     Q_OBJECT
  30     Q_PROPERTY(float pitch READ pitch WRITE setPitch NOTIFY pitchRollChanged)
  31     Q_PROPERTY(float roll  READ roll  WRITE setRoll NOTIFY pitchRollChanged)
  32     Q_PROPERTY(float yaw   READ yaw   WRITE setYaw NOTIFY pitchRollChanged)
  33     Q_PROPERTY(float altitude  READ altitude WRITE setAltitude)
  34     Q_PROPERTY(float vVelocity READ vVelocity WRITE setVVelocity NOTIFY pitchRollChanged)
  35     Q_PROPERTY(int   enabled   READ enabled WRITE setEnabled)
  36     Q_PROPERTY(bool  fly READ fly WRITE setFly)
  37     Q_PROPERTY(bool  ctrlActive READ ctrlActive WRITE setCtrlActive)
  38     Q_PROPERTY(float dronePitch READ dronePitch NOTIFY navDataChanged)
  39     Q_PROPERTY(float droneRoll READ droneRoll NOTIFY navDataChanged)
  40     Q_PROPERTY(float droneYaw READ droneYaw NOTIFY navDataChanged)
  41     Q_PROPERTY(float droneVBat READ droneVBat NOTIFY navDataChanged)
  42     Q_PROPERTY(float droneSpeed READ droneSpeed NOTIFY navDataChanged)
  43     Q_PROPERTY(float droneAltitude READ droneAltitude NOTIFY navDataChanged)
  44     Q_PROPERTY(bool  emergency READ emergency WRITE setEmergency)
  45     Q_PROPERTY(bool  connected READ connected WRITE setConnected NOTIFY connectedChanged)
  46     Q_PROPERTY(QString decodedStatus READ decodedStatus NOTIFY statusChanged)
  47
  48     Q_PROPERTY(int pwm_motor1 READ pwm_motor1 NOTIFY navDataChanged)
  49     Q_PROPERTY(int pwm_motor2 READ pwm_motor1 NOTIFY navDataChanged)
  50     Q_PROPERTY(int pwm_motor3 READ pwm_motor1 NOTIFY navDataChanged)
  51     Q_PROPERTY(int pwm_motor4 READ pwm_motor1 NOTIFY navDataChanged)
  52
  53
  54    // Config variables from QSettings
  55     Q_PROPERTY(QString  confDroneIp   READ confDroneIp      WRITE setConfDroneIp      NOTIFY configChanged)
  56     Q_PROPERTY(QString  confActiveUI  READ confActiveUI     WRITE setConfActiveUI     NOTIFY configChanged)
  57     Q_PROPERTY(bool  confShowDebug    READ confShowDebug    WRITE setConfShowDebug    NOTIFY configChanged)
  58     Q_PROPERTY(bool  confShowHorizon  READ confShowHorizon  WRITE setConfShowHorizon  NOTIFY configChanged)
  59     Q_PROPERTY(bool  confShowGauges   READ confShowGauges   WRITE setConfShowGauges   NOTIFY configChanged)
  60     Q_PROPERTY(bool  confUseAccel     READ confUseAccel     WRITE setConfUseAccel     NOTIFY configChanged)
  61     Q_PROPERTY(bool  confUseJoyStick  READ confUseJoyStick  WRITE setConfUseJoyStick  NOTIFY configChanged)
  62     Q_PROPERTY(bool  confShowJSIndicators  READ confShowJSIndicators WRITE setConfShowJSIndicators   NOTIFY configChanged)
  63     Q_PROPERTY(bool  confFullScreen   READ confFullScreen   WRITE setConfFullScreen   NOTIFY configChanged)
  64     Q_PROPERTY(float confForwardGain  READ confForwardGain  WRITE setConfForwardGain  NOTIFY configChanged)
  65     Q_PROPERTY(float confBackwardGain READ confBackwardGain WRITE setConfBackwardGain NOTIFY configChanged)
  66     Q_PROPERTY(float confLeftGain     READ confLeftGain     WRITE setConfLeftGain     NOTIFY configChanged)
  67     Q_PROPERTY(float confRightGain    READ confRightGain    WRITE setConfRightGain    NOTIFY configChanged)
  68     Q_PROPERTY(float confCtrlTrsh     READ confCtrlTrsh     WRITE setConfCtrlTrsh     NOTIFY configChanged)
  69
  70 public:
  71     explicit DroneControl();
  72
  73
  74     float pitch();    void setPitch(float val_);
  75     float roll() ;    void setRoll(float val_);
  76     float yaw() ;     void setYaw(float val_);
  77     float altitude(); void setAltitude(float val_) ;
  78     float vVelocity();void setVVelocity(float val_) ;
  79     int   enabled() ; void setEnabled(int val_) ;
  80     bool  emergency();void setEmergency(bool val_) ;
  81     bool  fly() ;     void setFly(bool val_) ;
  82     bool  ctrlActive();void setCtrlActive(bool val_) ;
  83
  84 // Read only telemetry and drone status
  85     QString decodedStatus();
  86
  87     bool connected()  { return m_connected; };
  88     void setConnected(bool val_);
  89
  90     int pwm_motor1();
  91     int pwm_motor2();
  92     int pwm_motor3();
  93     int pwm_motor4();
  94     float droneAltitude();
  95     float dronePitch();
  96     float droneRoll();
  97     float droneYaw();
  98     float droneVBat();
  99     float droneSpeed();
 100
 101 //Config variables
 102  QString confDroneIp();      void setConfDroneIp(QString ip);
 103  QString confActiveUI();     void setConfActiveUI(QString ui);
 104  bool    confShowDebug();    void setConfShowDebug(bool val);
 105  bool    confShowHorizon();  void setConfShowHorizon(bool val);
 106  bool    confShowJSIndicators();  void setConfShowJSIndicators(bool val);
 107  bool    confShowGauges();   void setConfShowGauges(bool val);
 108  bool    confUseAccel();     void setConfUseAccel(bool val);
 109  bool    confFullScreen();   void setConfFullScreen(bool val);
 110  bool    confUseJoyStick();  void setConfUseJoyStick(bool val);
 111  float   confForwardGain();  void setConfForwardGain(float val);
 112  float   confBackwardGain(); void setConfBackwardGain(float val);
 113  float   confLeftGain();     void setConfLeftGain(float val);
 114  float   confRightGain();    void setConfRightGain(float val);
 115  float   confCtrlTrsh();     void setConfCtrlTrsh(float val);
 116 signals:
 117     void navDataChanged();
 118     void statusChanged();
 119     void configChanged();
 120     void pitchRollChanged();
 121     void joyStickChanged();
 122     void connectedChanged();
 123 public slots:
 124     void navDataUpdated();
 125     void statusUpdated();
 126     void rotationReadingsChanged();
 127 #ifdef JOYSTICK
 128     void updateJoyStick();
 129 #endif
 130     void axisValueChanged(int axis, int value);
 131 private:
 132
 133     float m_pitch;     // pitch in horizon
 134     float m_roll;      // Roll in horizon
 135     float m_yaw;       // low value to display
 136     float m_vv;        // Verticl velocity
 137     float m_altitude;  // altitude
 138     float m_rotRefX;   // Rotation zero reference
 139     float m_rotRefY;
 140     float m_rotRefZ;
 141     int   m_enabled;   // Enable flag
 142     bool  m_emergency; // Emergency flag
 143     QString _emgReason; // Reason of emergency
 144     bool  m_fly;       // Fly flag
 145     bool  m_ctrlActive;
 146     bool  m_useAccel;
 147     bool  m_useJoyStick;
 148     bool  m_connected;
 149
 150
 151
 152
 153     float m_ctrlTrsh;  // accel controll treshold
 154
 155     QTcpSocket *ctlSock;  // TCP port for control/config data
 156     QUdpSocket *navSock;  // Navigation data receive socket port 5554
 157     QHostAddress droneHost;  // Ip address of the drone
 158
 159     DroneThread *droneThread;
 160     QSettings *droneSettings;
 161
 162 #ifdef QT_SENSORS
 163     QRotationSensor *rSensor; // used for controllind drone with device rotation
 164 #endif
 165 };
 166
 167
 168
 169 class DroneThread:public QThread {
 170     Q_OBJECT
 171 public:
 172     DroneThread(DroneControl *parentp,QHostAddress host);
 173
 174  //   ~DroneThread ();
 175     void run();
 176     void sendNav(QString cmd);
 177     void sendCmd(QString cmd);
 178     NavData *navData() { return &nd; };
 179
 180     enum droneState {
 181         notInitialized,
 182         initialized,
 183         bootstrap,
 184         ready,
 185         flying
 186
 187     };
 188 public slots:
 189     void navDataReady();
 190     void timer();
 191     void setFly(bool fly);
 192     void setEmergency(bool emg);
 193     void setDroneControl(float pitch,float roll,float yaw,float vv);
 194     void setDroneGain(float fgain,float bgain,float lgain,float rgain);
 195
 196 private:
 197
 198      QHostAddress droneHost;  // Ip address of the drone
 199      QTimer *stateTimer;
 200      volatile bool stopped;
 201      DroneControl *parent;
 202      QUdpSocket *navSock;  // Navigation data receive socket port 5554
 203      QUdpSocket *cmdSock; // Ay command socket port 5556
 204      int state;
 205      int retry;         // retry counter
 206      int noreply;       // No reply counter incremented every sedcmd, cleared when  got reply
 207      int seq;           // Drone command seq number
 208      bool m_fly;
 209      bool m_emergency;
 210      float m_pitch;
 211      float m_roll;
 212      float m_yaw;
 213      float m_vv;
 214      float m_fgain;
 215      float m_bgain;
 216      float m_rgain;
 217      float m_lgain;
 218      NavData nd;
 219 };
 220
 221 #endif // DRONECONTROL_H