rusefi
/
RabbitECUTeensyMCUXpresso
mirror of https://github.com/rusefi/RabbitECUTeensyMCUXpresso.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
RabbitECUTeensyMCUXpresso/source/UserServices/CEM.c

1026 lines
30 KiB
C
Raw Permalink Blame History

/******************************************************************************/
/* Copyright (c) 2016 MD Automotive Controls. Original Work. */
/* License: http://www.gnu.org/licenses/gpl.html GPL version 2 or higher */
/******************************************************************************/
/* CONTEXT:KERNEL */
/* PACKAGE TITLE: XXX */
/* DESCRIPTION: XXX */
/* FILE NAME: XXX.c */
/* REVISION HISTORY: 19-08-2016 | 1.0 | Initial revision */
/* */
/******************************************************************************/
#define _CEM_C
#include "CEM.h"
#include "declarations.h"
#include "EST.h"//matthew breaking layers
#include "SETUP.h"//matthew breaking layers
#include "os.h"
#include "tepmha.h"
#include "KERNELIO.h"
uint16 CEM_au16RisingCrankEdge[80];
uint16 CEM_au16FallingCrankEdge[80];
uint16 CEM_au16AllEdge[160];
bool CEM_aboSyncEdge[160];
uint16 CEM_au16SyncPoints[36];
uint8 CEM_u8RisingEdgesCount;
uint8 CEM_u8FallingEdgesCount;
uint32 CEM_u8CrankEdgeCounter;
bool CEM_boCrankEdgeFirstRising;
uint32 CEM_u32CrankEdgeTimeoutCount;
uint8 CEM_au8AutocorrPeak[4];
uint8 CEM_au8ConfirmedAutocorrPeak[4];
IOAPI_tenEdgePolarity CEM_enEdgePolarity;
TEPMAPI_tstTimerMeasurements CEM_stTimerMeasurements[TEPMHA_nEventChannels];
bool CEM_boEdgesReady;
uint32 CEM_au32TimerOffsets[9];
CEM_tenTriggerType CEM_enTriggerType;
uint8 CEM_u8SimpleMissingSync;
TEPMAPI_tstSimpleCamSync stSimpleCamSync;
extern IOAPI_tenEHIOResource TEPM_astTEPMVVTInputs[];
TEPMAPI_ttEventTime CEM_atLastVVTTimer[4];
TEPMAPI_ttEventTime CEM_atLastVVTHighTime[4];
TEPMAPI_ttEventTime CEM_atLastVVTLowTime[4];
static void CEM_vSequenceReset(IOAPI_tenEHIOResource enEHIOResource, TEPMAPI_ttEventTime tEventTime, uint32 u32OriginGlobalCycleFraction, bool boLatePhase);
static TEPMAPI_ttEventTime CEM_tCalculateGlobalTime(TEPMAPI_ttEventTime tEventTime, uint16 u16LastGapFraction, bool boGlobalTimeEnable);
static void CEM_vProcessAllEdges(void);
static void CEM_vPhaseError(int);
static bool CEM_boGetLatePhaseSimpleCamSync(void);
void CEM_vStart(uint32* const u32Stat)
{
CEM_u32GlobalCycleFraction = 0;
CEM_u32GlobalCycleTime = 0x0000ffff;
CEM_u32GlobalCycleOriginCount = 0;
CEM_u32CrankEdgeTimeoutCount = 0;
CEM_tEventTimeLast = ~0;
CEM_u8CrankEdgeCounter = 0xff;
CEM_boEdgesReady = FALSE;
CTS_boCTSReady = FALSE;
CEM_u8SimpleMissingSync = 0;
CEM_u32CrankErrorCounts = 0;
//CEM_u8PhaseRepeats = 1;//matthew must be dynamic!!!
memset((void*)&CEM_au16RisingCrankEdge, 0, sizeof(CEM_au16RisingCrankEdge));
memset((void*)&CEM_au16FallingCrankEdge, 0, sizeof(CEM_au16FallingCrankEdge));
CEM_u8RisingEdgesCount = 0;
CEM_u8FallingEdgesCount = 0;
CEM_u32SyncPoints = 0;
CEM_boCrankEdgeFirstRising = FALSE;
CEM_au8AutocorrPeak[0] = 128;
CEM_au8AutocorrPeak[1] = 128;
CEM_au8AutocorrPeak[2] = 128;
CEM_au8AutocorrPeak[3] = 128;
CEM_au8ConfirmedAutocorrPeak[0] = 128;
CEM_au8ConfirmedAutocorrPeak[1] = 128;
CEM_au8ConfirmedAutocorrPeak[2] = 128;
CEM_au8ConfirmedAutocorrPeak[3] = 128;
}
void CEM_vRun(uint32* const u32Stat)
{
if (10 < CEM_u32CrankEdgeTimeoutCount)
{
CEM_tEventTimeLast = ~0;
CEM_u32GlobalCycleOriginCount = 0;
}
else
{
CEM_u32CrankEdgeTimeoutCount++;
}
}
void CEM_vTerminate(uint32* const u32Stat)
{
}
bool CEM_boPopulateSyncPointsArray(puint16 const pu16EdgeFractionArray)
{
bool boRetVal = true;
CEM_u32SyncPoints = 0;
while (((0 != *(pu16EdgeFractionArray + CEM_u32SyncPoints)) && (36 > CEM_u32SyncPoints)) | (0 == CEM_u32SyncPoints))
{
CEM_au16SyncPoints[CEM_u32SyncPoints] = *(pu16EdgeFractionArray + CEM_u32SyncPoints);
CEM_u32SyncPoints++;
}
CEM_u32SequenceFraction = 0x2000ul / CEM_u32SyncPoints;
return boRetVal;
}
bool CEM_boPopulateCrankEdgeArrays(puint16 const pu16EdgeFractionArray, const bool boFirstRising, IOAPI_tenEdgePolarity enEdgePolarity, uint32_t u32TriggerType)
{
uint32 u32EdgeCount = 0;
bool boStat = FALSE;
uint8 u8EdgeCount = 0;
CEM_u8RisingEdgesCount = 0;
CEM_u8FallingEdgesCount = 0;
CEM_boCrankEdgeFirstRising = boFirstRising;
CEM_enEdgePolarity = enEdgePolarity;
while (((0 != *(pu16EdgeFractionArray + u32EdgeCount)) && (80 > u8EdgeCount)) | (0 == u32EdgeCount))
{
if (IOAPI_enEdgeBoth == enEdgePolarity)
{
if (TRUE == boFirstRising)
{
CEM_au16RisingCrankEdge[u32EdgeCount / 2] = *(pu16EdgeFractionArray + u32EdgeCount);
}
else
{
CEM_au16FallingCrankEdge[u32EdgeCount / 2] = *(pu16EdgeFractionArray + u32EdgeCount);
}
CEM_au16AllEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
u32EdgeCount++;
if (TRUE == boFirstRising)
{
CEM_au16FallingCrankEdge[u32EdgeCount / 2] = *(pu16EdgeFractionArray + u32EdgeCount);
}
else
{
CEM_au16RisingCrankEdge[u32EdgeCount / 2] = *(pu16EdgeFractionArray + u32EdgeCount);
}
CEM_au16AllEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
u32EdgeCount++;
CEM_u8RisingEdgesCount++;
CEM_u8FallingEdgesCount++;
}
if (IOAPI_enEdgeFalling == enEdgePolarity)
{
CEM_au16FallingCrankEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
CEM_au16AllEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
u32EdgeCount++;
CEM_u8FallingEdgesCount++;
}
if (IOAPI_enEdgeRising == enEdgePolarity)
{
CEM_au16RisingCrankEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
CEM_au16AllEdge[u32EdgeCount] = *(pu16EdgeFractionArray + u32EdgeCount);
u32EdgeCount++;
CEM_u8RisingEdgesCount++;
}
}
CEM_enTriggerType = (0 != u32TriggerType) ? CEM_enTypeSuzukiM15A + u32TriggerType - 1 : 0;
CEM_vProcessAllEdges();
boStat = TRUE;
CEM_boEdgesReady = TRUE;
return boStat;
}
static void CEM_vProcessAllEdges(void)
{
uint32 u32OldData;
uint32 u32DeltaCount = 0;
sint32 s32Temp;
for (uint32 u32ArrayIDX = 0; u32ArrayIDX < (CEM_xEdgesCount - 1); u32ArrayIDX++)
{
u32OldData = CEM_au16AllEdge[u32ArrayIDX];
CEM_au16AllEdge[u32ArrayIDX] = CEM_au16AllEdge[u32ArrayIDX + 1] - u32OldData;
}
CEM_au16AllEdge[CEM_xEdgesCount - 1] = ~CEM_au16AllEdge[CEM_xEdgesCount - 1] + 1;
CEM_aboSyncEdge[0] = TRUE;
for (uint32 u32ArrayIDX = 1; u32ArrayIDX < CEM_xEdgesCount; u32ArrayIDX++)
{
s32Temp = CEM_au16AllEdge[u32ArrayIDX] - CEM_au16AllEdge[u32ArrayIDX - 1];
if (1 < s32Temp)
{
u32DeltaCount++;
CEM_u8SimpleMissingSync = (u32ArrayIDX + 1) % CEM_xEdgesCount;
}
CEM_aboSyncEdge[u32ArrayIDX] = TRUE;
}
if (CEM_enTriggerType < CEM_enTypeSuzukiM15A)
{
CEM_enTriggerType = (1 == u32DeltaCount) && (8 < CEM_xEdgesCount) ? CEM_enOneGroupMissing : CEM_enAutocorrelationMode;
}
if (CEM_enTriggerType == CEM_enOneGroupMissing)
{
CEM_aboSyncEdge[CEM_u8SimpleMissingSync] = FALSE;
CEM_aboSyncEdge[CEM_u8SimpleMissingSync - 1] = FALSE;
}
}
void CEM_vSetSyncPhaseRepeats(uint32 u32SyncPhaseRepeats)
{
CEM_u8PhaseRepeats = (uint8)u32SyncPhaseRepeats;
}
TEPMAPI_ttEventTime CEM_ttGetModulePhase(uint32 u32ChannelIDX)
{
TEPMAPI_ttEventTime tPhaseOffset = 0;
tPhaseOffset = CEM_au32TimerOffsets[u32ChannelIDX];
return tPhaseOffset;
}
void CEM_vPrimaryEventCB(IOAPI_tenEHIOResource enEHIOResource, TEPMAPI_ttEventTime tEventTime)
{
uint16 u16LastGapFraction;
TEPMAPI_ttEventTime tLastGapTime;
static uint32 au32AutoCorr[CEM_nEdgesMax];
static uint32 au32InputArray[CEM_nEdgesMax];
static bool aboConfirmedFlags[4] = {0, 0, 0, 0};
static uint8 au8ErrorCount[4];
uint8 u8PhasePrev;
uint32 u32Temp;
static uint32 u32EdgeCount = 0;
bool boPhaseLocked = FALSE;
static bool boLatePhase;
static uint8 u8SameGapCount;
if (FALSE == CEM_boEdgesReady) return;
u8PhasePrev = CEM_u8CrankEdgeCounter;
/* Sanity check the edges count */
if (CEM_xEdgesCount > CEM_nEdgesMax) return;
u32EdgeCount++;
#ifdef CEM_CHECK_POL
/* Sanity check edge polarity */
if (IOAPI_enEdgeFalling == CEM_enEdgePolarity)
{
enTriState = TEPM_enGetTimerDigitalState(enEHIOResource);
/* Invert the polarity because the circuit inverts */
if (IOAPI_enLow == enTriState)
{
u32EdgePolAbortCount++;
return;
}
}
else if (IOAPI_enEdgeRising == CEM_enEdgePolarity)
{
enTriState = TEPM_enGetTimerDigitalState(enEHIOResource);
/* Invert the polarity because the circuit inverts */
if (IOAPI_enHigh == enTriState)
{
u32EdgePolAbortCount++;
return;
}
}
#endif //CEM_CHECK_POL
#ifdef BUILD_SAM3X8E
CEM_au32TimerOffsets[8] = ((tstTimerModule*)TC2)->TC_CHANNEL[2].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[7] = ((tstTimerModule*)TC2)->TC_CHANNEL[1].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[6] = ((tstTimerModule*)TC2)->TC_CHANNEL[0].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[5] = ((tstTimerModule*)TC1)->TC_CHANNEL[2].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[4] = ((tstTimerModule*)TC1)->TC_CHANNEL[1].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[3] = ((tstTimerModule*)TC1)->TC_CHANNEL[0].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[2] = ((tstTimerModule*)TC0)->TC_CHANNEL[2].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[1] = ((tstTimerModule*)TC0)->TC_CHANNEL[1].TC_CV - ((tstTimerModule*)TC0)->TC_CHANNEL[0].TC_CV;
CEM_au32TimerOffsets[0] = 0;
#endif //BUILD_SAM3X8E
/* Increment the confirmed channels */
uint32 u32PhaseMin = 0;
uint32 u32PhaseMax = (CEM_xEdgesCount / CEM_u8PhaseRepeats) - 1;
if (CEM_enAutocorrelationMode == CEM_enTriggerType)
{
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (TRUE == aboConfirmedFlags[u8ArrayIDX])
{
CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX]++;
if (u32PhaseMax < CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX])
{
CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX] -= (CEM_xEdgesCount / CEM_u8PhaseRepeats);
}
}
else
{
CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX] = 128;
}
u32PhaseMin += (CEM_xEdgesCount / CEM_u8PhaseRepeats);
u32PhaseMax += (CEM_xEdgesCount / CEM_u8PhaseRepeats);
u32PhaseMin = u32PhaseMin % CEM_xEdgesCount;
u32PhaseMax = u32PhaseMax % CEM_xEdgesCount;
}
}
u16LastGapFraction = CEM_au16AllEdge[(CEM_u8CrankEdgeCounter - 1) % CEM_xEdgesCount] / CEM_u8PhaseRepeats;
tLastGapTime = CEM_tCalculateGlobalTime(tEventTime, u16LastGapFraction, CEM_aboSyncEdge[CEM_u8CrankEdgeCounter]);
if (CEM_enAutocorrelationMode == CEM_enTriggerType)
{
/* Increment the edge counter if possible */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (TRUE == aboConfirmedFlags[u8ArrayIDX])
{
CEM_u8CrankEdgeCounter = (CEM_u8CrankEdgeCounter + 1) % CEM_xEdgesCount;
break;
}
}
}
CEM_tEventTimeLast = tEventTime;
CEM_u32CrankEdgeTimeoutCount = 0;
/* Shift the input array */
puint32 pu32 = au32InputArray;
pu32 += (CEM_nEdgesMax - CEM_xEdgesCount);
memcpy(pu32, pu32 + 1, sizeof(uint32) * CEM_xEdgesCount);
/* Buffer the new gap time */
au32InputArray[CEM_nEdgesMax - 1] = tLastGapTime;
/* If enough edges have arrived to proceed */
if ((5 / CEM_u8PhaseRepeats) <= u32EdgeCount)//matthew
{
switch (CEM_enTriggerType)
{
case CEM_enAutocorrelationMode:
{
CEM_au8AutocorrPeak[0] = 0x80;
CEM_au8AutocorrPeak[1] = 0x80;
CEM_au8AutocorrPeak[2] = 0x80;
CEM_au8AutocorrPeak[3] = 0x80;
/* Calculate the autocorrelation array */
memset((void*)&au32AutoCorr, 0, CEM_nEdgesMax * sizeof(uint32));
uint32 u32ControlArrayIDX = 0;
for (uint32 u32AutocorrPhase = 0; u32AutocorrPhase < CEM_xEdgesCount; u32AutocorrPhase++)
{
uint32 u32InputArrayIDX = CEM_nEdgesMax - CEM_xEdgesCount;
for (uint32 u32ShiftCount = 0; u32ShiftCount < CEM_xEdgesCount; u32ShiftCount++)
{
u32Temp = (au32InputArray[u32InputArrayIDX] * (uint32)CEM_au16AllEdge[u32ControlArrayIDX]);
u32Temp /= 0x100u;
au32AutoCorr[u32AutocorrPhase] += u32Temp;
u32ControlArrayIDX = (u32ControlArrayIDX + 1) % CEM_xEdgesCount;
u32InputArrayIDX++;
}
u32ControlArrayIDX++;
}
/* Find the autocorrelation peak */
uint32 u32AutoPeak = 0;
for (uint32 u32ArrayIDX = 0; u32ArrayIDX < CEM_xEdgesCount; u32ArrayIDX++)
{
if (u32AutoPeak < au32AutoCorr[u32ArrayIDX])
{
u32AutoPeak = au32AutoCorr[u32ArrayIDX];
}
}
/* Set the auto-peaks indices array */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < CEM_xEdgesCount; u8ArrayIDX++)
{
if (u32AutoPeak == au32AutoCorr[u8ArrayIDX])
{
if (128u == CEM_au8AutocorrPeak[0])
{
CEM_au8AutocorrPeak[0] = u8ArrayIDX;
}
else if (128u == CEM_au8AutocorrPeak[1])
{
CEM_au8AutocorrPeak[1] = u8ArrayIDX;
}
else if (128u == CEM_au8AutocorrPeak[2])
{
CEM_au8AutocorrPeak[2] = u8ArrayIDX;
}
else if (128u == CEM_au8AutocorrPeak[3])
{
CEM_au8AutocorrPeak[3] = u8ArrayIDX;
}
}
}
u32PhaseMin = 0;
u32PhaseMax = (CEM_xEdgesCount / CEM_u8PhaseRepeats) - 1;
/* Buffer the confirmed array if is not initialized */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (128u > CEM_au8AutocorrPeak[u8ArrayIDX])
{
if (127 < CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX])
{
if ((u32PhaseMin <= CEM_au8AutocorrPeak[u8ArrayIDX]) &&
(u32PhaseMax >= CEM_au8AutocorrPeak[u8ArrayIDX]))
{
CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX] = CEM_au8AutocorrPeak[u8ArrayIDX];
if (0xff == CEM_u8CrankEdgeCounter)
{
CEM_u8CrankEdgeCounter = CEM_au8AutocorrPeak[u8ArrayIDX];
}
}
}
}
u32PhaseMin += (CEM_xEdgesCount / CEM_u8PhaseRepeats);
u32PhaseMax += (CEM_xEdgesCount / CEM_u8PhaseRepeats);
u32PhaseMin = u32PhaseMin % CEM_xEdgesCount;
u32PhaseMax = u32PhaseMax % CEM_xEdgesCount;
}
/* Clear the confirmed flags */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
aboConfirmedFlags[u8ArrayIDX] = FALSE;
}
/* Check if the confirmed array is still confirmed */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
for (uint8 u8ConfirmedArrayIDX = 0; u8ConfirmedArrayIDX < 4; u8ConfirmedArrayIDX++)
{
if (CEM_au8ConfirmedAutocorrPeak[u8ConfirmedArrayIDX] == CEM_au8AutocorrPeak[u8ArrayIDX])
{
aboConfirmedFlags[u8ConfirmedArrayIDX] = TRUE;
au8ErrorCount[u8ConfirmedArrayIDX] = 0;
}
}
}
/* Increment error counter of unconfirmed */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (FALSE == aboConfirmedFlags[u8ArrayIDX])
{
au8ErrorCount[u8ArrayIDX] = (au8ErrorCount[u8ArrayIDX] + 1) % 0x80;
}
}
/* Check for phase lock on any index */
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (CEM_u8CrankEdgeCounter == CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX]) {boPhaseLocked = TRUE;}
}
/* Buffer and edge count if we are phase locked */
if (FALSE == boPhaseLocked)
{
for (uint8 u8ArrayIDX = 0; u8ArrayIDX < 4; u8ArrayIDX++)
{
if (TRUE == aboConfirmedFlags[u8ArrayIDX])
{
CEM_u8CrankEdgeCounter = CEM_au8ConfirmedAutocorrPeak[u8ArrayIDX];
}
}
}
CEM_u32GlobalCycleFraction += CEM_au16AllEdge[(CEM_u8CrankEdgeCounter - 1) % CEM_xEdgesCount];
/* Are we at a calculation edge? */
if (0 == CEM_u8CrankEdgeCounter)
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (1 == CEM_u8PhaseRepeats)
{
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[1])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[2])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[3])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
}
break;
}
case CEM_enOneGroupMissing:
case CEM_enTypeWVEA888:
{
CEM_u32GlobalCycleFraction += CEM_au16AllEdge[(CEM_u8CrankEdgeCounter - 1) % CEM_xEdgesCount];
if (au32InputArray[CEM_nEdgesMax - 1] > au32InputArray[CEM_nEdgesMax - 2] * 2)
{
if ((CEM_u8CrankEdgeCounter != CEM_u8SimpleMissingSync) &&
(100 < u32EdgeCount))
{
CEM_vPhaseError(CEM_u8CrankEdgeCounter);
}
CEM_u8CrankEdgeCounter = CEM_u8SimpleMissingSync;
}
/* Sync over 720 degrees */
if (CEM_enTypeWVEA888 == CEM_enTriggerType)
{
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[1])
{
if (0x10000 <= CEM_u32GlobalCycleFraction)
{
if ((CEM_atLastVVTHighTime[0] < (CEM_u32GlobalCycleTime / 8))
&& (CEM_atLastVVTHighTime[0] > (CEM_u32GlobalCycleTime / 32)))
{
boLatePhase = false;
}
else if ((CEM_atLastVVTLowTime[0] < (CEM_u32GlobalCycleTime / 8))
&& (CEM_atLastVVTLowTime[0] > (CEM_u32GlobalCycleTime / 32)))
{
boLatePhase = true;
}
else
{
boLatePhase = !boLatePhase;
}
}
else
{
if (CEM_atLastVVTHighTime[0] < (CEM_u32GlobalCycleTime / 8))
{
boLatePhase = false;
}
else if (CEM_atLastVVTLowTime[0] < (CEM_u32GlobalCycleTime / 8))
{
boLatePhase = true;
}
else
{
boLatePhase = !boLatePhase;
}
}
}
}
else if (CEM_u8CrankEdgeCounter == stSimpleCamSync.u32CamSyncSampleToothCount)
{
boLatePhase = CEM_boGetLatePhaseSimpleCamSync();
}
if (0 == CEM_u8CrankEdgeCounter)
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (1 == CEM_u8PhaseRepeats)
{
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[1])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0x4000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[2])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0x8000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[3])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0xc000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
}
else if (2 == CEM_u8PhaseRepeats)
{
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[1])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[2])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[3])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
}
if ((25 == CEM_u8CrankEdgeCounter) || (53 == CEM_u8CrankEdgeCounter))
{
TEPM_vStartEventProgramKernelQueues(FALSE, 0x80);
}
CEM_u8CrankEdgeCounter = (CEM_u8CrankEdgeCounter + 1) % CEM_xEdgesCount;
break;
}
case CEM_enTypeSuzukiM15A:
{
CEM_u32GlobalCycleFraction += CEM_au16AllEdge[(CEM_u8CrankEdgeCounter - 1) % CEM_xEdgesCount];
if (au32InputArray[CEM_nEdgesMax - 1] > (au32InputArray[CEM_nEdgesMax - 2] * 2))
{
/* Crank sensor gap */
if (((CEM_u8CrankEdgeCounter != 21) && (CEM_u8CrankEdgeCounter != 7)) &&
(100 < u32EdgeCount))
{
CEM_vPhaseError(CEM_u8CrankEdgeCounter);
}
CEM_u8CrankEdgeCounter++;
u8SameGapCount = 0;
CEM_u8CrankEdgeCounter = 30 <= CEM_u8CrankEdgeCounter ? 1 : CEM_u8CrankEdgeCounter;
}
else if (au32InputArray[CEM_nEdgesMax - 1] < (au32InputArray[CEM_nEdgesMax - 2] / 2))
/* Crank sensor end gap */
{
if (((CEM_u8CrankEdgeCounter != 22) && (CEM_u8CrankEdgeCounter != 9)) &&
(100 < u32EdgeCount))
{
CEM_vPhaseError(CEM_u8CrankEdgeCounter);
}
if (0 < u8SameGapCount)
{
CEM_u8CrankEdgeCounter = 10;
}
else
{
CEM_u8CrankEdgeCounter = 23;
CEM_u32GlobalCycleFraction = CEM_au16AllEdge[0] + CEM_au16AllEdge[1];
}
u8SameGapCount = 0;
}
else
{
/* Crank sensor same gap */
u8SameGapCount++;
CEM_u8CrankEdgeCounter++;
}
if (31 == CEM_u8CrankEdgeCounter)
{
CEM_u32GlobalCycleFraction &= 0x10000;
CEM_vSequenceReset(enEHIOResource, tEventTime, 0, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
CEM_u8CrankEdgeCounter = 1;
}
if (1 == CEM_u8PhaseRepeats)
{
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[1])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0x4000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[2])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0x8000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if (CEM_u32GlobalCycleFraction == CEM_au16SyncPoints[3])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, 0xc000, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
}
else if (2 == CEM_u8PhaseRepeats)
{
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[1])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[2])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
if ((0xffff & CEM_u32GlobalCycleFraction) == CEM_au16SyncPoints[3])
{
CEM_vSequenceReset(enEHIOResource, tEventTime, CEM_u32GlobalCycleFraction, boLatePhase);
TEPM_vSynchroniseEventProgramKernelQueues();
}
}
break;
}
default:
{
break;
}
}
if ((1u != ((CEM_u8CrankEdgeCounter - u8PhasePrev) % CEM_xEdgesCount)) && (TRUE == boPhaseLocked))
{
CEM_vPhaseError(0);
}
}
}
static bool CEM_boGetLatePhaseSimpleCamSync(void)
{
bool boLatePhase;
IOAPI_tenTriState enTriState;
enTriState = IO_enGetDIOResourceState(stSimpleCamSync.enEHIOResource);
if (true == stSimpleCamSync.boCamSyncHighLate)
{
boLatePhase = (IOAPI_enHigh == enTriState);
}
else
{
boLatePhase = (IOAPI_enLow == enTriState);
}
return boLatePhase;
}
static void CEM_vPhaseError(int code_in)
{
/* TODO
volatile static int code;
code = code_in;
*/
CEM_u32CrankErrorCounts++;
}
static TEPMAPI_ttEventTime CEM_tCalculateGlobalTime(TEPMAPI_ttEventTime tEventTime, uint16 u16LastGapFraction, bool boGlobalTimeEnable)
{
TEPMAPI_ttEventTime tTemp;
TEPMAPI_ttEventTime tGlobalTimeNew = 0;
uint32 u32Temp;
tTemp = (tEventTime - CEM_tEventTimeLast) & TEPMHA_nCounterMask;
if (0xffff >= tTemp)
{
tGlobalTimeNew = (0x10000u * tTemp) / (uint32)u16LastGapFraction;
//tGlobalTimeNew = 220000;
}
else if (0xfffff >= tTemp)
{
u16LastGapFraction = u16LastGapFraction >> 4;
tGlobalTimeNew = (0x1000u * tTemp) / (uint32)u16LastGapFraction;
}
else if (0xffffff >= tTemp)
{
u16LastGapFraction = u16LastGapFraction >> 8;
tGlobalTimeNew = (0x100u * tTemp) / (uint32)u16LastGapFraction;
}
else if (0xfffffff >= tTemp)
{
u16LastGapFraction = u16LastGapFraction >> 12;
tGlobalTimeNew = (0x10u * tTemp) / (uint32)u16LastGapFraction;
}
else
{
CEM_u32GlobalCycleTime = ~0u;
}
if (TRUE == boGlobalTimeEnable)
{
if (TRUE == TPS_boThrottleClosed)
{
u32Temp = USERCAL_stRAMCAL.u16ESTFilterClosed * CEM_u32GlobalCycleTime;
u32Temp += ((0x100 - USERCAL_stRAMCAL.u16ESTFilterClosed) * tGlobalTimeNew);
u32Temp /= 0x100;
}
else
{
u32Temp = USERCAL_stRAMCAL.u16ESTFilterOpen * tGlobalTimeNew;
u32Temp += ((0x100 - (uint8)USERCAL_stRAMCAL.u16ESTFilterOpen) * CEM_u32GlobalCycleTime);
u32Temp /= 0x100;
}
CEM_u32GlobalCycleTime = u32Temp;
if (tGlobalTimeNew < CEM_u32GlobalCycleTime)
{
CEM_u32GlobalCycleTime--;
}
else if (tGlobalTimeNew > CEM_u32GlobalCycleTime)
{
CEM_u32GlobalCycleTime++;
}
}
return tTemp;
}
static void CEM_vSequenceReset(IOAPI_tenEHIOResource enEHIOResource, TEPMAPI_ttEventTime tEventTime, uint32 u32OriginGlobalCycleFraction, bool boLatePhase)
{
uint32 u32Temp;
static uint32 u32SequenceIDX;
static IOAPI_tenTriState enTriState;
IOAPI_tenEHIOResource enLinkedResource;
CEM_tSyncTimeLast = tEventTime;
u32SequenceIDX++;
u32Temp = CEM_u32GlobalCycleTime / (4 * CEM_u8PhaseRepeats);
u32Temp = 0x10000 > u32Temp ? u32Temp : 0xffff;
if (1 == CEM_u8PhaseRepeats)
{
if (0 == u32OriginGlobalCycleFraction)
{
u32SequenceIDX = 0;
CEM_u32GlobalCycleFraction = u32OriginGlobalCycleFraction;
CEM_u32GlobalCycleOriginCount++;
}
}
else
{
if (0 == u32OriginGlobalCycleFraction)
{
if (((0x10000 <= CEM_u32GlobalCycleFraction) && (false == boLatePhase)) ||
((0x10000 > CEM_u32GlobalCycleFraction) && (true == boLatePhase)))
{
CEM_u32CamErrorCounts++;
CEM_u32CamRunningErrorCounts = 5 > CEM_u32CamRunningErrorCounts ? CEM_u32CamRunningErrorCounts + 1 : 5;
}
if (5 == CEM_u32CamRunningErrorCounts)
/* Many cam errors */
{
//if (0 == (CEM_u32GlobalCycleOriginCount % 8))
if (true)
/* Try swap phase */
{
if (0x10000 > CEM_u32GlobalCycleFraction)
{
u32SequenceIDX = 4;
CEM_u32GlobalCycleFraction = 0x10000 + u32OriginGlobalCycleFraction;
}
else
{
u32SequenceIDX = 0;
CEM_u32GlobalCycleFraction = u32OriginGlobalCycleFraction;
}
CEM_u32CamRunningErrorCounts = 0;
}
else
/* Keep phase and reset */
{
if (0x10000 <= CEM_u32GlobalCycleFraction)
{
u32SequenceIDX = 0;
CEM_u32GlobalCycleFraction = u32OriginGlobalCycleFraction;
}
else
{
u32SequenceIDX = 4;
CEM_u32GlobalCycleFraction = 0x10000 + u32OriginGlobalCycleFraction;
}
}
}
else if ((0x10000 <= CEM_u32GlobalCycleFraction) && (true == boLatePhase))
{
u32SequenceIDX = 0;
CEM_u32GlobalCycleFraction = u32OriginGlobalCycleFraction;
}
else
{
CEM_u32GlobalCycleFraction = 0x10000 + u32OriginGlobalCycleFraction;
}
CEM_u32GlobalCycleOriginCount++;
if (0 == (CEM_u32GlobalCycleOriginCount % 8))
{
CEM_u32CamRunningErrorCounts = 0 < CEM_u32CamRunningErrorCounts ? CEM_u32CamRunningErrorCounts - 1 : 0;
}
/* grab the primary linked resource for the phase tell-tale to slave uP */
enLinkedResource = TEPM_enGetPrimaryLinkedResource();
/* set the phase tell-tale */
enTriState = true == boLatePhase ? IOAPI_enHigh : IOAPI_enLow;
IO_vAssertDIOResource(enLinkedResource, enTriState);
}
}
if (1 < CEM_u32GlobalCycleOriginCount)//matthew
{
TEPM_vStartEventProgramKernelQueues(FALSE, u32SequenceIDX);
}
if ((0 == (0xffff & CEM_u32GlobalCycleFraction)) || (0x8000 == (0xffff & CEM_u32GlobalCycleFraction)))
{
TEPM_vInitiateUserCallBack(enEHIOResource, (uint16)u32Temp);
}
}
void CEM_vPhaseEventCB(IOAPI_tenEHIOResource enEHIOResource, TEPMAPI_ttEventTime tEventTime)
{
IOAPI_tenTriState enTriState;
if (enEHIOResource == TEPM_astTEPMVVTInputs[0])
{
enTriState = IO_enGetDIOResourceState(enEHIOResource);
if (IOAPI_enLow == enTriState)
{
CEM_atLastVVTLowTime[0] = TEPMHA_nCounterMask & (tEventTime - CEM_atLastVVTTimer[0]);
}
else
{
CEM_atLastVVTHighTime[0] = TEPMHA_nCounterMask & (tEventTime - CEM_atLastVVTTimer[0]);
}
CEM_atLastVVTTimer[0] = tEventTime;
}
else if (enEHIOResource == TEPM_astTEPMVVTInputs[1])
{
}
else if (enEHIOResource == TEPM_astTEPMVVTInputs[2])
{
}
else if (enEHIOResource == TEPM_astTEPMVVTInputs[3])
{
}
}
void CEM_vSetupSimpleCamSync(IOAPI_tenEHIOResource enEHIOResource, bool boCamSyncHighLate, uint32 u32CamSyncSampletoothCount)
{
stSimpleCamSync.enEHIOResource = enEHIOResource;
stSimpleCamSync.boCamSyncHighLate = boCamSyncHighLate;
stSimpleCamSync.u32CamSyncSampleToothCount = u32CamSyncSampletoothCount;
}
void CEM_vFreqEventCB(IOAPI_tenEHIOResource enEHIOResource, TEPMAPI_ttEventTime tEventTime)
{
static uint32 u32OldOSTick;
volatile static uint32 samples[100];
static uint32 counter;
static uint32 u32TimeOld;
static uint32 u32Temp;
static uint32 u32ErrCount;
samples[counter] = (tEventTime - u32TimeOld) & TEPMHA_nCounterMask;
u32Temp -= 5;
u32Temp *= 2;
if ((u32Temp < samples[counter]) &&
(2 > u32ErrCount))
{
samples[counter] /= 2;
u32ErrCount++;
}
else
{
u32ErrCount = 0;
}
u32Temp = samples[counter];
if (OS_u32TickCounter > (u32OldOSTick + 1))
{
TEPM_vInitiateUserCallBack(enEHIOResource, samples[counter]);
u32OldOSTick = OS_u32TickCounter;
}
counter = (counter + 1) % 100;
u32TimeOld = tEventTime;
}
Reference in New Issue View Git Blame Copy Permalink