Mercurial > public > ostc4
view Discovery/Src/motion.c @ 519:2c49561c8062
Reset Safety Stop display in case of a new descend:
In previous version safety stop was displayed in while the stop depth was reached. The count down was interrupted in case of a new descend but the safety stop was still displayed, blocking the NDL visualization in the big font view. To avoid this scenario a restart function has been added which will return to NDL view in case of a descend > 2m. If just a short drop happend then the countdown will be continued when the safety stop depth is reached again. If that takes more then a minute the safety stop counter is restarted.
author | Ideenmodellierer |
---|---|
date | Sun, 06 Sep 2020 22:11:51 +0200 |
parents | 0cd862e501f6 |
children | e3237f580ae9 |
line wrap: on
line source
/* * motion.c * * Created on: 20.05.2019 * Author: Thorsten Sonntag */ #include <stdint.h> #include <string.h> #include <stdlib.h> #include <math.h> #include "motion.h" #include "data_central.h" #include "t7.h" #include "t3.h" #include "settings.h" #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */ #define SECTOR_WINDOW 80.0 /* Pitch window which is used for custom view projection */ #define SECTOR_WINDOW_MAX 120.0 /* Pitch window which will be greater than the divers field of view */ #define SECTOR_HYSTERY 2 /* Additional offset to avoid fast changing displays */ #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */ #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */ #define SECTOR_MAX 24 /* maximum number of sectors */ #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ detectionState_t detectionState = DETECT_NOTHING; SSector sectorDetection; uint8_t GetSectorForPitch(float pitch) { static uint8_t lastsector = 0; float newPitch; uint8_t sector = 0; 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 > sectorDetection.window) /* clip value */ { newPitch = sectorDetection.window; } /* switch to other sector? */ if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder)) { sector = (uint16_t) newPitch / sectorDetection.size; sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY; sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY; lastsector = sector; } return lastsector; } void DefinePitchSectors(float centerPitch,uint8_t numOfSectors) { if(numOfSectors == CUSTOMER_DEFINED_VIEWS) { if(settingsGetPointer()->design == 3) /* Big font view ? */ { sectorDetection.count = t3_GetEnabled_customviews(); } else { sectorDetection.count = t7_GetEnabled_customviews(); } if(sectorDetection.count > 7) { sectorDetection.count = 7; /* more views are hard to manually control */ } } else if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) { sectorDetection.count = numOfSectors; } if(sectorDetection.count == SECTOR_MAX) { sectorDetection.window = SECTOR_WINDOW_MAX; } else { sectorDetection.window = SECTOR_WINDOW; } 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 */ 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) { sectorDetection.target = 0; sectorDetection.current = 0; sectorDetection.size = 0; sectorDetection.count = 0; switch(settingsGetPointer()->MotionDetection) { case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS); break; case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX); break; case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL); break; default: break; } } /* Map the current pitch value to a sector and create button event in case the sector is left */ detectionState_t detectSectorButtonEvent(float curPitch) { static uint8_t lastTargetSector = 0; uint8_t newTargetSector; uint8_t PitchEvent = DETECT_NOTHING; /* only change sector if reading is stable */ newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); if(lastTargetSector == newTargetSector) { sectorDetection.target = newTargetSector; } lastTargetSector = newTargetSector; if(sectorDetection.target != sectorDetection.current) { if(sectorDetection.target > sectorDetection.current) { sectorDetection.current++; PitchEvent = DETECT_POS_PITCH; } else { sectorDetection.current--; PitchEvent = DETECT_NEG_PITCH; } } return PitchEvent; } /* Check if pitch is not in center position and trigger a button action if needed */ detectionState_t detectScrollButtonEvent(float curPitch) { static uint8_t delayscroll = 0; /* slow down the number of scroll events */ uint8_t PitchEvent = DETECT_NOTHING; uint8_t newSector; if(delayscroll == 0) { newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); /* for scroll detection the motion windoe is split into 6 sectors => set event accoring to the sector number*/ switch(newSector) { case 0: case 1: PitchEvent = DETECT_POS_PITCH; break; case 5: case 6: PitchEvent = DETECT_NEG_PITCH; break; default: break; } if(PitchEvent != DETECT_NOTHING) { delayscroll = 5; } } else { delayscroll--; } return PitchEvent; } /* Detect if user is generating an pitch including return to starting position */ /* This is done by feeding the past movements value per value into a state machine */ detectionState_t detectPitch(float currentPitch) { static uint8_t lastSector = 0; 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; } curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); /* feed value into state machine */ switch (detectionState) { case DETECT_NOTHING: if(curSector != lastSector) /* detect a stable condition before evaluating for the next move */ { stableCnt=0; } if(stableCnt > STABLE_STATE_COUNT) { detectionState = DETECT_START; stableCnt = 0; startSector = lastSector; } break; case DETECT_START: if(curSector != lastSector) { if(abs(curSector - startSector) > 1) { if(curSector > lastSector) { detectionState = DETECT_POS_MOVE; } else { detectionState = DETECT_NEG_MOVE; } stableCnt = 0; startSector = lastSector; } } break; case DETECT_NEG_MOVE: case DETECT_POS_MOVE: if(curSector == lastSector) /* Moved to a max? */ { if(abs(startSector - curSector) > 2) { detectionState++; stableCnt = 0; } if(stableCnt > 2) { detectionState = DETECT_NOTHING; stableCnt = 0; } } break; case DETECT_MAXIMA: case DETECT_MINIMA: if(curSector != lastSector) /* reset timeout detection */ { detectionState++; stableCnt = 0; } break; case DETECT_RISEBACK: case DETECT_FALLBACK: if(curSector == lastSector) /* check if we are back at start position at end of movement */ { if(abs(startSector - curSector) <= 1) { if(stableCnt > 2) { detectionState++; stableCnt = 0; } } } break; default: detectionState = DETECT_NOTHING; break; } if(detectionState != DETECT_START) { stableCnt++; } lastSector = curSector; if(stableCnt > STABLE_STATE_TIMEOUT) { detectionState = DETECT_NOTHING; stableCnt = 0; } return detectionState; }