41 lines
1.7 KiB
Arduino
41 lines
1.7 KiB
Arduino
|
/*
|
||
|
|
* Converts a crank angle into a time from or since that angle occurred.
|
||
|
|
* Positive angles are assumed to be in the future, negative angles in the past:
|
||
|
|
* * Future angle calculations will use a predicted speed/acceleration
|
||
|
|
* * Past angle calculations will use the known speed
|
||
|
|
*
|
||
|
|
* Multiple prediction methods will be implemented here for testing:
|
||
|
|
* * Last interval using both last full revolution and gap between last teeth
|
||
|
|
* * 2nd derivative prediction (Speed + acceleration)
|
||
|
|
* * Closed loop error correction (Alpha-beta filter).
|
||
|
|
*/
|
||
|
|
|
||
|
|
#define fastDegreesToUS(degrees) (degrees * (unsigned long)timePerDegree)
|
||
|
|
unsigned long angleToTime(int16_t angle)
|
||
|
|
{
|
||
|
|
//#define degreesToUS(degrees) (decoderIsLowRes == true ) ? ((degrees * 166666UL) / currentStatus.RPM) : (degrees * (unsigned long)timePerDegree)
|
||
|
|
//#define degreesToUS(degrees) ((degrees * revolutionTime) / 360)
|
||
|
|
//#define degreesToUS(degrees) ((degrees * revolutionTime * 3054198967ULL) >> 40) //Fast version of divide by 360
|
||
|
|
//#define degreesToUS(degrees) (degrees * (unsigned long)timePerDegree)
|
||
|
|
return ((angle * revolutionTime) / 360);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Convert a time (uS) into an angle at current speed
|
||
|
|
*/
|
||
|
|
uint16_t timeToAngle(unsigned long time)
|
||
|
|
{
|
||
|
|
|
||
|
|
//#define uSToDegrees(time) (((unsigned long)time * currentStatus.RPM) / 166666)
|
||
|
|
//#define uSToDegrees(time) ( (((uint64_t)time * currentStatus.RPM * 211107077ULL) >> 45) ) //Crazy magic numbers method from Hackers delight (www.hackersdelight.org/magic.htm)
|
||
|
|
return (((unsigned long)time * currentStatus.RPM) / 166666);
|
||
|
|
|
||
|
|
/*
|
||
|
|
switch(calculationAlgorithm)
|
||
|
|
{
|
||
|
|
case CRANKMATH_METHOD_INTERVAL_TOOTH:
|
||
|
|
returnValue = ((elapsedTime * triggerToothAngle) / toothTime);
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
}
|