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