(#641) added memory mapping for LH calibration data

bitcraze · Nov 12, 2020 · 20c40c4 · 20c40c4
1 parent 1d65f0e
commit 20c40c4
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 22 deletions.
diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c
@@ -54,9 +54,10 @@ static const float t30 = M_PI / 6;
 static void lighthousePositionGeometryDataUpdated();
 static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Rotor2Rot, mat3d lh1Rotor2RotInverted);
 
-
-// Gometry memory handling for the memory module
-static uint32_t handleMemGetSize(void) { return sizeof(lighthouseCoreState.bsGeometry); }
+// Geometry memory handling for the memory module
+static const uint32_t calibStartAddr = 0x1000;
+static const uint32_t pageSize = 0x100;
+static uint32_t handleMemGetSize(void) { return calibStartAddr + sizeof(lighthouseCoreState.bsCalibration); }
 static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
 static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer);
 static const MemoryHandlerDef_t memDef = {
@@ -74,10 +75,25 @@ void lighthousePositionEstInit() {
 static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
   bool result = false;
 
-  if (memAddr + readLen <= sizeof(lighthouseCoreState.bsGeometry)) {
-    uint8_t* start = (uint8_t*)lighthouseCoreState.bsGeometry;
-    memcpy(buffer, start + memAddr, readLen);
-    result = true;
+  if (memAddr < calibStartAddr) {
+    if (memAddr + readLen <= sizeof(lighthouseCoreState.bsGeometry)) {
+      uint8_t* start = (uint8_t*)lighthouseCoreState.bsGeometry;
+      memcpy(buffer, start + memAddr, readLen);
+
+      result = true;
+    }
+  } else {
+    uint32_t calibOffsetAddr = memAddr - calibStartAddr;
+    uint32_t index = calibOffsetAddr / pageSize;
+    uint32_t inPageAddr = calibOffsetAddr % pageSize;
+    if (index < PULSE_PROCESSOR_N_BASE_STATIONS) {
+      if (inPageAddr + readLen <= sizeof(lighthouseCalibration_t)) {
+        uint8_t* start = (uint8_t*)&lighthouseCoreState.bsCalibration[index];
+        memcpy(buffer, start + inPageAddr, readLen);
+
+        result = true;
+      } 
+    }
   }
 
   return result;
@@ -86,13 +102,27 @@ static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t
 static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) {
   bool result = false;
 
-  if ((memAddr + writeLen) <= sizeof(lighthouseCoreState.bsGeometry)) {
-    uint8_t* start = (uint8_t*)lighthouseCoreState.bsGeometry;
-    memcpy(start + memAddr, buffer, writeLen);
-
-    lighthousePositionGeometryDataUpdated();
-
-    result = true;
+  if (memAddr < calibStartAddr) {
+    if ((memAddr + writeLen) <= sizeof(lighthouseCoreState.bsGeometry)) {
+      uint8_t* start = (uint8_t*)lighthouseCoreState.bsGeometry;
+      memcpy(start + memAddr, buffer, writeLen);
+
+      lighthousePositionGeometryDataUpdated();
+
+      result = true;
+      } 
+  } else {
+    uint32_t calibOffsetAddr = memAddr - calibStartAddr;
+    uint32_t index = calibOffsetAddr / pageSize;
+    uint32_t inPageAddr = calibOffsetAddr % pageSize;
+    if (index < PULSE_PROCESSOR_N_BASE_STATIONS) {
+      if (inPageAddr + writeLen <= sizeof(lighthouseCalibration_t)) {
+        uint8_t* start = (uint8_t*)&lighthouseCoreState.bsCalibration[index];
+        memcpy(start + inPageAddr, buffer, writeLen);
+
+        result = true;
+      }
+    }
   }
 
   return result;

diff --git a/src/utils/interface/lighthouse/lighthouse_calibration.h b/src/utils/interface/lighthouse/lighthouse_calibration.h
@@ -11,12 +11,12 @@ typedef struct {
   // Lh2 extra params
   float ogeemag;
   float ogeephase;
-} lighthouseCalibrationSweep_t;
+} __attribute__((packed)) lighthouseCalibrationSweep_t;
 
 typedef struct {
-  bool valid;
   lighthouseCalibrationSweep_t sweep[2];
-} lighthouseCalibration_t;
+  bool valid;
+} __attribute__((packed)) lighthouseCalibration_t;
 
 /**
  * @brief Initialize calibration structure from basestation ootx frame

diff --git a/src/utils/interface/lighthouse/lighthouse_geometry.h b/src/utils/interface/lighthouse/lighthouse_geometry.h
@@ -5,14 +5,14 @@
 #include "stabilizer_types.h"
 
 typedef struct {
-  vec3d origin;
-  mat3d mat;
+  __attribute__((aligned(4))) vec3d origin;
+  __attribute__((aligned(4))) mat3d mat;
 } __attribute__((packed)) baseStationGeometry_t;
 
 typedef struct {
-  mat3d baseStationInvertedRotationMatrixes;
-  mat3d lh1Rotor2RotationMatrixes;
-  mat3d lh1Rotor2InvertedRotationMatrixes;
+  __attribute__((aligned(4))) mat3d baseStationInvertedRotationMatrixes;
+  __attribute__((aligned(4))) mat3d lh1Rotor2RotationMatrixes;
+  __attribute__((aligned(4))) mat3d lh1Rotor2InvertedRotationMatrixes;
 } baseStationGeometryCache_t;
 
 /**