Mercurial > public > ostc4
view Discovery/Src/motion.c @ 474:4be72d55b09a Improve_Button_Sleep
Added error detection for reading of ADC values in sleep mode:
ADC value were used for pressure calculation without making sure that the I2C operations used for reading the values were called successfully. As result a invalid pressure could be calculated causing the OSTC to wakeup or (worstcase) to enter dive mode. To avoid this the return values are now evaluated and pressure calculation is only done if I2C signaled no error during transmission
author | ideenmodellierer |
---|---|
date | Tue, 12 May 2020 22:42:52 +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; }