Fixed issues

NetworkSync.cpp

@@ -89,9 +89,9 @@ void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
   data[startIndex + 1] = value >> 16 & 0xFF;
   data[startIndex + 2] = value >> 8 & 0xFF;
   data[startIndex + 3] = value & 0xFF;
-  Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
-                data[startIndex + 1], data[startIndex + 2],
-                data[startIndex + 3]);
+  // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
+  //               data[startIndex + 1], data[startIndex + 2],
+  //               data[startIndex + 3]);
 }
 
 void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,

Perception.cpp

@@ -6,7 +6,7 @@
 
 #include <math.h>
 
-// #define RC_DEBUG2
+#define RC_DEBUG2
 #ifdef RC_DEBUG2
 #include <Arduino.h>
 #endif
@@ -252,10 +252,17 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   }
 }
 
-// void Perception::AddTrackedObject(Sensor *sensor, uint8_t objectType,
-//                                   Vector3 position, Quaternion orientation) {
-
-//                                   }
+InterestingThing *Perception::FindTrackedObject(char objectId) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->id == objectId) {
+      return thing;
+    }
+  }
+  return nullptr;
+}
 
 unsigned char Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
@@ -326,10 +333,12 @@ void Perception::Update(float currentTimeMs) {
 
     if (obj->DegradeConfidence(deltaTime) == false) {
       // delete obj
-      if (roboid != nullptr && roboid->networkSync != nullptr)
-        roboid->networkSync->DestroyObject(obj);
-      this->trackedObjects[objIx] = nullptr;
-      delete obj;
+      // if (roboid != nullptr && roboid->networkSync != nullptr) {
+      //   printf("Cleanup obj %d\n", obj->id);
+      //   roboid->networkSync->DestroyObject(obj);
+      // }
+      // this->trackedObjects[objIx] = nullptr;
+      // delete obj;
     }
   }
 }

Perception.h

@@ -90,6 +90,8 @@ public:
   AddTrackedObject(Sensor *sensor, Spherical position,
                    Quaternion orientation = Quaternion::identity);
 
+  InterestingThing *FindTrackedObject(char objectId);
+
   /// @brief Retrieve the number of objects currently being tracked by the
   /// roboid
   /// @return The object of objects, which is always lower than maxObjectCount

TrackedObject.cpp

@@ -54,7 +54,8 @@ void InterestingThing::Refresh(Polar position) {
   this->confidence = maxConfidence;
 }
 
-void InterestingThing::Refresh(Spherical position) {
+void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
   this->position = position;
+  this->orientation = orientation;
   this->confidence = maxConfidence;
 }

TrackedObject.h

@@ -23,7 +23,8 @@ public:
   /// @details This will also update the confidence of the object to the
   /// maxConfidence value
   void Refresh(Polar position);
-  void Refresh(Spherical position);
+  void Refresh(Spherical position,
+               Quaternion orientation = Quaternion::identity);
 
   /// @brief Decrease the confidence based on the elapsed time
   /// @param deltaTime The time since the last DegradeConfidence call