Skip to content

Commit

Permalink
#290 Refactored and fixed automatic mode scanning
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Aug 20, 2019
1 parent 4244778 commit 62e0bf7
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 71 deletions.
6 changes: 3 additions & 3 deletions src/deck/drivers/interface/locodeck.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,12 @@ typedef struct {
volatile uint16_t rangingState;

// Requested and current ranging mode
lpsMode_t rangingMode;
lpsMode_t userRequestedMode;
lpsMode_t currentRangingMode;

// State of the ranging mode auto detection
bool rangingModeDetected;
bool autoStarted;
bool modeAutoSearchDoInitialize;
bool modeAutoSearchActive;
uint32_t nextSwitchTick;
} lpsAlgoOptions_t;

Expand Down
152 changes: 84 additions & 68 deletions src/deck/drivers/src/locodeck.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,20 +88,22 @@
// combinedAnchorPositionOk to enable sending the anchor rangings to the Kalman filter

static lpsAlgoOptions_t algoOptions = {
// .rangingMode is the wanted algorithm, available as a parameter
// .userRequestedMode is the wanted algorithm, available as a parameter
#if LPS_TDOA_ENABLE
.rangingMode = lpsMode_TDoA2,
.userRequestedMode = lpsMode_TDoA2,
#elif LPS_TDOA3_ENABLE
.rangingMode = lpsMode_TDoA3,
.userRequestedMode = lpsMode_TDoA3,
#elif defined(LPS_TWR_ENABLE)
.rangingMode = lpsMode_TWR,
.userRequestedMode = lpsMode_TWR,
#else
.rangingMode = lpsMode_auto,
.userRequestedMode = lpsMode_auto,
#endif
// .currentRangingMode is the currently running algorithm, available as a log
// -1 is an impossible mode which forces initialization of the requested mode
// lpsMode_auto is an impossible mode which forces initialization of the requested mode
// at startup
.currentRangingMode = -1,
.currentRangingMode = lpsMode_auto,
.modeAutoSearchActive = true,
.modeAutoSearchDoInitialize = true,
};

struct {
Expand Down Expand Up @@ -188,76 +190,90 @@ uint8_t locoDeckGetActiveAnchorIdList(uint8_t unorderedAnchorList[], const int m
return result;
}

static void uwbTask(void* parameters)
{
static bool switchToMode(const lpsMode_t newMode) {
bool result = false;

if (lpsMode_auto != newMode && newMode <= LPS_NUMBER_OF_ALGORITHMS) {
algoOptions.currentRangingMode = newMode;
algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;

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

result = true;
}

return result;
}

static void autoModeSearchTryMode(const lpsMode_t newMode, const uint32_t now) {
// Set up next time to check
algoOptions.nextSwitchTick = now + LPS_AUTO_MODE_SWITCH_PERIOD;
switchToMode(newMode);
}

static lpsMode_t autoModeSearchGetNextMode() {
lpsMode_t newMode = algoOptions.currentRangingMode + 1;
if (newMode > LPS_NUMBER_OF_ALGORITHMS) {
newMode = lpsMode_TWR;
}

return newMode;
}

static void processAutoModeSwitching() {
uint32_t now = xTaskGetTickCount();

if (algoOptions.modeAutoSearchActive) {
if (algoOptions.modeAutoSearchDoInitialize) {
autoModeSearchTryMode(lpsMode_TDoA2, now);
algoOptions.modeAutoSearchDoInitialize = false;
} else {
if (now > algoOptions.nextSwitchTick) {
if (algorithm->isRangingOk()) {
// We have found an algorithm, stop searching and lock to it.
algoOptions.modeAutoSearchActive = false;
DEBUG_PRINT("Automatic mode: detected %s\n", algorithmsList[algoOptions.currentRangingMode].name);
} else {
lpsMode_t newMode = autoModeSearchGetNextMode();
autoModeSearchTryMode(newMode, now);
}
}
}
}
}

static void resetAutoSearchMode() {
algoOptions.modeAutoSearchActive = true;
algoOptions.modeAutoSearchDoInitialize = true;
}

static void handleModeSwitch() {
if (algoOptions.userRequestedMode == lpsMode_auto) {
processAutoModeSwitching();
} else {
resetAutoSearchMode();
if (algoOptions.userRequestedMode != algoOptions.currentRangingMode) {
if (switchToMode(algoOptions.userRequestedMode)) {
DEBUG_PRINT("Switching to mode %s\n", algorithmsList[algoOptions.currentRangingMode].name);
}
}
}
}

static void uwbTask(void* parameters) {
lppShortQueue = xQueueCreate(10, sizeof(lpsLppShortPacket_t));

algoOptions.currentRangingMode = lpsMode_auto;

systemWaitStart();

while(1) {
// 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
xSemaphoreTake(algoSemaphore, portMAX_DELAY);
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_TDoA2;
algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
algorithm->init(dwm);
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_ALGORITHMS) {
algoOptions.currentRangingMode = 1;
} else {
algoOptions.currentRangingMode++;
}

algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
algorithm->init(dwm);
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_ALGORITHMS) {
DEBUG_PRINT("Trying to select wrong LPS algorithm, defaulting to TDoA2!\n");
algoOptions.currentRangingMode = algoOptions.rangingMode;
algorithm = algorithmsList[lpsMode_TDoA2].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);
timeout = algorithm->onEvent(dwm, eventTimeout);
}
handleModeSwitch();
xSemaphoreGive(algoSemaphore);

if (xSemaphoreTake(irqSemaphore, timeout/portTICK_PERIOD_MS)) {
if (xSemaphoreTake(irqSemaphore, timeout / portTICK_PERIOD_MS)) {
do{
xSemaphoreTake(algoSemaphore, portMAX_DELAY);
dwHandleInterrupt(dwm);
Expand Down Expand Up @@ -509,5 +525,5 @@ LOG_ADD(LOG_UINT8, mode, &algoOptions.currentRangingMode)
LOG_GROUP_STOP(loco)

PARAM_GROUP_START(loco)
PARAM_ADD(PARAM_UINT8, mode, &algoOptions.rangingMode)
PARAM_ADD(PARAM_UINT8, mode, &algoOptions.userRequestedMode)
PARAM_GROUP_STOP(loco)

0 comments on commit 62e0bf7

Please sign in to comment.