1
0

fp.c 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183
  1. /*
  2. * Fixed-Point number library
  3. * Contains functions for decimal numbers encoded in 16.16 format
  4. */
  5. #define fixed_point_t char
  6. // Fixed point arithmetic operations using 16.16 format
  7. #define FP_FRACT_BITS 16
  8. #define FP_MULTIPLY_CONST 15259
  9. #define FP_FRACT_MASK 0xFFFF
  10. // Convert a fixed-point variable to a string with a given number of decimals (truncated, not rounded)
  11. void FP_FPtoString(word fixedPointValue, char* outputString, word numDecimals) {
  12. word fractMask = FP_FRACT_MASK; // Needed to enforce a load(32) instruction, as constants have limited bits in arith operations
  13. word integerPart = fixedPointValue >> FP_FRACT_BITS; // Extract the integer part
  14. word fractionalPart = fixedPointValue & fractMask; // Extract the fractional part
  15. // Convert the integer part, which can be signed, to a string using itoa()
  16. if (integerPart < 0)
  17. {
  18. outputString[0] = '-';
  19. if (fractionalPart > 0)
  20. {
  21. itoa(- (integerPart + 1), &outputString[1]); // integerPart-1 to fix neg numbers
  22. }
  23. else
  24. {
  25. itoa(- integerPart, &outputString[1]);
  26. }
  27. }
  28. else
  29. {
  30. itoa(integerPart, outputString);
  31. }
  32. // Find the end of the integer part string
  33. char* endOfString = outputString;
  34. while (*endOfString)
  35. {
  36. endOfString++;
  37. }
  38. // Add a dot to separate the integer and fractional parts
  39. *endOfString = '.';
  40. endOfString++;
  41. char* decimalPosition = endOfString;
  42. char decbuf[10]; // 10 with current multiply constant, as number can be 9 decimal max
  43. // Convert the fractional part to intermediate buffer
  44. if (integerPart < 0 && fractionalPart > 0)
  45. {
  46. // Fix for neg numbers (2's complement)
  47. fractionalPart = ( (~fractionalPart) & fractMask) + 1;
  48. }
  49. fractionalPart *= FP_MULTIPLY_CONST;
  50. itoa(fractionalPart, decbuf);
  51. // Get length of decimal part string
  52. int declen = strlen(decbuf);
  53. // Prepend with 9-(length of decimal part) zeros before adding decimal part
  54. while (declen < 9)
  55. {
  56. *endOfString = '0';
  57. endOfString++;
  58. declen++;
  59. }
  60. // Terminate string
  61. *endOfString = 0;
  62. // Convert and append the fractional part directly to the result
  63. strcat(endOfString, decbuf);
  64. // Truncate at number of decimals given by placing string terminator
  65. decimalPosition[numDecimals] = 0;
  66. }
  67. // Convert a string to a fixed-point number
  68. fixed_point_t FP_StringToFP(char* decimalString)
  69. {
  70. // Get index of the dot, also return int part if no dot
  71. char* dotp = strchr(decimalString, '.');
  72. if (dotp == 0) return strToInt(decimalString) << FP_FRACT_BITS;
  73. word dotIndex = dotp - decimalString;
  74. // Copy integer part
  75. char integerPart[12];
  76. memcpy(integerPart, decimalString, dotIndex);
  77. // Terminate integerPart
  78. integerPart[dotIndex] = 0;
  79. // Copy decimal part
  80. char decimalPart[12];
  81. strcpy(decimalPart, decimalString + dotIndex + 1);
  82. // Add integer part to result
  83. word result = strToInt(integerPart) << FP_FRACT_BITS;
  84. // Get decimal part as an integer
  85. word decimalPartInt = strToInt(decimalPart);
  86. // Return if no or invalid decimal part
  87. if (decimalPartInt == 0)
  88. return result;
  89. // Get number of decimals of the decimal part string which we need later
  90. word lenDecimalPart = strlen(decimalPart);
  91. // Alternative for 10^lenDecimalPart
  92. word divisionNumber = 1;
  93. while (lenDecimalPart > 0)
  94. {
  95. divisionNumber *= 10;
  96. lenDecimalPart--;
  97. }
  98. // Calculate decimal part
  99. word decResult = decimalPartInt << FP_FRACT_BITS;
  100. decResult = MATH_div(decResult, divisionNumber);
  101. // Correct for negative numbers (look at string because -0.x)
  102. if (decimalString[0] == '-')
  103. {
  104. result -= (1 << FP_FRACT_BITS);
  105. decResult = (1 << FP_FRACT_BITS) - decResult;
  106. }
  107. // Add decimal part to integer part
  108. word fractMask = FP_FRACT_MASK; // Needed to enforce a load(32) instruction, as constants have limited bits in arith operations
  109. result |= (decResult & fractMask);
  110. return result;
  111. }
  112. // Convert an integer to a fixed-point number
  113. fixed_point_t FP_intToFP(word x)
  114. {
  115. return x << FP_FRACT_BITS;
  116. }
  117. // Convert a fixed-point number to an integer (truncate)
  118. word FP_FPtoInt(fixed_point_t x)
  119. {
  120. return x >> FP_FRACT_BITS;
  121. }
  122. // Multiply two fixed-point numbers using special instruction
  123. fixed_point_t FP_Mult(fixed_point_t a, fixed_point_t b)
  124. {
  125. // r4: a, r5: b
  126. fixed_point_t retval = 0;
  127. asm(
  128. "multfp r4 r5 r2 ; r2 = a*b (FP signed)\n"
  129. "write -4 r14 r2 ; write result to stack for return\n"
  130. );
  131. return retval;
  132. }
  133. // Divide two fixed-point numbers using MU (a/b)
  134. fixed_point_t FP_Div(fixed_point_t a, fixed_point_t b)
  135. {
  136. // r4: a, r5: b
  137. fixed_point_t retval = 0;
  138. asm(
  139. "load32 0xC02742 r2 ; r2 = addr fpdiv_writea\n"
  140. "write 0 r2 r4 ; write a to divider\n"
  141. "write 1 r2 r5 ; write b to divider and perform division\n"
  142. "read 1 r2 r2 ; read result to r2\n"
  143. "write -4 r14 r2 ; write result to stack for return\n"
  144. );
  145. return retval;
  146. }