Acceleration sensor control and basic vision framework

[mardrone] / mardrone / navdata.cpp

diff --git a/mardrone/navdata.cpp b/mardrone/navdata.cpp
index 5954c93..85011b4 100644 (file)
--- a/mardrone/navdata.cpp
+++ b/mardrone/navdata.cpp
@@ -112,8 +112,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
@@ -129,6 +129,19 @@ void NavData::parseOption(navdata_option_t *op)
         case NAVDATA_TRACKERS_SEND_TAG:
                 break;
         case NAVDATA_VISION_DETECT_TAG: //16
+            {
+                _navdata_vision_detect_t *nvisd=(_navdata_vision_detect_t*)op;
+                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],
+                       nvisd->width[0],nvisd->height[0],nvisd->dist[0]);
+                for(int i=0;i++;i<4) {
+                    tags[i].type=nvisd->type[i];
+                    tags[i].xc=nvisd->xc[i];
+                    tags[i].yc=nvisd->yc[i];
+                    tags[i].height=nvisd->height[i];
+                    tags[i].width=nvisd->width[i];
+                    tags[i].dist=nvisd->dist[i];
+                }
+           }
                 break;
         case NAVDATA_WATCHDOG_TAG:
                 break;