crazyflie uwbTask logic

[justinleeyang]

hi, arnaud:

I don't understand the main function of the following code:

Code: Select all

 // Change and init algorithm uppon request
    // The first time this loop enters, currentRangingMode is set to auto which forces
    // the initialization of the set algorithm
    if (algoOptions.rangingMode == lpsMode_auto) { // Auto switch
      if (algoOptions.rangingModeDetected == false) {
        if (algoOptions.currentRangingMode == lpsMode_auto) {
          // Initialize the algorithm, set time for next switch
          algoOptions.nextSwitchTick = xTaskGetTickCount() + LPS_AUTO_MODE_SWITCH_PERIOD;

          // Defaults to TDoA algorithm
          algoOptions.currentRangingMode = lpsMode_TDoA;
          algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
          algorithm->init(dwm, &algoOptions);
          timeout = algorithm->onEvent(dwm, eventTimeout);
        } else if (xTaskGetTickCount() > algoOptions.nextSwitchTick) {
          // Test if we have detected anchors
          if (algoOptions.autoStarted && algorithm->isRangingOk()) {
            algoOptions.rangingModeDetected = true;
            DEBUG_PRINT("Automatic mode: detected %s\n", algorithmsList[algoOptions.currentRangingMode].name);
          } else {
            // We have started the auto mode by initializing the next modes
            algoOptions.autoStarted = true;

            // Setting up next switching time
            algoOptions.nextSwitchTick = xTaskGetTickCount() + LPS_AUTO_MODE_SWITCH_PERIOD;

            // Switch to next algorithm!
            if ((algoOptions.currentRangingMode+1) > LPS_NUMBER_OF_ALGORITHM) {
              algoOptions.currentRangingMode = 1;
            } else {
              algoOptions.currentRangingMode++;
            }

            algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
            algorithm->init(dwm, &algoOptions);
            timeout = algorithm->onEvent(dwm, eventTimeout);
          }
        }
      }
    } else if (algoOptions.currentRangingMode != algoOptions.rangingMode) {  // Set modes
      // Reset auto mode
      algoOptions.rangingModeDetected = false;
      algoOptions.autoStarted = false;

      if (algoOptions.rangingMode < 1 || algoOptions.rangingMode > LPS_NUMBER_OF_ALGORITHM) {
        DEBUG_PRINT("Trying to select wrong LPS algorithm, defaulting to TDoA!\n");
        algoOptions.currentRangingMode = algoOptions.rangingMode;
        algorithm = algorithmsList[lpsMode_TDoA].algorithm;
      } else {
        algoOptions.currentRangingMode = algoOptions.rangingMode;
        algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
        DEBUG_PRINT("Switching mode to %s\n", algorithmsList[algoOptions.currentRangingMode].name);
      }

      algorithm->init(dwm, &algoOptions);
      timeout = algorithm->onEvent(dwm, eventTimeout);
    }

please help explain, thanks!

[arnaud]

The function is to detect the mode of the system: it tries TWR and TDoA in sequence until one of the mode is working. This allows us to build one Crazyflie 2.0 binary that works with both TDoA and TWR, no need of compile flag anymore .

[justinleeyang]

Got it! if I want to force one mode, how to define?

[arnaud]

The same compile flag as before should work. One new flash has been added to force TWR: https://github.com/bitcraze/crazyflie-f ... #L118-L125

[justinleeyang]

sorry, is it currentRangingMode ?

[arnaud]

If ranging mode is lpsMode_auto then the mode will be detected (see block of code you copy-pasted earlier). By defining LPS_TDOA_ENABLE or LPS_TWR_ENABLE you can set tdoa or twr by default which will bypass the automatic detection. You can add defines in tools/make/config.mk.

[justinleeyang]

yeah, thanks!