Flow Deck V2 Not Stable

Post here to get support
tobias
Bitcraze
Posts: 2339
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Flow Deck V2 Not Stable

Post by tobias »

There might be a software fix for this by putting the sensor in short range mode which potentially consumes less current. In zranger2.c change to this:

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);
    }
  }
}
Post Reply