From 69ac57abb48421f931e611e88c532eebcc09ff72 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Xose=20P=C3=A9rez?= Date: Thu, 14 Jun 2018 11:32:35 +0200 Subject: [PATCH] Using alternative methods for maths, saving ~8Kb with lights --- code/espurna/libs/fs_math.c | 636 ++++++++++++++++++++++++++++ code/espurna/libs/fs_math.h | 116 +++++ code/espurna/{ => libs}/pwm.c | 0 code/espurna/light.ino | 13 +- code/espurna/sensors/EmonSensor.h | 6 +- code/espurna/sensors/V9261FSensor.h | 5 +- 6 files changed, 768 insertions(+), 8 deletions(-) create mode 100644 code/espurna/libs/fs_math.c create mode 100644 code/espurna/libs/fs_math.h rename code/espurna/{ => libs}/pwm.c (100%) diff --git a/code/espurna/libs/fs_math.c b/code/espurna/libs/fs_math.c new file mode 100644 index 00000000..8f45eb26 --- /dev/null +++ b/code/espurna/libs/fs_math.c @@ -0,0 +1,636 @@ +/** + * This code is available at + * http://www.mindspring.com/~pfilandr/C/fs_math/ + * and it is believed to be public domain. + */ + +/* BEGIN fs_math.c */ + +#include "fs_math.h" + +#include +/* +** pi == (atan(1.0 / 3) + atan(1.0 / 2)) * 4 +*/ +static double fs_pi(void); +static long double fs_pil(void); + +double fs_sqrt(double x) +{ + int n; + double a, b; + + if (x > 0 && DBL_MAX >= x) { + for (n = 0; x > 2; x /= 4) { + ++n; + } + while (0.5 > x) { + --n; + x *= 4; + } + a = x; + b = (1 + x) / 2; + do { + x = b; + b = (a / x + x) / 2; + } while (x > b); + while (n > 0) { + x *= 2; + --n; + } + while (0 > n) { + x /= 2; + ++n; + } + } else { + if (x != 0) { + x = DBL_MAX; + } + } + return x; +} + +double fs_log(double x) +{ + int n; + double a, b, c, epsilon; + static double A, B, C; + static int initialized; + + if (x > 0 && DBL_MAX >= x) { + if (!initialized) { + initialized = 1; + A = fs_sqrt(2); + B = A / 2; + C = fs_log(A); + } + for (n = 0; x > A; x /= 2) { + ++n; + } + while (B > x) { + --n; + x *= 2; + } + a = (x - 1) / (x + 1); + x = C * n + a; + c = a * a; + n = 1; + epsilon = DBL_EPSILON * x; + if (0 > a) { + if (epsilon > 0) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (epsilon > b); + } else { + if (0 > epsilon) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (b > epsilon); + } + x *= 2; + } else { + x = -DBL_MAX; + } + return x; +} + +double fs_log10(double x) +{ + static double log_10; + static int initialized; + + if (!initialized) { + initialized = 1; + log_10 = fs_log(10); + } + return x > 0 && DBL_MAX >= x ? fs_log(x) / log_10 : fs_log(x); +} + +double fs_exp(double x) +{ + unsigned n, square; + double b, e; + static double x_max, x_min, epsilon; + static int initialized; + + if (!initialized) { + initialized = 1; + x_max = fs_log(DBL_MAX); + x_min = fs_log(DBL_MIN); + epsilon = DBL_EPSILON / 4; + } + if (x_max >= x && x >= x_min) { + for (square = 0; x > 1; x /= 2) { + ++square; + } + while (-1 > x) { + ++square; + x /= 2; + } + e = b = n = 1; + do { + b /= n++; + b *= x; + e += b; + b /= n++; + b *= x; + e += b; + } while (b > epsilon); + while (square-- != 0) { + e *= e; + } + } else { + e = x > 0 ? DBL_MAX : 0; + } + return e; +} + +double fs_modf(double value, double *iptr) +{ + double a, b; + const double c = value; + + if (0 > c) { + value = -value; + } + if (DBL_MAX >= value) { + for (*iptr = 0; value >= 1; value -= b) { + a = value / 2; + b = 1; + while (a >= b) { + b *= 2; + } + *iptr += b; + } + } else { + *iptr = value; + value = 0; + } + if (0 > c) { + *iptr = -*iptr; + value = -value; + } + return value; +} + +double fs_fmod(double x, double y) +{ + double a, b; + const double c = x; + + if (0 > c) { + x = -x; + } + if (0 > y) { + y = -y; + } + if (y != 0 && DBL_MAX >= y && DBL_MAX >= x) { + while (x >= y) { + a = x / 2; + b = y; + while (a >= b) { + b *= 2; + } + x -= b; + } + } else { + x = 0; + } + return 0 > c ? -x : x; +} + +double fs_pow(double x, double y) +{ + double p = 0; + + if (0 > x && fs_fmod(y, 1) == 0) { + if (fs_fmod(y, 2) == 0) { + p = fs_exp(fs_log(-x) * y); + } else { + p = -fs_exp(fs_log(-x) * y); + } + } else { + if (x != 0 || 0 >= y) { + p = fs_exp(fs_log( x) * y); + } + } + return p; +} + +static double fs_pi(void) +{ + unsigned n; + double a, b, epsilon; + static double p; + static int initialized; + + if (!initialized) { + initialized = 1; + epsilon = DBL_EPSILON / 4; + n = 1; + a = 3; + do { + a /= 9; + b = a / n; + n += 2; + a /= 9; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + epsilon = DBL_EPSILON / 2; + n = 1; + a = 2; + do { + a /= 4; + b = a / n; + n += 2; + a /= 4; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + p *= 4; + } + return p; +} + +double fs_cos(double x) +{ + unsigned n; + int negative, sine; + double a, b, c; + static double pi, two_pi, half_pi, third_pi, epsilon; + static int initialized; + + if (0 > x) { + x = -x; + } + if (DBL_MAX >= x) { + if (!initialized) { + initialized = 1; + pi = fs_pi(); + two_pi = 2 * pi; + half_pi = pi / 2; + third_pi = pi / 3; + epsilon = DBL_EPSILON / 2; + } + if (x > two_pi) { + x = fs_fmod(x, two_pi); + } + if (x > pi) { + x = two_pi - x; + } + if (x > half_pi) { + x = pi - x; + negative = 1; + } else { + negative = 0; + } + if (x > third_pi) { + x = half_pi - x; + sine = 1; + } else { + sine = 0; + } + c = x * x; + x = n = 0; + a = 1; + do { + b = a; + a *= c; + a /= ++n; + a /= ++n; + b -= a; + a *= c; + a /= ++n; + a /= ++n; + x += b; + } while (b > epsilon); + if (sine) { + x = fs_sqrt((1 - x) * (1 + x)); + } + if (negative) { + x = -x; + } + } else { + x = -DBL_MAX; + } + return x; +} + +double fs_log2(double x) +{ + static double log_2; + static int initialized; + + if (!initialized) { + initialized = 1; + log_2 = fs_log(2); + } + return x > 0 && DBL_MAX >= x ? fs_log(x) / log_2 : fs_log(x); +} + +double fs_exp2(double x) +{ + static double log_2; + static int initialized; + + if (!initialized) { + initialized = 1; + log_2 = fs_log(2); + } + return fs_exp(x * log_2); +} + +long double fs_powl(long double x, long double y) +{ + long double p; + + if (0 > x && fs_fmodl(y, 1) == 0) { + if (fs_fmodl(y, 2) == 0) { + p = fs_expl(fs_logl(-x) * y); + } else { + p = -fs_expl(fs_logl(-x) * y); + } + } else { + if (x != 0 || 0 >= y) { + p = fs_expl(fs_logl( x) * y); + } else { + p = 0; + } + } + return p; +} + +long double fs_sqrtl(long double x) +{ + long int n; + long double a, b; + + if (x > 0 && LDBL_MAX >= x) { + for (n = 0; x > 2; x /= 4) { + ++n; + } + while (0.5 > x) { + --n; + x *= 4; + } + a = x; + b = (1 + x) / 2; + do { + x = b; + b = (a / x + x) / 2; + } while (x > b); + while (n > 0) { + x *= 2; + --n; + } + while (0 > n) { + x /= 2; + ++n; + } + } else { + if (x != 0) { + x = LDBL_MAX; + } + } + return x; +} + +long double fs_logl(long double x) +{ + long int n; + long double a, b, c, epsilon; + static long double A, B, C; + static int initialized; + + if (x > 0 && LDBL_MAX >= x) { + if (!initialized) { + initialized = 1; + B = 1.5; + do { + A = B; + B = 1 / A + A / 2; + } while (A > B); + B /= 2; + C = fs_logl(A); + } + for (n = 0; x > A; x /= 2) { + ++n; + } + while (B > x) { + --n; + x *= 2; + } + a = (x - 1) / (x + 1); + x = C * n + a; + c = a * a; + n = 1; + epsilon = LDBL_EPSILON * x; + if (0 > a) { + if (epsilon > 0) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (epsilon > b); + } else { + if (0 > epsilon) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (b > epsilon); + } + x *= 2; + } else { + x = -LDBL_MAX; + } + return x; +} + +long double fs_expl(long double x) +{ + long unsigned n, square; + long double b, e; + static long double x_max, x_min, epsilon; + static int initialized; + + if (!initialized) { + initialized = 1; + x_max = fs_logl(LDBL_MAX); + x_min = fs_logl(LDBL_MIN); + epsilon = LDBL_EPSILON / 4; + } + if (x_max >= x && x >= x_min) { + for (square = 0; x > 1; x /= 2) { + ++square; + } + while (-1 > x) { + ++square; + x /= 2; + } + e = b = n = 1; + do { + b /= n++; + b *= x; + e += b; + b /= n++; + b *= x; + e += b; + } while (b > epsilon); + while (square-- != 0) { + e *= e; + } + } else { + e = x > 0 ? LDBL_MAX : 0; + } + return e; +} + +static long double fs_pil(void) +{ + long unsigned n; + long double a, b, epsilon; + static long double p; + static int initialized; + + if (!initialized) { + initialized = 1; + epsilon = LDBL_EPSILON / 4; + n = 1; + a = 3; + do { + a /= 9; + b = a / n; + n += 2; + a /= 9; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + epsilon = LDBL_EPSILON / 2; + n = 1; + a = 2; + do { + a /= 4; + b = a / n; + n += 2; + a /= 4; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + p *= 4; + } + return p; +} + +long double fs_cosl(long double x) +{ + long unsigned n; + int negative, sine; + long double a, b, c; + static long double pi, two_pi, half_pi, third_pi, epsilon; + static int initialized; + + if (0 > x) { + x = -x; + } + if (LDBL_MAX >= x) { + if (!initialized) { + initialized = 1; + pi = fs_pil(); + two_pi = 2 * pi; + half_pi = pi / 2; + third_pi = pi / 3; + epsilon = LDBL_EPSILON / 2; + } + if (x > two_pi) { + x = fs_fmodl(x, two_pi); + } + if (x > pi) { + x = two_pi - x; + } + if (x > half_pi) { + x = pi - x; + negative = 1; + } else { + negative = 0; + } + if (x > third_pi) { + x = half_pi - x; + sine = 1; + } else { + sine = 0; + } + c = x * x; + x = n = 0; + a = 1; + do { + b = a; + a *= c; + a /= ++n; + a /= ++n; + b -= a; + a *= c; + a /= ++n; + a /= ++n; + x += b; + } while (b > epsilon); + if (sine) { + x = fs_sqrtl((1 - x) * (1 + x)); + } + if (negative) { + x = -x; + } + } else { + x = -LDBL_MAX; + } + return x; +} + +long double fs_fmodl(long double x, long double y) +{ + long double a, b; + const long double c = x; + + if (0 > c) { + x = -x; + } + if (0 > y) { + y = -y; + } + if (y != 0 && LDBL_MAX >= y && LDBL_MAX >= x) { + while (x >= y) { + a = x / 2; + b = y; + while (a >= b) { + b *= 2; + } + x -= b; + } + } else { + x = 0; + } + return 0 > c ? -x : x; +} + +/* END fs_math.c */ diff --git a/code/espurna/libs/fs_math.h b/code/espurna/libs/fs_math.h new file mode 100644 index 00000000..1884fcdd --- /dev/null +++ b/code/espurna/libs/fs_math.h @@ -0,0 +1,116 @@ +/** + * This code is available at + * http://www.mindspring.com/~pfilandr/C/fs_math/ + * and it is believed to be public domain. + */ + +/* BEGIN fs_math.h */ +/* +** Portable freestanding code. +*/ +#ifndef H_FS_MATH_H +#define H_FS_MATH_H + +double fs_sqrt(double x); +double fs_log(double x); +double fs_log10(double x); +/* +** exp(x) = 1 + x + x^2/2! + x^3/3! + ... +*/ +double fs_exp(double x); +double fs_modf(double value, double *iptr); +double fs_fmod(double x, double y); +double fs_pow(double x, double y); +double fs_cos(double x); +/* +** C99 +*/ +double fs_log2(double x); +double fs_exp2(double x); +long double fs_powl(long double x, long double y); +long double fs_sqrtl(long double x); +long double fs_logl(long double x); +long double fs_expl(long double x); +long double fs_cosl(long double x); +long double fs_fmodl(long double x, long double y); + +#endif + +/* END fs_math.h */ + +#if 0 + +/* +> > Anybody know where I can get some source code for a +> > reasonably fast double +> > precision square root algorithm in C. +> > I'm looking for one that is not IEEE +> > compliant as I am running on a Z/OS mainframe. +> > +> > I would love to use the standard library but +> > unfortunatly I'm using a +> > stripped down version of C that looses the the runtime library +> > (we have to write our own). +> +> long double Ssqrt(long double x) +> { +> long double a, b; +> size_t c; + +size_t is a bug here. +c needs to be a signed type: + long c; + +> if (x > 0) { +> c = 0; +> while (x > 4) { +> x /= 4; +> ++c; +> } +> while (1.0 / 4 > x) { +> x *= 4; +> --c; +> } +> a = x; +> b = ((4 > a) + a) / 2; + +Not a bug, but should be: + b = (1 + a) / 2; + +> do { +> x = b; +> b = (a / x + x) / 2; +> } while (x > b); +> if (c > 0) { + +The above line is why c needs to be a signed type, +otherwise the decremented values of c, are greater than zero, +and the function won't work if the initial value of x +is less than 0.25 + +> while (c--) { +> x *= 2; +> } +> } else { +> while (c++) { +> x /= 2; +> } +> } +> } +> return x; +> } + +> +> > +> > That algorithm was actually 4 times slower +> > then the one below, and more +> > code. It was accurate though. +> > +> +> Sorry Pete, I wasn't looking very carefully. +> When I converted your function +> to double precision it's was much quicker, the best I've seen yet. + +*/ + +#endif diff --git a/code/espurna/pwm.c b/code/espurna/libs/pwm.c similarity index 100% rename from code/espurna/pwm.c rename to code/espurna/libs/pwm.c diff --git a/code/espurna/light.ino b/code/espurna/light.ino index 28a6293d..8d33f637 100644 --- a/code/espurna/light.ino +++ b/code/espurna/light.ino @@ -12,6 +12,10 @@ Copyright (C) 2016-2018 by Xose Pérez #include #include +extern "C" { + #include "libs/fs_math.h" +} + #if LIGHT_PROVIDER == LIGHT_PROVIDER_DIMMER #define PWM_CHANNEL_NUM_MAX LIGHT_CHANNELS extern "C" { @@ -288,19 +292,18 @@ void _fromKelvin(unsigned long kelvin, bool setMireds) { return; } - // Calculate colors unsigned int red = (kelvin <= 66) ? LIGHT_MAX_VALUE - : 329.698727446 * pow((kelvin - 60), -0.1332047592); + : 329.698727446 * fs_pow((double) (kelvin - 60), -0.1332047592); unsigned int green = (kelvin <= 66) - ? 99.4708025861 * log(kelvin) - 161.1195681661 - : 288.1221695283 * pow(kelvin, -0.0755148492); + ? 99.4708025861 * fs_log(kelvin) - 161.1195681661 + : 288.1221695283 * fs_pow((double) kelvin, -0.0755148492); unsigned int blue = (kelvin >= 66) ? LIGHT_MAX_VALUE : ((kelvin <= 19) ? 0 - : 138.5177312231 * log(kelvin - 10) - 305.0447927307); + : 138.5177312231 * fs_log(kelvin - 10) - 305.0447927307); _setRGBInputValue(red, green, blue); } diff --git a/code/espurna/sensors/EmonSensor.h b/code/espurna/sensors/EmonSensor.h index 0bf2674c..527a4bda 100644 --- a/code/espurna/sensors/EmonSensor.h +++ b/code/espurna/sensors/EmonSensor.h @@ -10,9 +10,11 @@ #undef I2C_SUPPORT #define I2C_SUPPORT 1 // Explicitly request I2C support. - #include "Arduino.h" #include "I2CSensor.h" +extern "C" { + #include "libs/fs_math.h" +} class EmonSensor : public I2CSensor { @@ -197,7 +199,7 @@ class EmonSensor : public I2CSensor { } // Calculate current - double rms = _samples > 0 ? sqrt(sum / _samples) : 0; + double rms = _samples > 0 ? fs_sqrt(sum / _samples) : 0; double current = _current_factor[channel] * rms; current = (double) (int(current * _multiplier[channel]) - 1) / _multiplier[channel]; if (current < 0) current = 0; diff --git a/code/espurna/sensors/V9261FSensor.h b/code/espurna/sensors/V9261FSensor.h index c80ab44b..67068c17 100644 --- a/code/espurna/sensors/V9261FSensor.h +++ b/code/espurna/sensors/V9261FSensor.h @@ -9,6 +9,9 @@ #include "Arduino.h" #include "BaseSensor.h" +extern "C" { + #include "libs/fs_math.h" +} #include @@ -203,7 +206,7 @@ class V9261FSensor : public BaseSensor { if (_voltage < 0) _voltage = 0; if (_current < 0) _current = 0; - _apparent = sqrt(_reactive * _reactive + _active * _active); + _apparent = fs_sqrt(_reactive * _reactive + _active * _active); }