Merge pull request bitcraze#751 from bitcraze/dev-kalman-eventtriggers

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/deck/drivers/src/usddeck.c

@@ -87,8 +87,8 @@
 #define SPI_END_TRANSACTION     spiEndTransaction
 #endif
 
-#define MAX_USD_LOG_VARIABLES_PER_EVENT   (50)
-#define MAX_USD_LOG_EVENTS                (10)
+#define MAX_USD_LOG_VARIABLES_PER_EVENT   (20)
+#define MAX_USD_LOG_EVENTS                (20)
 #define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
 #define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
 
@@ -572,9 +572,11 @@ static void usdLogTask(void* prm)
       // loop over event triggers "on:<name>"
       usdLogConfig.numEventConfigs = 0;
       usdLogConfig.fixedFrequencyEventIdx = MAX_USD_LOG_EVENTS;
+      usdLogConfig.frequency = 10; // use non-zero default value for task loop below
+      usdLogEventConfig_t *cfg = &usdLogConfig.eventConfigs[0];
+      const char* eventName = 0;
       line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
-      while (line && usdLogConfig.numEventConfigs < MAX_USD_LOG_EVENTS) {
-        usdLogEventConfig_t* cfg = &usdLogConfig.eventConfigs[usdLogConfig.numEventConfigs];
+      while (line) {
         if (strncmp(line, "on:", 3) == 0) {
           // special mode for non-event-based logging
           if (strcmp(&line[3], FIXED_FREQUENCY_EVENT_NAME) == 0) {
@@ -587,12 +589,14 @@ static void usdLogTask(void* prm)
             if (!line) break;
             usdLogConfig.mode = strtol(line, &endptr, 10);
             cfg->eventId = FIXED_FREQUENCY_EVENT_ID;
+            eventName = FIXED_FREQUENCY_EVENT_NAME;
             usdLogConfig.fixedFrequencyEventIdx = usdLogConfig.numEventConfigs;
           } else {
             // handle event triggers
             const eventtrigger *et = eventtriggerGetByName(&line[3]);
             if (et) {
               cfg->eventId = eventtriggerGetId(et);
+              eventName = et->name;
             } else {
               DEBUG_PRINT("Unknown event %s\n", &line[3]);
               line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
@@ -603,7 +607,7 @@ static void usdLogTask(void* prm)
           // Add log variables
           cfg->numVars = 0;
           cfg->numBytes = 0;
-          while (cfg->numVars < MAX_USD_LOG_VARIABLES_PER_EVENT) {
+          while (true) {
             line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
             if (!line || strncmp(line, "on:", 3) == 0)
               break;
@@ -622,11 +626,24 @@ static void usdLogTask(void* prm)
               DEBUG_PRINT("Unknown log variable %s.%s\n", group, name);
               continue;
             }
-            cfg->varIds[cfg->numVars] = varid;
-            ++cfg->numVars;
-            cfg->numBytes += logVarSize(logGetType(varid));
+            if (cfg->numVars < MAX_USD_LOG_VARIABLES_PER_EVENT) {
+              cfg->varIds[cfg->numVars] = varid;
+              ++cfg->numVars;
+              cfg->numBytes += logVarSize(logGetType(varid));
+            } else {
+              DEBUG_PRINT("Skip log variable %s: %s.%s (out of storage)\n", eventName, group, name);
+              continue;
+            }
+          }
+          if (usdLogConfig.numEventConfigs < MAX_USD_LOG_EVENTS - 1) {
+            ++usdLogConfig.numEventConfigs;
+            cfg = &usdLogConfig.eventConfigs[usdLogConfig.numEventConfigs];
+          } else {
+            DEBUG_PRINT("Skip config after event %s (out of storage)\n", eventName);
+            break;
           }
-          ++usdLogConfig.numEventConfigs;
+        } else {
+          line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
         }
       }
       f_close(&logFile);

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;