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(QString decodedStatus READ decodedStatus NOTIFY statusChanged)
  46
  47     Q_PROPERTY(int pwm_motor1 READ pwm_motor1 NOTIFY navDataChanged)
  48     Q_PROPERTY(int pwm_motor2 READ pwm_motor1 NOTIFY navDataChanged)
  49     Q_PROPERTY(int pwm_motor3 READ pwm_motor1 NOTIFY navDataChanged)
  50     Q_PROPERTY(int pwm_motor4 READ pwm_motor1 NOTIFY navDataChanged)
  51
  52 #if 0
  53     Q_PROPERTY(bool confHaveJoyStick READ confHaveJoyStick NOTIFY configChanged)
  54     Q_PROPERTY(QString joyStickName READ joyStickName NOTIFY statusChanged)
  55     Q_PROPERTY(int joyStickRawAxis1  READ joyStickRawAxis1 NOTIFY joyStickChanged)
  56     Q_PROPERTY(int joyStickRawAxis2  READ joyStickRawAxis2 NOTIFY joyStickChanged)
  57     Q_PROPERTY(int joyStickRawAxis3  READ joyStickRawAxis3 NOTIFY joyStickChanged)
  58     Q_PROPERTY(int joyStickRawAxis4  READ joyStickRawAxis4 NOTIFY joyStickChanged)
  59     Q_PROPERTY(int joyStickRawAxis5  READ joyStickRawAxis5 NOTIFY joyStickChanged)
  60     Q_PROPERTY(int joyStickRawAxis6  READ joyStickRawAxis6 NOTIFY joyStickChanged)
  61     Q_PROPERTY(int joyStickMaxAxis1  READ joyStickMaxAxis1 NOTIFY joyStickChanged)
  62     Q_PROPERTY(int joyStickMaxAxis2  READ joyStickMaxAxis2 NOTIFY joyStickChanged)
  63     Q_PROPERTY(int joyStickMaxAxis3  READ joyStickMaxAxis3 NOTIFY joyStickChanged)
  64     Q_PROPERTY(int joyStickMaxAxis4  READ joyStickMaxAxis4 NOTIFY joyStickChanged)
  65     Q_PROPERTY(int joyStickMaxAxis5  READ joyStickMaxAxis5 NOTIFY joyStickChanged)
  66     Q_PROPERTY(int joyStickMaxAxis6  READ joyStickMaxAxis6 NOTIFY joyStickChanged)
  67     Q_PROPERTY(int joyStickMinAxis1  READ joyStickMinAxis1 NOTIFY joyStickChanged)
  68     Q_PROPERTY(int joyStickMinAxis2  READ joyStickMinAxis2 NOTIFY joyStickChanged)
  69     Q_PROPERTY(int joyStickMinAxis3  READ joyStickMinAxis3 NOTIFY joyStickChanged)
  70     Q_PROPERTY(int joyStickMinAxis4  READ joyStickMinAxis4 NOTIFY joyStickChanged)
  71     Q_PROPERTY(int joyStickMinAxis5  READ joyStickMinAxis5 NOTIFY joyStickChanged)
  72     Q_PROPERTY(int joyStickMinAxis6  READ joyStickMinAxis6 NOTIFY joyStickChanged)
  73     Q_PROPERTY(float joyStickAxis1   READ joyStickAxis1    NOTIFY joyStickChanged)
  74     Q_PROPERTY(float joyStickAxis2   READ joyStickAxis2    NOTIFY joyStickChanged)
  75     Q_PROPERTY(float joyStickAxis3   READ joyStickAxis3    NOTIFY joyStickChanged)
  76     Q_PROPERTY(float joyStickAxis4   READ joyStickAxis4    NOTIFY joyStickChanged)
  77     Q_PROPERTY(float joyStickAxis5   READ joyStickAxis5    NOTIFY joyStickChanged)
  78     Q_PROPERTY(float joyStickAxis6   READ joyStickAxis6    NOTIFY joyStickChanged)
  79     Q_PROPERTY(bool  joyStickCalibrate READ joyStickCalibrate  WRITE  setJoyStickCalibrate)
  80 #endif
  81    // Config variables from QSettings
  82     Q_PROPERTY(QString  confDroneIp   READ confDroneIp      WRITE setConfDroneIp      NOTIFY configChanged)
  83     Q_PROPERTY(QString  confActiveUI  READ confActiveUI     WRITE setConfActiveUI     NOTIFY configChanged)
  84     Q_PROPERTY(bool  confShowDebug    READ confShowDebug    WRITE setConfShowDebug    NOTIFY configChanged)
  85     Q_PROPERTY(bool  confShowHorizon  READ confShowHorizon  WRITE setConfShowHorizon  NOTIFY configChanged)
  86     Q_PROPERTY(bool  confShowGauges   READ confShowGauges   WRITE setConfShowGauges   NOTIFY configChanged)
  87     Q_PROPERTY(bool  confUseAccel     READ confUseAccel     WRITE setConfUseAccel     NOTIFY configChanged)
  88     Q_PROPERTY(bool  confUseJoyStick  READ confUseJoyStick  WRITE setConfUseJoyStick  NOTIFY configChanged)
  89     Q_PROPERTY(bool  confShowJSIndicators  READ confShowJSIndicators WRITE setConfShowJSIndicators   NOTIFY configChanged)
  90     Q_PROPERTY(bool  confFullScreen   READ confFullScreen   WRITE setConfFullScreen   NOTIFY configChanged)
  91     Q_PROPERTY(float confForwardGain  READ confForwardGain  WRITE setConfForwardGain  NOTIFY configChanged)
  92     Q_PROPERTY(float confBackwardGain READ confBackwardGain WRITE setConfBackwardGain NOTIFY configChanged)
  93     Q_PROPERTY(float confLeftGain     READ confLeftGain     WRITE setConfLeftGain     NOTIFY configChanged)
  94     Q_PROPERTY(float confRightGain    READ confRightGain    WRITE setConfRightGain    NOTIFY configChanged)
  95     Q_PROPERTY(float confCtrlTrsh     READ confCtrlTrsh     WRITE setConfCtrlTrsh     NOTIFY configChanged)
  96
  97 public:
  98     explicit DroneControl();
  99
 100
 101     float pitch();    void setPitch(float val_);
 102     float roll() ;    void setRoll(float val_);
 103     float yaw() ;     void setYaw(float val_);
 104     float altitude(); void setAltitude(float val_) ;
 105     float vVelocity();void setVVelocity(float val_) ;
 106     int   enabled() ; void setEnabled(int val_) ;
 107     bool  emergency();void setEmergency(bool val_) ;
 108     bool  fly() ;     void setFly(bool val_) ;
 109     bool  ctrlActive();void setCtrlActive(bool val_) ;
 110
 111 // Read only telemetry and drone status
 112     QString decodedStatus();
 113 #ifdef JOYSTICK
 114 // JoyStick configuration
 115     QString joyStickName() { return m_joyStickName; };
 116     int joyStickRawAxis1() { return m_joyStickRawAxis1; };
 117     int joyStickRawAxis2() { return m_joyStickRawAxis2; };
 118     int joyStickRawAxis3() { return m_joyStickRawAxis3; };
 119     int joyStickRawAxis4() { return m_joyStickRawAxis4; };
 120     int joyStickRawAxis5() { return m_joyStickRawAxis5; };
 121     int joyStickRawAxis6() { return m_joyStickRawAxis6; };
 122     int joyStickMaxAxis1() { return m_joyStickMaxAxis1; };
 123     int joyStickMaxAxis2() { return m_joyStickMaxAxis2; };
 124     int joyStickMaxAxis3() { return m_joyStickMaxAxis3; };
 125     int joyStickMaxAxis4() { return m_joyStickMaxAxis4; };
 126     int joyStickMaxAxis5() { return m_joyStickMaxAxis5; };
 127     int joyStickMaxAxis6() { return m_joyStickMaxAxis6; };
 128     int joyStickMinAxis1() { return m_joyStickMinAxis1; };
 129     int joyStickMinAxis2() { return m_joyStickMinAxis2; };
 130     int joyStickMinAxis3() { return m_joyStickMinAxis3; };
 131     int joyStickMinAxis4() { return m_joyStickMinAxis4; };
 132     int joyStickMinAxis5() { return m_joyStickMinAxis5; };
 133     int joyStickMinAxis6() { return m_joyStickMinAxis6; };
 134     float joyStickAxis1()  { return m_joyStickAxis1; };
 135     float joyStickAxis2()  { return m_joyStickAxis2; };
 136     float joyStickAxis3()  { return m_joyStickAxis3; };
 137     float joyStickAxis4()  { return m_joyStickAxis4; };
 138     float joyStickAxis5()  { return m_joyStickAxis5; };
 139     float joyStickAxis6()  { return m_joyStickAxis6; };
 140     bool joyStickCalibrate()  { return m_joyCal; };
 141     void setJoyStickCalibrate(bool val) { m_joyCal=val;};
 142 #endif
 143
 144     int pwm_motor1();
 145     int pwm_motor2();
 146     int pwm_motor3();
 147     int pwm_motor4();
 148     float droneAltitude();
 149     float dronePitch();
 150     float droneRoll();
 151     float droneYaw();
 152     float droneVBat();
 153     float droneSpeed();
 154
 155 //Config variables
 156  QString confDroneIp();      void setConfDroneIp(QString ip);
 157  QString confActiveUI();     void setConfActiveUI(QString ui);
 158  bool    confShowDebug();    void setConfShowDebug(bool val);
 159  bool    confShowHorizon();  void setConfShowHorizon(bool val);
 160  bool    confShowJSIndicators();  void setConfShowJSIndicators(bool val);
 161  bool    confShowGauges();   void setConfShowGauges(bool val);
 162  bool    confUseAccel();     void setConfUseAccel(bool val);
 163  bool    confFullScreen();   void setConfFullScreen(bool val);
 164  bool    confUseJoyStick();  void setConfUseJoyStick(bool val);
 165  float   confForwardGain();  void setConfForwardGain(float val);
 166  float   confBackwardGain(); void setConfBackwardGain(float val);
 167  float   confLeftGain();     void setConfLeftGain(float val);
 168  float   confRightGain();    void setConfRightGain(float val);
 169  float   confCtrlTrsh();     void setConfCtrlTrsh(float val);
 170 signals:
 171     void navDataChanged();
 172     void statusChanged();
 173     void configChanged();
 174     void pitchRollChanged();
 175     void joyStickChanged();
 176 public slots:
 177     void navDataUpdated();
 178     void statusUpdated();
 179     void rotationReadingsChanged();
 180 #ifdef JOYSTICK
 181     void updateJoyStick();
 182 #endif
 183     void axisValueChanged(int axis, int value);
 184 private:
 185
 186     float m_pitch;     // pitch in horizon
 187     float m_roll;      // Roll in horizon
 188     float m_yaw;       // low value to display
 189     float m_vv;        // Verticl velocity
 190     float m_altitude;  // altitude
 191     float m_rotRefX;   // Rotation zero reference
 192     float m_rotRefY;
 193     float m_rotRefZ;
 194     int   m_enabled;   // Enable flag
 195     bool  m_emergency; // Emergency flag
 196     QString _emgReason; // Reason of emergency
 197     bool  m_fly;       // Fly flag
 198     bool  m_ctrlActive;
 199     bool  m_useAccel;
 200     bool  m_useJoyStick;
 201
 202 #ifdef JOYSTICK
 203 // Joystick
 204      bool  m_haveJoyStick;
 205     QString m_joyStickName;
 206     bool m_joyCal;
 207     int m_joyStickRawAxis1;
 208     int m_joyStickRawAxis2;
 209     int m_joyStickRawAxis3;
 210     int m_joyStickRawAxis4;
 211     int m_joyStickRawAxis5;
 212     int m_joyStickRawAxis6;
 213     float m_joyStickAxis1;
 214     float m_joyStickAxis2;
 215     float m_joyStickAxis3;
 216     float m_joyStickAxis4;
 217     float m_joyStickAxis5;
 218     float m_joyStickAxis6;
 219     int m_joyStickMinAxis1;
 220     int m_joyStickMinAxis2;
 221     int m_joyStickMinAxis3;
 222     int m_joyStickMinAxis4;
 223     int m_joyStickMinAxis5;
 224     int m_joyStickMinAxis6;
 225     int m_joyStickMaxAxis1;
 226     int m_joyStickMaxAxis2;
 227     int m_joyStickMaxAxis3;
 228     int m_joyStickMaxAxis4;
 229     int m_joyStickMaxAxis5;
 230     int m_joyStickMaxAxis6;
 231     int m_joyStickMapAxis[6];
 232 #endif
 233
 234
 235
 236     float m_ctrlTrsh;  // accel controll treshold
 237
 238     QTcpSocket *ctlSock;  // TCP port for control/config data
 239     QUdpSocket *navSock;  // Navigation data receive socket port 5554
 240     QHostAddress droneHost;  // Ip address of the drone
 241
 242     DroneThread *droneThread;
 243     QSettings *droneSettings;
 244 #ifdef HAVE_SDL
 245     QTimer joyStickTimer;
 246     SDL_Joystick *joyStick;
 247 #endif
 248 #ifdef QT_SENSORS
 249     QRotationSensor *rSensor; // used for controllind drone with device rotation
 250 #endif
 251 };
 252
 253
 254
 255 class DroneThread:public QThread {
 256     Q_OBJECT
 257 public:
 258     DroneThread(DroneControl *parentp,QHostAddress host);
 259
 260  //   ~DroneThread ();
 261     void run();
 262     void sendNav(QString cmd);
 263     void sendCmd(QString cmd);
 264     NavData *navData() { return &nd; };
 265
 266     enum droneState {
 267         notInitialized,
 268         initialized,
 269         ready,
 270         flying
 271
 272     };
 273 public slots:
 274     void navDataReady();
 275     void timer();
 276     void setFly(bool fly);
 277     void setEmergency(bool emg);
 278     void setDroneControl(float pitch,float roll,float yaw,float vv);
 279     void setDroneGain(float fgain,float bgain,float lgain,float rgain);
 280
 281 private:
 282
 283      QHostAddress droneHost;  // Ip address of the drone
 284      QTimer *stateTimer;
 285      volatile bool stopped;
 286      DroneControl *parent;
 287      QUdpSocket *navSock;  // Navigation data receive socket port 5554
 288      QUdpSocket *cmdSock; // Ay command socket port 5556
 289      int state;
 290      int seq;           // Drone command seq number
 291      bool m_fly;
 292      bool m_emergency;
 293      float m_pitch;
 294      float m_roll;
 295      float m_yaw;
 296      float m_vv;
 297      float m_fgain;
 298      float m_bgain;
 299      float m_rgain;
 300      float m_lgain;
 301      NavData nd;
 302 };
 303
 304 #endif // DRONECONTROL_H