Mercurial > public > ostc4
view Discovery/Src/motion.c @ 862:974648b5ccfe Evo_2_23
Only use deco gas for calculation if option is enabled:
The gas menu meanwhile provides the possibility to select a gas for deco (having the gas switching reminder visible) without the need to visualize the reduced deco time. If the visualization of the deco considering all gas changes is wanted then this may be enabled by selecting the gas additionaly for deco calculation.
author | Ideenmodellierer |
---|---|
date | Tue, 02 Jul 2024 19:24:03 +0200 |
parents | ff2b393e290f |
children |
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" #include "base.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_MAX 24 /* maximum number of sectors */ #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ #define SECTOR_MAX_CNT 5 /* max number of views used for sector control */ typedef enum { MOTION_DELTA_STABLE = 0, MOTION_DELTA_JITTER, MOTION_DELTA_RAISE, MOTION_DELTA_RAISE_FAST, MOTION_DELTA_FALL, MOTION_DELTA_FALL_FAST } MotionDeltaState_t; #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */ #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */ #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */ #define MOTION_DELTA_FAST_LEVEL 6.0 /* Movement causing a fast change detected */ #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */ #define MOTION_FOCUS_LIMIT 0.5 /* +/- value which defines the border of the focus area */ #define MOTION_FOCUS_USE_SECTOR 0.4 /* +/- value for the focus area used to map secors to views */ #define MOTION_FOCUS_SCROLL_IDLE 0.3 /* +/- value for starting generation of scroll events */ detectionState_t detectionState = DETECT_NOTHING; SSector sectorDetection; static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */ static uint8_t motionDeltaHistoryIdx; /* Current index of history data */ static uint8_t focusCnt = 0; static uint8_t inFocus = 0; static uint8_t sectorMap[SECTOR_MAX_CNT]; static uint8_t suspendMotionDetectionSec = 0; void resetMotionDeltaHistory() { motionDeltaHistoryIdx = 0; memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory)); } void evaluateMotionDelta(float roll, float pitch, float yaw) { static float lastValue[3] = {0.0,0.0,0.0}; uint8_t nextIndex = motionDeltaHistoryIdx + 1; uint8_t axis; float curValue; if(nextIndex == MOTION_DELTA_HISTORY_SIZE) { nextIndex = 0; } for(axis=0; axis < 3; axis++) { switch(axis) { case MOTION_HISTORY_ROLL: curValue = roll; break; case MOTION_HISTORY_PITCH: curValue = pitch; break; default: case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */ { lastValue[MOTION_HISTORY_YAW] -= 360; } else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */ { lastValue[MOTION_HISTORY_YAW] += 360; } curValue = yaw; break; } if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL) { motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE; } if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL) { motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER; } if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL) { motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE; } if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL) { motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL; } if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL) { motionDeltaHistory[axis][nextIndex]++; } lastValue[axis] = curValue; } motionDeltaHistoryIdx = nextIndex; } SDeltaHistory GetDeltaHistory(uint8_t stepback) { uint8_t loop; uint8_t index = motionDeltaHistoryIdx; SDeltaHistory result = {0,0,0}; loop = stepback + 1; /* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */ if(stepback < MOTION_DELTA_HISTORY_SIZE) { while(loop != 0) /* find requested entry */ { loop--; index--; if(index == 0) { index = MOTION_DELTA_HISTORY_SIZE - 1; } } result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index]; result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index]; result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index]; } return result; } uint8_t GetSectorForFocus(float focusOffset) { uint8_t sector = 0; float compare = -1.0 * MOTION_FOCUS_USE_SECTOR + sectorDetection.size ; /* start with first sector upper limit */ while(compare <= MOTION_FOCUS_USE_SECTOR) { if(focusOffset > compare) { sector++; } else { break; } compare += sectorDetection.size; } if(sector >= sectorDetection.count) { sector = sectorDetection.count - 1; } return sector; } void DefineSectorCount(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 > SECTOR_MAX_CNT) { sectorDetection.count = SECTOR_MAX_CNT; /* more views are hard to manually control */ } } else if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) { sectorDetection.count = numOfSectors; } sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count; } uint8_t GetCVForSector(uint8_t selSector) { if(selSector < sectorDetection.count) { return sectorMap[selSector]; } else { return 0; } } void MapCVToSector() { uint8_t ViewIndex = 0; memset(sectorMap, 0, sizeof(sectorMap)); while(ViewIndex < (sectorDetection.count / 2)) /* define center sector */ { ViewIndex++; } if(settingsGetPointer()->design == 3) /* Big font view ? */ { t3_set_customview_to_primary(); sectorMap[ViewIndex] = t3_change_customview(ACTION_END); } else { t7_set_customview_to_primary(); sectorMap[ViewIndex] = t7_change_customview(ACTION_END); } ViewIndex++; while(sectorMap[ViewIndex] == 0) { if(settingsGetPointer()->design == 3) /* Big font view ? */ { sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER); } else { sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER); } ViewIndex++; if(ViewIndex == sectorDetection.count) { ViewIndex = 0; } } } void InitMotionDetection(void) { sectorDetection.target = 0; sectorDetection.current = 0; sectorDetection.size = 0; sectorDetection.count = 0; switch(settingsGetPointer()->MotionDetection) { case MOTION_DETECT_SECTOR: DefineSectorCount(CUSTOMER_DEFINED_VIEWS); MapCVToSector(); break; case MOTION_DETECT_MOVE: DefineSectorCount(SECTOR_MAX); break; case MOTION_DETECT_SCROLL: DefineSectorCount(SECTOR_SCROLL); break; default: break; } resetMotionDeltaHistory(); } /* Map the current pitch value to a sector and create button event in case the sector is left */ detectionState_t detectSectorButtonEvent(float focusOffset) { static uint8_t lastTargetSector = 0xFF; static float lastfocusOffset = 1000.0; uint8_t newTargetSector; newTargetSector = GetSectorForFocus(focusOffset); /* take a small hysteresis into account to avoid fast display changes (flicker) */ if((newTargetSector != lastTargetSector) && (fabsf(focusOffset - lastfocusOffset) > (sectorDetection.size / 3))) { lastfocusOffset = focusOffset; lastTargetSector = newTargetSector; if(settingsGetPointer()->design == 3) /* Big font view ? */ { t3_select_customview(GetCVForSector(newTargetSector)); } else { t7_select_customview(GetCVForSector(newTargetSector)); } } return DETECT_NOTHING; } /* Check if pitch is not in center position and trigger a button action if needed */ detectionState_t detectScrollButtonEvent(float focusOffset) { static uint8_t delayscroll = 0; /* slow down the number of scroll events */ uint8_t PitchEvent = DETECT_NOTHING; if(delayscroll == 0) { if(focusOffset > MOTION_FOCUS_SCROLL_IDLE) { PitchEvent = DETECT_POS_PITCH; delayscroll = 7; } if(focusOffset < (-1.0 * MOTION_FOCUS_SCROLL_IDLE)) { PitchEvent = DETECT_NEG_PITCH; delayscroll = 7; } } 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 int8_t lastStart = 0; uint8_t exit = 0; int8_t step = 0; uint8_t duration = 0; SDeltaHistory test; if(lastStart < 0) { detectionState = DETECT_NOTHING; lastStart = 0; } else { detectionState = DETECT_START; } step = lastStart; do { test = GetDeltaHistory(step); duration++; switch (detectionState) { case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE) { exit = 1; lastStart = -2; } else { detectionState = DETECT_START; lastStart = -1; } break; case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE) { detectionState = DETECT_POS_MOVE; lastStart = step; } else if(test.pitch == MOTION_DELTA_FALL) { detectionState = DETECT_NEG_MOVE; lastStart = step; } else { lastStart = -1; } duration = 0; break; case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE) || (test.pitch == MOTION_DELTA_RAISE_FAST)) { detectionState++; } break; case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL) || (test.pitch == MOTION_DELTA_FALL_FAST)) { detectionState++; } break; case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL) { detectionState = DETECT_FALLBACK; } break; case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE) { detectionState = DETECT_RISEBACK; } break; case DETECT_RISEBACK: case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE) { if(duration > 4) /* avoid detection triggered by short moves */ { detectionState++; } exit = 1; lastStart = -2; } break; default: detectionState = DETECT_NOTHING; exit = 1; break; } step--; } while((step >= 0) && (!exit)); if((lastStart < MOTION_DELTA_HISTORY_SIZE)) { lastStart++; /* prepare value for next iteration (history index will be increased) */ } else { lastStart = -1; } if((detectionState != DETECT_POS_PITCH) && (detectionState != DETECT_NEG_PITCH)) /* nothing found */ { detectionState = DETECT_NOTHING; } else /* dont detect the same event twice */ { resetMotionDeltaHistory(); } return detectionState; } void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord) { pCoord->x = ((cosf(yaw) * cosf(pitch)) * pCoord->x + (cosf(yaw)*sinf(pitch)*sinf(roll) - (sinf(yaw)* cosf(roll))) * pCoord->y + (cosf(yaw)*sinf(pitch)*cosf(roll) + sinf(yaw)*sinf(roll)) * pCoord->z); pCoord->y = ((sinf(yaw) * cosf(pitch)) * pCoord->x + (sinf(yaw)*sinf(pitch)*sinf(roll) + cosf(yaw) * cosf(roll)) * pCoord->y + ( sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll))* pCoord->z); pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z); } SCoord CoordAdd(SCoord cA, SCoord cB) { SCoord result; result.x = cA.x + cB.x; result.y = cA.y + cB.y; result.z = cA.z + cB.z; return result; } SCoord CoordSub(SCoord cA, SCoord cB) { SCoord result; result.x = cA.x - cB.x; result.y = cA.y - cB.y; result.z = cA.z - cB.z; return result; } SCoord CoordCross(SCoord cA, SCoord cB) { SCoord result; result.x = (cA.y * cB.z) - (cA.z * cB.y); result.y = (cA.z * cB.x) - (cA.x * cB.z); result.z = (cA.x * cB.y) - (cA.y * cB.x); return result; } SCoord CoordMulF(SCoord op, float factor) { SCoord result; result.x = (op.x * factor); result.y = (op.y * factor); result.z = (op.z * factor); return result; } SCoord CoordDivF(SCoord op, float factor) { SCoord result; result.x = (op.x / factor); result.y = (op.y / factor); result.z = (op.z / factor); return result; } float CoordDot(SCoord cA, SCoord cB) { float result; result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z; return result; } void calibrateViewport(float roll, float pitch, float yaw) { SSettings* pSettings = settingsGetPointer(); pSettings->viewPitch = pitch + 180; pSettings->viewRoll = roll+ 180; pSettings->viewYaw = yaw; } float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis) { uint8_t retval = 0; float angleYaw; float anglePitch; float angleRoll; float distance = 0; float _a, _b; SCoord u,v,n; float r = 0.0; float focusLimit = 0; SCoord refVec; SCoord axis_1; SCoord axis_2; SCoord curVec; SCoord resultVec; SDeltaHistory movementDelta; SSettings* pSettings = settingsGetPointer(); roll += 180; pitch += 180; /* calculate base vector taking calibration delta into account yaw (heading) */ float compYaw; if(enableAxis & MOTION_ENABLE_YAW) { compYaw = 360.0 - yaw; /* turn to 0° */ compYaw += pSettings->viewYaw; /* consider calib yaw value */ compYaw += yaw; if (compYaw < 0.0) { compYaw = 360.0 + compYaw; } if (compYaw > 360.0) { compYaw = compYaw - 360.0; } if (compYaw > 360.0) { compYaw = compYaw - 360.0; } angleYaw = pSettings->viewYaw * M_PI / 180.0; } else { compYaw = 0.0; angleYaw = 0.0; } if(enableAxis & MOTION_ENABLE_PITCH) { anglePitch = pSettings->viewPitch * M_PI / 180.0; } else { anglePitch = 0; } if(enableAxis & MOTION_ENABLE_ROLL) { angleRoll = pSettings->viewRoll * M_PI / 180.0; } else { angleRoll = 0; } refVec.x = 0; refVec.y = 0; refVec.z = 1.0; anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec); /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */ axis_1.x = 0; if(refVec.y >=0) { axis_2.y = 1; /* => Spawn y == refVec y */ } else axis_1.y = -1; axis_1.z = -1.0 * refVec.y / refVec.z; axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */ /* check if detection plane is correct */ u = CoordSub(axis_1,refVec); v = CoordSub(axis_2,refVec); n = CoordCross(u,v); if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001)) { retval = 2; } else { if(enableAxis & MOTION_ENABLE_PITCH) { anglePitch = pitch * M_PI / 180.0; } else { anglePitch = 0.0; } if(enableAxis & MOTION_ENABLE_ROLL) { angleRoll = roll * M_PI / 180.0; } else { angleRoll = 0.0; } if(enableAxis & MOTION_ENABLE_YAW) { angleYaw = compYaw * M_PI / 180.0; } else { angleYaw = 0.0; } curVec.x = 0; curVec.y = 0; curVec.z = 1.0; anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec); _a = CoordDot(curVec,n); _b = CoordDot(refVec,n); if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */ { retval = 3; } else { r=_a/_b; if(r<0.00||r>1.40) /* are we looking into wrong direction? */ { retval = 4; } } distance = retval * 1.0; /* just for debugging */ if(retval == 0) { /* start calculating the matchpoint */ curVec = CoordMulF(curVec,r); resultVec = CoordSub(refVec,curVec); /* calculate the distance between reference and actual vector */ resultVec.x = resultVec.x * resultVec.x; resultVec.y = resultVec.y * resultVec.y; resultVec.z = resultVec.z * resultVec.z; if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0)) { distance = 0.0; } else { distance = sqrtf((resultVec.x + resultVec.y + resultVec.z)); } } } movementDelta = GetDeltaHistory(0); if(inFocus == 0) /* consider option to use smaller spot to detect focus state */ { focusLimit = MOTION_FOCUS_LIMIT - (((pSettings->viewPortMode >> 5) & 0x03) / 10.0); } else { focusLimit = MOTION_FOCUS_LIMIT; /* use standard spot to detect diver interactions */ } if((distance <= focusLimit) && (movementDelta.yaw != MOTION_DELTA_RAISE_FAST) && (movementDelta.yaw != MOTION_DELTA_FALL_FAST)) /* handle focus counter to avoid fast in/out focus changes */ { if(focusCnt < 10) { if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */ { resetMotionDeltaHistory(); } focusCnt++; } if((focusCnt == 10) && (inFocus == 0)) { inFocus = 1; } } else { if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5)) { focusCnt--; } if(focusCnt >= 5) /* Reset focus faster then setting focus */ { focusCnt--; } else { focusCnt = 0; inFocus = 0; } } if ((r<1) && (retval == 0)) /* add direction information to distance */ { distance *= -1.0; } return distance; } uint8_t viewInFocus(void) { return inFocus; } void resetFocusState(void) { inFocus = 0; } uint8_t viewDetectionSuspended(void) { uint8_t retVal = 0; if(suspendMotionDetectionSec) { retVal = 1; } return retVal; } void suspendMotionDetection(uint8_t seconds) { suspendMotionDetectionSec = seconds * 10; /* detection function is called every 100ms */ } void HandleMotionDetection(void) { detectionState_t pitchstate = DETECT_NOTHING; static uint8_t wasInFocus = 0; float focusOffset = 0.0; evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading); if(viewInFocus()) { focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, (MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW)); } else { focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL); } if(viewInFocus()) { wasInFocus = 1; set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03); if(suspendMotionDetectionSec == 0) /* suspend detection while diver is manually operating the OSTC */ { switch(settingsGetPointer()->MotionDetection) { case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch); break; case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset); break; case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(fabs(focusOffset)); break; default: pitchstate = DETECT_NOTHING; break; } } if(DETECT_NEG_PITCH == pitchstate) { StoreButtonAction((uint8_t)ACTION_PITCH_NEG); } if(DETECT_POS_PITCH == pitchstate) { StoreButtonAction((uint8_t)ACTION_PITCH_POS); } } else { if(wasInFocus) { wasInFocus = 0; if(suspendMotionDetectionSec == 0) { if(settingsGetPointer()->design == 7) { t7_set_customview_to_primary(); } else { t3_set_customview_to_primary(); } } } set_Backlight_Boost(0); } if(suspendMotionDetectionSec != 0) { suspendMotionDetectionSec--; } }