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

   1 /*==================================================================
   2   !
   3   !  mardrone application AR-Drone for MeeGo
   4
   5   ! Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
   6   ! All rights reserved.
   7   !
   8   !  Author:Kate Alhola  kate.alhola@nokia.com
   9   !
  10   ! GNU Lesser General Public License Usage
  11   ! This file may be used under the terms of the GNU Lesser
  12   ! General Public License version 2.1 as published by the Free Software
  13   ! Foundation and appearing in the file LICENSE.LGPL included in the
  14   ! packaging of this file.  Please review the following information to
  15   ! ensure the GNU Lesser General Public License version 2.1 requirements
  16   ! will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
  17   !
  18   !
  19   !
  20   *===================================================================*/
  21 #include <QDebug>
  22 #include "navdata.h"
  23
  24
  25 NavData::NavData(QObject *parent) :
  26     QObject(parent)
  27 {
  28      vx=0;vy=0;vz=0;pwm_motor1=0;pwm_motor2=0;pwm_motor3=0;pwm_motor4=0;
  29      vbat=0.0;pitch=0.0;roll=0.0;yaw=0.0;altitude=0.0;navSeq=0;
  30 }
  31 void NavData::setState(QString _state)
  32 {
  33     decodedState=_state;
  34     oldState=0;
  35     emit stateUpdated();
  36 };
  37
  38 void NavData::parseRawNavData(char *buf,unsigned int len)
  39 {
  40     int opt=0;
  41     navdata_option_t *op;
  42     _navdata_t *nd=(_navdata_t *)buf;
  43     state=nd->ardrone_state;
  44     if(state!=oldState) {
  45         oldState=state;
  46         decodedState=decodeState(state,0);
  47         emit stateUpdated();
  48     };
  49 #if 0
  50     qDebug("parseRawNavData hdr=%8x state=%08x seq=%06d opt[0].tag=%d size=%d",
  51            nd->header,nd->ardrone_state,nd->sequence,nd->options[0].tag,nd->options[0].size);
  52 #endif
  53 //    qDebug() << len << decodedState;
  54     op=&(nd->options[0]);
  55     while((((unsigned long)op-(unsigned long)buf)<len) && (op->size>0))
  56     {
  57        parseOption(op);
  58        op=(navdata_option_t *)((unsigned long)op+op->size);
  59     }
  60 };
  61
  62 void NavData::parseOption(navdata_option_t *op)
  63 {
  64 //    qDebug("parseOption tag=%d size=%d",op->tag,op->size);
  65     switch(op->tag) {
  66     case NAVDATA_DEMO_TAG:
  67                 {
  68                     _navdata_demo_t *ndemo=(_navdata_demo_t*)op;
  69                     vbat=ndemo->vbat_flying_percentage;
  70                     pitch=ndemo->theta/1000.0;
  71                     roll=ndemo->phi/1000.0;
  72                     yaw=ndemo->psi/1000.0;
  73                     altitude=ndemo->altitude;
  74                     vx=ndemo->vx;
  75                     vy=ndemo->vy;
  76                     vz=ndemo->vz;
  77                     emit navDataUpdated();
  78     //                qDebug("NAVDATA_DEMO: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);
  79                 }
  80                 break;
  81         case NAVDATA_TIME_TAG:
  82                 {
  83                 _navdata_time_t *ntdime=(_navdata_time_t*)op;
  84                 }
  85                 break;
  86         case NAVDATA_RAW_MEASURES_TAG:
  87                 {
  88                 _navdata_raw_measures_t *nraw=(_navdata_raw_measures_t*)op;
  89                 }
  90                 break;
  91         case NAVDATA_PHYS_MEASURES_TAG:
  92                 {
  93                 _navdata_phys_measures_t *nphys=(_navdata_phys_measures_t*)op;
  94                 }
  95                 break;
  96         case NAVDATA_GYROS_OFFSETS_TAG:
  97                 break;
  98         case NAVDATA_EULER_ANGLES_TAG:
  99                 break;
 100         case NAVDATA_REFERENCES_TAG:
 101                 break;
 102         case NAVDATA_TRIMS_TAG: // 7
 103                 {
 104                 _navdata_trims_t *ntrim=( _navdata_trims_t*)op;
 105                 }
 106                 break;
 107         case NAVDATA_RC_REFERENCES_TAG: //8
 108                 break;
 109         case NAVDATA_PWM_TAG:
 110                 {
 111                 _navdata_pwm_t *npwm=( _navdata_pwm_t*)op;
 112                 current_motor1=npwm->current_motor1;
 113                 pwm_motor1=npwm->motor1;
 114                 current_motor2=npwm->current_motor2;
 115                 pwm_motor2=npwm->motor2;
 116                 current_motor3=npwm->current_motor3;
 117                 pwm_motor3=npwm->motor3;
 118                 current_motor4=npwm->current_motor4;
 119                 pwm_motor4=npwm->motor4;
 120                 emit navDataUpdated();
 121   //              qDebug("pwm1=%3d I1=%4f pwm2=%3d I2=%4f pwm3=%3d I3=%4f pwm4=%3d I4=%4f",
 122   //                      pwm_motor1,current_motor1,pwm_motor2,current_motor2,pwm_motor3,current_motor3,pwm_motor4,current_motor4);
 123                  }
 124                 break;
 125         case NAVDATA_ALTITUDE_TAG: //10
 126                 break;
 127         case NAVDATA_VISION_RAW_TAG:
 128                 break;
 129         case NAVDATA_VISION_OF_TAG: //12
 130                 break;
 131         case NAVDATA_VISION_TAG:
 132                 break;
 133         case NAVDATA_VISION_PERF_TAG:
 134                 break;
 135         case NAVDATA_TRACKERS_SEND_TAG:
 136                 break;
 137         case NAVDATA_VISION_DETECT_TAG: //16
 138             {
 139                 _navdata_vision_detect_t *nvisd=(_navdata_vision_detect_t*)op;
 140 //                qDebug("nb_detected=%d type=%d xc=%d yc=%d width=%d height=%d dist=%d",nvisd->nb_detected,nvisd->type[0],nvisd->xc[0],nvisd->yc[0],
 141 //                       nvisd->width[0],nvisd->height[0],nvisd->dist[0]);
 142                 for(int i=0;i++;i<4) {
 143                     tags[i].type=nvisd->type[i];
 144                     tags[i].xc=nvisd->xc[i];
 145                     tags[i].yc=nvisd->yc[i];
 146                     tags[i].height=nvisd->height[i];
 147                     tags[i].width=nvisd->width[i];
 148                     tags[i].dist=nvisd->dist[i];
 149                 }
 150            }
 151                 break;
 152         case NAVDATA_WATCHDOG_TAG:
 153                 break;
 154         case NAVDATA_ADC_DATA_FRAME_TAG:
 155                 break;
 156         case NAVDATA_CKS_TAG: //65535
 157                 break;
 158     }
 159 };
 160
 161 QString NavData::decodeState(unsigned int state,int level)
 162 {
 163    QString s;
 164    s=QString("%1 %2 ").arg(navSeq++).arg(state,8,16);
 165    s+=(state & ARDRONE_FLY_MASK)==0 ? "landed ":"flying ";
 166    if(level>1) s+=(state & ARDRONE_VIDEO_MASK)==0 ?"video disable ":"video ena ";
 167    if(level>1)s+=(state & ARDRONE_VISION_MASK)==0 ? "vision disable ":"vision ena ";
 168    if(level>1)s+=(state & ARDRONE_CONTROL_MASK)==0 ?  "euler angles ":"angular speed ";
 169    if(level>1)s+=(state & ARDRONE_ALTITUDE_MASK)==0 ? "alt ctrl inact ":"alt ctrl act ";
 170  //s+=(state & ARDRONE_USER_FEEDBACK_START)==0 ?  /*!< USER feedback : Start button state */
 171    s+=(state & ARDRONE_COMMAND_MASK)==0 ?  "CMD NAK ":"CMD ACK ";
 172  //  s+=(state & ARDRONE_FW_FILE_MASK)==0 ?  /* Firmware file is good;
 173  //  s+=(state & ARDRONE_FW_VER_MASK )==0 ?  /* Firmware update is newer;
 174  //  ARDRONE_FW_UPD_MASK         = 1 << 9,  /* Firmware update is ongoing (1;
 175    s+=(state & ARDRONE_NAVDATA_DEMO_MASK )==0 ? "All navdata ":"navdata demo ";
 176    s+=(state & ARDRONE_NAVDATA_BOOTSTRAP)==0 ? "":"Navdata bootstrap ";
 177  //  s+=(state & ARDRONE_MOTORS_MASK)==0 ? /*!< Motors status : (0) Ok, (1) Motors problem */
 178    s+=(state & ARDRONE_COM_LOST_MASK)? "Com Lost ":"Com ok ";
 179    if(level>1)s+=(state & ARDRONE_VBAT_LOW) ? "VBat low ":"Vbat Ok ";
 180    if(level>1)s+=(state & ARDRONE_USER_EL) ? "User EL ":"";
 181    s+=(state & ARDRONE_TIMER_ELAPSED) ? "Timer elapsed ":"";
 182    s+=(state & ARDRONE_ANGLES_OUT_OF_RANGE) ? "Angles out of range ":"";
 183    s+=(state & ARDRONE_ULTRASOUND_MASK) ? "Ultrasonic sensor deaf ":"";
 184    s+=(state & ARDRONE_CUTOUT_MASK) ? "Cutout system detection detected":"";
 185 //   s+=(state & ARDRONE_PIC_VERSION_MASK)==0 ? /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
 186 //   s+=(state & ARDRONE_ATCODEC_THREAD_ON)==0 ? /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
 187    if(level>2)s+=(state & ARDRONE_NAVDATA_THREAD_ON)==0 ? "Navdata thread OFF ":"Navdata thread ON ";
 188    if(level>2)s+=(state & ARDRONE_VIDEO_THREAD_ON )==0 ? "Video thread OFF ":"Video thread ON ";
 189 //   s+=+(state & ARDRONE_ACQ_THREAD_ON)==0 ?  "Acquisition thread OFF ":"Acquisition thread ON ";
 190    s+=(state & ARDRONE_CTRL_WATCHDOG_MASK) ? "CTRL watchdog ":"";
 191    s+=(state & ARDRONE_ADC_WATCHDOG_MASK) ? "ADC Watchdog ":"";
 192    s+=(state & ARDRONE_COM_WATCHDOG_MASK) ? "Comm Watchdog ":"";
 193    s+=(state & ARDRONE_EMERGENCY_MASK)?  "Emg landing":"";
 194    return s;
 195 }