Mercurial > public > ostc4
view Discovery/Src/motion.c @ 860:3e499569baf3 Evo_2_23
Code cleanup
author | Ideenmodellierer |
---|---|
date | Tue, 07 May 2024 21:20:56 +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--; } }