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)