#632 Added support for trajectories in the app API

src/modules/interface/crtp_commander_high_level.h

@@ -46,12 +46,6 @@ Header file for high-level commander that computes smooth setpoints based on hig
 
 #include "stabilizer_types.h"
 
-// allocate memory to store trajectories
-// 4k allows us to store 31 poly4d pieces
-// other (compressed) formats might be added in the future
-#define TRAJECTORY_MEMORY_SIZE 4096
-extern uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];
-
 #define NUM_TRAJECTORY_DEFINITIONS 10
 
 typedef enum {
@@ -150,10 +144,40 @@ void crtpCommanderHighLevelStartTrajectory(const uint8_t trajectoryId, const flo
  *
  * @param trajectoryId The id of the trajectory
  * @param type         The type of trajectory that is stored in memory.
- * @param offset       offset in uploaded memory
+ * @param offset       offset in uploaded memory (bytes)
  * @param nPieces      Nr of pieces in the trajectory
  */
 void crtpCommanderHighLevelDefineTrajectory(const uint8_t trajectoryId, const crtpCommanderTrajectoryType_t type, const uint32_t offset, const uint8_t nPieces);
 
+/**
+ * @brief Get the size of the allocated trajectory memory
+ *
+ * @return uint32_t The size of the trajectory memory in bytes
+ */
+uint32_t crtpCommanderHighLevelTrajectoryMemSize();
+
+/**
+ * @brief Copy trajectory data to the trajectory memeory. After the copy crtpCommanderHighLevelDefineTrajectory()
+ *        must be called before the trajectory can be used.
+ *
+ * @param offset    offset in uploaded memory (bytes)
+ * @param length    Length of the data (bytes) to copy to the trajectory memory
+ * @param data[in]  pointer to the trajectory data source
+ *
+ * @return true   If data was copied
+ * @return false  If data is too large
+ */
+bool crtpCommanderHighLevelWriteTrajectory(const uint32_t offset, const uint32_t length, const uint8_t* data);
+
+/**
+ * @brief Copy data from the trajectory memory.
+ *
+ * @param offset             Offset in the trajectory memory (bytes)
+ * @param length             Length of the data to copy
+ * @param destination [out]  Pointer to copy data to
+ * @return true              If data was copied
+ * @return false             If length is too large
+ */
+bool crtpCommanderHighLevelReadTrajectory(const uint32_t offset, const uint32_t length, uint8_t* destination);
 
 #endif /* CRTP_COMMANDER_HIGH_LEVEL_H_ */

src/modules/src/crtp_commander_high_level.c

@@ -75,6 +75,12 @@ struct trajectoryDescription
   } trajectoryIdentifier;
 } __attribute__((packed));
 
+// allocate memory to store trajectories
+// 4k allows us to store 31 poly4d pieces
+// other (compressed) formats might be added in the future
+#define TRAJECTORY_MEMORY_SIZE 4096
+extern uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];
+
 #define ALL_GROUPS 0
 
 // Global variables
@@ -719,6 +725,34 @@ void crtpCommanderHighLevelDefineTrajectory(const uint8_t trajectoryId, const cr
   handleCommand(COMMAND_DEFINE_TRAJECTORY, (const uint8_t*)&data);
 }
 
+uint32_t crtpCommanderHighLevelTrajectoryMemSize()
+{
+  return sizeof(trajectories_memory);
+}
+
+bool crtpCommanderHighLevelWriteTrajectory(const uint32_t offset, const uint32_t length, const uint8_t* data)
+{
+  bool result = false;
+
+  if ((offset + length) <= sizeof(trajectories_memory)) {
+    memcpy(&(trajectories_memory[offset]), data, length);
+    result = true;
+  }
+
+  return result;
+}
+
+bool crtpCommanderHighLevelReadTrajectory(const uint32_t offset, const uint32_t length, uint8_t* destination)
+{
+  bool result = false;
+
+  if (offset + length <= sizeof(trajectories_memory) && memcpy(destination, &(trajectories_memory[offset]), length)) {
+    result = true;
+  }
+
+  return result;
+}
+
 PARAM_GROUP_START(hlCommander)
 PARAM_ADD(PARAM_FLOAT, vtoff, &defaultTakeoffVelocity)
 PARAM_ADD(PARAM_FLOAT, vland, &defaultLandingVelocity)

src/modules/src/mem.c

@@ -250,7 +250,7 @@ static void createInfoResponse(CRTPPacket* p, uint8_t memId) {
       createInfoResponseBody(p, MEM_TYPE_LOCO, MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * LOCO_MESSAGE_NR_OF_ANCHORS, noData);
       break;
     case TRAJ_ID:
-      createInfoResponseBody(p, MEM_TYPE_TRAJ, sizeof(trajectories_memory), noData);
+      createInfoResponseBody(p, MEM_TYPE_TRAJ, crtpCommanderHighLevelTrajectoryMemSize(), noData);
       break;
     case LOCO2_ID:
       createInfoResponseBody(p, MEM_TYPE_LOCO2, MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * 256, noData);
@@ -573,8 +573,7 @@ static uint8_t handleLedMemWrite(uint32_t memAddr, uint8_t writeLen, uint8_t* st
 static uint8_t handleTrajectoryMemRead(uint32_t memAddr, uint8_t readLen, uint8_t* startOfData) {
   uint8_t status = EIO;
 
-  if (memAddr + readLen <= sizeof(trajectories_memory) &&
-      memcpy(startOfData, &(trajectories_memory[memAddr]), readLen)) {
+  if (crtpCommanderHighLevelReadTrajectory(memAddr, readLen, startOfData)) {
     status = STATUS_OK;
   }
 
@@ -584,8 +583,7 @@ static uint8_t handleTrajectoryMemRead(uint32_t memAddr, uint8_t readLen, uint8_
 static uint8_t handleTrajectoryMemWrite(uint32_t memAddr, uint8_t writeLen, uint8_t* startOfData) {
   uint8_t status = EIO;
 
-  if ((memAddr + writeLen) <= sizeof(trajectories_memory)) {
-    memcpy(&(trajectories_memory[memAddr]), startOfData, writeLen);
+  if (crtpCommanderHighLevelWriteTrajectory(memAddr, writeLen, startOfData)) {
     status = STATUS_OK;
   }