Bitcraze Forums

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