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