equalizer.c 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229
  1. /*
  2. * Squeezelite for esp32
  3. *
  4. * (c) Philippe G. 2020, philippe_44@outlook.com
  5. *
  6. * This software is released under the MIT License.
  7. * https://opensource.org/licenses/MIT
  8. *
  9. */
  10. #include "math.h"
  11. #include "platform_config.h"
  12. #include "squeezelite.h"
  13. #include "equalizer.h"
  14. #include "esp_equalizer.h"
  15. #define EQ_BANDS 10
  16. static log_level loglevel = lINFO;
  17. static EXT_RAM_ATTR struct {
  18. void *handle;
  19. float loudness, volume;
  20. uint32_t samplerate;
  21. int8_t gain[EQ_BANDS];
  22. float loudness_gain[EQ_BANDS];
  23. bool update;
  24. } equalizer;
  25. #define POLYNOME_COUNT 6
  26. static const float loudness_envelope_coefficients[EQ_BANDS][POLYNOME_COUNT] = {
  27. {5.5169301499257067e+001, 6.3671410796029004e-001,
  28. -4.2663226432095233e-002, 8.1063072336581246e-004,
  29. -7.3621858933917722e-006, 2.5349489594339575e-008},
  30. {3.7716143859944118e+001, 1.2355293276538579e+000,
  31. -6.6435374582217863e-002, 1.2976763440259382e-003,
  32. -1.1978732496353172e-005, 4.1664114634622593e-008},
  33. {2.5103632377146837e+001, 1.3259150615414637e+000,
  34. -6.6332442135695099e-002, 1.2845279812261677e-003,
  35. -1.1799885217545631e-005, 4.0925911584040685e-008},
  36. {1.3159168212144563e+001, 8.8149357628440639e-001,
  37. -4.0384121097225931e-002, 7.3843501027501322e-004,
  38. -6.5508794453097008e-006, 2.2221997141120518e-008},
  39. {5.1337853800151700e+000, 4.0817077967582394e-001,
  40. -1.4107826528626457e-002, 1.5251066311713760e-004,
  41. -3.6689819583740298e-007, -2.0390798774727989e-009},
  42. {3.1432364156464315e-001, 9.1260548140023004e-002,
  43. -3.5012124633183438e-004, -8.6023911664606992e-005,
  44. 1.6785606828245921e-006, -8.8269731094371646e-009},
  45. {-4.0965062397075833e+000, 1.3667010948271402e-001,
  46. 2.4775896786988390e-004, -9.6620399661858641e-005,
  47. 1.7733690952379155e-006, -9.1583104942496635e-009},
  48. {-9.0275786029994176e+000, 2.6226938845184250e-001,
  49. -6.5777547972402156e-003, 1.0045957188977551e-004,
  50. -7.8851000325128971e-007, 2.4639885209682384e-009},
  51. {-4.4275018199195815e+000, 4.5399572638241725e-001,
  52. -2.4034902766833462e-002, 5.9828953622534668e-004,
  53. -6.2893971217140864e-006, 2.3133296592719627e-008},
  54. {1.4243299202697818e+001, 3.6984458807056630e-001,
  55. -3.0413994109395680e-002, 7.6700105080386904e-004,
  56. -8.2777185209388079e-006, 3.1352890650784970e-008} };
  57. /****************************************************************************************
  58. * calculate loudness gains
  59. */
  60. static void calculate_loudness(void) {
  61. char trace[EQ_BANDS * 5 + 1];
  62. size_t n = 0;
  63. for (int i = 0; i < EQ_BANDS; i++) {
  64. for (int j = 0; j < POLYNOME_COUNT && equalizer.loudness != 0; j++) {
  65. equalizer.loudness_gain[i] +=
  66. loudness_envelope_coefficients[i][j] * pow(equalizer.volume, j);
  67. }
  68. equalizer.loudness_gain[i] *= equalizer.loudness / 2;
  69. n += sprintf(trace + n, "%.2g%c", equalizer.loudness_gain[i], i < EQ_BANDS ? ',' : '\0');
  70. }
  71. LOG_INFO("loudness %s", trace);
  72. }
  73. /****************************************************************************************
  74. * initialize equalizer
  75. */
  76. void equalizer_init(void) {
  77. // handle equalizer
  78. char *config = config_alloc_get(NVS_TYPE_STR, "equalizer");
  79. char *p = strtok(config, ", !");
  80. for (int i = 0; p && i < EQ_BANDS; i++) {
  81. equalizer.gain[i] = atoi(p);
  82. p = strtok(NULL, ", :");
  83. }
  84. free(config);
  85. // handle loudness
  86. config = config_alloc_get(NVS_TYPE_STR, "loudness");
  87. equalizer.loudness = atof(config) / 10.0;
  88. free(config);
  89. }
  90. /****************************************************************************************
  91. * close equalizer
  92. */
  93. void equalizer_close(void) {
  94. if (equalizer.handle) {
  95. esp_equalizer_uninit(equalizer.handle);
  96. equalizer.handle = NULL;
  97. }
  98. }
  99. /****************************************************************************************
  100. * change sample rate
  101. */
  102. void equalizer_set_samplerate(uint32_t samplerate) {
  103. #if BYTES_PER_FRAME == 4
  104. if (equalizer.samplerate != samplerate) equalizer_close();
  105. equalizer.samplerate = samplerate;
  106. equalizer.update = true;
  107. LOG_INFO("equalizer sample rate %u", samplerate);
  108. #else
  109. LOG_INFO("no equalizer with 32 bits samples");
  110. #endif
  111. }
  112. /****************************************************************************************
  113. * get volume update and recalculate loudness according to
  114. */
  115. void equalizer_set_volume(unsigned left, unsigned right) {
  116. #if BYTES_PER_FRAME == 4
  117. float volume = (left + right) / 2;
  118. // do classic dB conversion and scale it 0..100
  119. if (volume) volume = log2(volume);
  120. volume = volume / 16.0 * 100.0;
  121. // LMS has the bad habit to send multiple volume commands
  122. if (volume != equalizer.volume && equalizer.loudness) {
  123. equalizer.volume = volume;
  124. calculate_loudness();
  125. equalizer.update = true;
  126. }
  127. #endif
  128. }
  129. /****************************************************************************************
  130. * change gains from LMS
  131. */
  132. void equalizer_set_gain(int8_t *gain) {
  133. #if BYTES_PER_FRAME == 4
  134. char config[EQ_BANDS * 4 + 1] = { };
  135. int n = 0;
  136. if (memcmp(equalizer.gain, gain, EQ_BANDS) != 0) equalizer.update = true;
  137. for (int i = 0; i < EQ_BANDS; i++) {
  138. equalizer.gain[i] = gain[i];
  139. n += sprintf(config + n, "%d,", gain[i]);
  140. }
  141. config[n-1] = '\0';
  142. config_set_value(NVS_TYPE_STR, "equalizer", config);
  143. LOG_INFO("equalizer gain %s", config);
  144. #else
  145. LOG_INFO("no equalizer with 32 bits samples");
  146. #endif
  147. }
  148. /****************************************************************************************
  149. * change loudness from LMS
  150. */
  151. void equalizer_set_loudness(uint8_t loudness) {
  152. #if BYTES_PER_FRAME == 4
  153. char p[4];
  154. itoa(loudness, p, 10);
  155. config_set_value(NVS_TYPE_STR, "loudness", p);
  156. // update loudness gains as a factor of loudness and volume
  157. if (equalizer.loudness != loudness / 10.0) {
  158. equalizer.loudness = loudness / 10.0;
  159. calculate_loudness();
  160. equalizer.update = true;
  161. }
  162. LOG_INFO("loudness %u", (unsigned) loudness);
  163. #else
  164. LOG_INFO("no equalizer with 32 bits samples");
  165. #endif
  166. }
  167. /****************************************************************************************
  168. * process equalizer
  169. */
  170. void equalizer_process(uint8_t *buf, uint32_t bytes) {
  171. #if BYTES_PER_FRAME == 4
  172. // don't want to process with output locked, so take the small risk to miss one parametric update
  173. if (equalizer.update) {
  174. equalizer.update = false;
  175. if (equalizer.samplerate != 11025 && equalizer.samplerate != 22050 && equalizer.samplerate != 44100 && equalizer.samplerate != 48000) {
  176. LOG_WARN("equalizer only supports 11025, 22050, 44100 and 48000 sample rates, not %u", equalizer.samplerate);
  177. return;
  178. }
  179. if (!equalizer.handle && ((equalizer.handle = esp_equalizer_init(2, equalizer.samplerate, EQ_BANDS, 0)) == NULL)) {
  180. LOG_WARN("can't init equalizer");
  181. return;
  182. }
  183. bool active = false;
  184. for (int i = 0; i < EQ_BANDS; i++) {
  185. float gain = equalizer.gain[i] + equalizer.loudness_gain[i];
  186. esp_equalizer_set_band_value(equalizer.handle, gain, i, 0);
  187. esp_equalizer_set_band_value(equalizer.handle, gain, i, 1);
  188. active |= gain != 0;
  189. }
  190. // at the end do not activate equalizer if all gain are 0
  191. if (!active) equalizer_close();
  192. LOG_INFO("equalizer %s", active ? "actived" : "deactivated");
  193. }
  194. if (equalizer.handle) {
  195. esp_equalizer_process(equalizer.handle, buf, bytes, equalizer.samplerate, 2);
  196. }
  197. #endif
  198. }