Code: Select all
In zranger2.c change to this:
void zRanger2Task(void* arg)
{
TickType_t lastWakeTime;
systemWaitStart();
// Restart sensor
VL53L1_StopMeasurement(&dev);
// Change to short range mode
VL53L1_SetDistanceMode(&dev, VL53L1_DISTANCEMODE_SHORT);
VL53L1_StartMeasurement(&dev);
lastWakeTime = xTaskGetTickCount();
while (1) {
vTaskDelayUntil(&lastWakeTime, M2T(100));
range_last = zRanger2GetMeasurementAndRestart(&dev);
rangeSet(rangeDown, range_last / 1000.0f);
// check if range is feasible and push into the kalman filter
// the sensor should not be able to measure >5 [m], and outliers typically
// occur as >8 [m] measurements
if (getStateEstimator() == kalmanEstimator &&
range_last < RANGE_OUTLIER_LIMIT) {
// Form measurement
tofMeasurement_t tofData;
tofData.timestamp = xTaskGetTickCount();
tofData.distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
tofData.stdDev = expStdA * (1.0f + expf( expCoeff * ( tofData.distance - expPointA)));
estimatorKalmanEnqueueTOF(&tofData);
}
}
}