Add Eventtriggers for kalman filter enqueue functions.

src/deck/drivers/src/lpsTdoa2Tag.c

@@ -274,7 +274,7 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
 }
 
 
-static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement, const uint8_t idA, const uint8_t idB) {
+static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
   estimatorEnqueueTDOA(tdoaMeasurement);
 
   #ifdef LPS_2D_POSITION_HEIGHT
@@ -287,6 +287,8 @@ static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement, cons
   estimatorEnqueueAbsoluteHeight(&heightData);
   #endif
 
+  const uint8_t idA = tdoaMeasurement->anchorIds[0];
+  const uint8_t idB = tdoaMeasurement->anchorIds[1];
   if (isConsecutiveIds(idA, idB)) {
     logUwbTdoaDistDiff[idB] = tdoaMeasurement->distanceDiff;
   }

src/deck/drivers/src/lpsTdoa3Tag.c

@@ -257,7 +257,7 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
   return MAX_TIMEOUT;
 }
 
-static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement, const uint8_t idA, const uint8_t idB) {
+static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
   estimatorEnqueueTDOA(tdoaMeasurement);
 
   #ifdef LPS_2D_POSITION_HEIGHT

src/deck/drivers/src/lpsTwrTag.c

@@ -121,7 +121,7 @@ static dwTime_t final_rx;
 
 static packet_t txPacket;
 static volatile uint8_t curr_seq = 0;
-static int current_anchor = 0;
+static uint8_t current_anchor = 0;
 
 static bool ranging_complete = false;
 static bool lpp_transaction = false;
@@ -256,6 +256,7 @@ static uint32_t rxcallback(dwDevice_t *dev) {
         dist.x = options->anchorPosition[current_anchor].x;
         dist.y = options->anchorPosition[current_anchor].y;
         dist.z = options->anchorPosition[current_anchor].z;
+        dist.anchorId = current_anchor;
         dist.stdDev = 0.25;
         estimatorEnqueueDistance(&dist);
       }

src/modules/interface/range.h

@@ -21,8 +21,8 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * ranges.h: Centralize range measurements for different directions
- *           and make them available as log
+ * range.h: Centralize range measurements for different directions
+ *          and make them available as log
  */
 
 #pragma once
@@ -56,7 +56,7 @@ float rangeGet(rangeDirection_t direction);
 /**
  * Enqueue a range measurement for distance to the ground in the current estimator.
  *
- * @param dstance Distance to the ground (m)
+ * @param distance Distance to the ground (m)
  * @param stdDev The standard deviation of the range sample
  * @param timeStamp The time when the range was sampled (in sys ticks)
  */

src/modules/interface/stabilizer_types.h

@@ -83,8 +83,14 @@ typedef struct quaternion_s {
   };
 } quaternion_t;
 
+typedef enum {
+  MeasurementSourceLocationService  = 0,
+  MeasurementSourceLighthouse       = 1,
+} measurementSource_t;
+
 typedef struct tdoaMeasurement_s {
-  point_t anchorPosition[2];
+  point_t anchorPositions[2];
+  uint8_t anchorIds[2];
   float distanceDiff;
   float stdDev;
 } tdoaMeasurement_t;
@@ -105,6 +111,7 @@ typedef struct positionMeasurement_s {
     float pos[3];
   };
   float stdDev;
+  measurementSource_t source;
 } positionMeasurement_t;
 
 typedef struct poseMeasurement_s {
@@ -130,6 +137,7 @@ typedef struct distanceMeasurement_s {
     };
     float pos[3];
   };
+  uint8_t anchorId;
   float distance;
   float stdDev;
 } distanceMeasurement_t;
@@ -252,6 +260,9 @@ typedef struct {
   const vec3d* rotorPos;     // Pos of rotor origin in global reference frame
   const mat3d* rotorRot;     // Rotor rotation matrix
   const mat3d* rotorRotInv;  // Inverted rotor rotation matrix
+  uint8_t sensorId;
+  uint8_t basestationId;
+  uint8_t sweepId;
   float t;                   // t is the tilt angle of the light plane on the rotor
   float measuredSweepAngle;
   float stdDev;

src/modules/src/crtp_localization_service.c

@@ -154,13 +154,22 @@ static void locSrvCrtpCB(CRTPPacket* pk)
   }
 }
 
+static void updateLogFromExtPos()
+{
+  ext_pose.x = ext_pos.x;
+  ext_pose.y = ext_pos.y;
+  ext_pose.z = ext_pos.z;
+}
+
 static void extPositionHandler(CRTPPacket* pk) {
   const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;
 
   ext_pos.x = data->x;
   ext_pos.y = data->y;
   ext_pos.z = data->z;
   ext_pos.stdDev = extPosStdDev;
+  ext_pos.source = MeasurementSourceLocationService;
+  updateLogFromExtPos();
 
   estimatorEnqueuePosition(&ext_pos);
   tickOfLastPacket = xTaskGetTickCount();
@@ -301,7 +310,9 @@ static void extPositionPackedHandler(CRTPPacket* pk)
     ext_pos.y = item->y / 1000.0f;
     ext_pos.z = item->z / 1000.0f;
     ext_pos.stdDev = extPosStdDev;
+    ext_pos.source = MeasurementSourceLocationService;
     if (item->id == my_id) {
+      updateLogFromExtPos();
       estimatorEnqueuePosition(&ext_pos);
       tickOfLastPacket = xTaskGetTickCount();
     }
@@ -363,13 +374,23 @@ void locSrvSendLighthouseAngle(int basestation, pulseProcessorResult_t* angles)
   }
 }
 
-
+// This logging group is deprecated
 LOG_GROUP_START(ext_pos)
   LOG_ADD(LOG_FLOAT, X, &ext_pos.x)
   LOG_ADD(LOG_FLOAT, Y, &ext_pos.y)
   LOG_ADD(LOG_FLOAT, Z, &ext_pos.z)
 LOG_GROUP_STOP(ext_pos)
 
+LOG_GROUP_START(locSrv)
+  LOG_ADD(LOG_FLOAT, x, &ext_pose.x)
+  LOG_ADD(LOG_FLOAT, y, &ext_pose.y)
+  LOG_ADD(LOG_FLOAT, z, &ext_pose.z)
+  LOG_ADD(LOG_FLOAT, qx, &ext_pose.quat.x)
+  LOG_ADD(LOG_FLOAT, qy, &ext_pose.quat.y)
+  LOG_ADD(LOG_FLOAT, qz, &ext_pose.quat.z)
+  LOG_ADD(LOG_FLOAT, qw, &ext_pose.quat.w)
+LOG_GROUP_STOP(locSrv)
+
 LOG_GROUP_START(locSrvZ)
   LOG_ADD(LOG_UINT16, tick, &tickOfLastPacket)  // time when data was received last (ms/ticks)
 LOG_GROUP_STOP(locSrvZ)

src/modules/src/estimator.c

@@ -11,7 +11,8 @@
 #include "estimator_kalman.h"
 #include "log.h"
 #include "statsCnt.h"
-
+#include "eventtrigger.h"
+#include "quatcompress.h"
 
 #define DEFAULT_ESTIMATOR complementaryEstimator
 static StateEstimatorType currentEstimator = anyEstimator;
@@ -26,6 +27,20 @@ STATIC_MEM_QUEUE_ALLOC(measurementsQueue, MEASUREMENTS_QUEUE_SIZE, sizeof(measur
 static STATS_CNT_RATE_DEFINE(measurementAppendedCounter, ONE_SECOND);
 static STATS_CNT_RATE_DEFINE(measurementNotAppendedCounter, ONE_SECOND);
 
+// events
+EVENTTRIGGER(estTDOA, uint8, idA, uint8, idB, float, distanceDiff)
+EVENTTRIGGER(estPosition, uint8, source)
+EVENTTRIGGER(estPose)
+EVENTTRIGGER(estDistance, uint8, id, float, distance)
+EVENTTRIGGER(estTOF)
+EVENTTRIGGER(estAbsoluteHeight)
+EVENTTRIGGER(estFlow)
+EVENTTRIGGER(estYawError, float, yawError)
+EVENTTRIGGER(estSweepAngle, uint8, sensorId, uint8, basestationId, uint8, sweepId, float, t, float, sweepAngle)
+EVENTTRIGGER(estGyroscope)
+EVENTTRIGGER(estAcceleration)
+EVENTTRIGGER(estBarometer)
+
 static void initEstimator(const StateEstimatorType estimator);
 static void deinitEstimator(const StateEstimatorType estimator);
 
@@ -144,6 +159,68 @@ void estimatorEnqueue(const measurement_t *measurement) {
   } else {
     STATS_CNT_RATE_EVENT(&measurementNotAppendedCounter);
   }
+
+  // events
+  switch (measurement->type) {
+    case MeasurementTypeTDOA:
+      eventTrigger_estTDOA_payload.idA = measurement->data.tdoa.anchorIds[0];
+      eventTrigger_estTDOA_payload.idB = measurement->data.tdoa.anchorIds[1];
+      eventTrigger_estTDOA_payload.distanceDiff = measurement->data.tdoa.distanceDiff;
+      eventTrigger(&eventTrigger_estTDOA);
+      break;
+    case MeasurementTypePosition:
+      // for additional data, see locSrv.{x,y,z} and lighthouse.{x,y,z}
+      eventTrigger_estPosition_payload.source = measurement->data.position.source;
+      eventTrigger(&eventTrigger_estPosition);
+      break;
+    case MeasurementTypePose:
+      // no payload needed, see locSrv.{x,y,z,qx,qy,qz,qw}
+      eventTrigger(&eventTrigger_estPose);
+      break;
+    case MeasurementTypeDistance:
+      eventTrigger_estDistance_payload.id = measurement->data.distance.anchorId;
+      eventTrigger_estDistance_payload.distance = measurement->data.distance.distance;
+      eventTrigger(&eventTrigger_estDistance);
+      break;
+    case MeasurementTypeTOF:
+      // no payload needed, see range.zrange
+      eventTrigger(&eventTrigger_estTOF);
+      break;
+    case MeasurementTypeAbsoluteHeight:
+      // no payload needed, see LPS_2D_POSITION_HEIGHT
+      eventTrigger(&eventTrigger_estAbsoluteHeight);
+      break;
+    case MeasurementTypeFlow:
+      // no payload needed, see motion.{deltaX,deltaY}
+      eventTrigger(&eventTrigger_estFlow);
+      break;
+    case MeasurementTypeYawError:
+      eventTrigger_estYawError_payload.yawError = measurement->data.yawError.yawError;
+      eventTrigger(&eventTrigger_estYawError);
+      break;
+    case MeasurementTypeSweepAngle:
+      eventTrigger_estSweepAngle_payload.sensorId = measurement->data.sweepAngle.sensorId;
+      eventTrigger_estSweepAngle_payload.basestationId = measurement->data.sweepAngle.basestationId;
+      eventTrigger_estSweepAngle_payload.sweepId = measurement->data.sweepAngle.sweepId;
+      eventTrigger_estSweepAngle_payload.t = measurement->data.sweepAngle.t;
+      eventTrigger_estSweepAngle_payload.sweepAngle = measurement->data.sweepAngle.measuredSweepAngle;
+      eventTrigger(&eventTrigger_estSweepAngle);
+      break;
+    case MeasurementTypeGyroscope:
+      // no payload needed, see gyro.{x,y,z}
+      eventTrigger(&eventTrigger_estGyroscope);
+      break;
+    case MeasurementTypeAcceleration:
+      // no payload needed, see acc.{x,y,z}
+      eventTrigger(&eventTrigger_estAcceleration);
+      break;
+    case MeasurementTypeBarometer:
+      // no payload needed, see baro.asl
+      eventTrigger(&eventTrigger_estBarometer);
+      break;
+    default:
+      break;
+  }
 }
 
 bool estimatorDequeue(measurement_t *measurement) {

src/modules/src/kalman_core/mm_tdoa.c

@@ -46,8 +46,8 @@ void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa)
     float y = this->S[KC_STATE_Y];
     float z = this->S[KC_STATE_Z];
 
-    float x1 = tdoa->anchorPosition[1].x, y1 = tdoa->anchorPosition[1].y, z1 = tdoa->anchorPosition[1].z;
-    float x0 = tdoa->anchorPosition[0].x, y0 = tdoa->anchorPosition[0].y, z0 = tdoa->anchorPosition[0].z;
+    float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
+    float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
 
     float dx1 = x - x1;
     float dy1 = y - y1;

src/modules/src/kalman_core/mm_tdoa_robust.c

@@ -72,8 +72,8 @@ void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *t
     float y = this->S[KC_STATE_Y];   
     float z = this->S[KC_STATE_Z];
 
-    float x1 = tdoa->anchorPosition[1].x, y1 = tdoa->anchorPosition[1].y, z1 = tdoa->anchorPosition[1].z;
-    float x0 = tdoa->anchorPosition[0].x, y0 = tdoa->anchorPosition[0].y, z0 = tdoa->anchorPosition[0].z;
+    float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
+    float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
 
     float dx1 = x - x1;   float  dy1 = y - y1;   float dz1 = z - z1;
     float dx0 = x - x0;   float  dy0 = y - y0;   float dz0 = z - z0;

src/modules/src/lighthouse/lighthouse_position_est.c

@@ -264,6 +264,7 @@ static void estimatePositionCrossingBeams(const pulseProcessor_t *state, pulsePr
     // Make sure we feed sane data into the estimator
     if (isfinite(ext_pos.pos[0]) && isfinite(ext_pos.pos[1]) && isfinite(ext_pos.pos[2])) {
       ext_pos.stdDev = 0.01;
+      ext_pos.source = MeasurementSourceLighthouse;
       estimatorEnqueuePosition(&ext_pos);
     }
   } else {
@@ -278,8 +279,10 @@ static void estimatePositionSweepsLh1(const pulseProcessor_t* appState, pulsePro
   sweepInfo.rotorPos = &appState->bsGeometry[baseStation].origin;
   sweepInfo.t = 0;
   sweepInfo.calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh1;
+  sweepInfo.basestationId = baseStation;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+    sweepInfo.sensorId = sensor;
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[baseStation];
     if (bsMeasurement->validCount == PULSE_PROCESSOR_N_SWEEPS) {
       sweepInfo.sensorPos = &sensorDeckPositions[sensor];
@@ -289,6 +292,7 @@ static void estimatePositionSweepsLh1(const pulseProcessor_t* appState, pulsePro
         sweepInfo.rotorRot = &appState->bsGeometry[baseStation].mat;
         sweepInfo.rotorRotInv = &appState->bsGeoCache[baseStation].baseStationInvertedRotationMatrixes;
         sweepInfo.calib = &bsCalib->sweep[0];
+        sweepInfo.sweepId = 0;
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
@@ -300,6 +304,7 @@ static void estimatePositionSweepsLh1(const pulseProcessor_t* appState, pulsePro
         sweepInfo.rotorRot = &appState->bsGeoCache[baseStation].lh1Rotor2RotationMatrixes;
         sweepInfo.rotorRotInv = &appState->bsGeoCache[baseStation].lh1Rotor2InvertedRotationMatrixes;
         sweepInfo.calib = &bsCalib->sweep[1];
+        sweepInfo.sweepId = 1;
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
@@ -317,8 +322,10 @@ static void estimatePositionSweepsLh2(const pulseProcessor_t* appState, pulsePro
   sweepInfo.rotorRot = &appState->bsGeometry[baseStation].mat;
   sweepInfo.rotorRotInv = &appState->bsGeoCache[baseStation].baseStationInvertedRotationMatrixes;
   sweepInfo.calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh2;
+  sweepInfo.basestationId = baseStation;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+    sweepInfo.sensorId = sensor;
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh2[sensor].baseStatonMeasurements[baseStation];
     if (bsMeasurement->validCount == PULSE_PROCESSOR_N_SWEEPS) {
       sweepInfo.sensorPos = &sensorDeckPositions[sensor];
@@ -327,6 +334,7 @@ static void estimatePositionSweepsLh2(const pulseProcessor_t* appState, pulsePro
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.t = -t30;
         sweepInfo.calib = &bsCalib->sweep[0];
+        sweepInfo.sweepId = 0;
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
         STATS_CNT_RATE_EVENT(&positionRate);
@@ -336,6 +344,7 @@ static void estimatePositionSweepsLh2(const pulseProcessor_t* appState, pulsePro
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.t = t30;
         sweepInfo.calib = &bsCalib->sweep[1];
+        sweepInfo.sweepId = 1;
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
         STATS_CNT_RATE_EVENT(&positionRate);

src/modules/src/outlierFilter.c

@@ -147,7 +147,7 @@ bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const fl
 
 
 static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(const tdoaMeasurement_t* tdoa) {
-  float anchorDistanceSq = distanceSq(&tdoa->anchorPosition[0], &tdoa->anchorPosition[1]);
+  float anchorDistanceSq = distanceSq(&tdoa->anchorPositions[0], &tdoa->anchorPositions[1]);
   float distanceDiffSq = sq(tdoa->distanceDiff);
   return (distanceDiffSq < anchorDistanceSq);
 }

src/utils/interface/tdoa/tdoaEngine.h

@@ -4,7 +4,7 @@
 #include "tdoaStorage.h"
 #include "tdoaStats.h"
 
-typedef void (*tdoaEngineSendTdoaToEstimator)(tdoaMeasurement_t* tdoaMeasurement, const uint8_t idA, const uint8_t idB);
+typedef void (*tdoaEngineSendTdoaToEstimator)(tdoaMeasurement_t* tdoaMeasurement);
 
 typedef enum {
   TdoaEngineMatchingAlgorithmNone = 0,

src/utils/src/tdoa/tdoaEngine.c

@@ -73,7 +73,7 @@ static void enqueueTDOA(const tdoaAnchorContext_t* anchorACtx, const tdoaAnchorC
     .distanceDiff = distanceDiff
   };
 
-  if (tdoaStorageGetAnchorPosition(anchorACtx, &tdoa.anchorPosition[0]) && tdoaStorageGetAnchorPosition(anchorBCtx, &tdoa.anchorPosition[1])) {
+  if (tdoaStorageGetAnchorPosition(anchorACtx, &tdoa.anchorPositions[0]) && tdoaStorageGetAnchorPosition(anchorBCtx, &tdoa.anchorPositions[1])) {
     STATS_CNT_RATE_EVENT(&stats->packetsToEstimator);
 
     uint8_t idA = tdoaStorageGetId(anchorACtx);
@@ -84,8 +84,10 @@ static void enqueueTDOA(const tdoaAnchorContext_t* anchorACtx, const tdoaAnchorC
     if (idB == stats->anchorId && idA == stats->remoteAnchorId) {
       stats->tdoa = -distanceDiff;
     }
+    tdoa.anchorIds[0] = idA;
+    tdoa.anchorIds[1] = idB;
 
-    engineState->sendTdoaToEstimator(&tdoa, idA, idB);
+    engineState->sendTdoaToEstimator(&tdoa);
   }
 }