X-Git-Url: https://vcs.maemo.org/git/?a=blobdiff_plain;f=mardrone%2Fdronelib%2Fnavdata.cpp;h=f3b726179cd68c59e9959befe47879628b95813e;hb=f19da612ad0dd01a6f6339386625ce2bdc259220;hp=7b61e4300f90ef0d80d28e22bfbd02737cb97f32;hpb=196f6d56fb98af5849cc98d389de421822d3d5e0;p=mardrone

diff --git a/mardrone/dronelib/navdata.cpp b/mardrone/dronelib/navdata.cpp
index 7b61e43..f3b7261 100644
--- a/mardrone/dronelib/navdata.cpp
+++ b/mardrone/dronelib/navdata.cpp
@@ -26,8 +26,14 @@ NavData::NavData(QObject *parent) :
     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)
 {
@@ -44,18 +50,18 @@ 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:
                 {
@@ -69,7 +75,7 @@ void NavData::parseOption(navdata_option_t *op)
                     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:
@@ -112,8 +118,8 @@ void NavData::parseOption(navdata_option_t *op)
                 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
@@ -154,8 +160,8 @@ void NavData::parseOption(navdata_option_t *op)
 
 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 ";
@@ -167,7 +173,7 @@ QString NavData::decodeState(unsigned int state,int level)
  //  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 ";