MC100/Applications/math/fix_math.h

#ifndef _FIX_MATH_H__
#define _FIX_MATH_H__
#include "bsp/bsp.h"
#include "os/os_types.h"
#include "libs/utils.h"

typedef signed short s16q14_t;
typedef signed short s16q10_t;
typedef signed short s16q5_t;
//typedef signed short s16q4_t;
typedef int32_t   s32q4_t;
typedef int32_t   s32q14_t;
typedef int32_t   s32q5_t;
typedef int32_t   s32q19_t;

//#define S16Q4(A) (signed short)((A) * 16.0F)
#define S16Q5(A) (signed short)((A) * 32.0F)
#define S16Q10(A) (signed short)((A) * 1024.0F)
#define S16Q14(A) (signed short)((A) * 16384.0F)
#define S32Q4(A) (signed int)((A) * 16.0F)
#define S32Q14(A) (signed int)((A) * 16384.0F)
#define S32Q14_MUL(a, b) (((a)*(b)) >>14)
#define S16_mul(a, b, q) (((u32)(a)*(b)) >> q) 
/*
static __INLINE float S16Q4toF(s16q4_t v) {
	s16 num = (v >> 4) & 0xFFFF;
	u16 tail = v & 0x000F;
	float f = num + (float)tail / 16.0f;
	return f;
} 
*/
static __INLINE float S16Q5toF(s16q5_t v) {
	s16 num = (v >> 5) & 0xFFFF;
	u16 tail = v & 0x001F;
	float f = num + (float)tail / 32.0f;
	return f;
} 

static __INLINE float S16Q10toF(s16q5_t v) {
	s16 num = (v >> 10) & 0xFFFF;
	u16 tail = v & 0x03FF;
	float f = num + (float)tail / 1024.0f;
	return f;
}

static __INLINE float S32Q4toF(s32q4_t v) {
	s32 num = (v >> 4) & 0xFFFFFFFF;
	u16 tail = v & 0x000F;
	float f = num + (float)tail / 16.0f;
	return f;
} 

#define MATH_sat(in, minOut, maxOut) (min((in), MAX((in), (minOut))))

static __INLINE u16 Sqrt_Fix( u32 wInput )
{
	s32 wtemprootnew;

	u8 biter = 0u;
	s32 wtemproot;
	if ( wInput <= ( s32 )2097152 ) {
      	wtemproot = ( s32 )128;
    } else {
      	wtemproot = ( s32 )8192;
    }

    do {
      	wtemprootnew = ( wtemproot + wInput / wtemproot ) / ( s32 )2;
      	if ( wtemprootnew == wtemproot ) {
        	biter = 6u;
      	} else {
        	biter ++;
        	wtemproot = wtemprootnew;
      	}
    } while ( biter < 6u );
  	return ( wtemprootnew );
}

void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c);

#endif /* _FIX_MATH_H__ */