Generic case

bindings/cffirmware.i

@@ -133,23 +133,40 @@ void collisionAvoidanceUpdateSetpointWrap(
  free(workspace);
 }
 
-void state_set_neighbor_position(state_t *state, int idx, uint8_t id, float x, float y, float z)
+void state_set_position(state_t *state, int idx, uint8_t id, float x, float y, float z)
 {
- state->neighbors[idx].id = id;
- state->neighbors[idx].pos.x = x;
- state->neighbors[idx].pos.y = y;
- state->neighbors[idx].pos.z = z;
+ state->team_state[idx].id = id;
+ state->team_state[idx].pos.x = x;
+ state->team_state[idx].pos.y = y;
+ state->team_state[idx].pos.z = z;
 }
 
-void controller_lee_payload_set_attachement(controllerLeePayload_t* self, int idx, uint8_t id, float x, float y, float z, float mdx, float mdy, float mdz)
+struct vec controller_lee_payload_get_n(controllerLeePayload_t* self, int idx) 
+{
+ struct vec n = mkvec(self->n[idx].x, self->n[idx].y, self->n[idx].z);
+ return n;
+}
+void set_setpoint_qi_ref(setpoint_t *setpoint, int idx, uint8_t id, float qx, float qy, float qz, float qdx, float qdy, float qdz) 
+{ 
+ setpoint->cablevectors[idx].id = id;
+ setpoint->cablevectors[idx].mu_ref.x = qx;
+ setpoint->cablevectors[idx].mu_ref.y = qy;
+ setpoint->cablevectors[idx].mu_ref.z = qz;
+ setpoint->cablevectors[idx].qid_ref.x = qdx;
+ setpoint->cablevectors[idx].qid_ref.y = qdy;
+ setpoint->cablevectors[idx].qid_ref.z = qdz;
+}
+
+// void controller_lee_payload_set_attachement(controllerLeePayload_t* self, int idx, uint8_t id, float x, float y, float z, float mdx, float mdy, float mdz)
+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;
- self->attachement_points[idx].mu_desired.x = mdx;
- self->attachement_points[idx].mu_desired.y = mdy;
- self->attachement_points[idx].mu_desired.z = mdz;
+ // self->attachement_points[idx].mu_desired.x = mdx;
+ // self->attachement_points[idx].mu_desired.y = mdy;
+ // self->attachement_points[idx].mu_desired.z = mdz;
 }
 
 void controller_lee_payload_set_Pinv(controllerLeePayload_t* self, int idx, int id1, int id2, int row, int column, float value)

bindings/setup.py

@@ -43,7 +43,22 @@
  "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_4uav_5hp.c",
+ "src/lib/osqp/src/osqp/workspace_5uav_9hp.c",
+ "src/lib/osqp/src/osqp/workspace_6uav_14hp.c",
+ "src/lib/osqp/src/osqp/workspace_7uav_42hp.c",
+ "src/lib/osqp/src/osqp/workspace_8uav_56hp.c",
+ "src/lib/osqp/src/osqp/workspace_9uav_72hp.c",
+ "src/lib/osqp/src/osqp/workspace_10uav_90hp.c",
  "src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c",
+ "src/lib/osqp/src/osqp/workspace_3uav_6hp_rig.c",
+ "src/lib/osqp/src/osqp/workspace_4uav_12hp_rig.c",
+ "src/lib/osqp/src/osqp/workspace_5uav_20hp_rig.c", 
+ "src/lib/osqp/src/osqp/workspace_6uav_30hp_rig.c",
+ "src/lib/osqp/src/osqp/workspace_7uav_42hp_rig.c", 
+ "src/lib/osqp/src/osqp/workspace_8uav_56hp_rig.c", 
+ "src/lib/osqp/src/osqp/workspace_9uav_72hp_rig.c", 
+ "src/lib/osqp/src/osqp/workspace_10uav_90hp_rig.c", 
  "src/lib/osqp/src/osqp/workspace_2uav_1hp_rod.c",
  "src/lib/osqp/src/osqp/workspace_hyperplane.c",
  "src/lib/osqp/src/osqp/workspace_hyperplane_rb.c",

src/hal/src/radiolink.c

@@ -46,7 +46,7 @@
 #include "cfassert.h"
 
 #define RADIOLINK_TX_QUEUE_SIZE (1)
-#define RADIOLINK_CRTP_QUEUE_SIZE (5)
+#define RADIOLINK_CRTP_QUEUE_SIZE (10)
 #define RADIO_ACTIVITY_TIMEOUT_MS (1000)
 
 #define RADIOLINK_P2P_QUEUE_SIZE (5)
@@ -67,6 +67,8 @@ static int radiolinkReceiveCRTPPacket(CRTPPacket *p);
 static uint8_t rssi;
 static bool isConnected;
 static uint32_t lastPacketTick;
+static uint16_t count_rx_broadcast;
+static uint16_t count_rx_unicast;
 
 static volatile P2PCallback p2p_callback;
 
@@ -162,6 +164,7 @@ void radiolinkSyslinkDispatch(SyslinkPacket *slp)
  slp->length--; // Decrease to get CRTP size.
  // Assert that we are not dopping any packets
  ASSERT(xQueueSend(crtpPacketDelivery, &slp->length, 0) == pdPASS);
+ ++count_rx_unicast;
  ledseqRun(&seq_linkUp);
  // If a radio packet is received, one can be sent
  if (xQueueReceive(txQueue, &txPacket, 0) == pdTRUE)
@@ -173,7 +176,11 @@ void radiolinkSyslinkDispatch(SyslinkPacket *slp)
  {
  slp->length--; // Decrease to get CRTP size.
  // broadcasts are best effort, so no need to handle the case where the queue is full
- xQueueSend(crtpPacketDelivery, &slp->length, 0);
+ BaseType_t result = xQueueSend(crtpPacketDelivery, &slp->length, 0);
+ // only increment the received counter, if we were able to put it in the queue
+ if (result == pdPASS) {
+ ++count_rx_broadcast;
+ }
  ledseqRun(&seq_linkUp);
  // no ack for broadcasts
  } else if (slp->type == SYSLINK_RADIO_RSSI)
@@ -259,7 +266,28 @@ static int radiolinkSetEnable(bool enable)
  return 0;
 }
 
+/**
+ * Radio link log variables.
+ */
 LOG_GROUP_START(radio)
+/**
+ * @brief Radio Signal Strength Indicator [dBm]
+ */
 LOG_ADD_CORE(LOG_UINT8, rssi, &rssi)
+/**
+ * @brief Indicator if a packet was received from the radio within the last RADIO_ACTIVITY_TIMEOUT_MS.
+ */
 LOG_ADD_CORE(LOG_UINT8, isConnected, &isConnected)
+/**
+ * @brief Number of broadcast packets received.
+ * 
+ * Note that this is only 16 bits and overflows. Use overflow correction on the client side.
+ */
+LOG_ADD_CORE(LOG_UINT16, numRxBc, &count_rx_broadcast)
+/**
+ * @brief Number of unicast packets received.
+ * 
+ * Note that this is only 16 bits and overflows. Use overflow correction on the client side.
+ */
+LOG_ADD_CORE(LOG_UINT16, numRxUc, &count_rx_unicast)
 LOG_GROUP_STOP(radio)

src/lib/Kbuild

@@ -58,8 +58,16 @@ 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
-obj-y += osqp/src/osqp/workspace_2uav_1hp_rod.o
+obj-y += osqp/src/osqp/workspace_4uav_5hp.o
+# obj-y += osqp/src/osqp/workspace_5uav_9hp.o
+# obj-y += osqp/src/osqp/workspace_6uav_14hp.o
+# obj-y += osqp/src/osqp/workspace_7uav_42hp.o
+# obj-y += osqp/src/osqp/workspace_8uav_56hp.o
+# obj-y += osqp/src/osqp/workspace_9uav_72hp.o
+# obj-y += osqp/src/osqp/workspace_10uav_90hp.o
+# obj-y += osqp/src/osqp/workspace_3uav_2hp_rig.o
+# obj-y += osqp/src/osqp/workspace_3uav_6hp_rig.o
+# obj-y += osqp/src/osqp/workspace_2uav_1hp_rod.o
 obj-y += osqp/src/osqp/workspace_hyperplane.o
 obj-y += osqp/src/osqp/workspace_hyperplane_rb.o
 obj-y += osqp/src/osqp/workspace_compute_Fd_pair.o