s32fp fp_atoi(const char *str)
{
int nat = 0;
int frac = 0;
int div = 10;
int sign = 1;
if ('-' == *str)
{
sign = -1;
str++;
}
for (; *str >= '0' && *str <= '9'; str++)
{
nat *= 10;
nat += *str - '0';
}
if (*str != 0)
{
for (str++; *str >= '0' && *str <= '9'; str++)
{
frac += FP_FROMINT(*str - '0') / div;
div *= 10;
}
}
return sign * (FP_FROMINT(nat) + frac);
}
/** Get amplitude for given frequency multiplied with given percentage */
uint32_t MotorVoltage::GetAmpPerc(u32fp frq, uint32_t perc)
{
uint32_t amp = (perc * (FP_TOINT(FP_MUL(fac, frq)) + boost)) / 100;
if (frq < minFrq)
{
amp = 0;
}
if (amp > maxAmp)
{
amp = maxAmp;
}
if (frq > (maxFrq - FRQ_DRT_STR))
{
s32fp diff = maxFrq - frq;
diff = diff < 0 ? 0 : diff;
amp = FP_TOINT(FP_MUL(FP_FROMINT(amp), FP_DIV(diff, FRQ_DRT_STR)));
}
return amp;
}
/** Calculate slope of u/f */
void MotorVoltage::CalcFac()
{
fac = FP_DIV(FP_FROMINT(maxAmp - boost), endFrq);
}
/**
* Set a parameters digit value
*
* @param[in] ParamNum Parameter index
* @param[in] ParamVal New value of parameter
* @return 0 if set ok, -1 if ParamVal out of allowed range
*/
char parm_SetDig(PARAM_NUM ParamNum, int ParamVal)
{
values[ParamNum] = FP_FROMINT(ParamVal);
return 0;
}
u32fp Encoder::GetFrq()
{
//GetPulseTimeFiltered();
if (ignore > 0) return 0;
return FP_FROMINT(1000000) / (last_pulse_timespan * imp_per_rev);
}
{
for (str++; *str >= '0' && *str <= '9'; str++)
{
frac += FP_FROMINT(*str - '0') / div;
div *= 10;
}
}
return sign * (FP_FROMINT(nat) + frac);
}
u32fp fp_sqrt(u32fp rad)
{
u32fp sqrt = rad >> (rad<1000?4:8); //Starting value for newton iteration
u32fp sqrtl;
sqrt = sqrt>FP_FROMINT(1)?sqrt:FP_FROMINT(1); //Must be > 0
do {
sqrtl = sqrt;
sqrt = (sqrt + FP_DIV(rad, sqrt)) >> 1;
} while ((sqrtl - sqrt) > 1);
return sqrt;
}
s32fp fp_ln(unsigned int x)
{
int n = 0;
const s32fp ln2 = FP_FROMFLT(0.6931471806);
if (x == 0)
