Xose Pérez
6 years ago
5 changed files with 1088 additions and 1173 deletions
Unified View
Diff Options
-
+0 -636code/espurna/libs/fs_math.c
-
+630 -80code/espurna/libs/fs_math.h
-
+0 -447code/espurna/libs/pwm.c
-
+457 -9code/espurna/libs/pwm.h
-
+1 -1code/espurna/web.ino
+ 0
- 636
code/espurna/libs/fs_math.c
View File
| @ -1,636 +0,0 @@ | |||||
| /** | |||||
| * 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 <float.h> | |||||
| /* | |||||
| ** 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 */ | |||||
+ 630
- 80
code/espurna/libs/fs_math.h
View File
+ 0
- 447
code/espurna/libs/pwm.c
View File
| @ -1,447 +0,0 @@ | |||||
| /* | |||||
| * Copyright (C) 2016 Stefan Brüns <stefan.bruens@rwth-aachen.de> | |||||
| * | |||||
| * This program is free software; you can redistribute it and/or modify | |||||
| * it under the terms of the GNU General Public License as published by | |||||
| * the Free Software Foundation; either version 2 of the License, or | |||||
| * (at your option) any later version. | |||||
| * | |||||
| * This program is distributed in the hope that it will be useful, | |||||
| * but WITHOUT ANY WARRANTY; without even the implied warranty of | |||||
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |||||
| * GNU General Public License for more details. | |||||
| * | |||||
| * You should have received a copy of the GNU General Public License | |||||
| * along with this program; if not, write to the Free Software | |||||
| * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | |||||
| */ | |||||
| /* Set the following three defines to your needs */ | |||||
| #ifndef SDK_PWM_PERIOD_COMPAT_MODE | |||||
| #define SDK_PWM_PERIOD_COMPAT_MODE 0 | |||||
| #endif | |||||
| #ifndef PWM_MAX_CHANNELS | |||||
| #define PWM_MAX_CHANNELS 8 | |||||
| #endif | |||||
| #define PWM_DEBUG 0 | |||||
| #define PWM_USE_NMI 1 | |||||
| /* no user servicable parts beyond this point */ | |||||
| #define PWM_MAX_TICKS 0x7fffff | |||||
| #if SDK_PWM_PERIOD_COMPAT_MODE | |||||
| #define PWM_PERIOD_TO_TICKS(x) (x * 0.2) | |||||
| #define PWM_DUTY_TO_TICKS(x) (x * 5) | |||||
| #define PWM_MAX_DUTY (PWM_MAX_TICKS * 0.2) | |||||
| #define PWM_MAX_PERIOD (PWM_MAX_TICKS * 5) | |||||
| #else | |||||
| #define PWM_PERIOD_TO_TICKS(x) (x) | |||||
| #define PWM_DUTY_TO_TICKS(x) (x) | |||||
| #define PWM_MAX_DUTY PWM_MAX_TICKS | |||||
| #define PWM_MAX_PERIOD PWM_MAX_TICKS | |||||
| #endif | |||||
| #include <c_types.h> | |||||
| #include <pwm.h> | |||||
| #include <eagle_soc.h> | |||||
| #include <ets_sys.h> | |||||
| // from SDK hw_timer.c | |||||
| #define TIMER1_DIVIDE_BY_16 0x0004 | |||||
| #define TIMER1_ENABLE_TIMER 0x0080 | |||||
| struct pwm_phase { | |||||
| uint32_t ticks; ///< delay until next phase, in 200ns units | |||||
| uint16_t on_mask; ///< GPIO mask to switch on | |||||
| uint16_t off_mask; ///< GPIO mask to switch off | |||||
| }; | |||||
| /* Three sets of PWM phases, the active one, the one used | |||||
| * starting with the next cycle, and the one updated | |||||
| * by pwm_start. After the update pwm_next_set | |||||
| * is set to the last updated set. pwm_current_set is set to | |||||
| * pwm_next_set from the interrupt routine during the first | |||||
| * pwm phase | |||||
| */ | |||||
| typedef struct pwm_phase (pwm_phase_array)[PWM_MAX_CHANNELS + 2]; | |||||
| static pwm_phase_array pwm_phases[3]; | |||||
| static struct { | |||||
| struct pwm_phase* next_set; | |||||
| struct pwm_phase* current_set; | |||||
| uint8_t current_phase; | |||||
| } pwm_state; | |||||
| static uint32_t pwm_period; | |||||
| static uint32_t pwm_period_ticks; | |||||
| static uint32_t pwm_duty[PWM_MAX_CHANNELS]; | |||||
| static uint16_t gpio_mask[PWM_MAX_CHANNELS]; | |||||
| static uint8_t pwm_channels; | |||||
| // 3-tuples of MUX_REGISTER, MUX_VALUE and GPIO number | |||||
| typedef uint32_t (pin_info_type)[3]; | |||||
| struct gpio_regs { | |||||
| uint32_t out; /* 0x60000300 */ | |||||
| uint32_t out_w1ts; /* 0x60000304 */ | |||||
| uint32_t out_w1tc; /* 0x60000308 */ | |||||
| uint32_t enable; /* 0x6000030C */ | |||||
| uint32_t enable_w1ts; /* 0x60000310 */ | |||||
| uint32_t enable_w1tc; /* 0x60000314 */ | |||||
| uint32_t in; /* 0x60000318 */ | |||||
| uint32_t status; /* 0x6000031C */ | |||||
| uint32_t status_w1ts; /* 0x60000320 */ | |||||
| uint32_t status_w1tc; /* 0x60000324 */ | |||||
| }; | |||||
| static struct gpio_regs* gpio = (struct gpio_regs*)(0x60000300); | |||||
| struct timer_regs { | |||||
| uint32_t frc1_load; /* 0x60000600 */ | |||||
| uint32_t frc1_count; /* 0x60000604 */ | |||||
| uint32_t frc1_ctrl; /* 0x60000608 */ | |||||
| uint32_t frc1_int; /* 0x6000060C */ | |||||
| uint8_t pad[16]; | |||||
| uint32_t frc2_load; /* 0x60000620 */ | |||||
| uint32_t frc2_count; /* 0x60000624 */ | |||||
| uint32_t frc2_ctrl; /* 0x60000628 */ | |||||
| uint32_t frc2_int; /* 0x6000062C */ | |||||
| uint32_t frc2_alarm; /* 0x60000630 */ | |||||
| }; | |||||
| static struct timer_regs* timer = (struct timer_regs*)(0x60000600); | |||||
| static void ICACHE_RAM_ATTR | |||||
| pwm_intr_handler(void) | |||||
| { | |||||
| if ((pwm_state.current_set[pwm_state.current_phase].off_mask == 0) && | |||||
| (pwm_state.current_set[pwm_state.current_phase].on_mask == 0)) { | |||||
| pwm_state.current_set = pwm_state.next_set; | |||||
| pwm_state.current_phase = 0; | |||||
| } | |||||
| do { | |||||
| // force write to GPIO registers on each loop | |||||
| asm volatile ("" : : : "memory"); | |||||
| gpio->out_w1ts = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].on_mask); | |||||
| gpio->out_w1tc = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].off_mask); | |||||
| uint32_t ticks = pwm_state.current_set[pwm_state.current_phase].ticks; | |||||
| pwm_state.current_phase++; | |||||
| if (ticks) { | |||||
| if (ticks >= 16) { | |||||
| // constant interrupt overhead | |||||
| ticks -= 9; | |||||
| timer->frc1_int &= ~FRC1_INT_CLR_MASK; | |||||
| WRITE_PERI_REG(&timer->frc1_load, ticks); | |||||
| return; | |||||
| } | |||||
| ticks *= 4; | |||||
| do { | |||||
| ticks -= 1; | |||||
| // stop compiler from optimizing delay loop to noop | |||||
| asm volatile ("" : : : "memory"); | |||||
| } while (ticks > 0); | |||||
| } | |||||
| } while (1); | |||||
| } | |||||
| /** | |||||
| * period: initial period (base unit 1us OR 200ns) | |||||
| * duty: array of initial duty values, may be NULL, may be freed after pwm_init | |||||
| * pwm_channel_num: number of channels to use | |||||
| * pin_info_list: array of pin_info | |||||
| */ | |||||
| void ICACHE_FLASH_ATTR | |||||
| pwm_init(uint32_t period, uint32_t *duty, uint32_t pwm_channel_num, | |||||
| uint32_t (*pin_info_list)[3]) | |||||
| { | |||||
| int i, j, n; | |||||
| pwm_channels = pwm_channel_num; | |||||
| if (pwm_channels > PWM_MAX_CHANNELS) | |||||
| pwm_channels = PWM_MAX_CHANNELS; | |||||
| for (i = 0; i < 3; i++) { | |||||
| for (j = 0; j < (PWM_MAX_CHANNELS + 2); j++) { | |||||
| pwm_phases[i][j].ticks = 0; | |||||
| pwm_phases[i][j].on_mask = 0; | |||||
| pwm_phases[i][j].off_mask = 0; | |||||
| } | |||||
| } | |||||
| pwm_state.current_set = pwm_state.next_set = 0; | |||||
| pwm_state.current_phase = 0; | |||||
| uint32_t all = 0; | |||||
| // PIN info: MUX-Register, Mux-Setting, PIN-Nr | |||||
| for (n = 0; n < pwm_channels; n++) { | |||||
| pin_info_type* pin_info = &pin_info_list[n]; | |||||
| PIN_FUNC_SELECT((*pin_info)[0], (*pin_info)[1]); | |||||
| gpio_mask[n] = 1 << (*pin_info)[2]; | |||||
| all |= 1 << (*pin_info)[2]; | |||||
| if (duty) | |||||
| pwm_set_duty(duty[n], n); | |||||
| } | |||||
| GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, all); | |||||
| GPIO_REG_WRITE(GPIO_ENABLE_W1TS_ADDRESS, all); | |||||
| pwm_set_period(period); | |||||
| #if PWM_USE_NMI | |||||
| ETS_FRC_TIMER1_NMI_INTR_ATTACH(pwm_intr_handler); | |||||
| #else | |||||
| ETS_FRC_TIMER1_INTR_ATTACH(pwm_intr_handler, NULL); | |||||
| #endif | |||||
| TM1_EDGE_INT_ENABLE(); | |||||
| timer->frc1_int &= ~FRC1_INT_CLR_MASK; | |||||
| timer->frc1_ctrl = 0; | |||||
| pwm_start(); | |||||
| } | |||||
| __attribute__ ((noinline)) | |||||
| static uint8_t ICACHE_FLASH_ATTR | |||||
| _pwm_phases_prep(struct pwm_phase* pwm) | |||||
| { | |||||
| uint8_t n, phases; | |||||
| for (n = 0; n < pwm_channels + 2; n++) { | |||||
| pwm[n].ticks = 0; | |||||
| pwm[n].on_mask = 0; | |||||
| pwm[n].off_mask = 0; | |||||
| } | |||||
| phases = 1; | |||||
| for (n = 0; n < pwm_channels; n++) { | |||||
| uint32_t ticks = PWM_DUTY_TO_TICKS(pwm_duty[n]); | |||||
| if (ticks == 0) { | |||||
| pwm[0].off_mask |= gpio_mask[n]; | |||||
| } else if (ticks >= pwm_period_ticks) { | |||||
| pwm[0].on_mask |= gpio_mask[n]; | |||||
| } else { | |||||
| if (ticks < (pwm_period_ticks/2)) { | |||||
| pwm[phases].ticks = ticks; | |||||
| pwm[0].on_mask |= gpio_mask[n]; | |||||
| pwm[phases].off_mask = gpio_mask[n]; | |||||
| } else { | |||||
| pwm[phases].ticks = pwm_period_ticks - ticks; | |||||
| pwm[phases].on_mask = gpio_mask[n]; | |||||
| pwm[0].off_mask |= gpio_mask[n]; | |||||
| } | |||||
| phases++; | |||||
| } | |||||
| } | |||||
| pwm[phases].ticks = pwm_period_ticks; | |||||
| // bubble sort, lowest to hightest duty | |||||
| n = 2; | |||||
| while (n < phases) { | |||||
| if (pwm[n].ticks < pwm[n - 1].ticks) { | |||||
| struct pwm_phase t = pwm[n]; | |||||
| pwm[n] = pwm[n - 1]; | |||||
| pwm[n - 1] = t; | |||||
| if (n > 2) | |||||
| n--; | |||||
| } else { | |||||
| n++; | |||||
| } | |||||
| } | |||||
| #if PWM_DEBUG | |||||
| int t = 0; | |||||
| for (t = 0; t <= phases; t++) { | |||||
| ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); | |||||
| } | |||||
| #endif | |||||
| // shift left to align right edge; | |||||
| uint8_t l = 0, r = 1; | |||||
| while (r <= phases) { | |||||
| uint32_t diff = pwm[r].ticks - pwm[l].ticks; | |||||
| if (diff && (diff <= 16)) { | |||||
| uint16_t mask = pwm[r].on_mask | pwm[r].off_mask; | |||||
| pwm[l].off_mask ^= pwm[r].off_mask; | |||||
| pwm[l].on_mask ^= pwm[r].on_mask; | |||||
| pwm[0].off_mask ^= pwm[r].on_mask; | |||||
| pwm[0].on_mask ^= pwm[r].off_mask; | |||||
| pwm[r].ticks = pwm_period_ticks - diff; | |||||
| pwm[r].on_mask ^= mask; | |||||
| pwm[r].off_mask ^= mask; | |||||
| } else { | |||||
| l = r; | |||||
| } | |||||
| r++; | |||||
| } | |||||
| #if PWM_DEBUG | |||||
| for (t = 0; t <= phases; t++) { | |||||
| ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); | |||||
| } | |||||
| #endif | |||||
| // sort again | |||||
| n = 2; | |||||
| while (n <= phases) { | |||||
| if (pwm[n].ticks < pwm[n - 1].ticks) { | |||||
| struct pwm_phase t = pwm[n]; | |||||
| pwm[n] = pwm[n - 1]; | |||||
| pwm[n - 1] = t; | |||||
| if (n > 2) | |||||
| n--; | |||||
| } else { | |||||
| n++; | |||||
| } | |||||
| } | |||||
| // merge same duty | |||||
| l = 0, r = 1; | |||||
| while (r <= phases) { | |||||
| if (pwm[r].ticks == pwm[l].ticks) { | |||||
| pwm[l].off_mask |= pwm[r].off_mask; | |||||
| pwm[l].on_mask |= pwm[r].on_mask; | |||||
| pwm[r].on_mask = 0; | |||||
| pwm[r].off_mask = 0; | |||||
| } else { | |||||
| l++; | |||||
| if (l != r) { | |||||
| struct pwm_phase t = pwm[l]; | |||||
| pwm[l] = pwm[r]; | |||||
| pwm[r] = t; | |||||
| } | |||||
| } | |||||
| r++; | |||||
| } | |||||
| phases = l; | |||||
| #if PWM_DEBUG | |||||
| for (t = 0; t <= phases; t++) { | |||||
| ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); | |||||
| } | |||||
| #endif | |||||
| // transform absolute end time to phase durations | |||||
| for (n = 0; n < phases; n++) { | |||||
| pwm[n].ticks = | |||||
| pwm[n + 1].ticks - pwm[n].ticks; | |||||
| // subtract common overhead | |||||
| pwm[n].ticks--; | |||||
| } | |||||
| pwm[phases].ticks = 0; | |||||
| // do a cyclic shift if last phase is short | |||||
| if (pwm[phases - 1].ticks < 16) { | |||||
| for (n = 0; n < phases - 1; n++) { | |||||
| struct pwm_phase t = pwm[n]; | |||||
| pwm[n] = pwm[n + 1]; | |||||
| pwm[n + 1] = t; | |||||
| } | |||||
| } | |||||
| #if PWM_DEBUG | |||||
| for (t = 0; t <= phases; t++) { | |||||
| ets_printf("%d +%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); | |||||
| } | |||||
| ets_printf("\n"); | |||||
| #endif | |||||
| return phases; | |||||
| } | |||||
| void ICACHE_FLASH_ATTR | |||||
| pwm_start(void) | |||||
| { | |||||
| pwm_phase_array* pwm = &pwm_phases[0]; | |||||
| if ((*pwm == pwm_state.next_set) || | |||||
| (*pwm == pwm_state.current_set)) | |||||
| pwm++; | |||||
| if ((*pwm == pwm_state.next_set) || | |||||
| (*pwm == pwm_state.current_set)) | |||||
| pwm++; | |||||
| uint8_t phases = _pwm_phases_prep(*pwm); | |||||
| // all with 0% / 100% duty - stop timer | |||||
| if (phases == 1) { | |||||
| if (pwm_state.next_set) { | |||||
| #if PWM_DEBUG | |||||
| ets_printf("PWM stop\n"); | |||||
| #endif | |||||
| timer->frc1_ctrl = 0; | |||||
| ETS_FRC1_INTR_DISABLE(); | |||||
| } | |||||
| pwm_state.next_set = NULL; | |||||
| GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, (*pwm)[0].on_mask); | |||||
| GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, (*pwm)[0].off_mask); | |||||
| return; | |||||
| } | |||||
| // start if not running | |||||
| if (!pwm_state.next_set) { | |||||
| #if PWM_DEBUG | |||||
| ets_printf("PWM start\n"); | |||||
| #endif | |||||
| pwm_state.current_set = pwm_state.next_set = *pwm; | |||||
| pwm_state.current_phase = phases - 1; | |||||
| ETS_FRC1_INTR_ENABLE(); | |||||
| RTC_REG_WRITE(FRC1_LOAD_ADDRESS, 0); | |||||
| timer->frc1_ctrl = TIMER1_DIVIDE_BY_16 | TIMER1_ENABLE_TIMER; | |||||
| return; | |||||
| } | |||||
| pwm_state.next_set = *pwm; | |||||
| } | |||||
| void ICACHE_FLASH_ATTR | |||||
| pwm_set_duty(uint32_t duty, uint8_t channel) | |||||
| { | |||||
| if (channel > PWM_MAX_CHANNELS) | |||||
| return; | |||||
| if (duty > PWM_MAX_DUTY) | |||||
| duty = PWM_MAX_DUTY; | |||||
| pwm_duty[channel] = duty; | |||||
| } | |||||
| uint32_t ICACHE_FLASH_ATTR | |||||
| pwm_get_duty(uint8_t channel) | |||||
| { | |||||
| if (channel > PWM_MAX_CHANNELS) | |||||
| return 0; | |||||
| return pwm_duty[channel]; | |||||
| } | |||||
| void ICACHE_FLASH_ATTR | |||||
| pwm_set_period(uint32_t period) | |||||
| { | |||||
| pwm_period = period; | |||||
| if (pwm_period > PWM_MAX_PERIOD) | |||||
| pwm_period = PWM_MAX_PERIOD; | |||||
| pwm_period_ticks = PWM_PERIOD_TO_TICKS(period); | |||||
| } | |||||
| uint32_t ICACHE_FLASH_ATTR | |||||
| pwm_get_period(void) | |||||
| { | |||||
| return pwm_period; | |||||
| } | |||||
| uint32_t ICACHE_FLASH_ATTR | |||||
| get_pwm_version(void) | |||||
| { | |||||
| return 1; | |||||
| } | |||||
| void ICACHE_FLASH_ATTR | |||||
| set_pwm_debug_en(uint8_t print_en) | |||||
| { | |||||
| (void) print_en; | |||||
| } | |||||