Mercurial > public > ostc4
changeset 383:49a02dea8ae3 MotionDetection
Combine variables in a structure to improve readability of code
Add function to compensate offsets from calculated to real sector center position
author | ideenmodellierer |
---|---|
date | Thu, 17 Oct 2019 21:15:42 +0200 |
parents | 14fd5f35cb50 |
children | 427ae9f8e28e |
files | Discovery/Inc/motion.h Discovery/Src/motion.c |
diffstat | 2 files changed, 52 insertions(+), 46 deletions(-) [+] |
line wrap: on
line diff
--- a/Discovery/Inc/motion.h Thu Oct 10 22:26:03 2019 +0200 +++ b/Discovery/Inc/motion.h Thu Oct 17 21:15:42 2019 +0200 @@ -38,10 +38,22 @@ typedef struct { - float upperlimit; - float lowerlimit; + float upperborder; /* current sector is changed if pitch exceeds these borders */ + float lowerborder; + + uint8_t current; /* number of the current visible sector */ + uint8_t target; /* used for sector switch => number of the sector which shall be finnaly displayed */ + + float offset; /* offset to adjust minimum pitch value used for detection to zero */ + float size; /* delta of upper and lower boarder defining the sector in degree */ + float window; /* defines which range of pitch values are used for detection */ + float center; /* defines the offset from pitch to middle of active sector (avoid center position is close to the calculated borders) */ + uint8_t count; /* number of sectors used for detection */ } SSector; + + + void InitMotionDetection(void); void DefinePitchSectors(float centerAngle, uint8_t numOfSectors); detectionState_t detectPitch(float currentPitch);
--- a/Discovery/Src/motion.c Thu Oct 10 22:26:03 2019 +0200 +++ b/Discovery/Src/motion.c Thu Oct 17 21:15:42 2019 +0200 @@ -27,47 +27,31 @@ #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ static detectionState_t detectionState = DETECT_NOTHING; - -uint8_t curSector; -static uint8_t targetSector; -static float sectorSize; -static float sectorwindow; -static uint8_t sectorCount; - -static float sector_upperborder; -static float sector_lowerborder; +static SSector sectorDetection; uint8_t GetSectorForPitch(float pitch) { - static uint8_t lastsector = 0xFF; + static uint8_t lastsector = 0; float newPitch; uint8_t sector = 0; - - newPitch = pitch + (sectorwindow / 2.0); /* do not use negativ values */ + newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */ if (newPitch < 0.0) /* clip value */ { newPitch = 0.0; } - if (newPitch > sectorwindow) /* clip value */ + if (newPitch > sectorDetection.window) /* clip value */ { - newPitch = sectorwindow; - } - - if(lastsector == 0xFF) /* First call of function => make sure a new sector is set */ - { - sector_lowerborder = SECTOR_BORDER; - sector_upperborder = SECTOR_BORDER * -1.0; - + newPitch = sectorDetection.window; } /* switch to other sector? */ - if((newPitch > sector_upperborder) || (newPitch <= sector_lowerborder)) + if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder)) { - sector = (uint16_t) newPitch / sectorSize; - sector_lowerborder = sector * sectorSize - SECTOR_HYSTERY; - sector_upperborder = (sector + 1) * sectorSize + SECTOR_HYSTERY; + sector = (uint16_t) newPitch / sectorDetection.size; + sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY; + sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY; lastsector = sector; } @@ -78,40 +62,48 @@ { if(numOfSectors == CUSTOMER_DEFINED_VIEWS) { - sectorCount = t7_GetEnabled_customviews(); - if(sectorCount > 7) + sectorDetection.count = t7_GetEnabled_customviews(); + if(sectorDetection.count > 7) { - sectorCount = 7; /* more views are hard to manually control */ + sectorDetection.count = 7; /* more views are hard to manually control */ } } else if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) { - sectorCount = numOfSectors; + sectorDetection.count = numOfSectors; } - if(sectorCount == SECTOR_MAX) + if(sectorDetection.count == SECTOR_MAX) { - sectorwindow = SECTOR_WINDOW_MAX; + sectorDetection.window = SECTOR_WINDOW_MAX; } else { - sectorwindow = SECTOR_WINDOW; + sectorDetection.window = SECTOR_WINDOW; } - sectorSize = sectorwindow / sectorCount; + sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0; + sectorDetection.size = sectorDetection.window / sectorDetection.count; + sectorDetection.center = 0; +/* reset border values */ + sectorDetection.lowerborder = SECTOR_BORDER; + sectorDetection.upperborder = SECTOR_BORDER * -1.0; /* get the current sector */ - curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); - targetSector = curSector; + sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); + sectorDetection.target = sectorDetection.current; +/* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */ + sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset); + } void InitMotionDetection(void) { - targetSector = 0; - curSector = 0; - sectorSize = 0; - sectorCount = 0; + sectorDetection.target = 0; + sectorDetection.current = 0; + sectorDetection.size = 0; + sectorDetection.count = 0; switch(settingsGetPointer()->MotionDetection) { @@ -138,19 +130,19 @@ newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); if(lastTargetSector == newTargetSector) { - targetSector = newTargetSector; + sectorDetection.target = newTargetSector; } lastTargetSector = newTargetSector; - if(targetSector != curSector) + if(sectorDetection.target != sectorDetection.current) { - if(targetSector > curSector) + if(sectorDetection.target > sectorDetection.current) { - curSector++; + sectorDetection.current++; PitchEvent = DETECT_POS_PITCH; } else { - curSector--; + sectorDetection.current--; PitchEvent = DETECT_NEG_PITCH; } } @@ -203,6 +195,8 @@ static uint8_t startSector = 0; static uint8_t stableCnt = 0; + uint8_t curSector; + if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */ { detectionState = DETECT_NOTHING;