QObject(parent)
{
vx=0;vy=0;vz=0;pwm_motor1=0;pwm_motor2=0;pwm_motor3=0;pwm_motor4=0;
- vbat=0.0;pitch=0.0;roll=0.0;yaw=0.0;altitude=0.0;
+ vbat=0.0;pitch=0.0;roll=0.0;yaw=0.0;altitude=0.0;navSeq=0;
}
+void NavData::setState(QString _state)
+{
+ decodedState=_state;
+ oldState=0;
+ emit stateUpdated();
+};
void NavData::parseRawNavData(char *buf,unsigned int len)
{
qDebug("parseRawNavData hdr=%8x state=%08x seq=%06d opt[0].tag=%d size=%d",
nd->header,nd->ardrone_state,nd->sequence,nd->options[0].tag,nd->options[0].size);
#endif
- qDebug() << decodedState;
+// qDebug() << len << decodedState;
op=&(nd->options[0]);
- while((((unsigned int)op-(unsigned int)buf)<len) && (op->size>0))
+ while((((unsigned long)op-(unsigned long)buf)<len) && (op->size>0))
{
parseOption(op);
- op=(navdata_option_t *)((unsigned int)op+op->size);
+ op=(navdata_option_t *)((unsigned long)op+op->size);
}
};
void NavData::parseOption(navdata_option_t *op)
{
- // qDebug("parseOption tag=%d size=%d",op->tag,op->size);
+// qDebug("parseOption tag=%d size=%d",op->tag,op->size);
switch(op->tag) {
case NAVDATA_DEMO_TAG:
{
vy=ndemo->vy;
vz=ndemo->vz;
emit navDataUpdated();
-// 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);
+ // 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);
}
break;
case NAVDATA_TIME_TAG:
current_motor4=npwm->current_motor4;
pwm_motor4=npwm->motor4;
emit navDataUpdated();
- // qDebug("pwm1=%3d I1=%4f pwm2=%3d I2=%4f pwm3=%3d I3=%4f pwm4=%3d I4=%4f",
- // pwm_motor1,current_motor1,pwm_motor2,current_motor2,pwm_motor3,current_motor3,pwm_motor4,current_motor4);
+ // qDebug("pwm1=%3d I1=%4f pwm2=%3d I2=%4f pwm3=%3d I3=%4f pwm4=%3d I4=%4f",
+ // pwm_motor1,current_motor1,pwm_motor2,current_motor2,pwm_motor3,current_motor3,pwm_motor4,current_motor4);
}
break;
case NAVDATA_ALTITUDE_TAG: //10
QString NavData::decodeState(unsigned int state,int level)
{
- QString s="%1 ";
- s=s.arg(state,8,16);
+ QString s;
+ s=QString("%1 %2 ").arg(navSeq++).arg(state,8,16);
s+=(state & ARDRONE_FLY_MASK)==0 ? "landed ":"flying ";
if(level>1) s+=(state & ARDRONE_VIDEO_MASK)==0 ?"video disable ":"video ena ";
if(level>1)s+=(state & ARDRONE_VISION_MASK)==0 ? "vision disable ":"vision ena ";
// s+=(state & ARDRONE_FW_VER_MASK )==0 ? /* Firmware update is newer;
// ARDRONE_FW_UPD_MASK = 1 << 9, /* Firmware update is ongoing (1;
s+=(state & ARDRONE_NAVDATA_DEMO_MASK )==0 ? "All navdata ":"navdata demo ";
- s+=(state & ARDRONE_NAVDATA_BOOTSTRAP)==0 ? "":"Navdata bootstrap ";
+ s+=(state & ARDRONE_NAVDATA_BOOTSTRAP)==0 ? "":"Navdata bootstrap ";
// s+=(state & ARDRONE_MOTORS_MASK)==0 ? /*!< Motors status : (0) Ok, (1) Motors problem */
s+=(state & ARDRONE_COM_LOST_MASK)? "Com Lost ":"Com ok ";
if(level>1)s+=(state & ARDRONE_VBAT_LOW) ? "VBat low ":"Vbat Ok ";