I've downloaded firmware 2019 and I want to understand some of its code and how they work in aspect of getting information (height measurement) from flowdeck.
I know already that the height measurement is done in the z-ranger deck driver and pushed into the estimator ; but didn't understand well those two function in the z.ranger deck:
what's in while (1) is running infinity looping(I've read the code and it imply to me it's infinity and stuck in the code -that's wrong ofcourse ! but why?! ), when it stops or gets out from the while (1) ? and what does M2T(100) means? what's the role of those rows :
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);
what's the purpose of the second function " zRanger2ReadRange(zDistance_t* zrange, const uint32_t tick)" ?
in brief I'm asking about every row in that code what it does?
here's the link of my z-ranger deck https://github.com/bitcraze/crazyflie-f ... er2.c#L146 ,
attaching from it the code I'm asking about:
Code: Select all
while (1) {
vTaskDelayUntil(&lastWakeTime, M2T(100));
range_last = zRanger2GetMeasurementAndRestart(&dev);
// 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);
}
}
}
bool zRanger2ReadRange(zDistance_t* zrange, const uint32_t tick)
{
bool updated = false;
if (isInit) {
if (range_last != 0 && range_last < RANGE_OUTLIER_LIMIT) {
zrange->distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
zrange->timestamp = tick;
updated = true;
}
}
return updated;
}
second question, is our EKF , Z-ranger, and PID blocks of drivers are working in serial or in parallel? I mean lets assume I have output from Z-ranger which it's the height measurement, so now the EKF will be off until Z-ranger push its measurement data (push its height measurement) ? if the blocks of drivers EKF,Z-RANGER,PID aren't working in serial .. so there must be a Q memory which like Priority Queue or something like that .. am I right? I don't know how the pattern/process of work of those blocks EKF , Z-ranger, and PID blocks(in other words I'm asking how the topology connection between EKF-Z-RANGER-PID works in serial/parallel ?)
thanks alot guys!