164 lines
2.9 KiB
C++
164 lines
2.9 KiB
C++
/**** Includes ****/
|
|
#include "utils.h"
|
|
|
|
using namespace util;
|
|
|
|
/**** Private definitions ****/
|
|
/**** Private constants ****/
|
|
/**** Private variables ****/
|
|
/**** Private function declarations ****/
|
|
#ifndef TESTING
|
|
#endif
|
|
|
|
/**** Public function definitions ****/
|
|
uint8_t util::invert(uint8_t x)
|
|
{
|
|
if(x!=0) return 0;
|
|
else return 1;
|
|
}
|
|
|
|
uint8_t util::sat_add(uint8_t x, uint8_t y)
|
|
{
|
|
uint8_t z = x + y;
|
|
// Check for overflow
|
|
if((z < x)||(z < y)) return 0xFF;
|
|
else return z;
|
|
}
|
|
|
|
uint16_t util::sat_add(uint16_t x, uint16_t y)
|
|
{
|
|
uint16_t z = x + y;
|
|
// Check for overflow
|
|
if((z < x)||(z < y)) return 0xFF;
|
|
else return z;
|
|
}
|
|
|
|
uint32_t util::sat_add(uint32_t x, uint32_t y)
|
|
|
|
{
|
|
uint32_t z = x + y;
|
|
// Check for overflow
|
|
if((z < x)||(z < y)) return 0xFF;
|
|
else return z;
|
|
}
|
|
|
|
uint8_t util::sat_subtract(uint8_t x, uint8_t y)
|
|
{
|
|
uint8_t z = x - y;
|
|
// Check for underflow
|
|
if(z > x) return 0;
|
|
else return z;
|
|
}
|
|
|
|
uint16_t util::sat_subtract(uint16_t x, uint16_t y)
|
|
{
|
|
uint16_t z = x - y;
|
|
// Check for underflow
|
|
if(z > x) return 0;
|
|
else return z;
|
|
}
|
|
uint32_t util::sat_subtract(uint32_t x, uint32_t y)
|
|
{
|
|
uint32_t z = x - y;
|
|
// Check for underflow
|
|
if(z > x) return 0;
|
|
else return z;
|
|
}
|
|
|
|
uint8_t util::abs_subtract(uint8_t x, uint8_t y)
|
|
{
|
|
if(x > y) return x - y;
|
|
else return y-x;
|
|
}
|
|
|
|
uint16_t util::abs_subtract(uint16_t x, uint16_t y)
|
|
{
|
|
if(x > y) return x - y;
|
|
else return y-x;
|
|
}
|
|
|
|
uint32_t util::abs_subtract(uint32_t x, uint32_t y)
|
|
{
|
|
if(x > y) return x - y;
|
|
else return y-x;
|
|
}
|
|
|
|
|
|
uint16_t util::sat_cast(uint32_t x)
|
|
{
|
|
if(x > 0x0000FFFF) return 0xFFFF;
|
|
else return (uint16_t)x;
|
|
}
|
|
|
|
uint16_t util::sat_cast(int32_t x)
|
|
{
|
|
if(x < 0) return 0x0000;
|
|
else if(x > 0x0000FFFF) return 0xFFFF;
|
|
else return (uint16_t)x;
|
|
}
|
|
|
|
uint16_t util::convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
|
|
{
|
|
int32_t temp = (int32_t)raw;
|
|
|
|
temp = temp * mul;
|
|
if(div>1) temp /= div;
|
|
temp += offset;
|
|
|
|
return sat_cast(temp);
|
|
}
|
|
|
|
uint16_t util::sat_mul_kilo(uint16_t xk, uint16_t yk)
|
|
{
|
|
uint32_t temp = (uint32_t)xk * (uint32_t)yk;
|
|
temp /= 1000;
|
|
|
|
return sat_cast(temp);
|
|
}
|
|
|
|
uint16_t util::sat_div_kilo(uint16_t top, uint16_t bot)
|
|
{
|
|
//Sanity check bot
|
|
if(bot==0) return 0xFFFF; //aka infinity
|
|
|
|
uint32_t temp = (uint32_t)top * 1000;
|
|
temp /= (uint32_t)bot;
|
|
|
|
return sat_cast(temp);
|
|
}
|
|
|
|
uint16_t util::sat_ratio(uint16_t top, uint16_t bot)
|
|
{
|
|
//Sanity check bot
|
|
if(bot==0) return 0xFFFF; //aka infinity
|
|
|
|
//Easy option
|
|
if(top>=bot) return 0xFFFF;
|
|
|
|
uint32_t temp = (uint32_t)top * 0x0000FFFF;
|
|
temp /= (uint32_t)bot;
|
|
|
|
return sat_cast(temp);
|
|
}
|
|
|
|
uint16_t util::percent_to_16b(uint8_t percent)
|
|
{
|
|
uint32_t temp = (uint32_t)percent * 0x0000FFFF;
|
|
temp /= 100;
|
|
|
|
// Limit to 16 bits
|
|
uint16_t pwm = sat_cast(temp);
|
|
|
|
return pwm;
|
|
}
|
|
|
|
uint16_t util::percent_of(uint8_t percent, uint16_t value)
|
|
{
|
|
if(percent == 0) return 0;
|
|
else if(percent >= 100) return value;
|
|
|
|
uint32_t temp = (uint32_t)value * percent;
|
|
return temp/100;
|
|
}
|
|
|
|
/**** Private function definitions ****/ |