git.maemo.org Git - mardrone/blob - mardrone/navdata.cpp

   1 #include <QDebug>
   2 #include "navdata.h"
   3
   4
   5 NavData::NavData(QObject *parent) :
   6     QObject(parent)
   7 {
   8      vx=0;vy=0;vz=0;pwm_motor1=0;pwm_motor2=0;pwm_motor3=0;pwm_motor4=0;
   9      vbat=0.0;pitch=0.0;roll=0.0;yaw=0.0;altitude=0.0;
  10 }
  11
  12 void NavData::parseRawNavData(char *buf,unsigned int len)
  13 {
  14     int opt=0;
  15     navdata_option_t *op;
  16     _navdata_t *nd=(_navdata_t *)buf;
  17     state=nd->ardrone_state;
  18     if(state!=oldState) {
  19         oldState=state;
  20         decodedState=decodeState(state,0);
  21         emit stateUpdated();
  22     };
  23 #if 0
  24     qDebug("parseRawNavData hdr=%8x state=%08x seq=%06d opt[0].tag=%d size=%d",
  25            nd->header,nd->ardrone_state,nd->sequence,nd->options[0].tag,nd->options[0].size);
  26 #endif
  27     qDebug() << decodedState;
  28     op=&(nd->options[0]);
  29     while((((unsigned int)op-(unsigned int)buf)<len) && (op->size>0))
  30     {
  31        parseOption(op);
  32        op=(navdata_option_t *)((unsigned int)op+op->size);
  33     }
  34 };
  35
  36 void NavData::parseOption(navdata_option_t *op)
  37 {
  38   //  qDebug("parseOption tag=%d size=%d",op->tag,op->size);
  39     switch(op->tag) {
  40     case NAVDATA_DEMO_TAG:
  41                 {
  42                     _navdata_demo_t *ndemo=(_navdata_demo_t*)op;
  43                     vbat=ndemo->vbat_flying_percentage;
  44                     pitch=ndemo->theta/1000.0;
  45                     roll=ndemo->phi/1000.0;
  46                     yaw=ndemo->psi/1000.0;
  47                     altitude=ndemo->altitude;
  48                     vx=ndemo->vx;
  49                     vy=ndemo->vy;
  50                     vz=ndemo->vz;
  51                     emit navDataUpdated();
  52                     qDebug("pitch=%2.1f roll=%2.1f yaw=%2.1f alt=%2.1f v(%2.1f,%2.1f,%2.1f bat=%2.1f",pitch,roll,yaw,altitude,vx,vy,vz,vbat);
  53                 }
  54                 break;
  55         case NAVDATA_TIME_TAG:
  56                 {
  57                 _navdata_time_t *ntdime=(_navdata_time_t*)op;
  58                 }
  59                 break;
  60         case NAVDATA_RAW_MEASURES_TAG:
  61                 {
  62                 _navdata_raw_measures_t *nraw=(_navdata_raw_measures_t*)op;
  63                 }
  64                 break;
  65         case NAVDATA_PHYS_MEASURES_TAG:
  66                 {
  67                 _navdata_phys_measures_t *nphys=(_navdata_phys_measures_t*)op;
  68                 }
  69                 break;
  70         case NAVDATA_GYROS_OFFSETS_TAG:
  71                 break;
  72         case NAVDATA_EULER_ANGLES_TAG:
  73                 break;
  74         case NAVDATA_REFERENCES_TAG:
  75                 break;
  76         case NAVDATA_TRIMS_TAG: // 7
  77                 {
  78                 _navdata_trims_t *ntrim=( _navdata_trims_t*)op;
  79                 }
  80                 break;
  81         case NAVDATA_RC_REFERENCES_TAG: //8
  82                 break;
  83         case NAVDATA_PWM_TAG:
  84                 {
  85                 _navdata_pwm_t *npwm=( _navdata_pwm_t*)op;
  86                 current_motor1=npwm->current_motor1;
  87                 pwm_motor1=npwm->motor1;
  88                 current_motor2=npwm->current_motor2;
  89                 pwm_motor2=npwm->motor2;
  90                 current_motor3=npwm->current_motor3;
  91                 pwm_motor3=npwm->motor3;
  92                 current_motor4=npwm->current_motor4;
  93                 pwm_motor4=npwm->motor4;
  94                 emit navDataUpdated();
  95                 qDebug("pwm1=%3d I1=%4f pwm2=%3d I2=%4f pwm3=%3d I3=%4f pwm4=%3d I4=%4f",
  96                        pwm_motor1,current_motor1,pwm_motor2,current_motor2,pwm_motor3,current_motor3,pwm_motor4,current_motor4);
  97                  }
  98                 break;
  99         case NAVDATA_ALTITUDE_TAG: //10
 100                 break;
 101         case NAVDATA_VISION_RAW_TAG:
 102                 break;
 103         case NAVDATA_VISION_OF_TAG: //12
 104                 break;
 105         case NAVDATA_VISION_TAG:
 106                 break;
 107         case NAVDATA_VISION_PERF_TAG:
 108                 break;
 109         case NAVDATA_TRACKERS_SEND_TAG:
 110                 break;
 111         case NAVDATA_VISION_DETECT_TAG: //16
 112                 break;
 113         case NAVDATA_WATCHDOG_TAG:
 114                 break;
 115         case NAVDATA_ADC_DATA_FRAME_TAG:
 116                 break;
 117         case NAVDATA_CKS_TAG: //65535
 118                 break;
 119     }
 120 };
 121
 122 QString NavData::decodeState(unsigned int state,int level)
 123 {
 124    QString s="%1 ";
 125    s=s.arg(state,8,16);
 126    s+=(state & ARDRONE_FLY_MASK)==0 ? "landed ":"flying ";
 127    if(level>1) s+=(state & ARDRONE_VIDEO_MASK)==0 ?"video disable ":"video ena ";
 128    if(level>1)s+=(state & ARDRONE_VISION_MASK)==0 ? "vision disable ":"vision ena ";
 129    if(level>1)s+=(state & ARDRONE_CONTROL_MASK)==0 ?  "euler angles ":"angular speed ";
 130    if(level>1)s+=(state & ARDRONE_ALTITUDE_MASK)==0 ? "alt ctrl inact ":"alt ctrl act ";
 131  //s+=(state & ARDRONE_USER_FEEDBACK_START)==0 ?  /*!< USER feedback : Start button state */
 132    s+=(state & ARDRONE_COMMAND_MASK)==0 ?  "CMD NAK ":"CMD ACK ";
 133  //  s+=(state & ARDRONE_FW_FILE_MASK)==0 ?  /* Firmware file is good;
 134  //  s+=(state & ARDRONE_FW_VER_MASK )==0 ?  /* Firmware update is newer;
 135  //  ARDRONE_FW_UPD_MASK         = 1 << 9,  /* Firmware update is ongoing (1;
 136    s+=(state & ARDRONE_NAVDATA_DEMO_MASK )==0 ? "All navdata ":"navdata demo ";
 137       s+=(state & ARDRONE_NAVDATA_BOOTSTRAP)==0 ? "":"Navdata bootstrap ";
 138  //  s+=(state & ARDRONE_MOTORS_MASK)==0 ? /*!< Motors status : (0) Ok, (1) Motors problem */
 139    s+=(state & ARDRONE_COM_LOST_MASK)? "Com Lost ":"Com ok ";
 140    if(level>1)s+=(state & ARDRONE_VBAT_LOW) ? "VBat low ":"Vbat Ok ";
 141    if(level>1)s+=(state & ARDRONE_USER_EL) ? "User EL ":"";
 142    s+=(state & ARDRONE_TIMER_ELAPSED) ? "Timer elapsed ":"";
 143    s+=(state & ARDRONE_ANGLES_OUT_OF_RANGE) ? "Angles out of range ":"";
 144    s+=(state & ARDRONE_ULTRASOUND_MASK) ? "Ultrasonic sensor deaf ":"";
 145    s+=(state & ARDRONE_CUTOUT_MASK) ? "Cutout system detection detected":"";
 146 //   s+=(state & ARDRONE_PIC_VERSION_MASK)==0 ? /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
 147 //   s+=(state & ARDRONE_ATCODEC_THREAD_ON)==0 ? /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
 148    if(level>2)s+=(state & ARDRONE_NAVDATA_THREAD_ON)==0 ? "Navdata thread OFF ":"Navdata thread ON ";
 149    if(level>2)s+=(state & ARDRONE_VIDEO_THREAD_ON )==0 ? "Video thread OFF ":"Video thread ON ";
 150 //   s+=+(state & ARDRONE_ACQ_THREAD_ON)==0 ?  "Acquisition thread OFF ":"Acquisition thread ON ";
 151    s+=(state & ARDRONE_CTRL_WATCHDOG_MASK) ? "CTRL watchdog ":"";
 152    s+=(state & ARDRONE_ADC_WATCHDOG_MASK) ? "ADC Watchdog ":"";
 153    s+=(state & ARDRONE_COM_WATCHDOG_MASK) ? "Comm Watchdog ":"";
 154    s+=(state & ARDRONE_EMERGENCY_MASK)?  "Emg landing":"";
 155    return s;
 156 }