EKF firmware understanding

Firmware/software/electronics/mechanics
Post Reply
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

EKF firmware understanding

Post by RyanMco »

Hi guys !
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;
}
Moreover, is our crazyflie 2.1 using PID for linear estimation or horizontal estimation? what's the purpose of using PID at all in our crazyflie?!


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!
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: EKF firmware understanding

Post by RyanMco »

Any help guys?!! thanks alot
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: EKF firmware understanding

Post by RyanMco »

Hi guys, once again .. any help please?!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: EKF firmware understanding

Post by arnaud »

Hi,
I am nor sure where to start so I will answer your questions in the order they appear in your message:
(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?
Crazyflie is an embedded system and we are using a real-time operating system (FreeRTOS) in the main processor. What you are looking at is running in a task and is looping forever until the power to the processor is cut. Other tasks runs in parallel and all the tasks in the Crazyflie will have a "while(1)" or equivalent somewhere. "M2T" is a macro that converts Milliseconds to OS Tick (OS tick is the time unit used by the FreeRTOS functions).

When you have doubt about a function in the firmware, it is good to search for it in the full project, the implementation is usually helpful. For example for M2T searching it in the github project allows to find it: https://github.com/bitcraze/crazyflie-f ... fig.h#L123

The row you copy-pasted are responsible for converting the raw sensor measurement from millimeter to meters and pushing the required data into the estimator. The standard deviation is calculated this way to ensure the ranging sensor is not used by the estimator when the measured range is above what the sensor can measure reliably.
Moreover, is our crazyflie 2.1 using PID for linear estimation or horizontal estimation? what's the purpose of using PID at all in our crazyflie?!
When using the flow deck Crazyflie is using an EKF for state estimation.
By default the controller used is implemented by a PID.
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 ?)
There is a stabilizer loop that runs in series the required functions to control the Crazyflies: https://github.com/bitcraze/crazyflie-f ... #L248-L300
The general sequence is: "Get sensor values" -> "Estimator" -> "Controller" -> "Power distribution". The Commander is responsible for giving new setpoints to the controller.

The onboard sensors (ie. The IMU) is fetched in series in the loop and put in a structure. Some sensors are pushed in a queue and pulled from within the estimator. This is the case for the flow/zranger sensors. Generally speaking you have to look and follow the code to understand how one particular sensor is handled.
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: EKF firmware understanding

Post by RyanMco »

meaning full answer ! thanks alot and you're very good instructor-straight forward!
Post Reply