RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed issues

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-08 17:26:24 +02:00
parent 920c90163d
commit 28bf3b4682
5 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
NetworkSync.cpp
View File

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

27
Perception.cpp
View File

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

2
Perception.h
View File

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

3
TrackedObject.cpp
View File

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

3
TrackedObject.h
View File

@ -23,7 +23,8 @@ public:
/// @details This will also update the confidence of the object to the /// @details This will also update the confidence of the object to the
/// maxConfidence value /// maxConfidence value
void Refresh(Polar position); 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 /// @brief Decrease the confidence based on the elapsed time
/// @param deltaTime The time since the last DegradeConfidence call /// @param deltaTime The time since the last DegradeConfidence call