Add support to track rigid body payloads

bindings/cffirmware.i

@@ -133,13 +133,21 @@ void collisionAvoidanceUpdateSetpointWrap(
  free(workspace);
 }
 
-void state_set_neighbor_position(state_t *state, int idx, float x, float y, float z)
+void state_set_neighbor_position(state_t *state, int idx, uint8_t id, float x, float y, float z)
 {
- state->position_neighbors[idx].x = x;
- state->position_neighbors[idx].y = y;
- state->position_neighbors[idx].z = z;
+ state->neighbors[idx].id = id;
+ state->neighbors[idx].pos.x = x;
+ state->neighbors[idx].pos.y = y;
+ state->neighbors[idx].pos.z = z;
 }
 
+void controller_lee_payload_set_attachement(controllerLeePayload_t* self, int idx, uint8_t id, float x, float y, float z)
+{
+ self->attachement_points[idx].id = id;
+ self->attachement_points[idx].point.x = x;
+ self->attachement_points[idx].point.y = y;
+ self->attachement_points[idx].point.z = z;
+}
 %}
 
 %pythoncode %{

bindings/setup.py

@@ -43,6 +43,7 @@
  "src/lib/osqp/src/osqp/util.c",
  "src/lib/osqp/src/osqp/workspace_2uav_2hp.c",
  "src/lib/osqp/src/osqp/workspace_3uav_2hp.c",
+ "src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c",
 ]
 
 cffirmware = Extension(

src/hal/interface/usec_time.h

@@ -32,6 +32,11 @@
  */
 void initUsecTimer(void);
 
+/**
+ * Reset the microsecond-resolution timer to 0.
+ */
+void usecTimerReset(void);
+
 /**
  * Get microsecond-resolution timestamp.
  */

src/hal/src/usec_time.c

@@ -71,6 +71,16 @@ void initUsecTimer(void)
  isInit = true;
 }
 
+void usecTimerReset(void)
+{
+ IF_DEBUG_ASSERT(isInit);
+
+ const uint32_t zero = 0;
+ __atomic_store(&usecTimerHighCount, &zero, __ATOMIC_SEQ_CST);
+
+ TIM7->CNT = 0;
+}
+
 uint64_t usecTimestamp(void)
 {
  IF_DEBUG_ASSERT(isInit);

src/lib/Kbuild

@@ -58,3 +58,4 @@ obj-y += osqp/src/osqp/scaling.o
 obj-y += osqp/src/osqp/util.o
 obj-y += osqp/src/osqp/workspace_2uav_2hp.o
 obj-y += osqp/src/osqp/workspace_3uav_2hp.o
+obj-y += osqp/src/osqp/workspace_3uav_2hp_rig.o