diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c index 6cdd9b4133..a49993f789 100644 --- a/src/deck/drivers/src/lpsTdoa2Tag.c +++ b/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; } diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c index 891380f881..51b0a1038e 100644 --- a/src/deck/drivers/src/lpsTdoa3Tag.c +++ b/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 diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c index 73f53581d0..d7a78b571a 100644 --- a/src/deck/drivers/src/lpsTwrTag.c +++ b/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); } diff --git a/src/modules/interface/range.h b/src/modules/interface/range.h index 34b61bcc3c..3fa8708ef8 100644 --- a/src/modules/interface/range.h +++ b/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 . * - * 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) */ diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 834223212c..1b225110ce 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/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; diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index 7915a2bffa..d4908b5151 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -154,6 +154,13 @@ 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; @@ -161,6 +168,8 @@ static void extPositionHandler(CRTPPacket* pk) { 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) diff --git a/src/modules/src/estimator.c b/src/modules/src/estimator.c index 43dbe21d8a..d327608a68 100644 --- a/src/modules/src/estimator.c +++ b/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) { diff --git a/src/modules/src/kalman_core/mm_tdoa.c b/src/modules/src/kalman_core/mm_tdoa.c index e9626d9ecd..761e0e4b01 100644 --- a/src/modules/src/kalman_core/mm_tdoa.c +++ b/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; diff --git a/src/modules/src/kalman_core/mm_tdoa_robust.c b/src/modules/src/kalman_core/mm_tdoa_robust.c index 79d53c8483..bd6f15caf4 100644 --- a/src/modules/src/kalman_core/mm_tdoa_robust.c +++ b/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; diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index 4d223ae2bd..851740dfc2 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/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); diff --git a/src/modules/src/outlierFilter.c b/src/modules/src/outlierFilter.c index 7179845421..a6fab9f948 100644 --- a/src/modules/src/outlierFilter.c +++ b/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); } diff --git a/src/utils/interface/tdoa/tdoaEngine.h b/src/utils/interface/tdoa/tdoaEngine.h index b48dac326f..8406278818 100644 --- a/src/utils/interface/tdoa/tdoaEngine.h +++ b/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, diff --git a/src/utils/src/tdoa/tdoaEngine.c b/src/utils/src/tdoa/tdoaEngine.c index 928e73316f..4c2bd22f0b 100644 --- a/src/utils/src/tdoa/tdoaEngine.c +++ b/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); } } diff --git a/test/modules/src/kalman_core/test_mm_tdoa.c b/test/modules/src/kalman_core/test_mm_tdoa.c index 523e24ada4..e10075a7bc 100644 --- a/test/modules/src/kalman_core/test_mm_tdoa.c +++ b/test/modules/src/kalman_core/test_mm_tdoa.c @@ -42,7 +42,7 @@ void testThatScalarUpdateIsCalledInSimpleCase() { expectedHm[KC_STATE_Z] = 0.0; tdoaMeasurement_t measurement = { - .anchorPosition = { + .anchorPositions = { {.x = -1.0, .y = 0.0, .z = 0.0}, {.x = 1.0, .y = 0.0, .z = 0.0}, }, @@ -68,7 +68,7 @@ void testThatSampleWhereDroneIsInSamePositionAsAnchorIsIgnored() { this.S[KC_STATE_Z] = 0.0; tdoaMeasurement_t measurement = { - .anchorPosition = { + .anchorPositions = { {.x = -1.0, .y = 0.0, .z = 0.0}, {.x = 1.0, .y = 0.0, .z = 0.0}, }, @@ -91,7 +91,7 @@ void testThatScalarUpdateIsNotCalledWhenTheOutlierFilterIsBlocking() { this.S[KC_STATE_Z] = 0.0; tdoaMeasurement_t measurement = { - .anchorPosition = { + .anchorPositions = { {.x = -1.0, .y = 0.0, .z = 0.0}, {.x = 1.0, .y = 0.0, .z = 0.0}, }, diff --git a/test/modules/src/test_outlier_filter.c b/test/modules/src/test_outlier_filter.c index f595730b8c..a2a3b28c35 100644 --- a/test/modules/src/test_outlier_filter.c +++ b/test/modules/src/test_outlier_filter.c @@ -14,13 +14,13 @@ uint32_t fixtureOpenLhFilter(OutlierFilterLhState_t* this); void setUp(void) { // TDoA filter - tdoa.anchorPosition[0].x = 1.0; - tdoa.anchorPosition[0].y = 1.0; - tdoa.anchorPosition[0].z = 1.0; + tdoa.anchorPositions[0].x = 1.0; + tdoa.anchorPositions[0].y = 1.0; + tdoa.anchorPositions[0].z = 1.0; - tdoa.anchorPosition[1].x = 5.0; - tdoa.anchorPosition[1].y = 1.0; - tdoa.anchorPosition[1].z = 1.0; + tdoa.anchorPositions[1].x = 5.0; + tdoa.anchorPositions[1].y = 1.0; + tdoa.anchorPositions[1].z = 1.0; tdoa.distanceDiff = 0.0; } diff --git a/tools/usdlog/config_kalman.txt b/tools/usdlog/config_kalman.txt new file mode 100644 index 0000000000..62802bc5cf --- /dev/null +++ b/tools/usdlog/config_kalman.txt @@ -0,0 +1,39 @@ +1 # version +2048 # buffer size in bytes +log # file name +0 # enable on startup (0/1) +on:estTDOA +on:estPosition +locSrv.x +locSrv.y +locSrv.z +lighthouse.x +lighthouse.y +lighthouse.z +on:estPose +locSrv.x +locSrv.y +locSrv.z +locSrv.qx +locSrv.qy +locSrv.qz +locSrv.qw +on:estDistance +range.zrange +on:estTOF +on:estAbsoluteHeight +on:estFlow +motion.deltaX +motion.deltaY +on:estYawError +on:estSweepAngle +on:estGyroscope +gyro.x +gyro.y +gyro.z +on:estAcceleration +acc.x +acc.y +acc.z +on:estBarometer +baro.asl