Arduino s16.15 Fixed Point Math Routines

It is important to note, that fixed point math comes in many flavors. For example, a 16-bit integer can implement 31 different fixed point formats, signed and unsigned Q1 through Q16 (Q number format). A couple of popular 16-bit formats being Q8.8, Q16 and S15 (S representing a signed bit). Each version has distinct ranges of the numbers the format can represent. Additionally, each type will have significant differences in the precision that can be achieved. So, when one presents fixed-point functions or a library, there is a good chance the implementation is unique and specific to the programmer’s task. It’s highly doubtful you’ll find a single-solution that fits all purposes. Having said all that, here are some fixed point functions for the arduino that I cobbled together.

Most of this was copied from the source code of either the “avrfix” library, “fixedpt, a fixed point math library for C”, or the AVR GCC library sources. Very little of this is new, I simply massaged a few bytes here and there and combined it all together. I changed a few function names, but left enough hints inside the assembler routines if you want to discover where they came from.

This should provide very rudimentary fixed point math, allowing a base to build upon. It uses a signed long (int32_t) in the form of a s16.15 fixed point value. There is no support for overflow or saturation. I’ve added basic square root and trig functions.

I hope I left all of the necessary attributions in the file. I’ve conducted very minimal testing, and you need to determine if the accuracy fits your needs. Use this code at your own risk!

fix.h:

//fix.h:
#ifndef FIX_H
#define FIX_H

#include <arduino.h>
#include "stdint.h"

//Pragmas
#define FP_IBITS        16 //integer bits
#define FP_FBITS        15 //fraction bits
#define FP_BITS         32 //total bits (s16.15)
#define FP_MIN          -2147450880L
#define FP_MAX          2147450880L
#define FP_FMASK        (((int32_t)1<<FP_FBITS) - 1)
#define FP_ONE          ((int32_t)0x8000)
#define FP_CONST(R)     ((int32_t)((R)*FP_ONE + ((R) >= 0 ? 0.5 : -0.5)))
#define FP_PI           FP_CONST(3.14159265358979323846)
#define FP_TWO_PI       FP_CONST(2*3.14159265358979323846)
#define FP_HALF_PI      FP_CONST(3.14159265358979323846/2)
#define FP_ABS(A)       ((A) < 0 ? -(A) : (A))
#define FP_FRAC_PART(A) ((int32_t)(A)&FP_FMASK)
#define FP_DegToRad(D)  (FP_Divide(D, (int32_t)1877468))
#define FP_RadToDeg(R)  (FP_Multiply(R, (int32_t)18529868))

//conversion Functions
#define itok(i)         ( (int32_t)( (int32_t)i<<(int32_t)FP_FBITS ) )
#define ktoi(k)         ( ( (int16_t)( (int32_t)k>>(int32_t)FP_FBITS ) )&0x0000ffff )
#define ftok(f)         ( (int32_t)(float)( (f)*(32768) ) )
extern float FP_FixedToFloat(int32_t);
extern int32_t FP_FloatToFixed(float);
int32_t FP_StringToFixed(char *s);
void FP_FixedToString(int32_t f, char *s);

//basic math
extern int32_t FP_Multiply(int32_t, int32_t);
extern int32_t FP_Divide(int32_t, int32_t);

//special functions
int32_t FP_Round(int32_t, uint8_t);

//square root
extern int32_t _FP_SquareRoot(int32_t, int32_t);
#define FP_Sqrt(a)      _FP_SquareRoot(a, 15);

//trig
extern int32_t FP_Sin(int32_t);
#define FP_Cos(A)       (FP_Sin(FP_HALF_PI - A))
#define FP_Tan(A)       (FP_Division(FP_Sin(A), FP_Cos(A)))

#endif //FIX_H

fix.cpp:

/*
 * The ideas and algorithms have been cherry-picked from a large number
 * of previous implementations available on the Internet, and from the
 * AVR GCC lib sources.
 * Copyright (c) 2002  Michael Stumpf  <mistumpf@de.pepperl-fuchs.com>
 * Copyright (c) 2006  Dmitry Xmelkov
 * Copyright (C) 2012-2015 Free Software Foundation, Inc.
 * Contributed by Sean D'Epagnier  (sean@depagnier.com)
 * Georg-Johann Lay (avr@gjlay.de)
 * All rights reserved.
 * Maximilan Rosenblattl, Andreas Wolf 2007-02-07
 * Copyright (c) 2010-2012 Ivan Voras <ivoras@freebsd.org>
 * Copyright (c) 2012 Tim Hartrick <tim@edgecast.com>
 
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright
 notice, this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright
 notice, this list of conditions and the following disclaimer in
 the documentation and/or other materials provided with the
 distribution.
 * Neither the name of the copyright holders nor the names of
 contributors may be used to endorse or promote products derived
 from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
*/
//fix.cpp
#include <avr/io.h>
#include <stdlib.h>
#include <string.h>
#include "fix.h"

#define MAX_STRING_SIZE 8

//atol function ignores sign
int32_t _atol(const char* s) {
  int32_t v=0;
   
  //ignore whitespace
  while (*s == ' ' || (uint16_t)(*s - 9) < 5u) 
    ++s;
  //skip over sign
  if (*s == '-' || *s == '+') 
    ++s;
  //convert ascii to number
  while ((uint16_t)(*s - '0') < 10u) {
    v = v*10 + *s - '0';
    ++s;
  }
  return v;
}

//ltoa function ignores sign
char *_ltoa(int32_t n, char *s) {
  char temp[8];
  uint8_t i = 0;

  //construct backward string of number
  while ((uint32_t)n > 0) {
    temp[i++] = ((uint32_t)n%10) + '0';
    n = ((uint32_t)n)/10;
  }
  //reverse string
  while (i > 0) {
    *s++ = temp[--i];
  }
  //terminate string
  *s = 0;
  return s;
}

//basic string copy n
static inline void _strcpy(char *d, const char *s) {
  uint8_t n=0;
   
  while (*s != '\0') {
    if (n++ >= MAX_STRING_SIZE) 
      return; //destination @ max size
    *d++ = *s++;
  }
}

//basic string concatenation
void _concat(char *d, char *s) {
  uint8_t n=0;
  
  //find end of destination
  while(*d) {
    d++;
  }
  //copy source to destination
  while(*s && n<MAX_STRING_SIZE) {
    *d++ = *s++;
    n++;
  }
  //terminate destination
  *d = '\0';
}

int32_t FP_StringToFixed(char *s) {
  int32_t f, fpw, fpf, bit, r[15] = {
    0x2faf080, 0x17d7840, 0xbebc20, 0x5f5e10, 0x02faf08, 0x017d784, 0x0bebc2, 0x05f5e1,
    0x002faf1, 0x0017d78, 0x00bebc, 0x005f5e, 0x0002faf, 0x00017d8, 0x000bec //0x0005f6
  };
  uint8_t sign, i;
  char *p=s, temp[9] = "00000000";

  sign = 0;
  //separate whole & fraction portions
  while (*p != '.') {
    //check for negative sign
    if (*p == '-') 
      sign = 1;
    //no decimal found, return integer as fixed point
    if (*p == '\0') 
      return sign ? -(_atol(s)<<FP_FBITS) : (_atol(s)<<FP_FBITS);
    p++;
  }
 
  //whole part
  *p = '\0';
  fpw = (_atol(s)<<FP_FBITS);
 
  //pad fraction part with trailing zeros
  _strcpy(temp, (p + 1));
  //get fraction
  f = _atol(temp);
  //re-insert decimal point
  *p = '.';
 
  fpf = 0;
  bit = 0x4000;
  //convert base10 fraction to fixed point base2
  for (i=0; i<15; i++) {
    if (f - r[i] > 0) {
      f -= r[i];
      fpf += bit;
    }
    bit >>= 1;
  }
 
  //join fixed point whole and fractional parts
  return sign ? -(fpw + fpf) : (fpw + fpf);
}

void FP_FixedToString(int32_t f, char *s) {
  int32_t fp, bit=0x4000, r[16] = { 50000, 25000, 12500, 6250, 3125, 1563, 781, 391, 195, 98, 49, 24, 12, 6, 3 };
  int32_t d[5] = { 10000, 1000, 100, 10 };
  char *p=s, *sf, temp[12];
  uint8_t i;
  
  //zero?
  if (f == 0) {
    *p++ = '0';
    *p = 0;
    return;
  }
  
  //negate?
  if (f < 0) {
    *p++ = '-';
    f = -f;
  }
  
  //get whole part
  fp = ktoi(f);
  if (fp != 0) 
    p = _ltoa(fp, p);
  else 
    *p++ = '0';
    
  //get fractional part
  fp = FP_FRAC_PART(f);
  if (fp == 0) {
    *p = 0;   //terminate string
    return;
  }  
  *p++ = '.'; //add decimal to end of s
  *p = 0;     //terminate string
   
  f = 0;
  //convert fraction base 2 to base 10
  for (i=0; i<15; i++) {
    if (fp & bit) 
      f += r[i];
    bit >>= 1;
  }
  //temporary string storage space
  sf = temp;
  sf = ltoa(f, sf, 10);
   
  // if needed, add leading zeros to fractional portion
  for (i=0; i<4; i++) {
    if (f < d[i]) {
      *p++ = '0';
      *p = '\0';
    } else 
      break;
  }
   
  //combine whole & fractional parts
  _concat(s, sf);
}

int32_t __attribute__((naked)) FP_Multiply(int32_t a, int32_t b) {
  asm volatile (
    //move passed values into correct regs
    "movw r16, r18 \n"
    "movw r18, r20 \n"
    "movw r20, r22 \n"
    "movw r22, r24 \n"
    "call __mulsa3 \n"
    //move results into return regs
    "movw r22, r24 \n"
    "movw r24, r26 \n"
    "ret           \n"
  );
}

int32_t __attribute__((naked)) FP_Divide(int32_t a, int32_t b) {
  asm volatile (
    //move passed values into correct regs
    "movw r26, r24 \n"
    "movw r24, r22 \n"
    "call __divsa3 \n"
    "ret           \n"
  );
}

//Difference from ISO/IEC DTR 18037: using an uint8_t as second parameter according to microcontroller register size and maximum possible value
int32_t FP_Round(int32_t f, uint8_t n) {
  n = FP_FBITS - n;
  if (f >= 0)
    return (f&(0xFFFFFFFF<<n)) + ((f&(1<<(n - 1)))<<1);
  else
    return (f&(0xFFFFFFFF<<n)) - ((f&(1<<(n - 1)))<<1);
}

int32_t __attribute__((naked)) FP_FloatToFixed(float f) {
  asm volatile (
    //__fractsfsa
    "subi  r24, 0x80    \n"
    "sbci  r25, 0xf8    \n"
    "call __fixunssfsi \n"
    "set                \n"
    "cpse  r27, r1      \n"
    "rjmp  __fp_zero    \n"
    "ret                \n"
  );
}

float __attribute__((naked)) FP_FixedToFloat(int32_t k) {
  asm volatile (
    //__fractsasf
    "call __floatsisf \n"
    "tst   r25         \n"
    "breq  1f          \n"
    "subi  r24, 0x80   \n"
    "sbci  r25, 0x07   \n"
    "1:                \n"
    "ret               \n"
  );
}

int32_t FP_Sin(int32_t fp) {
  int16_t sign = 1;
  int32_t sqr, result;
  const int32_t SK[2] = {
    FP_CONST(7.61e-03),
    FP_CONST(1.6605e-01)
  };

  //normalize
  fp %= 2*FP_PI;
  if (fp < 0)
    fp = FP_TWO_PI + fp;
    //fp = FP_PI*2 + fp;
  if ((fp > FP_HALF_PI) && (fp <= FP_PI))
    fp = FP_PI - fp;
  else if ((fp > FP_PI) && (fp <= (FP_PI + FP_HALF_PI))) {
    fp = fp - FP_PI;
    sign = -1;
  } else if (fp > (FP_PI + FP_HALF_PI)) {
    fp = (FP_PI<<1) - fp;
    sign = -1;
  }
  
  //calculate sine
  sqr = FP_Multiply(fp, fp);
  result = FP_Multiply(SK[0], sqr);
  result = FP_Multiply((result - SK[1]), sqr);
  result = FP_Multiply((result + FP_ONE), fp);
  /*
  //taylor series
  // sin(x) = x − (x^3)/3! + (x^5)/5! − (x^7)/7! + ...
  sqr = FP_Multiply(fp, fp);
  fp = FP_Multiply(fp, sqr);
  result -= FP_Divide(fp, itok(6));
  fp = FP_Multiply(fp, sqr);
  result += FP_Divide(fp, itok(120));
  fp = FP_Multiply(fp, sqr);
  result -= FP_Divide(fp, itok(5040));
  fp = FP_Multiply(fp, sqr);
  result += FP_Divide(fp, itok(362880));
  fp = FP_Multiply(fp, sqr);
  result -= FP_Divide(fp, itok(39916800));
  */
  return (sign*result);
}

int32_t _FP_SquareRoot(int32_t val, int32_t Q) {
  int32_t sval = 0;

  //convert Q to even
  if (Q & 0x01) {
    Q -= 1;
    val >>= 1;
  }
  //integer square root math
  for (uint8_t i=0; i<=30; i+=2) {
    if ((0x40000001>>i) + sval <= val) {  
      val -= (0x40000001>>i) + sval;     
      sval = (sval>>1) | (0x40000001>>i);
    } else {
      sval = sval>>1;
    }
  }
  if (sval < val) 
    ++sval;  
  //this is the square root in Q format
  sval <<= (Q)/2;
  //convert the square root to Q15 format
  if (Q < 15)
    return(sval<<(15 - Q));
  else
    return(sval>>(Q - 15));
}

Test Code:

#include "fix.h"

void setup(void) {
  volatile int32_t fix1, fix2, fix3;
  volatile float float1 = 2.33f;
  char s[12];

  Serial.begin(9600);
  delay(1000);
  
  //0.66 + 2.33
  fix1 = ftok(0.66);
  fix2 = FP_FloatToFixed(float1);
  FP_FixedToString(fix3, s);
  Serial.print("0.66 + 2.33 = "); Serial.println(s);
  
  //2.33 - 0.66
  fix3 = fix2 - fix1;
  FP_FixedToString(fix3, s);
  Serial.print("2.33 - 0.66 = "); Serial.println(s);

  //2.33*0.66
  fix3 = FP_Multiply(fix2, fix1);
  FP_FixedToString(fix3, s);
  Serial.print("2.33 * 0.66 = "); Serial.println(s);

  //2.33/0.66
  fix3 = FP_Divide(fix2, fix1);
  FP_FixedToString(fix3, s);
  Serial.print("2.33 / 0.66 = "); Serial.println(s);

  //square root of 30.25
  fix1 = FP_Sqrt(FP_FloatToFixed(30.25));
  float1 = FP_FixedToFloat(fix1);
  Serial.print("sqrt(30.25) = "); Serial.println(float1);

  //sine of 45 degrees
  fix1 = FP_DegToRad(FP_FloatToFixed(45.0));
  fix2 = FP_Sin(fix1);
  FP_FixedToString(fix2, s);
  Serial.print("Sin(45.0) = "); Serial.println(s);
}

void loop(void) { }

[updated: 5/21/16]

Advertisements

About Jim Eli

µC experimenter
This entry was posted in Uncategorized and tagged , , , , , , , , , , , , , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s