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