1 /*==================================================================
3 ! mardrone application AR-Drone for MeeGo
5 ! Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
8 ! Author:Kate Alhola kate.alhola@nokia.com
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.
20 *===================================================================*/
25 NavData::NavData(QObject *parent) :
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;
31 void NavData::setState(QString _state)
38 void NavData::parseRawNavData(char *buf,unsigned int len)
42 _navdata_t *nd=(_navdata_t *)buf;
43 state=nd->ardrone_state;
46 decodedState=decodeState(state,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);
53 // qDebug() << len << decodedState;
55 while((((unsigned long)op-(unsigned long)buf)<len) && (op->size>0))
58 op=(navdata_option_t *)((unsigned long)op+op->size);
62 void NavData::parseOption(navdata_option_t *op)
64 // qDebug("parseOption tag=%d size=%d",op->tag,op->size);
66 case NAVDATA_DEMO_TAG:
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;
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);
81 case NAVDATA_TIME_TAG:
83 _navdata_time_t *ntdime=(_navdata_time_t*)op;
86 case NAVDATA_RAW_MEASURES_TAG:
88 _navdata_raw_measures_t *nraw=(_navdata_raw_measures_t*)op;
91 case NAVDATA_PHYS_MEASURES_TAG:
93 _navdata_phys_measures_t *nphys=(_navdata_phys_measures_t*)op;
96 case NAVDATA_GYROS_OFFSETS_TAG:
98 case NAVDATA_EULER_ANGLES_TAG:
100 case NAVDATA_REFERENCES_TAG:
102 case NAVDATA_TRIMS_TAG: // 7
104 _navdata_trims_t *ntrim=( _navdata_trims_t*)op;
107 case NAVDATA_RC_REFERENCES_TAG: //8
109 case NAVDATA_PWM_TAG:
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);
125 case NAVDATA_ALTITUDE_TAG: //10
127 case NAVDATA_VISION_RAW_TAG:
129 case NAVDATA_VISION_OF_TAG: //12
131 case NAVDATA_VISION_TAG:
133 case NAVDATA_VISION_PERF_TAG:
135 case NAVDATA_TRACKERS_SEND_TAG:
137 case NAVDATA_VISION_DETECT_TAG: //16
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];
152 case NAVDATA_WATCHDOG_TAG:
154 case NAVDATA_ADC_DATA_FRAME_TAG:
156 case NAVDATA_CKS_TAG: //65535
161 QString NavData::decodeState(unsigned int state,int level)
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":"";