From 22f1eabffb4a7f0b7269a6f8a98536a505476ef2 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 5 Jul 2023 13:50:56 +0200 Subject: [PATCH 01/14] full speed spi gyro, 8kHz --- lib/Espfc/src/Blackbox.h | 4 +- lib/Espfc/src/Cli.h | 6 +-- lib/Espfc/src/Controller.h | 11 +++-- lib/Espfc/src/Espfc.h | 8 +++- lib/Espfc/src/Model.h | 69 ++++++++++++++++++------------- lib/Espfc/src/ModelState.h | 5 ++- lib/Espfc/src/Msp/MspProcessor.h | 20 ++++++--- lib/Espfc/src/Output/Mixer.h | 5 ++- lib/Espfc/src/Sensor/GyroSensor.h | 36 +++++++++++++--- lib/Espfc/src/SensorManager.h | 29 +++++++++---- lib/Espfc/src/Stats.h | 60 +++++++++++++++++++++++---- lib/Espfc/src/Target/Queue.h | 8 ++-- lib/Espfc/src/Timer.h | 7 ---- 13 files changed, 190 insertions(+), 78 deletions(-) diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index bb4c6b8e..ccbf735c 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -291,9 +291,9 @@ class Blackbox { switch(e.type) { - case EVENT_MIXER_UPDATE: + case EVENT_MIXER_UPDATED: update(); - //_model.state.appQueue.send(Event(EVENT_BBLOG_UPDATE)); + //_model.state.appQueue.send(Event(EVENT_BBLOG_UPDATED)); return 1; default: break; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index b6cf0b68..56183852 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1182,16 +1182,16 @@ class Cli { s.print(FPSTR(_model.state.stats.getName((StatCounter)i))); s.print(": "); - s.print((int)(_model.state.stats.getTime((StatCounter)i) * _model.state.loopTimer.interval), 1); + s.print((int)(_model.state.stats.getTime((StatCounter)i)), 1); s.print("us, "); s.print(_model.state.stats.getLoad((StatCounter)i), 1); s.print("%"); s.println(); } s.print(F(" TOTAL: ")); - s.print((int)(_model.state.stats.getTotalTime() * _model.state.loopTimer.interval)); + s.print((int)(_model.state.stats.getCpuTime())); s.print(F("us, ")); - s.print(_model.state.stats.getTotalLoad(), 1); + s.print(_model.state.stats.getCpuLoad(), 1); s.print(F("%")); s.println(); } diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h index 2cffd93e..5cb75c53 100644 --- a/lib/Espfc/src/Controller.h +++ b/lib/Espfc/src/Controller.h @@ -23,11 +23,16 @@ class Controller { switch(e.type) { - case EVENT_IMU_UPDATE: - if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) { + case EVENT_GYRO_READ: + _model.state.loopUpdate = true; + return 1; + case EVENT_IMU_UPDATED: + if(_model.state.loopUpdate) + { update(); + _model.state.loopUpdate = false; + _model.state.appQueue.send(Event(EVENT_PID_UPDATED)); } - _model.state.appQueue.send(Event(EVENT_PID_UPDATE)); return 1; default: break; diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 392c2d40..623445f3 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -68,6 +68,7 @@ class Espfc { return 0; } + Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); _sensor.read(); _input.update(); @@ -82,6 +83,7 @@ class Espfc #else if(_model.state.gyroTimer.check()) { + Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); _sensor.update(); if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) { @@ -110,6 +112,7 @@ class Espfc #if defined(ESPFC_MULTI_CORE) Event e = _model.state.appQueue.receive(); //Serial2.write((uint8_t)e.type); + Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); _sensor.onAppEvent(e); _controller.onAppEvent(e); @@ -118,10 +121,11 @@ class Espfc #else if(_model.state.serialTimer.check()) { + Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); _serial.update(); + _buzzer.update(); + _model.state.stats.update(); } - _buzzer.update(); - _model.state.stats.update(); #endif return 1; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index b25dc070..c081fc18 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -258,12 +258,19 @@ class Model void sanitize() { - int loopSyncMax = ESPFC_GYRO_DENOM_MAX; // max 8kHz - //if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2; - config.loopSync = std::max((int)config.loopSync, loopSyncMax); + state.gyroRate = 1000; + int loopSyncMax = 1; + if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2; + + // for spi gyro allow full speed mode + if (state.gyroDev && state.gyroDev->getBus()->getType() == BUS_SPI) + { + loopSyncMax = ESPFC_GYRO_DENOM_MAX; // max 8kHz + state.gyroRate = state.gyroClock; + } - state.gyroRate = state.gyroClock / config.loopSync; - state.loopRate = state.gyroClock / config.loopSync; + config.loopSync = std::max((int)config.loopSync, loopSyncMax); + state.loopRate = state.gyroRate / config.loopSync; config.output.protocol = ESC_PROTOCOL_SANITIZE(config.output.protocol); @@ -309,7 +316,7 @@ class Model if(config.output.protocol == ESC_PROTOCOL_PWM && state.loopRate > 500) { config.loopSync = std::max(config.loopSync, (int8_t)((state.loopRate + 499) / 500)); // align loop rate to lower than 500Hz - state.loopRate = state.gyroClock / config.loopSync; + state.loopRate = state.gyroRate / config.loopSync; if(state.loopRate > 480 && config.output.maxThrottle > 1940) { config.output.maxThrottle = 1940; @@ -319,7 +326,7 @@ class Model if(config.output.protocol == ESC_PROTOCOL_ONESHOT125 && state.loopRate > 2000) { config.loopSync = std::max(config.loopSync, (int8_t)((state.loopRate + 1999) / 2000)); // align loop rate to lower than 2000Hz - state.loopRate = state.gyroClock / config.loopSync; + state.loopRate = state.gyroRate / config.loopSync; } } @@ -382,9 +389,9 @@ class Model // sample rate = clock / ( divider + 1) state.gyroTimer.setRate(state.gyroRate); state.accelTimer.setRate(constrain(state.gyroTimer.rate, 100, 500)); - state.accelTimer.setInterval(state.accelTimer.interval - 10); + state.accelTimer.setInterval(state.accelTimer.interval - 5); //state.accelTimer.setRate(state.gyroTimer.rate, 2); - state.loopTimer.setRate(state.gyroTimer.rate, 1); + state.loopTimer.setRate(state.gyroTimer.rate, config.loopSync); state.mixerTimer.setRate(state.loopTimer.rate, config.mixerSync); state.actuatorTimer.setRate(25); // 25 hz state.dynamicFilterTimer.setRate(50); @@ -396,27 +403,31 @@ class Model state.magTimer.setRate(state.magRate); } + const uint32_t gyroFilterRate = state.loopTimer.rate; + const uint32_t inputFilterRate = state.loopTimer.rate; + const uint32_t pidFilterRate = state.loopTimer.rate; + // configure filters for(size_t i = 0; i <= AXIS_YAW; i++) { - state.gyroAnalyzer[i].begin(state.gyroTimer.rate, config.dynamicFilter); + state.gyroAnalyzer[i].begin(gyroFilterRate, config.dynamicFilter); if(isActive(FEATURE_DYNAMIC_FILTER)) { - state.gyroDynamicFilter[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), state.gyroTimer.rate); + state.gyroDynamicFilter[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), gyroFilterRate); if(config.dynamicFilter.width > 0) { - state.gyroDynamicFilter2[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), state.gyroTimer.rate); + state.gyroDynamicFilter2[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), gyroFilterRate); } } - state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, state.gyroTimer.rate); - state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, state.gyroTimer.rate); + state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate); + state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, gyroFilterRate); if(config.gyroDynLpfFilter.cutoff > 0) { - state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), state.gyroTimer.rate); + state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), gyroFilterRate); } else { - state.gyroFilter[i].begin(config.gyroFilter, state.gyroTimer.rate); + state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate); } - state.gyroFilter2[i].begin(config.gyroFilter2, state.gyroTimer.rate); - state.gyroFilter3[i].begin(config.gyroFilter3, state.gyroTimer.rate); - state.accelFilter[i].begin(config.accelFilter, state.accelTimer.rate); - state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), state.gyroTimer.rate); + state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); + state.gyroFilter3[i].begin(config.gyroFilter3, gyroFilterRate); + state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); + state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), gyroFilterRate); if(magActive()) { state.magFilter[i].begin(config.magFilter, state.magTimer.rate); @@ -427,11 +438,11 @@ class Model { if (config.input.filterType == INPUT_FILTER) { - state.inputFilter[i].begin(config.input.filter, state.loopTimer.rate); + state.inputFilter[i].begin(config.input.filter, inputFilterRate); } else { - state.inputFilter[i].begin(FilterConfig(FILTER_PT3, 25), state.loopTimer.rate); + state.inputFilter[i].begin(FilterConfig(FILTER_PT3, 25), inputFilterRate); } } @@ -462,15 +473,15 @@ class Model pid.iLimit = 0.15f; pid.oLimit = 0.5f; pid.rate = state.loopTimer.rate; - pid.dtermNotchFilter.begin(config.dtermNotchFilter, state.loopTimer.rate); + pid.dtermNotchFilter.begin(config.dtermNotchFilter, pidFilterRate); if(config.dtermDynLpfFilter.cutoff > 0) { - pid.dtermFilter.begin(FilterConfig((FilterType)config.dtermFilter.type, config.dtermDynLpfFilter.cutoff), state.loopTimer.rate); + pid.dtermFilter.begin(FilterConfig((FilterType)config.dtermFilter.type, config.dtermDynLpfFilter.cutoff), pidFilterRate); } else { - pid.dtermFilter.begin(config.dtermFilter, state.loopTimer.rate); + pid.dtermFilter.begin(config.dtermFilter, pidFilterRate); } - pid.dtermFilter2.begin(config.dtermFilter2, state.loopTimer.rate); - pid.ftermFilter.begin(config.input.filterDerivative, state.loopTimer.rate); - if(i == AXIS_YAW) pid.ptermFilter.begin(config.yawFilter, state.loopTimer.rate); + pid.dtermFilter2.begin(config.dtermFilter2, pidFilterRate); + pid.ftermFilter.begin(config.input.filterDerivative, pidFilterRate); + if(i == AXIS_YAW) pid.ptermFilter.begin(config.yawFilter, pidFilterRate); pid.begin(); } @@ -485,7 +496,7 @@ class Model pid.iLimit = Math::toRad(config.angleRateLimit) * 0.1f; pid.oLimit = Math::toRad(config.angleRateLimit); pid.rate = state.loopTimer.rate; - pid.ptermFilter.begin(config.levelPtermFilter, state.loopTimer.rate); + pid.ptermFilter.begin(config.levelPtermFilter, pidFilterRate); //pid.iLimit = 0.3f; // ROBOT //pid.oLimit = 1.f; // ROBOT pid.begin(); diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index ca22570b..7babd55c 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -138,6 +138,7 @@ struct ModelState Device::BaroDevice* baroDev; VectorInt16 gyroRaw; + VectorFloat gyroSampled; VectorInt16 accelRaw; VectorInt16 magRaw; @@ -155,6 +156,7 @@ struct ModelState VectorFloat magPose; bool imuUpdate; + bool loopUpdate; VectorFloat pose; Quaternion poseQ; @@ -227,8 +229,9 @@ struct ModelState float gyroBiasAlpha; int gyroBiasSamples; int gyroCalibrationState; + int gyroCalibrationRate; - int32_t gyroClock = 2000; + int32_t gyroClock = 1000; int32_t gyroRate; Timer gyroTimer; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 40c3ed77..39b3f188 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -102,6 +102,15 @@ static uint8_t fromFilterTypeDerivative(uint8_t t) } } +static uint8_t fromGyroDlpf(uint8_t t) +{ + switch(t) { + case Espfc::GYRO_DLPF_256: return 0; + case Espfc::GYRO_DLPF_EX: return 1; + default: return 2; + } +} + static int8_t toVbatSource(uint8_t t) { switch(t) { @@ -193,7 +202,7 @@ class MspProcessor // 1.42 r.writeU8(2); // configuration state: configured // 1.43 - r.writeU16(_model.state.gyroClock); // sample rate + r.writeU16(_model.state.gyroTimer.rate); // sample rate r.writeU32(0); // configuration problems // 1.44 r.writeU8(0); // spi dev count @@ -214,13 +223,14 @@ class MspProcessor case MSP_STATUS_EX: case MSP_STATUS: - r.writeU16(_model.state.loopTimer.delta); + //r.writeU16(_model.state.loopTimer.delta); + r.writeU16(_model.state.stats.loopTime()); r.writeU16(_model.state.i2cErrorCount); // i2c error count // acc, baro, mag, gps, sonar, gyro r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); r.writeU32(_model.state.modeMask); // flight mode flags r.writeU8(0); // pid profile - r.writeU16(lrintf(_model.state.stats.getTotalLoad())); + r.writeU16(lrintf(_model.state.stats.getCpuLoad())); if (m.cmd == MSP_STATUS_EX) { r.writeU8(1); // max profile count r.writeU8(0); // current rate profile index @@ -235,7 +245,7 @@ class MspProcessor // Write arming disable flags r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count r.writeU32(_model.state.armingDisabledFlags); // 4 bytes, flags - r.writeU8(0); // rebbot required + r.writeU8(0); // reboot required break; case MSP_NAME: @@ -964,7 +974,7 @@ class MspProcessor r.writeU16(_model.config.gyroNotch2Filter.freq); // gyro notch 2 hz r.writeU16(_model.config.gyroNotch2Filter.cutoff); // gyro notch 2 cutoff r.writeU8(_model.config.dtermFilter.type); // dterm type - r.writeU8(_model.config.gyroDlpf == GYRO_DLPF_256 ? 0 : (_model.config.gyroDlpf == GYRO_DLPF_EX ? 1 : 2)); // dlfp type + r.writeU8(fromGyroDlpf(_model.config.gyroDlpf)); r.writeU8(0); // dlfp 32khz type r.writeU16(_model.config.gyroFilter.freq); // lowpass1 freq r.writeU16(_model.config.gyroFilter2.freq); // lowpass2 freq diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index b04fdf04..7ef88105 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -61,11 +61,12 @@ class Mixer { switch(e.type) { - case EVENT_PID_UPDATE: + case EVENT_PID_UPDATED: if(_model.state.mixerTimer.syncTo(_model.state.loopTimer)) { update(); } - _model.state.appQueue.send(Event(EVENT_MIXER_UPDATE)); + _model.state.stats.loopTick(); + _model.state.appQueue.send(Event(EVENT_MIXER_UPDATED)); return 1; default: break; diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 275db001..6d3ac13f 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -34,10 +34,14 @@ class GyroSensor: public BaseSensor _gyro->setFullScaleGyroRange(_model.config.gyroFsr); _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start - _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroTimer.rate; + _model.state.gyroCalibrationRate = _model.state.loopTimer.rate; + _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroCalibrationRate; _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); + _idx = 0; + _count = std::min(_model.config.loopSync, (int8_t)8); + return 1; } @@ -58,6 +62,25 @@ class GyroSensor: public BaseSensor _gyro->readGyro(_model.state.gyroRaw); + align(_model.state.gyroRaw, _model.config.gyroAlign); + + VectorFloat input = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale; + + // moving average filter + if(_count > 1) + { + _sum -= _samples[_idx]; + _sum += input; + _samples[_idx] = input; + if (++_idx == _count) _idx = 0; + + _model.state.gyroSampled = _sum / (float)_count; + } + else + { + _model.state.gyroSampled = input; + } + return 1; } @@ -67,9 +90,7 @@ class GyroSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); - align(_model.state.gyroRaw, _model.config.gyroAlign); - - _model.state.gyro = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale; + _model.state.gyro = _model.state.gyroSampled; calibrate(); @@ -159,7 +180,7 @@ class GyroSensor: public BaseSensor break; case CALIBRATION_START: //_model.state.gyroBias = VectorFloat(); - _model.state.gyroBiasSamples = 2 * _model.state.gyroTimer.rate; + _model.state.gyroBiasSamples = 2 * _model.state.gyroCalibrationRate; _model.state.gyroCalibrationState = CALIBRATION_UPDATE; break; case CALIBRATION_UPDATE: @@ -187,6 +208,11 @@ class GyroSensor: public BaseSensor } } + size_t _idx; + size_t _count; + VectorFloat _sum; + VectorFloat _samples[8]; + Model& _model; Device::GyroDevice * _gyro; }; diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h index 0384b786..e20c9fca 100644 --- a/lib/Espfc/src/SensorManager.h +++ b/lib/Espfc/src/SensorManager.h @@ -49,7 +49,6 @@ class SensorManager return 1; case EVENT_MAG_READ: _mag.filter(); - _model.state.imuUpdate = true; return 1; case EVENT_SENSOR_READ: if(_model.state.imuUpdate) @@ -57,7 +56,7 @@ class SensorManager _fusion.update(); _model.state.imuUpdate = false; } - _model.state.appQueue.send(Event(EVENT_IMU_UPDATE)); + _model.state.appQueue.send(Event(EVENT_IMU_UPDATED)); return 1; default: break; @@ -70,24 +69,40 @@ class SensorManager _model.state.appQueue.send(Event(EVENT_START)); _gyro.read(); - _model.state.appQueue.send(Event(EVENT_GYRO_READ)); + if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) + { + _model.state.appQueue.send(Event(EVENT_GYRO_READ)); + } - if(_accel.read()) + int status = _accel.read(); + if(status) { _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); } - if(_mag.read()) + if (!status) + { + status = _mag.read(); + } + if(status) { _model.state.appQueue.send(Event(EVENT_MAG_READ)); } - if(_baro.update()) + if(!status) + { + status = _baro.update(); + } + if(status) { _model.state.appQueue.send(Event(EVENT_BARO_READ)); } - if (_voltage.update()) + if(!status) + { + status = _voltage.update(); + } + if (status) { _model.state.appQueue.send(Event(EVENT_VOLTAGE_READ)); } diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 9550830c..9f371ee8 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -28,6 +28,8 @@ enum StatCounter : int8_t { COUNTER_TELEMETRY, COUNTER_SERIAL, COUNTER_WIFI, + COUNTER_CPU_0, + COUNTER_CPU_1, COUNTER_COUNT }; @@ -37,11 +39,11 @@ class Stats class Measure { public: - Measure(Stats& stats, StatCounter counter): _stats(stats), _counter(counter) + inline Measure(Stats& stats, StatCounter counter): _stats(stats), _counter(counter) { _stats.start(_counter); } - ~Measure() + inline ~Measure() { _stats.end(_counter); } @@ -50,7 +52,7 @@ class Stats StatCounter _counter; }; - Stats() + Stats(): _loop_last(0), _loop_time(0) { for(size_t i = 0; i < COUNTER_COUNT; i++) { @@ -60,13 +62,13 @@ class Stats } } - inline void start(StatCounter c) /* IRAM_ATTR */ + inline void start(StatCounter c) IRAM_ATTR { _start[c] = micros(); //Serial1.write((uint8_t)c); } - inline void end(StatCounter c) /* IRAM_ATTR */ + inline void end(StatCounter c) IRAM_ATTR { uint32_t diff = micros() - _start[c]; _sum[c] += diff; @@ -74,6 +76,19 @@ class Stats //Serial1.write(t); } + void loopTick() + { + uint32_t now = micros(); + uint32_t diff = now - _loop_last; + _loop_time += (((int32_t)diff - _loop_time + 8) >> 4); + _loop_last = now; + } + + uint32_t loopTime() const + { + return _loop_time; + } + void update() { if(!timer.check()) return; @@ -89,25 +104,50 @@ class Stats return _avg[c] * 100.f; } + /** + * @brief Get the Time of counter normalized to one ms + */ float getTime(StatCounter c) const { - return _avg[c]; + return _avg[c] * timer.interval * 0.001f; } float getTotalLoad() const { float ret = 0; - for(size_t i = 0; i < COUNTER_COUNT; i++) ret += getLoad((StatCounter)i); + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; + ret += getLoad((StatCounter)i); + } return ret; } float getTotalTime() const { float ret = 0; - for(size_t i = 0; i < COUNTER_COUNT; i++) ret += getTime((StatCounter)i); + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; + ret += getTime((StatCounter)i); + } return ret; } + float getCpuLoad() const + { + float load = getLoad(COUNTER_CPU_0) + getLoad(COUNTER_CPU_1); +#ifdef ESPFC_MULTI_CORE + load *= 0.5f; +#endif + return load; + } + + float getCpuTime() const + { + return getTime(COUNTER_CPU_0) + getTime(COUNTER_CPU_1); + } + const char * getName(StatCounter c) const { switch(c) @@ -133,6 +173,8 @@ class Stats case COUNTER_TELEMETRY: return PSTR(" tlm"); case COUNTER_SERIAL: return PSTR(" serial"); case COUNTER_WIFI: return PSTR(" wifi"); + case COUNTER_CPU_0: return PSTR(" cpu_0"); + case COUNTER_CPU_1: return PSTR(" cpu_1"); default: return PSTR("unknown"); } } @@ -143,6 +185,8 @@ class Stats uint32_t _start[COUNTER_COUNT]; uint32_t _sum[COUNTER_COUNT]; float _avg[COUNTER_COUNT]; + uint32_t _loop_last; + int32_t _loop_time; }; } diff --git a/lib/Espfc/src/Target/Queue.h b/lib/Espfc/src/Target/Queue.h index 65e28507..d849faed 100644 --- a/lib/Espfc/src/Target/Queue.h +++ b/lib/Espfc/src/Target/Queue.h @@ -29,10 +29,10 @@ enum EventType EVENT_SENSOR_READ, EVENT_INPUT_READ, EVENT_VOLTAGE_READ, - EVENT_IMU_UPDATE, - EVENT_PID_UPDATE, - EVENT_MIXER_UPDATE, - EVENT_BBLOG_UPDATE, + EVENT_IMU_UPDATED, + EVENT_PID_UPDATED, + EVENT_MIXER_UPDATED, + EVENT_BBLOG_UPDATED, }; class Event diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Timer.h index 9d498970..82e2dd29 100644 --- a/lib/Espfc/src/Timer.h +++ b/lib/Espfc/src/Timer.h @@ -58,13 +58,6 @@ class Timer return 1; } - /*bool sync(Timer& t) const - { - if(iteration % t.denom != 0) return false; - t.update(); - return true; - }*/ - bool syncTo(const Timer& t) { if(t.iteration % denom != 0) return false; From 7b6956688e65d08f885b9404dafd535b0f90b0fb Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 5 Jul 2023 14:00:34 +0200 Subject: [PATCH 02/14] remove gyro_sync param --- lib/Espfc/src/Blackbox.h | 2 +- lib/Espfc/src/Cli.h | 1 - lib/Espfc/src/ModelConfig.h | 3 --- lib/Espfc/src/Msp/MspProcessor.h | 4 ++-- test/test_fc/test_fc.cpp | 18 ++++++------------ 5 files changed, 9 insertions(+), 19 deletions(-) diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index ccbf735c..a0b6b09a 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -253,7 +253,7 @@ class Blackbox motorConfigMutable()->minthrottle = _model.state.minThrottle; motorConfigMutable()->maxthrottle = _model.state.maxThrottle; - gyroConfigMutable()->gyro_sync_denom = 1; //_model.config.gyroSync; + gyroConfigMutable()->gyro_sync_denom = 1; pidConfigMutable()->pid_process_denom = _model.config.loopSync; gyro.sampleLooptime = 125; //_model.state.gyroTimer.interval; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 56183852..ba13cf86 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -384,7 +384,6 @@ class Cli Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices), Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices), Param(PSTR("gyro_dlpf"), &c.gyroDlpf, gyroDlpfChoices), - Param(PSTR("gyro_sync"), &c.gyroSync), Param(PSTR("gyro_align"), &c.gyroAlign, alignChoices), Param(PSTR("gyro_lpf_type"), &c.gyroFilter.type, filterTypeChoices), Param(PSTR("gyro_lpf_freq"), &c.gyroFilter.freq), diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 713a8a01..bf943101 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -535,7 +535,6 @@ class ModelConfig int8_t gyroDev; int8_t gyroDlpf; int8_t gyroFsr; - int8_t gyroSync; int8_t gyroAlign; FilterConfig gyroFilter; FilterConfig gyroFilter2; @@ -702,10 +701,8 @@ class ModelConfig gyroAlign = ALIGN_DEFAULT; gyroDlpf = GYRO_DLPF_256; gyroFsr = GYRO_FS_2000; - gyroSync = 1; // unused, force 1 loopSync = 8; // MPU 1000Hz - //loopSync = 4; // LSM 833Hz mixerSync = 1; accelBus = BUS_AUTO; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 39b3f188..2832f72f 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -902,7 +902,7 @@ class MspProcessor break; case MSP_ADVANCED_CONFIG: - r.writeU8(_model.config.gyroSync); + r.writeU8(1); // gyroSync unused r.writeU8(_model.config.loopSync); r.writeU8(_model.config.output.async); r.writeU8(_model.config.output.protocol); @@ -921,7 +921,7 @@ class MspProcessor break; case MSP_SET_ADVANCED_CONFIG: - /*_model.config.gyroSync = */m.readU8(); // ignore, removed in 1.43 + m.readU8(); // ignore gyroSync, removed in 1.43 _model.config.loopSync = m.readU8(); _model.config.output.async = m.readU8(); _model.config.output.protocol = m.readU8(); diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 82e4bafa..bc23980d 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -105,8 +105,7 @@ void test_model_gyro_init_1k_256dlpf() Model model; model.state.gyroClock = 8000; model.config.gyroDlpf = GYRO_DLPF_256; - model.config.gyroSync = 1; // 1khz - model.config.loopSync = 8; + model.config.loopSync = 1; model.config.mixerSync = 1; model.begin(); @@ -123,14 +122,13 @@ void test_model_gyro_init_1k_188dlpf() Model model; model.state.gyroClock = 1000; model.config.gyroDlpf = GYRO_DLPF_188; - model.config.gyroSync = 1; // 1khz model.config.loopSync = 2; model.config.mixerSync = 2; model.begin(); TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroClock); - TEST_ASSERT_EQUAL_INT32( 500, model.state.gyroRate); - TEST_ASSERT_EQUAL_INT32( 500, model.state.gyroTimer.rate); + TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroRate); + TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroTimer.rate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopRate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopTimer.rate); TEST_ASSERT_EQUAL_INT32( 250, model.state.mixerTimer.rate); @@ -139,10 +137,9 @@ void test_model_gyro_init_1k_188dlpf() void test_model_inner_pid_init() { Model model; - model.state.gyroClock = 8000; + model.state.gyroClock = 1000; model.config.gyroDlpf = GYRO_DLPF_256; - model.config.gyroSync = 1; // 1khz - model.config.loopSync = 8; + model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = MIXER_QUADX; model.config.pid[PID_ROLL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; @@ -174,8 +171,7 @@ void test_model_outer_pid_init() Model model; model.state.gyroClock = 8000; model.config.gyroDlpf = GYRO_DLPF_256; - model.config.gyroSync = 1; // 1khz - model.config.loopSync = 8; + model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = MIXER_QUADX; model.config.pid[PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; @@ -199,7 +195,6 @@ void test_controller_rates() Model model; model.state.gyroClock = 8000; model.config.gyroDlpf = GYRO_DLPF_256; - model.config.gyroSync = 1; // 1khz model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = MIXER_QUADX; @@ -250,7 +245,6 @@ void test_controller_rates_limit() Model model; model.state.gyroClock = 8000; model.config.gyroDlpf = GYRO_DLPF_256; - model.config.gyroSync = 1; // 1khz model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = MIXER_QUADX; From e2b908eac4157f63f82b2c42e62d2c3d0ea13e79 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 5 Jul 2023 18:32:01 +0200 Subject: [PATCH 03/14] fft analyzer for dynamic filter --- lib/Espfc/src/Sensor/GyroSensor.h | 107 ++++++++++++++++++++++++++++- lib/Espfc/src/Target/TargetESP32.h | 2 + 2 files changed, 108 insertions(+), 1 deletion(-) diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 6d3ac13f..6c1355ac 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -4,6 +4,12 @@ #include "BaseSensor.h" #include "Device/GyroDevice.h" +#ifdef ESPFC_DSP +// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c +#include "dsps_fft4r.h" +#include "dsps_wind_hann.h" +#endif + #define ESPFC_FUZZY_ACCEL_ZERO 0.05 #define ESPFC_FUZZY_GYRO_ZERO 0.20 @@ -42,6 +48,10 @@ class GyroSensor: public BaseSensor _idx = 0; _count = std::min(_model.config.loopSync, (int8_t)8); +#ifdef ESPFC_DSP + dynamicFilterFFTInit(); +#endif + return 1; } @@ -113,9 +123,14 @@ class GyroSensor: public BaseSensor if(dynamicFilterEnabled || dynamicFilterDebug) { +#ifdef ESPFC_DSP + _fft_in[i][_fft_c] = _model.state.gyro[i]; +#else dynamicFilterAnalyze((Axis)i, dynamicFilterDebug); +#endif if(dynamicFilterUpdate) dynamicFilterApply((Axis)i); - if(dynamicFilterEnabled) { + if(dynamicFilterEnabled) + { _model.state.gyro.set(i, _model.state.gyroDynamicFilter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); } @@ -134,10 +149,82 @@ class GyroSensor: public BaseSensor } } +#ifdef ESPFC_DSP + dynamicFilterFFTAnalyze(dynamicFilterDebug); +#endif + return 1; } private: +#ifdef ESPFC_DSP + void dynamicFilterFFTInit() + { + _fft_c = 0; + _fft_bucket_width = (float)(_model.state.loopTimer.rate * 0.5) / N; + + dsps_fft4r_init_fc32(NULL, N >> 1); + + // Generate hann window + dsps_wind_hann_f32(_fft_wind, N); + } + + void dynamicFilterFFTAnalyze(bool debug) + { + if(++_fft_c < N) return; + + _fft_c = 0; + + for(size_t i = 0; i < 3; ++i) + { + // apply window + for (size_t j = 0; j < N; j++) + { + _fft_in[i][j] *= _fft_wind[j]; // real + } + + // FFT Radix-4 + dsps_fft4r_fc32(_fft_in[i], N >> 1); + + // Bit reverse + dsps_bit_rev4r_fc32(_fft_in[i], N >> 1); + + // Convert one complex vector with length N/2 to one real spectrum vector with length N/2 + dsps_cplx2real_fc32(_fft_in[i], N >> 1); + + // calculate magnitude + for (size_t j = 0; j < N >> 1; j++) + { + _fft_out[i][j] = _fft_in[i][j * 2 + 0] * _fft_in[i][j * 2 + 0] + _fft_in[i][j * 2 + 1] * _fft_in[i][j * 2 + 1]; + } + + // TODO: find max noise freq + float maxAmt = 0; + float maxFreq = 0; + float loFreq = 100; + float hiFreq = 400; + for (size_t j = 1; j < (N >> 1) - 1; j++) + { + const float freq = _fft_bucket_width * j; + if(freq < loFreq) continue; + if(freq > hiFreq) break; + const float amt = _fft_out[i][j]; + if(amt > maxAmt) + { + maxAmt = amt; + maxFreq = freq; + } + } + _fft_max_freq[i] = maxFreq; + + if(debug) + { + _model.state.debug[i] = lrintf(maxFreq); + } + } + } +#endif + void dynamicFilterAnalyze(Axis i, bool debug) { _model.state.gyroAnalyzer[i].update(_model.state.gyro[i]); @@ -158,7 +245,11 @@ class GyroSensor: public BaseSensor void dynamicFilterApply(Axis i) { +#ifdef ESPFC_DSP + const float freq = _fft_max_freq[i]; +#else const float freq = _model.state.gyroAnalyzer[i].freq; +#endif const float bw = 0.5f * (freq / (_model.config.dynamicFilter.q * 0.01)); // half bandwidth if(_model.config.dynamicFilter.width > 0 && _model.config.dynamicFilter.width < 30) { const float w = 0.005f * _model.config.dynamicFilter.width; // half witdh @@ -215,6 +306,20 @@ class GyroSensor: public BaseSensor Model& _model; Device::GyroDevice * _gyro; + +#ifdef ESPFC_DSP + static const size_t N = 128; + size_t _fft_c; + float _fft_bucket_width; + float _fft_max_freq[3]; + + // fft input and aoutput + __attribute__((aligned(16))) float _fft_in[3][N]; + __attribute__((aligned(16))) float _fft_out[3][N]; + // Window coefficients + __attribute__((aligned(16))) float _fft_wind[N]; +#endif + }; } diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index 45103a4c..b6dadfa7 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -84,6 +84,8 @@ #define ESPFC_MULTI_CORE #endif +#define ESPFC_DSP + #include "Device/SerialDevice.h" #define SERIAL_UART_PARITY_NONE 0B00000000 From 274d6fdc9aac28aefe419cab9a1edb8a807d8968 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 5 Jul 2023 19:16:29 +0200 Subject: [PATCH 04/14] dynamic notch improvement --- lib/Espfc/src/Sensor/GyroSensor.h | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 6c1355ac..10e3e194 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -120,10 +120,16 @@ class GyroSensor: public BaseSensor _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); } _model.state.gyro.set(i, _model.state.gyroFilter3[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); if(dynamicFilterEnabled || dynamicFilterDebug) { #ifdef ESPFC_DSP + if(dynamicFilterDebug && i == 0) + { + _model.state.debug[3] = _model.state.gyro[i]; + } _fft_in[i][_fft_c] = _model.state.gyro[i]; #else dynamicFilterAnalyze((Axis)i, dynamicFilterDebug); @@ -135,8 +141,6 @@ class GyroSensor: public BaseSensor _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); } } - _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); if(_model.config.debugMode == DEBUG_GYRO_FILTERED) { _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); @@ -161,7 +165,7 @@ class GyroSensor: public BaseSensor void dynamicFilterFFTInit() { _fft_c = 0; - _fft_bucket_width = (float)(_model.state.loopTimer.rate * 0.5) / N; + _fft_bucket_width = (float)_model.state.loopTimer.rate / N; dsps_fft4r_init_fc32(NULL, N >> 1); @@ -175,6 +179,10 @@ class GyroSensor: public BaseSensor _fft_c = 0; + const float loFreq = _model.config.dynamicFilter.min_freq; + const float hiFreq = _model.config.dynamicFilter.max_freq; + const float offset = _fft_bucket_width * 0.5f; // center of bucket + for(size_t i = 0; i < 3; ++i) { // apply window @@ -201,11 +209,9 @@ class GyroSensor: public BaseSensor // TODO: find max noise freq float maxAmt = 0; float maxFreq = 0; - float loFreq = 100; - float hiFreq = 400; for (size_t j = 1; j < (N >> 1) - 1; j++) { - const float freq = _fft_bucket_width * j; + const float freq = _fft_bucket_width * j + offset; if(freq < loFreq) continue; if(freq > hiFreq) break; const float amt = _fft_out[i][j]; From a1ae54d3ebe7bc8a743871a318b5052031c293ae Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 6 Jul 2023 10:09:46 +0200 Subject: [PATCH 05/14] extract SMA class --- lib/Espfc/src/Math/Sma.h | 48 ++++++++++++++++++++++++++++ lib/Espfc/src/Sensor/GyroSensor.h | 53 +++++++++++-------------------- 2 files changed, 67 insertions(+), 34 deletions(-) create mode 100644 lib/Espfc/src/Math/Sma.h diff --git a/lib/Espfc/src/Math/Sma.h b/lib/Espfc/src/Math/Sma.h new file mode 100644 index 00000000..0e5c0910 --- /dev/null +++ b/lib/Espfc/src/Math/Sma.h @@ -0,0 +1,48 @@ +#ifndef _ESPFC_MATH_SMA_H_ +#define _ESPFC_MATH_SMA_H_ + +namespace Espfc { + +namespace Math { + +template +class Sma +{ +public: + Sma(): _idx(0), _count(MaxSize) + { + begin(MaxSize); + } + + void begin(size_t count) + { + _count = std::min(count, MaxSize); + _inv_count = 1.f / _count; + } + + SampleType update(const SampleType& input) + { + if(_count > 1) + { + _sum -= _samples[_idx]; + _sum += input; + _samples[_idx] = input; + if (++_idx >= _count) _idx = 0; + return _sum * _inv_count; + } + return input; + } + +private: + SampleType _samples[MaxSize]; + SampleType _sum; + size_t _idx; + size_t _count; + float _inv_count; +}; + +} + +} + +#endif \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 10e3e194..32c65d46 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -3,6 +3,7 @@ #include "BaseSensor.h" #include "Device/GyroDevice.h" +#include "Math/Sma.h" #ifdef ESPFC_DSP // https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c @@ -45,8 +46,7 @@ class GyroSensor: public BaseSensor _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); - _idx = 0; - _count = std::min(_model.config.loopSync, (int8_t)8); + _sma.begin(_model.config.loopSync); #ifdef ESPFC_DSP dynamicFilterFFTInit(); @@ -77,19 +77,7 @@ class GyroSensor: public BaseSensor VectorFloat input = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale; // moving average filter - if(_count > 1) - { - _sum -= _samples[_idx]; - _sum += input; - _samples[_idx] = input; - if (++_idx == _count) _idx = 0; - - _model.state.gyroSampled = _sum / (float)_count; - } - else - { - _model.state.gyroSampled = input; - } + _model.state.gyroSampled = _sma.update(input); return 1; } @@ -165,17 +153,17 @@ class GyroSensor: public BaseSensor void dynamicFilterFFTInit() { _fft_c = 0; - _fft_bucket_width = (float)_model.state.loopTimer.rate / N; + _fft_bucket_width = (float)_model.state.loopTimer.rate / FFT_SIZE; - dsps_fft4r_init_fc32(NULL, N >> 1); + dsps_fft4r_init_fc32(NULL, FFT_SIZE >> 1); // Generate hann window - dsps_wind_hann_f32(_fft_wind, N); + dsps_wind_hann_f32(_fft_wind, FFT_SIZE); } void dynamicFilterFFTAnalyze(bool debug) { - if(++_fft_c < N) return; + if(++_fft_c < FFT_SIZE) return; _fft_c = 0; @@ -186,22 +174,22 @@ class GyroSensor: public BaseSensor for(size_t i = 0; i < 3; ++i) { // apply window - for (size_t j = 0; j < N; j++) + for (size_t j = 0; j < FFT_SIZE; j++) { _fft_in[i][j] *= _fft_wind[j]; // real } // FFT Radix-4 - dsps_fft4r_fc32(_fft_in[i], N >> 1); + dsps_fft4r_fc32(_fft_in[i], FFT_SIZE >> 1); // Bit reverse - dsps_bit_rev4r_fc32(_fft_in[i], N >> 1); + dsps_bit_rev4r_fc32(_fft_in[i], FFT_SIZE >> 1); - // Convert one complex vector with length N/2 to one real spectrum vector with length N/2 - dsps_cplx2real_fc32(_fft_in[i], N >> 1); + // Convert one complex vector with length FFT_SIZE/2 to one real spectrum vector with length FFT_SIZE/2 + dsps_cplx2real_fc32(_fft_in[i], FFT_SIZE >> 1); // calculate magnitude - for (size_t j = 0; j < N >> 1; j++) + for (size_t j = 0; j < FFT_SIZE >> 1; j++) { _fft_out[i][j] = _fft_in[i][j * 2 + 0] * _fft_in[i][j * 2 + 0] + _fft_in[i][j * 2 + 1] * _fft_in[i][j * 2 + 1]; } @@ -209,7 +197,7 @@ class GyroSensor: public BaseSensor // TODO: find max noise freq float maxAmt = 0; float maxFreq = 0; - for (size_t j = 1; j < (N >> 1) - 1; j++) + for (size_t j = 1; j < (FFT_SIZE >> 1) - 1; j++) { const float freq = _fft_bucket_width * j + offset; if(freq < loFreq) continue; @@ -305,25 +293,22 @@ class GyroSensor: public BaseSensor } } - size_t _idx; - size_t _count; - VectorFloat _sum; - VectorFloat _samples[8]; + Math::Sma _sma; Model& _model; Device::GyroDevice * _gyro; #ifdef ESPFC_DSP - static const size_t N = 128; + static const size_t FFT_SIZE = 128; size_t _fft_c; float _fft_bucket_width; float _fft_max_freq[3]; // fft input and aoutput - __attribute__((aligned(16))) float _fft_in[3][N]; - __attribute__((aligned(16))) float _fft_out[3][N]; + __attribute__((aligned(16))) float _fft_in[3][FFT_SIZE]; + __attribute__((aligned(16))) float _fft_out[3][FFT_SIZE]; // Window coefficients - __attribute__((aligned(16))) float _fft_wind[N]; + __attribute__((aligned(16))) float _fft_wind[FFT_SIZE]; #endif }; From c63b707f76f8d15d1995c9a44f3e9afff4359610 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 6 Jul 2023 14:53:06 +0200 Subject: [PATCH 06/14] refactor fft --- lib/Espfc/src/Filter.h | 56 +++++----- lib/Espfc/src/Math/FFTAnalyzer.h | 114 +++++++++++++++++++ lib/Espfc/src/Sensor/GyroSensor.h | 177 +++++++++--------------------- 3 files changed, 192 insertions(+), 155 deletions(-) create mode 100644 lib/Espfc/src/Math/FFTAnalyzer.h diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h index d6dd8cb6..af2c0048 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Filter.h @@ -64,11 +64,6 @@ class FilterConfig return FilterConfig(t, f, c); } - FilterConfig reconfigure(int16_t freq, int16_t cutoff = 0) const - { - return FilterConfig((FilterType)type, freq, cutoff); - } - int8_t type; int16_t freq; int16_t cutoff; @@ -354,10 +349,34 @@ class Filter void reconfigure(int16_t freq, int16_t cutoff = 0) { - reconfigure(_conf.reconfigure(freq, cutoff), _rate); + reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate); + } + + void reconfigure(int16_t freq, int16_t cutoff, float q) + { + reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q); } void reconfigure(const FilterConfig& config, int rate) + { + _rate = rate; + _conf = config.sanitize(_rate); + switch(_conf.type) + { + case FILTER_BIQUAD: + reconfigure(config, rate, 0.70710678118f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf + break; + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + case FILTER_BPF: + reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff)); + break; + default: + reconfigure(config, rate, 0.f); + } + } + + void reconfigure(const FilterConfig& config, int rate, float q) { _rate = rate; _conf = config.sanitize(_rate); @@ -367,14 +386,14 @@ class Filter _state.pt1.init(_rate, _conf.freq); break; case FILTER_BIQUAD: - initBiquadLpf(_rate, _conf.freq); + _state.bq.init(BIQUAD_FILTER_LPF, _rate, _conf.freq, q); break; case FILTER_NOTCH: case FILTER_NOTCH_DF1: - initBiquadNotch(_rate, _conf.freq, _conf.cutoff); + _state.bq.init(BIQUAD_FILTER_NOTCH, _rate, _conf.freq, q); break; case FILTER_BPF: - initBiquadBpf(_rate, _conf.freq, _conf.cutoff); + _state.bq.init(BIQUAD_FILTER_BPF, _rate, _conf.freq, q); break; case FILTER_FIR2: _state.fir2.init(); @@ -408,25 +427,6 @@ class Filter #if !defined(UNIT_TEST) private: #endif - // BIQUAD - void initBiquadLpf(float rate, float freq) - { - const float q = 0.70710678118f; - //const float q = 1.0f / sqrtf(2.0f); /* quality factor: butterworth for lpf */ - _state.bq.init(BIQUAD_FILTER_LPF, rate, freq, q); - } - - void initBiquadNotch(float rate, float freq, float cutoff) - { - const float q = getNotchQApprox(freq, cutoff); - _state.bq.init(BIQUAD_FILTER_NOTCH, rate, freq, q); - } - - void initBiquadBpf(float rate, float freq, float cutoff) - { - const float q = getNotchQApprox(freq, cutoff); - _state.bq.init(BIQUAD_FILTER_BPF, rate, freq, q); - } int _rate; FilterConfig _conf; diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h new file mode 100644 index 00000000..8b3834cf --- /dev/null +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -0,0 +1,114 @@ +#ifndef _ESPFC_MATH_FFT_ANALYZER_H_ +#define _ESPFC_MATH_FFT_ANALYZER_H_ + +// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c + +#include "Filter.h" +#include "dsps_fft4r.h" +#include "dsps_wind_hann.h" + +namespace Espfc { + +namespace Math { + +template +class FFTAnalyzer +{ +public: + FFTAnalyzer(): _idx(0) {} + + int begin(int16_t rate, const DynamicFilterConfig& config) + { + _rate = rate; + _freq_min = config.min_freq; + _freq_max = config.max_freq; + _peak_count = config.width; + + freq = (_freq_min + _freq_max) * 0.5f; + + _idx = 0; + _bin_width = (float)_rate / Size; // no need to dived by 2 as we next process `Size / 2` results + _bin_offset = _bin_width * 0.5f; // center of bin + + dsps_fft4r_init_fc32(NULL, Size >> 1); + + // Generate hann window + dsps_wind_hann_f32(_wind, Size); + + return 1; + } + + // calculate fft and find noise peaks + int update(float v) + { + _samples[_idx] = v; + + if(++_idx < Size) return 0; // not enough samples + + _idx = 0; + + // apply window function + for (size_t j = 0; j < Size; j++) + { + _samples[j] *= _wind[j]; // real + } + + // FFT Radix-4 + dsps_fft4r_fc32(_samples, Size >> 1); + + // Bit reverse + dsps_bit_rev4r_fc32(_samples, Size >> 1); + + // Convert one complex vector with length Size/2 to one real spectrum vector with length Size/2 + dsps_cplx2real_fc32(_samples, Size >> 1); + + // calculate magnitude squared + for (size_t j = 0; j < Size >> 1; j++) + { + size_t k = j * 2; + _samples[j] = _samples[k] * _samples[k] + _samples[k + 1] * _samples[k + 1]; + } + + // find highest noise peak freq + float maxAmt = 0; + float maxFreq = 0; + for (size_t j = 1; j < (Size >> 1) - 1; j++) + { + const float freq = _bin_width * j + _bin_offset; + if(freq < _freq_min) continue; + if(freq > _freq_max) break; + const float amt = _samples[j]; + if(amt > maxAmt) + { + maxAmt = amt; + maxFreq = freq; + } + } + if(maxFreq > 0) freq = maxFreq; + + return 1; + } + + float freq; + +private: + int16_t _rate; + int16_t _freq_min; + int16_t _freq_max; + int16_t _peak_count; + + size_t _idx; + float _bin_width; + float _bin_offset; + + // fft input and output + __attribute__((aligned(16))) float _samples[Size]; + // Window coefficients + __attribute__((aligned(16))) float _wind[Size]; +}; + +} + +} + +#endif diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 32c65d46..18389285 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -4,11 +4,9 @@ #include "BaseSensor.h" #include "Device/GyroDevice.h" #include "Math/Sma.h" - +#include "Math/FreqAnalyzer.h" #ifdef ESPFC_DSP -// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c -#include "dsps_fft4r.h" -#include "dsps_wind_hann.h" +#include "Math/FFTAnalyzer.h" #endif #define ESPFC_FUZZY_ACCEL_ZERO 0.05 @@ -49,7 +47,10 @@ class GyroSensor: public BaseSensor _sma.begin(_model.config.loopSync); #ifdef ESPFC_DSP - dynamicFilterFFTInit(); + for(size_t i = 0; i < 3; i++) + { + _fft[i].begin(_model.state.loopTimer.rate, _model.config.dynamicFilter); + } #endif return 1; @@ -96,6 +97,8 @@ class GyroSensor: public BaseSensor bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ; bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check(); + const int debugAxis = 1; + // filtering for(size_t i = 0; i < 3; ++i) { @@ -107,152 +110,81 @@ class GyroSensor: public BaseSensor { _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); } + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + { + _model.state.debug[0] = lrintf(degrees(_model.state.gyro[i])); + } + _model.state.gyro.set(i, _model.state.gyroFilter3[i].update(_model.state.gyro[i])); + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + { + _model.state.debug[1] = lrintf(degrees(_model.state.gyro[i])); + } + _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroFilter2[i].update(_model.state.gyro[i])); + + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + { + _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); + } if(dynamicFilterEnabled || dynamicFilterDebug) { #ifdef ESPFC_DSP - if(dynamicFilterDebug && i == 0) - { - _model.state.debug[3] = _model.state.gyro[i]; - } - _fft_in[i][_fft_c] = _model.state.gyro[i]; + int status = _fft[i].update(_model.state.gyro[i]); + dynamicFilterUpdate = dynamicFilterEnabled && status; + const float freq = _fft[i].freq; #else - dynamicFilterAnalyze((Axis)i, dynamicFilterDebug); + _model.state.gyroAnalyzer[i].update(_model.state.gyro[i]); + const float freq = _model.state.gyroAnalyzer[i].freq; #endif - if(dynamicFilterUpdate) dynamicFilterApply((Axis)i); + if (dynamicFilterDebug) + { + _model.state.debug[i] = lrintf(_fft[i].freq); + if (i == debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + if(dynamicFilterUpdate) dynamicFilterApply((Axis)i, freq); if(dynamicFilterEnabled) { _model.state.gyro.set(i, _model.state.gyroDynamicFilter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); } } + + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + { + _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + if(_model.config.debugMode == DEBUG_GYRO_FILTERED) { _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); } - _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroFilter2[i].update(_model.state.gyro[i])); if(_model.accelActive()) { _model.state.gyroImu.set(i, _model.state.gyroImuFilter[i].update(_model.state.gyro[i])); } } -#ifdef ESPFC_DSP - dynamicFilterFFTAnalyze(dynamicFilterDebug); -#endif - return 1; } private: -#ifdef ESPFC_DSP - void dynamicFilterFFTInit() - { - _fft_c = 0; - _fft_bucket_width = (float)_model.state.loopTimer.rate / FFT_SIZE; - - dsps_fft4r_init_fc32(NULL, FFT_SIZE >> 1); - - // Generate hann window - dsps_wind_hann_f32(_fft_wind, FFT_SIZE); - } - - void dynamicFilterFFTAnalyze(bool debug) - { - if(++_fft_c < FFT_SIZE) return; - - _fft_c = 0; - - const float loFreq = _model.config.dynamicFilter.min_freq; - const float hiFreq = _model.config.dynamicFilter.max_freq; - const float offset = _fft_bucket_width * 0.5f; // center of bucket - - for(size_t i = 0; i < 3; ++i) - { - // apply window - for (size_t j = 0; j < FFT_SIZE; j++) - { - _fft_in[i][j] *= _fft_wind[j]; // real - } - - // FFT Radix-4 - dsps_fft4r_fc32(_fft_in[i], FFT_SIZE >> 1); - - // Bit reverse - dsps_bit_rev4r_fc32(_fft_in[i], FFT_SIZE >> 1); - - // Convert one complex vector with length FFT_SIZE/2 to one real spectrum vector with length FFT_SIZE/2 - dsps_cplx2real_fc32(_fft_in[i], FFT_SIZE >> 1); - - // calculate magnitude - for (size_t j = 0; j < FFT_SIZE >> 1; j++) - { - _fft_out[i][j] = _fft_in[i][j * 2 + 0] * _fft_in[i][j * 2 + 0] + _fft_in[i][j * 2 + 1] * _fft_in[i][j * 2 + 1]; - } - - // TODO: find max noise freq - float maxAmt = 0; - float maxFreq = 0; - for (size_t j = 1; j < (FFT_SIZE >> 1) - 1; j++) - { - const float freq = _fft_bucket_width * j + offset; - if(freq < loFreq) continue; - if(freq > hiFreq) break; - const float amt = _fft_out[i][j]; - if(amt > maxAmt) - { - maxAmt = amt; - maxFreq = freq; - } - } - _fft_max_freq[i] = maxFreq; - - if(debug) - { - _model.state.debug[i] = lrintf(maxFreq); - } - } - } -#endif - - void dynamicFilterAnalyze(Axis i, bool debug) - { - _model.state.gyroAnalyzer[i].update(_model.state.gyro[i]); - if(debug) - { - if(i == 0) - { - _model.state.debug[0] = _model.state.gyroAnalyzer[0].freq; - _model.state.debug[2] = lrintf(degrees(_model.state.gyroAnalyzer[0].noise)); - _model.state.debug[3] = lrintf(degrees(_model.state.gyro[0])); - } - else if(i == 1) - { - _model.state.debug[1] = _model.state.gyroAnalyzer[1].freq; - } - } - } - - void dynamicFilterApply(Axis i) + void dynamicFilterApply(Axis i, const float freq) { -#ifdef ESPFC_DSP - const float freq = _fft_max_freq[i]; -#else - const float freq = _model.state.gyroAnalyzer[i].freq; -#endif - const float bw = 0.5f * (freq / (_model.config.dynamicFilter.q * 0.01)); // half bandwidth + const float q = _model.config.dynamicFilter.q * 0.01; + //const float bw = 0.5f * (freq / q)); // half bandwidth if(_model.config.dynamicFilter.width > 0 && _model.config.dynamicFilter.width < 30) { const float w = 0.005f * _model.config.dynamicFilter.width; // half witdh const float freq1 = freq * (1.0f - w); const float freq2 = freq * (1.0f + w); - _model.state.gyroDynamicFilter[i].reconfigure(freq1, freq1 - bw); - _model.state.gyroDynamicFilter2[i].reconfigure(freq2, freq2 - bw); + _model.state.gyroDynamicFilter[i].reconfigure(freq1, freq1, q); + _model.state.gyroDynamicFilter2[i].reconfigure(freq2, freq2, q); } else { - _model.state.gyroDynamicFilter[i].reconfigure(freq, freq - bw); + _model.state.gyroDynamicFilter[i].reconfigure(freq, freq, q); } } @@ -299,16 +231,7 @@ class GyroSensor: public BaseSensor Device::GyroDevice * _gyro; #ifdef ESPFC_DSP - static const size_t FFT_SIZE = 128; - size_t _fft_c; - float _fft_bucket_width; - float _fft_max_freq[3]; - - // fft input and aoutput - __attribute__((aligned(16))) float _fft_in[3][FFT_SIZE]; - __attribute__((aligned(16))) float _fft_out[3][FFT_SIZE]; - // Window coefficients - __attribute__((aligned(16))) float _fft_wind[FFT_SIZE]; + Math::FFTAnalyzer<128> _fft[3]; #endif }; @@ -316,4 +239,4 @@ class GyroSensor: public BaseSensor } } -#endif \ No newline at end of file +#endif From e1b147d65301dbe43cf6ee425f60420ac667a53e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 6 Jul 2023 14:58:18 +0200 Subject: [PATCH 07/14] build fix --- lib/Espfc/src/Sensor/GyroSensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 18389285..7303fa2f 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -143,7 +143,7 @@ class GyroSensor: public BaseSensor #endif if (dynamicFilterDebug) { - _model.state.debug[i] = lrintf(_fft[i].freq); + _model.state.debug[i] = lrintf(freq); if (i == debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); } if(dynamicFilterUpdate) dynamicFilterApply((Axis)i, freq); From 82b53a91bf4781da2a8aa82776d72e004249b5e0 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 7 Jul 2023 01:49:57 +0200 Subject: [PATCH 08/14] failsafe fix --- lib/Espfc/src/Actuator.h | 41 +++++++++++++++++++++++--------- lib/Espfc/src/Blackbox.h | 22 +++++++++++++---- lib/Espfc/src/Espfc.h | 15 ++++++++---- lib/Espfc/src/Input.h | 19 ++++++++------- lib/Espfc/src/Model.h | 15 ++++++++---- lib/Espfc/src/ModelConfig.h | 13 ++++++++++ lib/Espfc/src/ModelState.h | 1 + lib/Espfc/src/Msp/MspProcessor.h | 1 + lib/Espfc/src/Target/Queue.h | 1 + 9 files changed, 95 insertions(+), 33 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 66885f03..9a2dac68 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -21,8 +21,9 @@ class Actuator int update() { Stats::Measure(_model.state.stats, COUNTER_ACTUATOR); - updateArming(); + updateArmingDisabled(); updateModeMask(); + updateArmed(); updateAirMode(); updateScaler(); updateBuzzer(); @@ -73,7 +74,7 @@ class Actuator } } - void updateArming() + void updateArmingDisabled() { int errors = _model.state.i2cErrorDelta; _model.state.i2cErrorDelta = 0; @@ -119,11 +120,13 @@ class Actuator for(size_t i = 0; i < MODE_COUNT; i++) { - bool next = newMask & (1 << i); - bool prev = _model.state.modeMaskPrev & (1 << i); - if(next == prev) continue; // mode unchanged - if(next && canActivateMode((FlightMode)i)) continue; // mode can be set - newMask &= ~(1 << i); // block activation, clear bit + bool newVal = newMask & (1 << i); + bool oldVal = _model.state.modeMask & (1 << i); + if(newVal == oldVal) continue; // mode unchanged + if(newVal && !canActivateMode((FlightMode)i)) + { + newMask &= ~(1 << i); // block activation, clear bit + } } _model.updateModes(newMask); @@ -144,9 +147,25 @@ class Actuator } } + void updateArmed() + { + if((_model.hasChanged(MODE_ARMED))) + { + bool armed = _model.isModeActive(MODE_ARMED); + if(armed) + { + _model.state.disarmReason = DISARM_REASON_SYSTEM; + } + else if(!armed && _model.state.disarmReason == DISARM_REASON_SYSTEM) + { + _model.state.disarmReason = DISARM_REASON_SWITCH; + } + } + } + void updateAirMode() { - bool armed = _model.isActive(MODE_ARMED); + bool armed = _model.isModeActive(MODE_ARMED); if(!armed) { _model.state.airmodeAllowed = false; @@ -159,7 +178,7 @@ class Actuator void updateBuzzer() { - if(_model.isActive(MODE_FAILSAFE)) + if(_model.isModeActive(MODE_FAILSAFE)) { _model.state.buzzer.play(BEEPER_RX_LOST); } @@ -167,13 +186,13 @@ class Actuator { _model.state.buzzer.play(BEEPER_BAT_LOW); } - if(_model.isActive(MODE_BUZZER)) + if(_model.isModeActive(MODE_BUZZER)) { _model.state.buzzer.play(BEEPER_RX_SET); } if((_model.hasChanged(MODE_ARMED))) { - _model.state.buzzer.push(_model.isActive(MODE_ARMED) ? BEEPER_ARMING : BEEPER_DISARMING); + _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BEEPER_ARMING : BEEPER_DISARMING); } } diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index a0b6b09a..04cdd062 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -8,6 +8,7 @@ extern "C" { #include "blackbox/blackbox.h" +#include "blackbox/blackbox_fielddefs.h" } class BlackboxBuffer @@ -350,25 +351,36 @@ class Blackbox void updateArmed() { + // log arming beep event static uint32_t beep = 0; - if(beep != 0 && beep < _model.state.loopTimer.last) + if(beep != 0 && _model.state.loopTimer.last > beep) { setArmingBeepTimeMicros(_model.state.loopTimer.last); beep = 0; } - bool armed =_model.isActive(MODE_ARMED); + // stop logging + static uint32_t stop = 0; + if(stop != 0 && _model.state.loopTimer.last > stop) + { + blackboxFinish(); + stop = 0; + } + + bool armed = _model.isActive(MODE_ARMED); if(armed == ARMING_FLAG(ARMED)) return; if(armed) { ENABLE_ARMING_FLAG(ARMED); - beep = _model.state.loopTimer.last + 200000; // delay arming beep event ~50ms (200ms) + beep = _model.state.loopTimer.last + 200000; // schedule arming beep event ~200ms } else { DISABLE_ARMING_FLAG(ARMED); - setArmingBeepTimeMicros(micros()); - blackboxFinish(); + flightLogEventData_t eventData; + eventData.disarm.reason = _model.state.disarmReason; + blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, &eventData); + stop = _model.state.loopTimer.last + 500000; // schedule stop in 500ms } } diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 623445f3..59b2b376 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -72,11 +72,16 @@ class Espfc _sensor.read(); _input.update(); - _actuator.update(); - _serial.update(); - _buzzer.update(); - _model.state.stats.update(); - + if(_model.state.actuatorTimer.check()) + { + _actuator.update(); + } + if(_model.state.serialTimer.check()) + { + _serial.update(); + _buzzer.update(); + _model.state.stats.update(); + } _model.state.appQueue.send(Event(EVENT_IDLE)); return 1; diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 076ca173..1a675ee9 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -120,9 +120,8 @@ class Input _model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); _model.state.inputRxFailSafe = (status == INPUT_FAILSAFE); _model.state.inputFrameCount++; - uint32_t now = micros(); - updateFrameRate(now); + updateFrameRate(); processInputs(); @@ -189,7 +188,7 @@ class Input if(_model.isSwitchActive(MODE_FAILSAFE)) { failsafeStage2(); - return true; + return false; // not real failsafe, rx link is still valid } if(status == INPUT_RECEIVED) @@ -204,13 +203,16 @@ class Input return true; } - uint32_t lossTime = micros() - _model.state.inputFrameTime; + // stage 2 timeout + const uint32_t lossTime = micros() - _model.state.inputFrameTime; if(lossTime >= Math::clamp((uint32_t)_model.config.failsafe.delay, (uint32_t)1u, (uint32_t)200u) * TENTH_TO_US) { failsafeStage2(); return true; } - else if(lossTime >= 1 * TENTH_TO_US) + + // stage 1 timeout + if(lossTime >= 1 * TENTH_TO_US) { failsafeStage1(); return true; @@ -239,10 +241,10 @@ class Input _model.state.failsafe.phase = FAILSAFE_RX_LOSS_DETECTED; _model.state.inputRxLoss = true; _model.state.inputRxFailSafe = true; - if(_model.isActive(MODE_ARMED)) + if(_model.isModeActive(MODE_ARMED)) { _model.state.failsafe.phase = FAILSAFE_LANDED; - _model.disarm(); + _model.disarm(DISARM_REASON_FAILSAFE); } } @@ -276,8 +278,9 @@ class Input } } - void updateFrameRate(uint32_t now) + void updateFrameRate() { + const uint32_t now = micros(); const uint32_t frameDelta = now - _model.state.inputFrameTime; _model.state.inputFrameTime = now; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index c081fc18..4c833d03 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -74,10 +74,12 @@ class Model state.modeMaskSwitch = mask; } - void disarm() + void disarm(DisarmReason r) { + state.disarmReason = r; clearMode(MODE_ARMED); clearMode(MODE_AIRMODE); + state.appQueue.send(Event(EVENT_DISARM)); } /** @@ -95,7 +97,7 @@ class Model bool isAirModeActive() const { - return isActive(MODE_AIRMODE);// || isActive(FEATURE_AIRMODE); + return isModeActive(MODE_AIRMODE);// || isActive(FEATURE_AIRMODE); } bool isThrottleLow() const @@ -108,7 +110,7 @@ class Model return config.blackboxDev == 3 && config.blackboxPdenom > 0; } - bool gyroActive() const IRAM_ATTR + bool gyroActive() const /* IRAM_ATTR */ { return state.gyroPresent && config.gyroDev != GYRO_NONE; } @@ -168,7 +170,7 @@ class Model } } - bool armingDisabled() const IRAM_ATTR + bool armingDisabled() const /* IRAM_ATTR */ { return state.armingDisabledFlags != 0; } @@ -179,6 +181,11 @@ class Model else state.armingDisabledFlags &= ~flag; } + bool getArmingDisabled(ArmingDisabledFlags flag) + { + return state.armingDisabledFlags & flag; + } + Device::SerialDevice * getSerialStream(SerialPort i) { return state.serial[i].stream; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index bf943101..3a9a605c 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -457,6 +457,19 @@ class OutputConfig OutputChannelConfig channel[ESPFC_OUTPUT_COUNT]; }; +enum DisarmReason { + DISARM_REASON_ARMING_DISABLED = 0, + DISARM_REASON_FAILSAFE = 1, + DISARM_REASON_THROTTLE_TIMEOUT = 2, + DISARM_REASON_STICKS = 3, + DISARM_REASON_SWITCH = 4, + DISARM_REASON_CRASH_PROTECTION = 5, + DISARM_REASON_RUNAWAY_TAKEOFF = 6, + DISARM_REASON_GPS_RESCUE = 7, + DISARM_REASON_SERIAL_COMMAND = 8, + DISARM_REASON_SYSTEM = 255, +}; + enum ArmingDisabledFlags { ARMING_DISABLED_NO_GYRO = (1 << 0), ARMING_DISABLED_FAILSAFE = (1 << 1), diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 7babd55c..20170f7d 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -269,6 +269,7 @@ struct ModelState uint32_t modeMask; uint32_t modeMaskPrev; uint32_t modeMaskSwitch; + uint32_t disarmReason; bool airmodeAllowed; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 2832f72f..9bd24306 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1302,6 +1302,7 @@ class MspProcessor } (void)disableRunawayTakeoff; _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd); + if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED); } break; diff --git a/lib/Espfc/src/Target/Queue.h b/lib/Espfc/src/Target/Queue.h index d849faed..91790b15 100644 --- a/lib/Espfc/src/Target/Queue.h +++ b/lib/Espfc/src/Target/Queue.h @@ -33,6 +33,7 @@ enum EventType EVENT_PID_UPDATED, EVENT_MIXER_UPDATED, EVENT_BBLOG_UPDATED, + EVENT_DISARM, }; class Event From 7cc1254d45001eec750735b659f51d9f5cd569f9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 7 Jul 2023 09:31:24 +0200 Subject: [PATCH 09/14] fix arming tests --- test/test_fc/test_fc.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index bc23980d..d9624354 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -454,7 +454,7 @@ void test_actuator_arming_gyro_motor_calbration() TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); - actuator.updateArming(); + actuator.updateArmingDisabled(); TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_NO_GYRO | ARMING_DISABLED_MOTOR_PROTOCOL, model.state.armingDisabledFlags); } @@ -476,7 +476,7 @@ void test_actuator_arming_failsafe() TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); - actuator.updateArming(); + actuator.updateArmingDisabled(); TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_RX_FAILSAFE | ARMING_DISABLED_FAILSAFE | ARMING_DISABLED_CALIBRATING, model.state.armingDisabledFlags); } @@ -496,7 +496,7 @@ void test_actuator_arming_throttle() TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); - actuator.updateArming(); + actuator.updateArmingDisabled(); TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_THROTTLE, model.state.armingDisabledFlags); } From 4610b5221ad408286d029e157bc19fdebcf6edc5 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 7 Jul 2023 13:12:53 +0200 Subject: [PATCH 10/14] blackbox sensors --- lib/Espfc/src/Blackbox.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index 04cdd062..450bd865 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -257,6 +257,10 @@ class Blackbox gyroConfigMutable()->gyro_sync_denom = 1; pidConfigMutable()->pid_process_denom = _model.config.loopSync; + if(_model.accelActive()) enabledSensors |= SENSOR_ACC; + if(_model.magActive()) enabledSensors |= SENSOR_MAG; + if(_model.baroActive()) enabledSensors |= SENSOR_BARO; + gyro.sampleLooptime = 125; //_model.state.gyroTimer.interval; targetPidLooptime = _model.state.loopTimer.interval; activePidLoopDenom = _model.config.loopSync; From 5a84131f320d361fa75928f578796c0731e10278 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 12 Jul 2023 00:11:56 +0200 Subject: [PATCH 11/14] track more peaks --- docs/calculations.ods | Bin 34905 -> 66054 bytes lib/Espfc/src/Filter.h | 4 +- lib/Espfc/src/Math/FFTAnalyzer.h | 81 +++++++++++++++++++++++++----- lib/Espfc/src/Model.h | 3 +- lib/Espfc/src/ModelConfig.h | 1 + lib/Espfc/src/ModelState.h | 3 +- lib/Espfc/src/Sensor/GyroSensor.h | 67 ++++++++++++++++-------- 7 files changed, 122 insertions(+), 37 deletions(-) diff --git a/docs/calculations.ods b/docs/calculations.ods index b4a73ab2d077dc1f997e2c7fe2efa28c325fb2ef..e3b6862dc557ae02e48f5076b67840c96863f403 100644 GIT binary patch literal 66054 zcmZU4Wl)?!(=7yoYj6(^!QDN$y9RfcMHY9<;ubtO!QI^*7I$}99D?7x-&gOwKkig@ z)znl^*G$(_bDlGOMnxV578?o*0ScQpPto-dji3JU5!_HPKv!N$P?0Ccu6adx)1 zF*5Y2}v;osezKfL7A7TwgjFxLiOxd>TUq!?UmhEi3N z=tC51=zfQr&shvEM_7XxKC9p)I?3wqVO9*zPDt@8^JT%)*6ClqO%4 zw0rYtcXqW@6ccM8hjr!viQL&ir9zy zB9iLc<|8=%Cji9)E=VrB@0rSocbAHyLxtrBwn*^#=k+i%(l7&|mm}kIa`sl7&&--i z%2R0Jm!h1t6e($CM^;DeyJTxUd{P6UZq|-t!TxvdNG}MPCO?00hQG(O<`s{pvj&R` zu_D^AD8kI33Tb||+_#GEy8MJu{|C{NwXb;CVC;Eb(%^58Sz#qrd-7#ME(MFZI70$z ztflj|@c@n}jkDVW(-^EbAHiR}J>DhR<77J>>ZvmH0mU@&#ZuHUzD9LeQdQgXPt?ze zBv570=MMSIF%`)Gv>sTv%}$&*M)JQs`R`m3nFD$1(d1vl_^n74j)?~_k`NVfrk=7} znq%1R+nqmwB(o~mdK^JDks)iNY_SO_feBNh#_A_EYT}ohVJWU^&p%{k<#;>!7~Vv5 zN3pekmGn!Oq+hG@<*6Qp-aUnCD0y*=BA$H6%Ivww0W?NIu{T_6PUBKf8BDYVej^Ab zXe-+2afPAj6It=Xx$%WbSqZLEtJw)ED>4zWU*Oom0P(S458%=7-&?u-iB%88fc!N#tRBdSOV00&pFR*sE^c}ir@9uk5G-WY!+aF z`l)XRH#U78XYJz|y@h~42F^$Oe3fpPAVghmaX8#igBb<5V;)HMFdKz)O^ti>2K0JI zYY7Ea!{1m05@!ry1Y7%rKrXY%6DZnt7kF6SZ<>Kbg5e5AJzsP{HoAcLblA^reJ#6b zD@BDGZHQzfYorJwR{*TZ_)OQE8yK$)9~Y-}>JC9^h%xyth7_WARP*eXz=*D9^LHL+ z@;HM^$y8P*bgyy0)t7npQ5{e_i#YXr$f=?zN+icGyW!rS@OX6$Jz{WIq$P@hsOazn z4f8vhxW+vgQY(c`{lAHpeNV~t#~1VH+YvA@9VjB2CHK>NCuy*;KrsElhu|htvYsh& zFYV=LmR}VDUosq{&7xTB?bM4Y=R)@v_R(0}&OWhU_Y9*9+XC;{b7}98PCp}k@5c?; zG=fpUf3YmB_xXU!`Z|kaqHY?B%hvoLk~|+kBhp{<5{K}!TI+KU%nk~{;GYk1De>70 zpMmYox-oiWv2Ln?56wLF!|P7x@skj<=yo6rU3g5#G>tFXLQ#)KsI-dM8G$valv?~o zG#Ojy+KHUSA5KApE_C9NRnTf*e8*&=Lv(*+C_J;EfX&xWuun@Ru1A-qqo?=tnZRm? z5u>+D!&wNKVHVMSS>y4am|-Txh`RNpmAA9YR7U-f?K$iZI1e29rHk1)(HtIGGwAS= zBJiG~`^?Qz=TU0))6_%_+hUYy`jS_%i2S4Xf%It2S}eW5w0F@Ch}36cv-7WEPP=j( z5(Hg3b+dI)QNIpl4FG5t`uyF8t(rQ>&FF&5uiJ;ZU5M|T@(RZufW4~iBqtt$_ol}X z7w)f_^(ehmaB}RjFPK8M$lmu*9Fr?;JLYAmp;R zQjJxi)qPF7QA)y2TR&s*m_CC`i>!=E@?=y_`ZjC=%UvfsR>Qj(RXN>Jo8fu?hTUlC z=|li-nl9wCB~&%j7M!be#;-QrWhD>9Wl;T)E{!pVaw!V}{H>VRt;RMl zKdy|9ObrTK@JlJuU^H*hzS#PjXsY~zrPs)sno900GdU)&LzK8V_x*YrzLk;#d@sbE zZmsdhu6f0I4h#Qs)iqHKXkl|NhHXvL5ZW)MvUyDt=c|$V<+z6sOfm>(qqfGhJ?9gZ z8JB0((pHrEV$3xNLx)lUV&+i12>Q2#Ofe*maq?e1XeXkugU#sc_nm)Y6TDoRC38Wo8U>7NU# ztju>cC@AQHfA)a@^KWDlUR(|e3I<9=K|}Hr^e0#(Y$QxVY$Vt(=$HhU$ar{ou&7^= z@Mw`q*szJHF-h6TzLKGma}kn%!=>RRM}?!oMWQCcW+y>mC;jq`f{2>}jfV!0fq{XV zk?R{9A15O3W=@Lf+$PE%7;Qp;M# zz(w7_LebPy&(ucU%G+33%vSrmg@LAvfsBo*o`;D7&`QJ7($c~KVB-P=*jsqH*a83m zZTlbtmvD2>C~MyYSDz49|DQg7!S+F^-a(P>kr`ew`F;t-!45j%E(Xz_=AnKb3BFc= zeu2@!-f6)O84-XykY{m%Ur0zuP-t{mWK48;NK8~H2m}iLnG_n69vzz$osgao5u5}H zNsRrO5gYs~E;2qQEh8Z+H8nLjsURq|EHu3`IyEmQy%3aH9+_2}oLP{V{U;+kKPJB^ zJ-XU`A=s?acy>KLuN&LVRA@bYII?GOj&ABWqN3NW^_qrQeAdrS$=v` zeoR|QN?_PXBA*3quIna+~wp6ao_mW95`<&mb1nXaA5);(~?@bK`!#Qea_`XqRMY;J94 zxMzO6A3Qa-GTFB}JG!v2FuZ&?x_LFfzCX2nF}ZWIvUfVWcMU%HyKr*9aPqWrc)fml zyL$GtdG+^bd1!xQ{(5u#e0%b6XW?Lb{d{NsY=0GUyneeob$7Dze6_j1zrTBUa&)}C ze{ykpxO0B8e|mble{l=>d$fP`uz&k_vu=jQG=<()u!O%NXGtSeOw|t&G0lKT%F5w;U*&>fqLXYdW!nqhLNkE zze(A)X5(vCx2GF5R@JMWSY7;8eO|dfo95Y?vzUJPcv@;gXF97O+G=@TH}b#VSb8^E zYLrHEh)8og+GlY%>Uyh#yi}%j>tygBFa*G}82P*0vjiA1G+&RtQ%+yDiRrg#Hy8z6 z%{=Q5TCaW8Uqcgjr;Xd%*3Ohx^`5VX5y|i~?0EP6?75nQ&qTD~x#di6@v?AKC(-q? zIuf=-n%&8ip z@Qd2;>54pQ&{BY%Qms?^&KpnURLfzkfIVdsU(-9`4AkVpW^*+7HpBt{NkT&Xvc0#O zhA9savrbq4kbX8zMw81AOWDDq8JVzhG^us*=_l~;H**HDcg|r;c5#eYl%T0OAG}2j z58z}XtU*QQN))8k-YHh@i`$>l<>O76$e6mXa$evlf@<2J4B0Jb)pNclVR`oV4H?4e zlPs5B=a*7Ep+EB}v=_ck7ePVR|Hf49LuY~36JR4Tb~hZ9K5=Jj7VkiC)CW zj|k9s?K4s+UWKI01=;IEY&b9YL4XD`!!hMk0(yA{`Rni6kJFZl0bg>eU##fsjRKo} zrX)W5NtD}e-rWnSJW9~0ldX9j$6eL!_=T%JM)UkmGAtqb`2Mr`!(kKfs<^OzvQ{tg zyN|@JqKwWQ)q9JI-n{yS&)B0_D$O3OH%6* zK>${z*P&ZDCefPkjE>E+{9Rs~IM2ZG%CIv4WTO3^5(07D-U_AsBa0cSdgyDcO`}e4 z3a~aD%i4O=mdlEd>x9HI@nux4UdIneG#0 zHfzM zJ<0s={Zk3gzC+) zX6O-CDo^BoZmyV}*c4V)U+o07OT!ffWhI7(D^ABPjf86A;M8<+3^Q3>@+96FT%^klj{?_~=E5>VFXNGc2rq>i^QEAd`QIX&f&jJ;-W3 zeN%KuUOmGVB@UmR3?AzqI2|ECneeOEA10{7ZsC87nn((a;u-Kah&0+dRQeGFrscME# z?V9pm|2awpaM$2&n4J7xF!XgGsb(jx>1%ccmG4)|YK^Fgt*;wqLBv}~Xn z7+R$QHW|!u@ndLq0U@Cccv0p@>~|_27m*-;K6%4p8K~JGaWfj{JzS(etUfPNYx)yt z^#q!59?0>J4a7=H#RAOxWyT)XQ5$r;E6)DSVN$(VM)mlakX_x{{DaprA>CjA+&dZM?SI#Xu&`G$zhgo{G0vtN22yK8@nA@!FYJHA}63$sSTw1 zB|8!}p70av(yCsrpu4yl8Hrz{gSX-k!Z97{eK)a_ogQ3BBz?i;-}o)XYRCbsmrWw%F&{4%vSLW50Q0( zrueA9Yxe4Kg6j}~la%zxS*qw-UIyiUQBv(pi|ZrWesk#7A@au%@cvi9_*L6nk!S=z!04pnpsyfxa=ET_)B)<2VB)giQ<&O3hzfcS(;G zjg$wgQ^j95EIeY$CdP*r>zr`Y^oTTrYbStpzYM|pBVIchy)%QI$vpp(F@S4hv?kq(R0Y4rDj>Hp8$!*9V4{( zUmDb{FHw}uh>uMzbBokq*U52)M{r}vzhVB8_%@5VMH?ra=pJq!!**P2U9FaZNcYJm z_tS<#C_ibjT1Gs^!4s|Rp8NgRHId>;F~eHIuS@nOH(t=sd;}RrBSfYpA?8v|exkPm z9hWJ9c|s~eclzIWq}V9e;`#R|H5l=bl9QRRo*^}mOGdF8_teBae>OmC`Y87MS_!32TxK3s(PIZwE zP|)?x9Z4lQIoQG{{YRnTTviZ z-A$I@DzJF>F+F+X`ctC)0`L$1(3BWAr)~6BvzF&9%`Y$gvK?zPl8eeAStaVPZ_M#o zMQt&LJ(MvEybonRjd*E|n#WDs2Yf3cns!DvCDt;p49r{*r<@7xmXAch87w+)4aIPm z^5xUxMnu;+)YL~T+n3BK*b+(I15NT*A*U>XR3bvEAjYeG;8XrQIJGYEw&C-jlD@k; zb+VEcjIah4Y3Wc###I<@LActFsDPsrS>JNOMGE8!WNPYczcb4tn+3L^QhB-6t_rhB z(d^PggDpR0`|4yeZNv1QuAra4WBfFU8BaQ#6;3j2yOmCpP2YO8AxHdG!tvbC((RB7 z15oKF(ECwruS{7JiVTREr2SA-J;_!??PqjCbb_7+Xx>yCRCc>`sLfu~<>hmY%=^O& zPo!65-jW6TgQEI8qI{rp@T~cffxJ^-@v#4VztF&s#9?&Y>L>G{c4mN&C-&nBIL1}t z^jPcxnpwCdwQ`Wh@v?Pxpj5uC^^iDn;ccjxCp|+s64pT2T{P|bLd14#+~*^ejhz;&4~xCHWZF@%ElFxD*V)~HGLB4&^dktv-At35 zyTIQK!*|DDqJDoh*ZKkR>ehC6^igyA2aZCEP}4NujdI#7!b4Y*rCY5a^Qojl=S^ZK z)$3R$T-v6L4T;WeA)N?n?J6u*@s&g zceu=jqWAKSI0cpF<`!=PUHkMsbPZlF5?xdwQb~OFI_`bCj!L>N!~~m9{j6i;>Sb1m zgEwg!?Dqq%yZ85_`S};0#mpZGA@i7ZQQUTEhCtcRWTh7Yt4V|Z2(fCR2I0^}nXuLd zJ!-aG68!y)eVj70kxAamNGXFY9j)?@C5-e`k)w?Rh|>)0Use!=MjrjitvWMLRx+LH z

fVU$pxEMhD;hAc{fwUXA`!3}+K%nEAwk8?L@~hm?Nf`U6SFuy%8#Dz_MswdM-2 z$NF2pJ?u=v1^G@FLL0V7`+I~5wya@^va?&j^3Caq?q>FNZS&I~&biL|6eAhh zFas{ia-F4<>6{1PTsilQl>Vgfgc4GAN1&xkN!(SMs+WKat1v8JzKlc1#xA}$g9*cA zLW+cQofzk9b33mbHppoPPDC7FKQ7guUnMheOxFGKMb|aNo$t*QF{chkAWjZa*JLr>&d` zjAO1xuS!&xpL1mo%f9lgj+k;3kx#{TZAV%(nv}NyC9ZH2+ma$zTy06LEA*Nu_xZs6 zIP@2Gorvn7(6!BieyBGHy!K1=`VBq$c4jJKYVb&UxgqhGq|)jHY?gEbKCUhTL(N{$ z_IF_L9Vlf7i(d`~8yQ=2Ls1>W-~xv1?r4m*bx=E&+EBY3?bqh=2nHBB9XT*B(mSh7z0Aqka@&k*wHv*zKD^y%&dNCx=-8< zOtpX1#ke3C?DXfdk_LHHSj@VkvKoCA*)66N-2_wGQb@S1-*|(61e}+wE-c_0D41f0DTgvA3xj)!4`2uBCD59Ux4$9c{c^OM z7H(1Ih%0Zl`~Be-=}y4UHr6C7(uMssWfAIZ zrO7v+tS_85$lCVhG*LL$32v;TRL*dj@qo#+^AltY#dVZMy~ zZS0CE)qI;hj4n7M!+XMvt6fYyuha7rJDD!?WP~&@YLkmlqXoSxRoaM~oHl)+P;KyU zq0_I@@0H;KN(B-X2RfKHZEWn-c9P+0k)qUlvDC%@#F1q4$)iDG!=p_gPPKr9=rK!w z7jyE0=Kvm&FRu9)jAVW7wJ48o7GwDd;DiBe@${t?*5@qv49kuxOqz@sy=JsL0Z^4zp_A5@B)@*`LiS3Ey0 z$}NAEOvCWq8OdBz7*6`}Wo?e@KNjoxLJBL zw=FzA(q)6zKp}(Cw>f;#(-gI7(h)|-TzJS?isva(6cx96XUD|dqDhPh*vFfpew(en zoN`9SjJU4nj|q&c)jeNpS{F2=6c&{5P2wI9vr?)maQYee*Zda)A9&B%R%s;uV0hk4eoxZt)AW3I@%k4M zQDg901y>J~y|qRCHd)(nZ&1$L_~U8w10Tjk@g8IuM`G8`%MfW^dba7oe8}&!xJX!- zId{L;GvZp{3Z4ay=GOq4bkBA9tKU~W>_oZ;XDzpy0^~1vViL$@$nam80=%_8VZWg- zB6 zh;rPPKL|qc6VmP2dvES2IH6>_OCl$|@KAaHWUA8L`+}1!ruEF&wH3i|=TX{0JSptx zW9@{}f!vtP+&}qgdfgxNj61k${jtlfFQw0lu@AOP476JWkfqPgYk0FO0_);Z1bt!>_R#vM~^GHM2{mKB%TOQ z=o#f${h=;plXZb;lTGp$inK!siW2}T)B`08(`kiJ%xx4k4qDFS(*?=Q$14Wu1 z3m0f;RY0jVJF+X@;8y&}r*8ZeVNaUtiZq+85ZeoNsn=umx6mRV{u>tCb4&A$Ccul7 zt!5w7xIy6&wsBoChaW~_q6?ML0BjErE4o8v4z$YL$-nJnT=WCG(kllN?9EY zR{|?zPG%??S#VpxY#A?$SaYde&qx6=2}AqIZEx_>owr>3+;R(w#oUoW zGp>e3G!fm+G2DT4V==!39|t8`1^c3Go|VrlCPM;IEtG4-sOWJ-e$LeTsNT zMr)~+T~@YpBDzfpaO9U>gi!O=+=#|_F*M!KAUD!ISzQ$2s$f+;IbB>Nco&*UjZ)m- z)v0Da)u)_V-${g6@G6Q1vRa^{=9(=|2%hwd23G%xb|1b8RFpZ8i7FIVQAUc53*> zt#6YmSqV)%o|HP6ADSC3$BK}Sq{C|Y_o}*9f3K#t_QK&3b+UeQ_Rco__m4TEuy>_0 zba6Fv&deLNfsYeOp6FC=m_}*M6^cN(qd^Wn1_K?)u7|L|`%ztd=a@Njx9b75)03aT zi00-?!l>AN1~_^VUmy&3{0RFdO76d`Mez`tXS@GmW|Bp$#{T7d90Z*2|>xxTB9fIQ(F?hoZ;Y)wt2DxldEd^s?3w9?8?!B*$(lFmN^@L zAjjts-CN3V&|2K+X(LyRL{ckRQ22xKMOQFR42DmN>_FX>S2{R{!FS|iT&E`=zN%FF z`@ee8Jw2Z!$W*1IGx11MI^7P{qokAz!b0{3kx_~irf*m_9-c7IcnwtFXgA~%uk1vm z*sCRI`}>=qkS2n^kiR&}da^}S=JiLoen?zWIL&@glP(GuG-5xEX%R^!At0#bvO)d>3Wl+J!t+f`Humb;&1#BHzEG81s_V*Z!?{LktYZ%^e z#8DC)e^)?wWc5#DOU&~uAd((@t+d9dkrC;`VGhb@ob#r; z;ip$GNEgOU4@(DTF#M<QhRUC=x%w!woG;K^gE#I{;b8-?9)2LWFPZ(6rvYYHj(w< zZ0Sr2LrSj4R1R(n7^lZg1Rh*->&#g%6ILB@(@DH*Czv~!hi{eNBG8iJbP%k(m(*xY zzSAZuc4DQwK9DULH-H#xEEB`-;$1KmsfTVw+2ZOZVR+s3YOSC0$<3!;MyWdBqTRp9 zp7NdaCnFsu8a0|eMdJvYKa=qw$$RtE@j@|DgowX@x%qmbvqMkB)H-cGpN_5c>uMpd z3YwSrd2Or(pJ)6E*T~>1MZVRVL3(GhG68&{lnZLBzKm4)f+GK*P|zqeTSx@6M@fAFv#o+dQLe{ z5lCYgB+YuS|N1M3ewKigeK2XXnD%7D&@0V-5$1Kjv7U8EJkRn$ZtJOeiyPdsdvr6S zE9{s3c@ttXcYmbqJJU_uZE`EEmw7FOoVMj$zL=WvcKU`^aV&&X#fq|u`By~?tuQMPV&`}A`R|yG54<&jq)Fv#6&&`P5D9gy2)VD5Q&NvHj$7qBAny7p7+(eK_**#vT%W>jU9 zSjt~D!5OC!ae4n{fPUEn5OHy1w<2hh0A5Mrzt5T!i(@?@jy;&h6}-%#&_D_l%PfcY%jT z3*GbQc{s0qrxDJL9_dTS3moF!S*#*siI3D*q!)}Wrm2(3r>g)!4+Ciu4v~lnpG|W< z#};>$#MB`Yi`?;uAcS6__Yon(c!h$0{3#7rsXgL6^p9Ym`wLrQe~Vd#YE!`uw@IM_ zY5H5PY>KB6E9jSlQ%ZAI7ItM_^d86S+7bONE6R^f+m;mS!i`gK7(xF|IC8eQbOoxl zoQ@(p92`lJVv!BIRKmn;LQy7%{3H8Ca|**EL0#wC$0SH~O|VVJf1MZt$P;L5;_cPi zl$PcOB{~w3(CbzUeDxQvE~IpeHW?J$FuP;5oNo%L%(C?H7u9X^=qYs*dAxNKcx{>6yJvVNR1Fq~B~cNuzO5U%xPWLOm)9vv}#6RWd*)%8x$T;UIS z64PSP2-p#u+mlgaVwhVIsMJg7`dQ}k^+H`kphA8I?3pTuLuoY~*VWuI)7DIG(KKp8 zEy>AOb4)YsZoIbV9&6vO{c1lqX{KS|QCe3~cZ2cT{OF_G)r+~Xx`{q`n9_MxTCAP= zM$v1csjbyj9?`Ug$t}zU_d-&|@yg4_9su^Jx_Aw{#dA(n-GH37dFs29_%v%Q)x3@2 zmD!m`j0~CmVs27U&BFRqtT*MgqOp;tv&2Z7SNR75ppZ)Un=%QwB$Y;ZbXzJeID%E> zX#F{hq>3#iE@Fx7&1TF~w854|Cs?6UU5*sIiq|)f^KDMSBuU$i>Un@W7Uz53JP;oz zb9OH>litrmnubmIc>Kb}LjMU26XC*0$OF)z&&cc!0sJLwShLt<*qby7e2utsfQ zeri*^E^`XyPl=|)G9=a>I>~1U1SuriP!doly|8$~qb7rpO~~x@0g|Iva;W8+^No~P zR>#Tf;=dFuc_6ev{|uWT|kWFcq{UF zLF7xc;v+G9MKP(I@&_eNhEMpz6iV}xM^UU+7p_Kc?L^yMy-D2H5}!ifniPncQZJZb zYS5Z@u%dtSSeLgi>_p}UOe>6k`2Zb*82L760O2|AJ*S4UDyvDFzPa(b)>b$#ibM_+ zukW|vn*nnlVxMCRoOAsvR?^M)K2TEMQre8*gTAq*C|$wHc1;kO&jn{lT)ov9wNeOF zH1rl)wTbU!U5D@8MS4Y>G%V=)A8ZYsRj^n8^wi~VLw*#ojQgE5)9XR@XtOB8-LiRl zudA7~lHcLV5o2D%y(|*^+dE@@2j61ix3oMs8CD z(rxf7);e#Er~zz6?=T}M#v&P4tnX4Si)KBTPJYp#qFx@k)M$&RbG+3cZ*A@+;1EST ziI%$eN@oc`|8W3@e8P7h9@#M`R*)Xs<*){Pk^>zz@RQf%$k_yw>-ZuSR3i90#?fc> zcU>?XJ43u(D>9X^)U5gu3VLCG`u8!HR4wUd*fWqVq#POebyoeUkTS$76C7F$%s|uh zSY4f348|U%ly}#~s$#v@(q3Gq@X?23$QlDQhm`ao0 z(l(&$Y9<$2av3aiGd^e)tJmH_vUk>?{BI181{Ot;s{w?UtdSRy3Ji-L1*UK(3h~m5va|c z-8@qKxerh}DZckw7{w_z+JW7U%ly+;56B*e!|ztW~0k(+(i z^RWxc4(gY3t5NYE=oX-}aANq>FAfRg@SD zQ&Tz9G5C36%tV3y3+zL0ee>D#!UplA7IBxV=1PNI*N>eGKuVw|FeGF6^ZrnKXRm8# zEgi5Kr+}qA&Vf9ntUT^s0U%#%a>vAnHJR?3Be5EcNkOUmyc9*qo+T%|kJE-!GfE@R zrVA&eAU@SSiQepC*ZKp`GX8U-Nb9sXnV^;Yu?(>{|F(h4*L8{{aMCo+CEl& z57b~ePb1IoDVl#3Z8K3lUQt#Ag6Vl#Gb~rt=qxc&Y-E+(@vu!SXA8A9JF~elE}dRS z?S82@kyaZFP?FI1S|>6k2cUF0-PhEu7f8!GF#IgxXOkr$;d0M^`CH&BEH8J!ICTKY zmezr&eyoux-`9XJy~#!a`$6p%Zy1y`aXM&QhwsE)cN|7>bIDC;$WEf@N5rNE0;xXf zQ&duwnYE;w(B(3GdBA27U|Gu1J9PHZI}QyWj~Vx~<66qU{SW=h&+Yg*m|JvLy?$a| zSU&wUTQEV!&7lE&ZR^ZX=j5|b?QO{({L+KzJZ8h^`}k66 zNLcn~^M}mO5QqJdS`$UF0d-Lg@MB{ARQie!Xr+z@4RsW#dxB^Ud}t8qW%~`5Z454S zNge-;G*i#o`NJe0`IGqgYbJX@b_#FMH*3+J8i5aPSdIQ>U{7P3At>-7`U zI9y08zq<4cLVB#N1;4AF(i6jRq@Tr2RQ&-GQ#|WH7!mML3<%4$9}hXY~U= z;4q*hzQG_qkXyd|ppf4lGnQ%*t1mCXoqwX?!h zGsq)b2vB}3nE^tNPy}bx$WQ&gKD^v8aQykA_({>9F#+KfK?cQ|P6bNuQ*E)!9MaD3 zWMoxXOIDv1bU6Zxu^So0>dfQDw*l81_R^JGW&4>9g`#$8ZQQ@XpK2E_mS}f7yVdq7 zElIgQXKQbk{K-+)s)i-$SfKrL^O#}9iahurv1k15+|I^tDr3%p_8W8^z8p&lSL|#_ z=y6}Y)XA~Qa;`bij~Tnt-lw$2Y#$TcI#LAm1_zFo@JKxoEcW`yN9Vqq#J7f*)|9gL z+po5>kJzzSv0#6Hs?9R+I`~T!=AF&QAj)a#AiXg4N19e&gRR&cMaaOKcF_Ra-OipfL|| znTr8E!DY{w9cXZvSjWddnsUvNuIx{%4Jh9<<{IsVh(#f`{&owe@h6;Dm_)2sOeQfk ze~sDhXX_V|1S{azBX-`rp&1qT@XVo+JeOv*`E`CC{nKkV0BaT~La9ogvCntufn z*8@S*FmK$XCZE(&{m`F5|JV_?l#MT)4yV5{OQ&PO=JXZAlUZtl;>?cYk7kkr^NFxZ z%s259shk?Frl5GTCIR8Vt)`F1f@A@Tc(9zqdUo4SyL$axb!bEY~KrnA4hpeR+*eYH%CK_(i6F^+%6XOAd6qE&v*p z7?e`dymiQ}v6CV8tu^VDf^NxvfXfk-ZHLhvPA){VO+D*k1!?6!0D$}rg2TQ~Yizn- zYmIKSOHpghizC}Uq6Vac2*{GH^tCN*$ynnZ?p^W7WPUMK3b)DKl=*z@bZpGFT)vva zcZ${2hwR@Gq8U;(8s-qEldbk0?%UVAR9iYCp>J!`S1cw952Pj6ZPUOe3x+}wJdcR8 zLxz%R>H#c?KJjmpcM8hTGJ0>QcG^g{EJuA!KLx+dD;NpL8$bB*b$O<)Wnr(^o`)JN z1KX{Q;EwOR|7A3L>7eiCc$=jrss+B7m&EOSrYV?@;a|d?nwRZ5A^R{VA}Rfmb5e(8 zHD*uwW*RXhl~qfvXi}$bkJ(DnRpt!U!bdG?1ZO>C)3nLFH4kosbKL;ri`too%;pp# zQxfgFU>JrtoV(ZxAyI(&MMlqc?YC<6e_!f=%)PFMA87YSdMx8omwvV2@A%k4JG<`Z z(0bM}a1{S*$qP1*vQ^993|r0Q<1k@grt5s(_MfvN(ov?SLU%iIBH%09Fu=T!JT{EH zu0=4!8~lYPTSYP+0E*MMmT}WO4m}RHmcb`JUmL(~eKRzr z(B=CFl zjlhquR;E}?0C)aqgISFZw@4quULxYYTdSz5|4)T&&{e4O_p71<09to zFXo@Hkg`PJ{nPDs?W(_by@OvP;ip@`^Edwwb{0H`!!bZOmYthwK}ig-^~wDvkA`z% zZV3J|Af=33_yrqpY^Qv6%t=K6t%XBs<`A7&HP3gx(_BNVNY|O%&EM#WnUG&Y^-T=b$7OJa?{jC{ zntlnTA&Vv<3tyR|m!j8FLStH?LC3(U=l6ruPjTjR-P28`v|;OKM4SSB zCdHH{(;_w9YnRj2m$O2NH3o3huSWb0qKs&H9W~LD4O0a`v2#FnYh6Sq**E``hm9gM zkreDD0nCDluEK%8sckS`hgOVsS(uBeFY{>HrpcHftMP6BH2YF}D)FHT zTHMvX3sKS^Yw*%6&SHkE|KO5u6C1{Jaa`CGOfYtQ{M`?x-+DNg?q_e;r2e7rd8z#0 z$ZYBm*3Tz&+FO?!Wqui!5o0c2)xnonZtv8sPT(cAbV1wg=Q>YA;s*p!D9OoVM;!fl zLJrM@r?Awe$pg6m3f%ZcGr`eU2_Qg}kK4=1V;qM_zJF-my)ZJ|MmQXg*Q{m|&q`=x z!mJ;#wM|<1rpArohqZB&G=2~F42A>>vbfi{f1wFuy%k_UuoTyVylh3NdMa28ly&?T zXTCj7_WSq&U7)quk3=Tje)n1`rf~Xm=lf%VGKWR(!N!ODW+*zx@G@e7)W@GUvFvXn z5iflE?o}8P5`<%Zu=}0A+G<4X$7{msAb~0NE+5_uLyvGSU;zNu(1Bq-z@N%&{vx-x z>!L?ijnDI$ap+O1ao|aKyvP5@MzkNWL#&x3wU8~~2|>;{^~=h+%am&O!<|iyq?+I` zf_E!Y|CiFfR+#+*HwR9p!m2-Lki>&)VFwU?%=PlLe0oT`0Td;D30JNN(Rm zqSU?zj8%Z|1&qWwt8aJmq+I0`A-$QH>0HcF1?X#d-!it0V{f}{xP-#`ZWErlr@CqJ zi@usLDRQ^o9Sn9g3H(zN_)gKJF=u-odT-oYe0}LJtL4vT{&#)WQGQ734&-+&!asLG zjAf|Jhhb30=qJ`~@6B_-GsXJiNLt`Y@{7+;`j?s8m_Xgbr{Tk7mX@%rn?23!9XS3F z!kt)w<;Ca zwf<-_2Y!11wj#6S3AN~jt3Cjec(nkfVACF&Qg{|=a{vB1W8;q1Dnn?xA$yi7L(+~d zDXX5<)~;+zuH!88)Vkq_k$`e((gGt2lkiecrrCRZOSfW;*oh$~OWCG;T7B$LI z+^&pG-8slZnCUM5qUO*fZrk;Y)M~8271&v|T`sO2*Xxf+nX}m$cnFH`whB{e(%Bg6 z7S{z>NdoSQ95ymHEX2QnV1b<{2u}!xZ)q)}S-x`3t=<#*jQH|gKIYLz+x!fcbXF%a zTW8}t<*Y+re>yS*%qGt+7}f)VL{9R;7@4mJsJJ}t6jLd?HLEmgwT=_<~B><4HML(DDW`A(`m-+ut z_LpICyieFRNYLOxf@^U10D}bxZoz}Q1$T$w!QI{6-Q5Nq++7BDx1Hbr-FM|U_W7{i zd%9<;@0#wruIsEi=WB@M(*e7L434UQ7xI`)(!?DTyrv7kS?_^3+6cUf02Cd zJ`!?D$tc_UMG^h`6ccZGFZN^wi2m8(=OH6y(W&sl+uHCCg#b_RQc!|RfUfHZiIYm9 zcdYl=5a+oi|H_15x*cW50W{vk^b=+zbIn-LL&ncQ{>N~Ho2FT=wN4X;Cy5elYq#x( z6$yhq%cS#vgv*5yQ7(H-G~o*uanuoBthUx0@jVAPiB{84i(^-A9XxIg%8UL{c!yhB z2YYTtuX_W`xRZ!+n8`8v5qgq^eLS5Q6t=EY;Ycq`S@a~;**evBnFj7eDp@sIMRHLg zLg0AA78#6VWM4en9_JK3)6U7i z{_J3uZA@(E7k4Sh?>nMyb-Kl&9GLXkEL`;0hcggm#|<+HvgWSP4tIxuhIt_wIdyPS^oB=t;<{{NF#Z8ZKuv zx3>PZmAPo2_9tSWNPK%_RO9HU<FM7!yll1avh6JKdVoEF zT9V%&U9ryB62cxWZ!x0GT${aJAe4G+o++HG`@ox@kC---l~il=_k1vL>95cV1Fp-a zWF|(+7tVctyEX|8=F`-ad5vrLxv$_75gGOh=B^_8S;cW$x8;NPyYJ}J`%P_)4;Q?O z8OVfV5p-(we!qb5RzL);7cdx6y~iBWK>nQBIvor_pzz+N*i9K|Y=QT)b0C*oiU@P) zJae8qUw?Lhp{uYk*ePNh!}zKd`EW&BhlISE!4*M=d@P;0pV;mz)R$vtJg9x0cKyJ~ zlT?avF}No<$Mx%zCMH+29`umTw-#`R!Q499*=!R&({P!zvEVPW^qx#DWnyS-bbyfkm z_l4dH8}~KihQ#a)LQCxspFsp=L)^yIF*L0V%yh$5-&^iAas3Ul&9kVh5Y^ouD=-vX zQ`f{%u|^LWEpfyy)?K?m;12|~EYijizeL=EVL;tk+t;Tj?r6|;BO%d!a+PO~U*m}! zI-u&gk-8rqp-G3cQl}iEGu{9~p8JPNuO;VCd?=mfOvCq$&l|j(#dMC(8Ul39Eb5uK z8oW;s&5Tr-m!wM>F3cWfV#u}v+wD8Vds55?eIUCKn7{+1mA^VjM+p0v;7>f?_-<$CD*c>kjmN;hrTrir3&PJ; zYXhFVH4!x{1HH7U@uHgdA-Ef3wfbt+4+4QRfQNl4fsr$PZhknO=a$I`by^UKAxP?ShLMqUPE^aj@@*%k z_`jEkKJVj|Ygz{jXCI83mT@{JxadV?kHFP_Sx>W5>>C`~#AMv#@pl=hJA7o&_u6dd za7Uw!`&kTMLRO``MJYk?{tB{c4+1P4^RSr?*`xYAHu^1|#7=S$sak4ewZ7Oi^qkcD zYdx(If?Zq8=_AnD=mvTc@amBOHHDT zg<@Ma8401$U;&GH{e_L`?&>l0Ri`aOwKC+gqM&s?OW4dnJ``Xp8j3wM)I3R}GBeCC zJa3b3XUAJ?fRS*tieO26QhR+40lq6rwK8RNPBv8jFW~DaWLH0@81bsdhAwZ9YMNuCSooT|HOKD~0mzJ_XgD|AlXw}a1qLg(Ma*!?MR$OH& z5#wnt?y!5R66|yj!7KD`BRXCU_Ome~4-fA(SaQf()X5FnRpz>>s1w2{nl z6uH@;Q(mL0Ilz5&c}GEY!v&?4sWUxUuYW~1d_;2 z>uUGdBQrCst?2mHIrxI9ta9z{Xe4pR6E~5kY7gH9vS+WkL#ybZTm+A-z>qT1L8?w9 z8QjBuzC}(DzgEhh+1Y(Hz80gISwz+Fa@_x0$M~O;0nsu3zqcR%=RFF34<1)yLPt%0 z0m{RCmOiN-twr(a7_(Qrw6sS;!%zCrcI{7I%RW)_q1<&d$Z%q(x>Q{1ddg~stAAw( z_o?R{;!M2l{g#Q*MvDG%jRL)O#Ly_H^6uA&j#m8mHZE((Er9`f9aJ} z4r^T^KcEQo@9M0rYI$|McI4(aIj9RfNIfoVK`;`B@T3`frVgea7cVhnIGB4ugLXQU=OLX1%*{QT&z`U#>%D%9~**U6mhg^b~D z4@BhTDR}*D!F;O1Svi#}RO-48f#}osLT96|{;#i+b;@B79^~2HvmjUbsNkb!MwR4_ zd#EIX-#OsGYqEW8eKY6k+5}H5jtb#z}%<9pNP-U^1`$IL_ zYS#Y~4~du19n?Z&X&O<`ieWDPRI=4?!6EcWm?oFBby43kOJfq^p|*jD6X{Eq4?YzB zWmzt`1_4Cuf#L!YtT%1}DEryN;X8Zi;8ZdZXi`%c14T1R!@0k)3{#_oWh~F_-Xi(A zy{y4tvu8S0MMFI8cQkEFFVS;%p}wR0X@>wZYG_3(kPv>g(VMkAhI6+#;8>>$%PG7q zCJM=a;0Z_e_K*g?qIrfB^(w7Ck@6E_cJWN*42GfcJq_Qd{1--R9OST~`i^F2=m)*= zM9EL$ouYmQK0}^PsUhq>)Q$T+XZNww*xow`)62wWqj3&4>h`+p%@HHU*Z;qIHfjK?3{!2Wn+ysGJoacCA%2(IK4>8Nx|RHbHYzSZdP zv>s#3;z+zl&sMJi77#PgJySpsvAku#;B9q#y>LzpSz7l-0eawynX{f3b!LhXZuh?c>k)Iv#8ButVY zsx$(gD!VfH zZ-|!9`OGr3Slc%AnBT`48hLPu{lN9cg6ye^BR|AvM`DUbrX$n%H*$!LEQMkek|Hpo zm|PkDzF?n4!Y{(-0B|v5S)oPq6)l(}w$d{~R-mLIKqdqqQ+F5MhoIe#|_U zUk@8hgc4c$S5YaOe*3*~?U*U>UwrndUr{i{Q*b}nZxgDb8e|@<2!0Y5QhM%gmX_F- zyDN>@mLp4lzQb+p$cu@}SufWAix#&WXWYw7<7t%vN8r0X^Qo)hi_I>n4;Qfxop--R zzz-WM@vyj+0{q;~HDaWsYDY_m=lC%IdIPL zweH<)L=*n;LKty5l7BLmSo`C)@y&_SDDc}i1X|^VA9ltg9_zx&U(n~-I&D}w82dMW zR93SAG=q02*uNXeo#y+>x%4HLwQ&fRih9jj^MC3-M*19h|f9hU!)l)Iy{d7R}g z4gXqUG<9G_B-#J!v51#~M0_HVBpR_Ysqq_iqL}< z6~zgisr?Ucw0n$_uAo7IVIy?&P)}-xUEJ))L}2kmPc3VhoI?%;Fc*juBlY8#4RO-3 z{7Sm%Y;tTAC25B}`>r#-_whHfF*hA7{(h-SEd;gRFYCeIu29%13bcyOp1!gk_R6ySE;~qTFlPVfQpB;822C*?BhnG`*8B+7GS; ziGc$qO8r^R4djh$=*-!rPAc666QO)!6?M@%6#d^J-H3Y&po_jpYz^=i- z1KN$VZ9D^X0bboL&LK_Bs7|2II*;g>yWx1egCAjJ?h>b;ZOe*BW+O`yDTCh9&--pb zwGz|stYh&nL4u~C5^W(4$&02{t=q11p5DKEPO1}`wbiwJ9mfwD zxm)~RZ3Kn>E~=zARh#ipr7 z9ec2_DkJ~c;i;nJGmWzODr4WRqAGgT;J(Yt0P>$v&zyf^cl176_TO0mw>{5*qo!fx zw(xYv=)08w8ufvAIGAq|s<{w0gHec>JfOk}dF6_<35&Qh65eMDy#zbpqOk={`m_2m zY=r!35vak}KAj1Q8Sk@GLn>Zq`&V2U3GRhC&RjOnMd8X-3lp}bmn4eU(Pty9Q5A4R z&2G8fBJ72t#<-_ere7?li?M3LmrMG>iL1P!6I}+w1hy}GioM9@t%Itz?nWRVZO+V} zE-OJ%2Ep>^HD=w7&3F>Kp)q&$S?^{OZ`O zz9COJLHR{8HS_)2_({85Krla0R$}JY^%eWt5J&8(6Oy>7ohQ1&e{@H+=6Iss64;z< zZ{aE9s*Okl+G})cc6=>H3}=O|nhL6qX%JPwRtb>c#e>CtC;Lr-CnRb_BH=lvJO3AU zM0X<^p8K4HgsMABPU&IoJiB=KnV1CJ&Fc@CP_k+YV(*k-B2!FvpcLL43>$RL2HTN) z!z#*`|2|St5H}cP{#}789imJYem9l4$dHuwTeK@y&pmU8b{|Z|d+U4{cu6=bCgiyW zI~H*m!wE&MR%Dq&Rmy~Iy^k^9Iz5h?R=@~hg=9N^ogQ=!CI%6 zk<()n-rYF*Nl{pYe;X51&<#yj_~@vB;~zQF*_z8J9jRPLgl5h-hbD6`E_XsC)sX&P z(PR&;(gRyz>`rmRpANErj&1M}{F+pCb?NVdl3y@SXx|vczSxtd>lkh{jbk?IIs)k4 zWNNk=Nk$sk?K3d_t}t5F!Ko#1p! z^mir!o1Bq+!UR%PuM5OH=A7NOAo|P_t(}=xl&pu2yjCcM;yz4ln@Wp6-f1${eBB1|JR6O(c{bNw?Nla2^t5m8PqGoAc6>ZWnnmxHE*(3BqI0}L_o=28v=q8_0n* zXa_Y1iHh*b>()c&iNPi?bLN68GMoTNbC`3Q4?mi`Q7msf;%#1HTSsEsdnzBzJ|b)e zH}*AYj?f`=?=RmtiKZ~ll`R-C;5h58da*Wiz*f1lM>w-E{Gh8}rPy-(-cpj$?0}WB zCn+g{2f%uDs&ZZ2=Fjv-WtTmRLnD~O$^U*QO4#y1MTC;Z?vf_JfV9M#LG);a9AVGge zHYWf?$|4m{1@#ClxOq*)8Osdb0i7-;kfT^_K}bgNINp&_(BOt+)o0#rtVnW}jr%8% zUTS(Y{+sjk_JMT7l4i^vx>4CD3;1La=G!H0I>#0-NO4zSxsfN3X((b}9%Qm7_$qA8 zabZxX^=VmZFL8)~fy6UjTC{bA3CxlxdcZKA`mge;x%H7oG_v!uiy&l(`li-VdOtRZ zWPcjHc#-}or(ILNq_7m8Z`2IS!@2XEGU6SVdtN05<1a?CegyZQ7x&ENke9%$eE}=^ zs7lLSskpD^RETx~@a@gBB7GTb>iO%4O!`jHbOO*csp%)M-!L&RKXs<_%V~wgECNBr z9{5m&ZkYxwQ8p`1JD&`=zt=yzUm4*2adZO6DMc$~`kD9-*7mSUb44v8ej;;jc3;2w zap`qT8A0$1g&2-t#n!R#Y`Dbp^Np5EGZP*$IVG$n9>?RWhEI!X7^MM&*R*h4(;rZ` zb=>J~E7o>!n}`K0+Zv&mNJaTSnvQsN%|=Rl5_BTPQr(B99)51_KdoXsUTB{wuaM2n zBnc&fau5teFk)Qvih6X7xty1JPC7gQQ%*R%LcR~oi2u{t>6X;m+8X!U%Ra_5$YNzp zL4sAcaV@KI4?p#T3l_@&PMM=FwVvXJJBpH}GsG#dDiU{6gVoltl2=X?a~Qhcb2Zv+ zft_I)Q%kpduhE!s*-hb`Da-ds z67+e)LHh#lu0PY};?PEf5_#m6py~7`%TneH`-yz^A2nQ`@;_M%BIICALIh?-Yr22g z()w|lVOa+~&ckSMf6O;Wc*K87VPK1MzAzy(o2F>_W`JmJp*+bg79)|RprDW~wl;HO z_Rn*iV>-LjkBuKi4*Hq+3m!NWXUlzEP@B#>3gvhW+#Fc-sqGI4AmQZEUeogt+v2D7uujwhS(fz?mF+Z-yT8lEwGO z%MJNnDHm*lq?pry!w7}tV7mM*{H>6<>Wy;S5HayOH>l3tWzV_tHQz4z*Vb}6EB4sg zp)=~y$Al#7Z^UD}zfL*= zm+_b?EleQ2(?H@YpCc=kOFTdP=HL0A(L6(n6S5RiyO2rGpEN=r7TOcuzyw)^jT#K!+=HIh9wUy!l)+WJU3qUlF`YCO3JlxV`;9sCeMO9H1e}pe*Usjiw#mN zGE>x`cMVHB41;(>h#7$_v|$fTGg7*%W2SBme8LZthQW3b0d%>CW@>uB*poM8yp}jf zG&@n!(;_gm3MJzs6b*V>f`Ip>X%yVsVGsEY->&=>^mrc64GJ5$z$V(&t4FXAb@<|p z!TcWk8`KdApxEV)qdAMFsd!dR3;9QkYdSQptNoN9ml%n4E1H0gqA;pT7Q6T5)r;X_ z@Mq!Wu=07ZLfh#*q?l?cJXZ7zKYu8W(E!~Jkb5tH#wI&&#sI`U3 z`1c5;{WGEi@TL4=MJez<4tT?y0N6AlS6Ss}=)C>}N={5PN#qb$*UYzXmuebbqyf^8 zH94@%HTPWAflCvCr!^WBFW+MCK#FM@2>Pbx=+4yQDaQeYu@EZ9#-5nDYw~Z0jSy)U z`ydYNhk3SHwUX{4fRfe3LmG(Hz)uy#U7#;{ia5JRm+~n}Ve=d|0&O423xhUcWO~@W zS9h&!83}T}1JL>o4J}k~GI`7|G(N00C4aq8D&k0SyqF5|Z3pLiI$lgnh@l4@y0VQX zhYve`6s$bX-2sw&&7U)O(936Hr6sBuqr2)`rn9iTt*juR;`d4NYIsO4AbSkY6I;%O zL$tSY?xBqi;`@PCM>)B>!{hX{t?o=cnVsWF-aOqWuG|58XKt>qsSp+0KtlO#V%4Dg z*M4mI7zN+C8Mslq;Wx7eVB+#g8&$qR$3rNfP~69ccY@KJ3Pm-i zm&WYEra%*xz?}OFKKn~X|NDkOz?TV%D&lbX`b4U}j7Ffsbl}`Bpiv&hEnwyxpFsH6 z9|j@(3Q${ReI2mn=XZ{zZy@zrUTKPdEPM*p4Bu0Su(;z#=VGP$NT(f0TdsYADbmcq zQr*R+?qEYp*IQ^!VclFM`sdc@isGOQ_HKuxbGoxz-fMsyv91I|$|AlF6`45OkPF># z=Xz?}gRWCozvRr_e>}%EE$X6@-;327I+%A$P)j4mwko_*Stb^^gPESZ2z(zUbiHGm zC&o~R7B_oZG_KE9W+M=`#*hZ3?e$%o`!hZ0EtqSKu^pb^pj$HBf6L@lZJpF#SaATz zr%atI3oc1B{1)8%H#j-`Ss?ky;1S$*EqTo*t4nxz|0xqs-%UU1fIG!mi3WZ_l*KS> zu;N!CUZpI?2i7LC=@IL9Gx?TABCvV6L@__KHdH>wxY;F-HN=^L^24cHzdV$!e$ZjEZM&&g>K zlQJ3OIw_}f?Zeohf0LrIj?{9+e81y7u6ta*I~5P@y7FiZCmJv&xK}cHsnLZ-aq!l; za@FdbFp?iiI|AW1RZI4;+k7Wpji%y~_q_RC8D{Y{)pBa>c2YJH@lGj++d=9*k$HWS z9JFWCoF`5Z`bC_LJ;|%Pz?*!L7_o^i8FLu)XV+Wvwn1XB8EQmjLyWv;mAgzXv5Q|~ z)~$vJg@o%2a%`QZ(!Vy>+XM&{Te%L$dK^W~_%#^$uq2Ls{ZPq4ts!(&eReMHZy#6E*L95vv_B;4#uM_cqNIr z`-gmOMspe0ReCBB|CgrmKj+o|ssaBY9RDxj<9`STq+!H5kC12; zj8?74;fUE>G35O5kdzKTYW^E5`T$2vIO5r?P1EICK@ji;kN4(wzeudfkgV6RYbL9@ z-f}NR4PGF**pzwQ4>&rR(z|=vO;D?p9F|pXxbnLl=G$iAdxtJezMi=dh!PTLF`V>R zo7cLlzkRV^t!^cI+WS$7B zY7Crs^;%^m#ztP7IKO$yav5X{N?y0^O(lLeJm{A)9t{fptMOf9W?^9g@pEAIs(42w zM8*kDfTtCYk(>S;a3!$!!c^CC|Q9Jk@Z8x zb%rx8m(jZw!qoP|zRQerjO)$kO&SE}I;G18_{#L>&>Cy<)F z*QDkroS?jLR+M%QrX~An4{}d`{ELJIkdJD#O6*sR+&*hB;qGfM*$}^dvqGGi!Nb9L zMR}ggqj>(Ahj{bJP)kfaseBN0Qt!ED|C5!nVo`9_=OFQ zg0uhX)D-EpoQPEJ4RUGY6QfxW?MA%b{+NG5(Vw+nL17w_plUBd!V>vQjrS90CzXy) zxAqScfJlVku7DL@<8?|>2az%Zj5y=hH_yMI-n`wOzEQ0MbCE#(Yr_Xto5k)K$zW_z z5*yE?KuroZ%ut~=Z1txY%5afR8zp#1;masy(fa<;L+Anije52GL|`Z~iSGW51)HR4 z^aQmJtLMlAtQ(BcmMfQW;00(CC%JoeY-|t~L^45DE~d{}^fSeQ{;TsVfGOVwwubd2 zG;Y%T`am}VM^_mBgOob|V!^?AbWiLy_&tRL6cds-8GY|~)Y@tUbJ1w1Y0?(`!YOyJ zv9UgGXill1idTN4k=yWbQ@6b?sH}D{)LzNslOiVA<(YeP^v}Cf#4H6d!xGutTy~{D zw78TB^a}fzl3;`)oH1!A)H5bZke}zo!Xu7!uD@(qrbElEibUS-&QcP-xwe8Qh=hDLla0X=I zwzu9Ce`$oWt>gY&q-1x{WtFU+@{&ryncfipM~h?_ckK+HAYuAJH$Nzdyxk;|{>K!h zdFF7fNqy?pM!m#SLD=O(Q10&4`tC;4eL?XYt}}t_F$imSyVRCjT;f(4uMzLoqqBEf zk=$2qHhCtE=LwfD?{dOzutc;+Cakbd+4eE8R|!C znr<4J_bya9*TFA|@*CFc)i^_Mt9rX5D-jO3c;dhsC2rJ#SA%Bra(I;&)p)o}X75nR zF4Q%(M_rZQ$*`TfFsgmfZD2T_<>Ar(KY+t!UUS@;bkDHUz*{|AG4156-PRG}5ovl( zLd$Tz;G3^r%d7H)SflQlq6#V0uNi!&yCz%IpD_*(k8i5^Gd@H!W4hl04T2~-!*B;z zN`jwuf zH}DLb>xJ-K0W_xs7(ID!O*6Y3xXp%(K34oL6z9NcHCV1;`Dzj2D!?TH->0Kn>_o`T zw`jX!JzP>+L>(zxT3Q+(3>(o6PqCON5v$BLx+~J=VTW7DhIfMV?3CD?r~xUm@K;7e zeGlpGuWhN=R$C#gjjk)gAkoYC9!}AYzD4SbAj6AwN8aa}t5? zfG}^S+ik8erPljw(I*2Iz&%iM!r)}$YqAo~KL9?~hPgFeC9OKNgVz$2Yascez! zQDh2r(D(~$hen3sO$_df^*++g6nteuVbp1$ z1tj(^=D7?cv~!H;7&Sl1VNQ*DY(`qn!h-PWnS&<@%hX8gJ|VWCmM80U2-I|LX}-0| zZ&~bxRmV>djI*msv2VWfvZlbtwmHxY!XPjpO7B<4AXYVZ8I>db#KIvl>)kS|&r$QO zAyMykxxfjHVR%(R+|kS!US~HFA4}w@n~h-{|NU#Rf$_o#+)imWyrjLR@U)vtON}nB zfg8V&P<3F|{G?EWQhmB~?6%32C28*~;N$5$B31|D)6INM+}n6%_xM?VIjy~oZB6{W z@su2uwM#WiA?atdL$Gxo84mYx%C2GUc)a1Dnvud-#?=3dNt97@HrMnOVB z`;3Ny_L|@_g`~N)bCcga7zW62cP;+6<4+>}!LB^rQ^1R;N&l|#Wf2G+d32>qO z>xr|O`!bAh*pFRB2l1yI+mhMW$ExAayUJ^jmKSwJ08T}-TU1^J8iIfb;VjRRgh~!7 zu#57tS{I{ADRC1e%OCm`Z3XOs5GdxLPiKr%b$ld_VUZUe5!#ti1?1$J4f8?)@^*Y; z5G&-&#$!$6m~beM>@-sHSBR=zfbRoHs^-_CVf5#FN?dsb>QLd{TlYzhzJ6}Du5H-E zV(YabU#x|Hushp5j$XOfraN?2)m*l2rgI?3MEq9j6*GvFO-?OUn4eAu!e?(~6eV=$ zUF>}w`)^ZI$&Ym~Tjws+;oyk4+Qb@^9c`;l3r+>7dQ%JhVcI<4#&rZy!$b zdSnVCikEk;SlI4)#q8uz6a+)4D?n`2AA2E6>uK6>T&Lj8z>R5qNZ00ns$g!eUs-oN z3v>oqPj6(`8PTyqtJ>qvEv&?;`Oq^I2mwAZ#};9W9kN;h&=-1D6FrZ=#p5qHgDizL z(ngEOs+C#kkT@b2N1kqzOs}=i42!GaMltZLgf(v}tE`0BaT()`j?aF#Rwa4s z+1ujBlM|L%@QU%QbgTQN9O#y;NITs%AWf#<_^ z77F-}KY3zQZ|1F183NQ?Hw|D)EOf2!jjR~uLVDXN`jZ3geLXTMEEkUuY*xk0+yDQE zOB^77Ecjd2-A@8Ed6V{HPR9Gc+I^s!b&9(Sy@hck-pbkT`^xj*mFTY;L+$~4zM-YV ze%8@JP35gMi8$(Ej@O0j=c_7A$wsUw46a-6+Ugs6G?{L>X;gqR!*{QQ?@0;HKnly@ z7a-el<$0`-%2=Xq-Mmj}K|vJ27TwrXZwA!eT+@)tOpH*+`+M55kI%w2B3<`0k659s zJPQ&_e=28h4)J=v9lEjVmm8xzCi|50q!}3hjj0<{**RPs8b&)3afHQp&LFpnx1$vZ zQ{que>OPUU>q!&hG!sl7J3M;Zs=I-*4+ z98ZU+346#6p6i9N7hGA*yVwl(2AmLFE5@Gv)mFn#|HB6-RrsR=S;`E%vJYGa))qAI zBi;k$dhn{5?UtKHOiCdmjAba?GaTwUb$x&|*gq@BL^<5OZLF_sj$FJu97xf*V@FgM za}T5n{_5Cl7d~)Sx$fmW9TWfHHM!m;yEiOyl7`sOGovwz{+Xo3A(FX;l)XE zgK8Nk5d|<$xZxg2s$ZnosB>y0v<-tXW- zN6j342#G8PXtwGJ_h!JkArV3ML9T$^cL`z#bJu}94XUre35G+QnX6#66U+`Y$D1g7 zi;sNx?t-eWsd9d+(7aa7n;5wSxcDXpVv{^GC%7jt>lio2HWC|Wu*03xSB+Dkhfa7l zW5B^%68q+d*}fG_^|a(1KDB%!i?|ku!h>b8D|%4Bo39^^6-aHs&_Kqikok-T|4mYijO(=t zU_}YO@I*?ulSV20BLl0lVo)sIZZ%`0n$*HcNJ$$wGrTE@d{Xphg(<0f^WK(bHRG6` zC(k+aey6MC*svT!V;gI_4vvX1VYy?&FyPnG~QU!wCnf44VTrMaR(NsX76XOZ=9 z*M&v)FC&>02}Nis$#XY1m8H%2aKkT$1@&q88)Zorx<+Fe`bEM5P5PURH%6I8bVX`% zW(=E>NIgghipXq{2F41BGo;Pb;j^4@*~E%QKZAeWiCk^K;HW zjDDnbS*@;ibyUkI#29uA*JOUX;iFf~-=ADEEZ<-hbca&o%w+&%TPL2&w=do~G;71E z_f*v$5C;WNrU^Ke#clHk=-shDf>3SVf_AX}o6>a`$R;Bpkn(n!Ie~CKtLF_&Q|YBlvi8r@m#zkn}Ggoc#_*Y zltgz9{;99^V+EWb(~sT2qaWfx#i`Y$ppiroE~#NVe;a|L@905Pbxd`N1kk`VB5bfQ zuEV1Zg#V+Oln&BBhAWGmr?Qw@zI3jw7`fDNB{Cf%yry1aCg-C-*M$H3`I$5bK%h$( z7$ov_)v1K>g7e0Bv4idFn6o`ZNaoV`YSQp`u*$kUXQ%Q-H{^AFt2_7Tp-jCt#QpLQ zH(3(kPpn#ulDX#UBu%QK-P7T&wToWBaq#*z{NUL;)7S+7tMg^bx*-I#-oG}-B-Mp} zc_=IxS+<(>=uRHc87A6xegYwJiQc}7|3x+I|5y(il|8purCa3^}3wU(I@JIM$d+$9zs9!8o-iYHkh} zYp&#O7Y(b@W5UZdW;^5LBVt4G;y-_Xebwf_u~_VWKkT!(srA9f4e3<(R&7kaHx0ZT z{G*s?#QU)@H{}Z;y1M46Vj&T(Yz&tYIDMYJJ4vptFZOZt{BwU+q`la0{Z?1`GNfa_ zyx~Gj#ClN{;tqqm>AJc)i)d4vwYo}@#=Rnq>-)0L^3mv3u}GC+gf})(CrC7JJKWqJ zJtC%O+phefI<`BxTHKZCQl1pvR;gnY0>=cdeJG0B)YIqX?0;Vw+IQ*0UrFE+}%+QMI&B>}4-`&7NqquIc%lmXETi}JfZA;Vo z;qJz4u_fxop*9psbrnLe`X{FSx4QtIvs2Pecg@YkxA~^tn+(TGySJcz`%@l8zX7bH zw_~skXeF*E%ToqpItoJ379Fh;%b0#PkaJ->hEu>!6->RUw7$ z?_iTnt7@0P_RaugM;fAymhVkVqxI?DAMV1Rve${f{k^t0-AYT}_v-U$d8tcheQ~c} z^{7wBta8>f`w%t-$#cIboS1S%4kAUBr>Sy?tb(oVA584GGt}`6ZSl*FP3oCfpPHDG zx$yk(54pCz?_f~ZxDGlD#v}l%wr;eGaxcf(zGta?PbQ^;tm=6qp}8zfBdV@+5iAcbU0EE{Nj^hQ z^%waZcKjsZG+jM>)*fVbs+T*sk5t49n{^XXL^?E*hFo&6QP?_pk@ajc_cmSBOUQbC z{_5^o_ljor_q7J0-Ohdf_6DQ=*tJ&^p}r_}=;lL8lt#`M-lBM{eWBvXY6kk^0G}h7ZZ3W2G5WY9!)wLs%}fviE-7F4_L^n$J(_H~ zPTiDwZoI}0b?-!IlY9hRhv0wsvT_l&HI)joT(!0>Z%SPWHc0E5gB+fDZ9zK|>^-_I z7iEjPA*FJzdMkA8=FP0 zmw=OG>(*se*)8akF~}Ucnamn%KFI+x$&MC^2w)ZGQC*8WIwIlt-?HPABayMD){Ej5 z%`2GpJE8ZAquT9)8h`h*A~@xc=n%FTIIJBjl;6Uwt9LvuI*a{`%n~WncSxs1FGbS> z@YDmTh>EP9cFfa z)Ab)7{C+-vhF;`6I@byo$K;eYZR4WSRo9*EylMwHzD=`l7mf(!{L|= zQexhV@nTVA`jp3sgJ#fEKvD6h?4&a5MXMnrwZ_U%wSRy|3d!B=dklacpBfk+B*KT; zM&irmv!cBK)Q_jW3<=9e$K|Hv^s~r)QDl;xYu$0a8_?mp_wp6TTc??Y>(ABR!UW4iJ-SDCq4501bUn|zMjS_)YrD_i27cFDg?u^M=` zOFQ(itM$qk4d!FDk{5rMhtq0WM02?KGQj7X#o~$FLOqDCT7oa*LV1}_Gz}aduji?_ zX1h*^CE=nT6J&a`zn;0}C|s0vx8|F|WxsWK58mL{v-ZjEbnj}>UY)!dy`jVM-VP$7rzxIxev2r{j-i9xv=Hy zTFAg^3Jg{d=f6iX&FG)uoccv%+Vr_{TT=cLl1WkqiCb}?o_yGJ$E`ejX7yZ{RA}78 z3)JmET4CrZOhieUEM}BOpeD2Rmcci2gzxZlSU(Ur5Wjb@#&OUUeu-0NF^~F6X=f}P zGy`oVnHzC65}H5+CUb5R4&e|Z>R0V;FqY;3MigWbMt-Gqa?4Gf&alWYW!1IAW8j_6 zmF%^fR3KEaY4it0|{F z{WeXBi|`AH-DvyDpaJep<3P!tro6or(Uh(Tu2JdxhA{_*funO_gk?)&{Rf0|!@CGG zqCyssbmj#@&RFLVDuPAL!=bG#87AQI4X$)Sk+r;VMukoEUvi(g2#!n5A1vtXDatGf zoRvR$4TW=JcqDwW{zGU6NgReKu)pc`hjHs_0uqg+y#(VQiB%;lsX#HfglC(5kkGvZ z>C65eYAA?!Q73|YH(~m%CoT*|k@)F+Wcb>kTqQbsFE^1-rE0&-|CN_uWDwF-s6&M& zGM?(I(sBxB)0GAG2~bcp)MO|R@t|)ZViUboY>BW-C9rEUoK>blf=5z7K*IGHSkxl$ zDFUyQQawp|v=R}Fh+W{rSh6KzpBM2f@JU0mU@s?xd_v<7fWP}qklzF86K9ZvbEzcvYna`L|Wk%z0Hwkex~## ziydt_F;5Nd6&7h>Wssa_5ow0O=HF+W>i_PmQsh|lWzA=~pV1iNpZhBIHG^(pku=vB zOB2+v^2i#Mh1ZB^A$QfoPH77(tzg5598jc!?8+jAbx@s@>yOJ#*k8~dKUAS;uyy)h zgwr^BmAig79$@kg-6 zUtHn-cR-D1HJSwsa_IK9_Ln;IT=#xlloa||1 zXEI#Vo@m0-60|)!H)pcK$iJiyfj+OOo zg(IQta53|59@yJSmei>xJ_lI;56a#GIIbW@6f`q4Gcz+&%*+%sjG394nVFfHVzy&u z=9puK*fHN^ckkb?tG&JYucqcr&664=wO+ScQqyZcI@Uy*=vozjJD4Y5f0Qz@X~=4$^AUX?Xqtr^GEkrGqTui}6xIZ46O9G*zrIv={H=w{B1UlF zqro>>V{n?wrU+q`1T-tr%7ojm(WMOUMk@~#KnX6kC=4ASwoAiE{`QY0qtuPRw7O>7 z7$A&S9x#N;qF~6Ry79NOX_&-=!64&E_M%rg81TkSu)-U6%?j^$6f*1kphdOa2QglBB3G^iNbJ1 zt$a(jn2(97RVoft1KMdqK&dU3&K{5@-Kpv7(p;beN=s6!Ls%wBGEy0k5rW^V(OI%- z;K<<6%m*RgYsmctAerIrYiZ2iBPN)tbZCaM`=~&xtbdWvl{JMb!*~@(n-m(2ib}!A z)`n&3px6a#(MhNx`D3hJwgFp$0qyjh?jeI&##Rme0%u5Yq-T7f1xNl*Be82GL8MZV z!Q2xd(YC0lh;Cxv{=JJ$%q&oz8bVG!{X?$EAf;x0FD6!OU&fUlK|}-8MP9U-Xc8>4 z+x(Hi@136>jP4j&`*n}2dSeldNE5}1SwtVsk7dv%OOcK%@RXG1YAc8$!4N+QHLe?4^am!IPzKIFjS~HlI#|J> zfyKPj2Ufo0j4Gr@Ok<_fn<%S0v#m?3aVr}yJvo>lmE8r*?wd^)QxT0f)#>AH5C$A6 zhFCW`N&dEu^?r$ko>M6H!Aeu(&BP8B{Rc}wtQW@L>b(X1s{eO`Mo&NZKbuUsaRr7d z(E2IR|3BGe5P&urJ7arm3v(A&CTBB?nUoozWfUd!Mj*_5c3p%S64IGwvI|f!z!|O$ z$$X!Uw)-PKMN_4;`gg(dVZ9XlUVLlLl`+B3nC8ksbq-Tn%1LRfwsU&E9;U9cExyh5 zc6#D$!aG1nSpi+Qhdmwd_w+DEZUo!8mSP&u><0m*Ub4Xz9w%vpvDSDPSfV&F>pw@n zc)YW}U@f#%TrUa}y`D(~V|Mte3Y+1K+g1A_+DVdvMB-rOCqrFB2)Y&JM%mLJRCouA zu6~Uh^(}=I3mDzf$Iq?rMk16S8}9ctUh9YK`KDQUUz>WnEsfsok*6BvvRy0v?&0_P zI{jpt`@;Rz(CRK>D>7SR`I>mzdQYtJt>GW}TA!^JA%P~X5b*zrd@%pW_kSCeQxp=G zSWv=m-lKnBt~jCjERD%Fqd`L~k|nlFZ&xqpLxtFA#kbIY<@??+JCbjcbXX(dm-&Y6Rw~1DenTKW2XC=wmO%y>zwoSL z$ThX|btoppm68-1qi|9nnKn0K*G=Epq>SpMCpC2!u5MxylxMy(ap7eBN+SX$Je%&p zSD4;wz*1D{vh}#tJt;9gD7+U8db<+cNJu=0JmA?GWvzg<+)A|I?Fv?OPMmz^jx@g* z(|xha@HaWf9?Q2_2^i5U`b*BiLAmuFGWP;oKPb}BUhk_84gLKawi)`OBRZ8w!end4 zM}l3qlFlvMdO_X?g>`M@x{IA-PW@=}nx41M{HkKFbMC*Bvaoe9 zb~XP$t;^2b-tB*`>%Z%nql2TH<3H5@p=bXc-oKtX8rz%O{ukbVw{t-K-%!NZ)YRP8 z97tFPXJ%73XXpR25rYHm%FzEt8whYO;y1^)u|HC5U{|3|A z&e+o2g;~tn)y~+_<$vf691!{c+5Wh~-?HE0dGhNYQf3X=ftGa6HN2D|m~MC3vJNrX z-0&ip$Dnkfj{=h!l_7rid8#>)drHKho#>WmN^uP$#I_>MA9m?aQk`RVXm)j1=qzOD zzdfNY;wI;{K%Gf}zgbJzdu!;O`tyBOhd@nU6Lr{J%K04LD+i4=WTs0SGZ#i%LUoD8 z>xYT7a?_-W)QIdaOnPbij{0sN&FngzaWRIYu#U85ZOB3U4*m^BMI5EzgnkIP6FFyc z@R*qxJS|pM6Ttfti!YI*Dhm^%dnlo^%DsAtBo7I#N^q2sKbuGD9hMWX25#K;vY0-# z+A~;nf{tb&B^RwVS`^QwLRqonp!v+zq_SEE{q~9B7!DEZrB0wqbY3$Z7FA76;yNe&!a$gA_0FsBR||B`t!dyl#mw>00tU>Uycs+G%pN z?+BdEYAA}BFlF9b)sjWPgR)K;!Q+2BbbYDSIs4 zzfB@3x4OLr_-5F;^HctBt+O|K*72H6N{R>X{G_cjixw zZ3y-NSq|v)(W{`CD-VwxvRg5owr1_Z!@JZxUZ1R8(<^tTspPIfShOmKcd+BPoG!E( zgFzQ2;yDQXq zx-HK<)?TR3G#f)~__}+z9KI0UctR~0vE*FhKEgYu3Gzdys7)%6m`t#mVp}H904JCd z#E~5Z>`b?6PDYMY%Gb!Osn$y@rtOVWCafpLR2UT_6D%$8_ zM3#h>m~8DpVg&KhzemDFIdkHOZ282ucz57!b>p}B*SW@J>0msz6r*9OHk8pQ$)*md z8Yi+mqD_bnK0EaJnFysSq&_c~<0&L{ScOM{>Iz{!dD)O+io<*Qt(ez9)ue7ejs;nq zp?K@?ouyy1yA>AHyLvwU^^;|>dRDn4EAqB~fi%&To)x0?QiTdxY#H5%IX#=ssmcQJ zCJ&7Z>lH8<<5~S&2c<2*Jkd{M|1PMdH-aT;dn$egN9LzYCZHo?^$6;~=%~6}l_*wb zRxCnyWF?U)Drw#1cEUVKFr})c!-#oAqOt1wRKa`4olBD0A?gyHXSvLSOpPgQngl0s zt1?x_kr4U&=kLU-C>q{JV-Be*k{REn4N}0|A8XEh+AIbXN0};V!>Qp(Tq}+tnnvYz;zHhVT5<^GF0b~Me>FlqrqgBnbKi|UN;Xhh( zSJ|#+i@I$N+PK6uem(xBQ;9lhegSdY-W3fa7;BVuBmAxQd!5frQF}$SMkAt{irCvn zy5%=*Nf*&5`a1SPsO*`D%S>fNc}bAmtW3PaoA%7G){S`0D5o6h)5?ZQ#6}>>4%Rw` z1HmTdt{D34WHHj+0ISydk9=OlKc-qOTeichay5#zkvvdLUAw-gbzgs<0|-V1$u5Wn z{HD(lS7`J0%b7=KG}xEfS+llwofE+qV24uY{TC3Rnh z93b5sE2tyb9fb;%5WiYfpmTQ)0(+c29dgZ|iOm2A>v&aqoN~WW z3f{02ydrwI!sgsES+i@dAJD5)0RgZVs5 z0ep;|2QguM_7;0J(q{_~Unw3EXj*TDuMl@}4!#;lAF3fD*7VOS>sm;wUg`y!r=k!? z^xkH+Q|PX}$~|2dYjBCA2huza zS~1a!>mR;Ldx8f>3@t)y1wz;>{U&megNEM?4fCPjVpe3EP9p?u=%?Cx>?-hd`<0ol z#W*cogMVlNw2)=RAFfRVJBm41`4)9m8NcKc+lL4;xXwH)+;8)VLq5M08$W^b5oj2T z6%24I;N&Qt<$qeMU;^hS3XQf62i$*e(-^eJCq?NyrPn0zuEP3#f`;1Q zVwF}ML8F?QOFo2TLx)9!5cTmKb6&7T}l;n%=?_L z5R09I57_JkeydX>o|E|aHiMHDPoaUC)mJHg(T;QtDk6 zsaO>3Aw~qalPunl&xW^AHS?)K_n6hb%)+>m4m?YD-JzvjnfZ*4KM3u$*fnEcZ}HmC zP}iV@c1sw4xMEE$=f&=S#E@i=$?FHPN{QFRlg#QAP;0p-`v7sYQ0Z}rW%oa zH!xxgyS>5sAYMx%jGn-P<*=rEMy0azWz~#yI|LA1_ z4`lQI?p)z#Fk3C=GtkSdB3#t}q0 z`E<=egIfdyB73^F9>5{8U@Qainxv6NO;4|+xk~)?F!iVNfwo}g+tLK`oRw6*+x>(Q zAAtf>falX(K<`zS<=0Ea*WLK}=%`gzzb7w^^Fw}*_BPdA}}E}Y)n-J_J1K4y;_FP+|wcVCu*hf(CYdaRh73`@%b;ec78 z2V1@#fw%F!xp$8~$xm~q_U>0_E`s=HgBh3u%M8om&*Hr|j!EG!W4G~)n$bVI{FQA_ zwSV2+&PQF{Kg>6X)&k9mQ{yK4w~tzq9Fzjyo%!6jAIpbhc7M7}*?K=Xe+167`(-82 znUz_7eM}Vu5b7VV`8~(J6bs`X)$~|!Uwmp$HFc}y&)nyn+ctH3^K|z7J#pt^V!}t7 zy?43}%PUxj-Tkw>UoEUB0$tnb*~0mtlritKQ>JKM@33tZKyaFaj63^UP^5IX#@Dyq zyF(BD=pJx&(zkT(dMh#>-69$w&}nn|>)8p(zP|tOZjF%e-|D%RB(>a?v=u>$#Gb-r1Tj0cWTR5ZQ%alZIU$ zgr$1Fqn|R)O>~ilUjjZFW*bh^ns@pelUVioAKgBygwMZszk#6DGuul{20S*D?;2kI zbbFTE=_t?;b$apoaz8L^#X}`_IVnalton2R5CtR3d%sdxyw}MB7@?jncu1 zv78hIKzND*2W3tUErTl8Myf4E3qh4bb(jB=)GxE8ne!agpRAmSPxyyC8@4XZI<q zuUk4|14G95sj$c4r^bPfhhFYJo!I)f!`+jem<>Cm44;KP=I7tT7e8EnTlD38n(xxz zc;%6RP2@Ma~j@{3Wb_cq*zZZ>$7B=)SJ_-T!4m*lNE#)BM>mA$*wCSsLo4bU!Q} zpkA$6*akeNJ=}<*C2lEP>Eq6BSMS|FWliGOkGrrZ&y~$Ki-eYlw;pE=Z==6nO8$KK z-u3Zj5<3ih59gj7m+wIs!b%`PX>#{C+;#Y#O>kW)Td?0ird8esV{Xibu#m2n8x4oIppT}GFpCuxh?u~H)Gq%WvPd( zf6wA$&wS6FZYEkaT()X&kb(^_wEwXF+iS|v)9bufOLxatJ9Js^c??mwe8K&`(%~qO zJNnHbS3&s3Ysa<@XKJnMqFv8J$?>^baIVYc7ZK)%|F_zLXAxwrJq)r?1{A*rm%={C zmRUMtk2#O)m(QFWVZ^beK?lX@`wly=4;vd;XV&~D?5#Z> z`0LZlzUH)yy{HprTXi<@v}q&C5bJboEdS#*q;Z*WHln)mx`H9KI83Sd7@f}s9Oui0 zH*aMg3Gz+0N+;^($H2}M2mp_fCVBhBEkB0+_y=*m0<+=g?Q{R<19L+`|J)C+KZswB zC!cpE@3+DJZ+3E5Y?ZX9l4|)mH_@xUZ$qsmyl>06bZMaz6d`<`HQ3zh2MR@Sk@YQE zWScOGT^6d^+T5G<(WH-FmGi3RQml*=P2rA(^Qvxkk;~&|c{H{3*=>q&)_D{tGxs3v zGRAgl)nm&|@)-6^C7V=XAzd>Jq)*oYi9fjz4%5Augf{ z?P0j(@vJjDEkQtM9M?GQGdWgS$^i0=!=)V4EVP6xEY<{*<`$HP;kBO-6SdtcvFBce z(_)s{N{Q2A5xm~xW?6wWR;z5drRF9BqfqdAcFWE3P&QeroaOQ6CL$S8lI~du zXvbMA$!vpdMI&&T=E+Hn=&Ba>?1vxxGMOy8Lk#F01}?wTWd>|rThb>?7}a$5fkj~1 zWubAFr<=>#ywWqKNEQpUNTH1S&4=smPfAO)%WG#@TeMzq+h7(g4&DAdKaa=?e2{HP zk3zG>5V!)Aq+2`34MGi~w?R#Yu|m`bTBMGXgneN3f0Ng#;>FP$!!=!0t;aEkvzYSi zc*6TtG?p8|6<#F|5a*yC1AxJku&>;^k+$BkShfglG>z-3)V85Drd9Va<#*~)MHMTk zk|kYq%0-!au5g8u8E5Ix!<1>#lW{Qoogwd$v&!m1jzy(MWZiZ&n&zILu*qW#Qt1B! zp5bE)JG_Yo^{~I~7G(=7zLhLN7NX{dFeD7ZG~xMSMKIlOti5F#N*T)d73wAc5C3!j z1K-wE`(%P=Y*HApmNlr(I!`5-le@Am0hcnKQx+q$PbB0X8$>FqBo5djH5^J%eb=aR zECT|Y)X-yH8EcG#Auw#ZolQ}gOcgP%aNSc{)X7k`F3M!Ll9rg(OO&^l!sS zltFV1@>xb|%*_;Fi}We2<%Z7Yiv&%88p=eGIp5n%D`H?ic^ol|rZp$+B6Ev0(rEmd z%v6zaewM)^HzxEjfDc>+Y|aE?F0>t825eqUp42`DQVLi$&PwWV=uGl#s(9mZGMYFg zw0HH+rattv@eo(I<+j(t5K0L}$QT%y>Gc)6%)Gf2TuP|I9v6$lYAKvriCXItOzsec zb)n3nLKcOYH^&TwTC&ST=Mf%OPT|fbE)z>ks9lT=g1I~{0D&6eWMg4qwRMhcF!+sw zrsRz^CUjG>T0YRE41ZJGet~ zVq8-er><&MBu2U#$;;+bCEB(m1Ya1fJQ|m=-mm~)9$+V2aZ-`o5iZ3nh-!k%JUQ`Y zo}*;sKwEDTvIUK2!QIB;cPOx-rt%i8l(8-CSa3wI3Y zj!tOz-LJwJUb`&)%gHoRcldacrju_c?N~10*dgKnkoP6C@%$ z6ysyaF`y9*Q?dR8@LY5RXxyZgt%sTtCq8RT3Bl<*+1atHdfat8bMbx>^7#{|=jlGn zH^=SovruF?r*qmfyXWu!S@$;O_4K(9f`MG-XU$wqb0DQ>q$Z;^)mSIIf0FOO`7EI8 zl7_O1HXB;M zL8aq8KU7C4*OCtPM%&dV2jfn*QAtU|PNle`$05)!WBD0Ma%ZTDmCc9BRa3;Wdd!qX z^--PugH4?*;5@2rK*8v=oSCUnmHep{&C+0!Lr>ruw#P1ykO3vo(A}fU3jAv-j$=|b zAUTzSpnCeczO@8CPpfIhd>gGQ%jIk55{}5gV{I7p@JYTl4r-K4z7CH!a2A8M29kSw z!w7su%V9v5wap}_rmnySfn60i#hv-%(PjdW)XICOj_9mW`*x69H9;!p;~C#+UWh}7PZv0bwTGvW6ahGveDd3maGv)Dx?gy zvh*$EmcCVhm5^BX#5J}U6AyEWCuyP=nA1ooGen{L3 zl;^wAflMi(!Pzp9jx5Q#H+;0#FzYFf9)h;jo9t~|*;y#YfVTCjG+GOU3f(tY==Acd za!ssuL5|md7Ev{;tIO>K`s}X)ifK#Y4tf2^?h4D*n-R6d5745g(sK2QTd+r^#<29Kh0Zrn&^~TFYoY(q zclAhBxCFe<-s2n-NS%_R8=bTw%l~XWgwOlFaAik33YurMaHWvSxR8(AzQA#Z!_F`| z%cJ^tU#LwLA90q$)uSxWX@H%fyH6hW0*e=^XqJpr^THYxF5h_Js!U#vu2MKHJA^U0 zhu-cNgJ<|?`x^9Mn!97Fec<4NIIy$t!Is*tK3nH1f&cDN<*}*<>5Zx(Q49fE{1)1_ zK53fKbb!z_dsz70)k5M8|0g$Lo5i^JKahAQz$Mj9!>QVg601xo=Hklj9DGbuuemT&z8B0rt>P3tmrvZ0z1)mFwJdGpUg+lB-l2D~=)iY;@ z7hueH1+s|v?^Oggr!!ZLRhvs)$9F_lxte}??2ww*5SRWhIzY7Xn=-+u_e;RH^ZsnH z+|v|EA^O!TI}}UD9!I6K*op!@ikX!&Ht99|)w!$4&9_TTbYi$3zgf+ow&RGt9_0Ru zS|OjNR|GHVNRvsJYpqM&K=H#ZkAv5zeL1sAkZ6kma4WF6pgg%#M0P-&3rc1R+c zCap|~h5-dDC0@qKC%+j|BHA!t$UR@EWi@ZZaR#g}M@TO=d<1lqP;!~r8x5UM5&v|T zd^e^n^_ZrEO2or4HWO278cJ#!j@%*+wPgs#{#i4JH5p)~XBOg-Xe3LKi3ST51c8*l z=Xv(-?#Qu@iFXNwcL|5+9tYJU25vtr8MBbcK!O-fKF@7w{Nn`)%{B@`k8IE+5k&v` zw(-wwk;=dX8ss&x2hA<<)3p$T%Dym=o|h8JAi3zC)x1Hpm1%BTV_E3iXMHqZztc=% ziDd?h*fV?Xc}^h|wX6#9tqSQm<~i}s;1D|@#;{66K~5o(0E*Jn$=4)7>C%OiXf$B3 zQ8A0e1dz62VC_L*?P8#MC4qIy1Bo<&m}lh%W*@koRw$hmqeQuom;(d54!^AU=sEM! zvS(S6$RRIj+;X#LwX$cqIr18SAI^W@B^-I?winP`8y+1Fd<&RP!^ZU7&dihj-p4%c zQ~{^^G}=y6z9D)i^!8Z|UFJW2rSrz?S>Pu59zsV3U*({A2@Yh5lxD;7K>x-{2nkNs zI-zWsX66Ki>h?Cw;#(w65So3BS#U*jCOUYp4oI2%dK^Ef{dzeb zo!(iZ{d~FEwPKs@f4`k}lerr_5ofk^eW|($t1|`rI{A?m(Cz!;#i6g?Rem1JCfLa& z+&lR#LrW22jJ;&zx7ssWj*6GZ%FXc$@#kBa{!+phGcm&?w_q^JP`290^KQU!gy&fKrn779GIGdD9B@L? zp^~tLQMH?S|5x#-sE%`KXaQ?yWxjc>NgvVkX0{4dD zX4Q^i#74cFXp0nda$lHk7W}f^{$u1rw}O{gYAa}Aio>71kuR?_$h75ZIA> zT_zohJnS^S1Qi-OHpRlO%wl}YJiU4c)+xQ~M(aT{2d!~~qxSC)L16`o!<$pGGvCaA zZ8ZzQD?i(W}l!nRJJhpE`+H>vnS+f?IAffA|~wDrB2ADKx!=$KS^->8z)_XuKR)Y z5+Jg5@=~4ZV)$nS1M=LL>hU{Jo(P4>1u_=EzzCHyfdT0?Ax?~ec1fyDs0|J{c}h5z z=;J(iaUN1xlr#N(gV;he(jJ9do`F&!wLDxvTARM^nXic38U$gQ>xgvVRfME{xE(#3 zEhU<=&L08x|B9Me2joB4Yx5rMaCy=gITut-S1RlN9VUTzgwUY_H~sn+3#mOvf~c#f z1)ZuM44S6hz*bJFmaSqz5vEY9?>0<#cWoW|4SbYBL@nQ%ioK^?3{4O(CpWvhP=DpP%-1;Yx&D#dpXzt7jih!iQ4dmHkUQla-` zl>~|Cs|j9xACHA0@yLx9u+Nk(S-E7D?T476Sx|Q=YRjPrlrv{4z0C39;gWr;uMgSdRX|u>dCyWBlDEz&ckR&X* zg%9_UO`$R!)lc}ZM)c$M>p{um2ryQ#E$DYy)wq_a-mpp5LlBnXHUP$XOSGVK1Rq@$ z5HNA{>R27dwDM?Nu|SO{uoj7UT;u^qaXPr}bJ*hS5Gm`7#H{vg8avuQ{=5jt&G?q5 zl#EqY(WHokL@+J6sHWSC&_B7|nzmBniIGJts8C6F{=O^ENSCzARm#7nY_wSnSsAfa zEAKQ0e*o*-2NfrdmS>@2csOlmMY{A@z*J)T3BiuZfy%wZLsC6qo$Gxb%bTzb%)N~% zB5=kOlUyb$=bc?FJtE&jiTsB$BMt)#%jVSJW0{SU<`ySg0>SODR-aQcPL=tfg9e7N zR=6w^^cfJNy@hEdqpF6G^plN=B1M9>q-~J}J3+9o4juz1UQt4gJFj`L+8U7R%`5U> z(H@Mo=!7Kl7+Aq+0-WM^%ea{OG>WOH(JV$JWibYPM8YIScu_NM4i^Z6)lon^*c6p3 z8D9G;(t%nMV=~IersPTsnqofoX&2{c96(XhQ$@xCG(%W&HJa=px&)^}+#*v1v?2}m zW|urfHtX+O6w4BPJ$sw6`A7=2Rho(1DKpFz5v44FK0ybB_Db_ASvIb?NN6tJ5-Wg` z^UF#r)^?6Q4cU-%y@% z=SXGyq{&X!{BV%_Wvaz$zB7!fSh<7MrBrV`Fc_HEN6nRiV@x?90%9Z}AFs?12}KHr z6s9HJ1`89*qnayRrod;Ef#rFUvF0$J#}gtj@S0XQ3AZ@AN~$UY9HK>4 zTfE9S1;R1Bu_=88bo^VA{C@(o}(Y zCSJKsHsqO}O=q1}I1tGXTZ`d59U1t-@UAZFtOe{0kco{Wc#`k4@`?jYDRGBvkOb0_ z>?W|st2cfcSnD_STZgam@5%rqhFmu?0*-(}1n%e%cfc$Uw~&SmUl??a7r`*G<1dT} zV5G{cI^_YzF32-PPke&S3v&6{K=u}OGjz6=%T#B`WbQ7GaciL%9IPw0S>@`bwPev5 z&V7Pey6j}B(>)xbL0gK5RlCxhaVl#&pQ{a`H>Y{*B3{F3w#p#v1FNz&^{eKH&SrWl z>$szLpAVYLetxQ9<@}YJo6*M>dR56x&4f|vFVDR&)!EAB5&m<;o-<0^X;w3X1Y4PD z41|2erC~Gsf^uhmNec;3u% z@tVc_@DiqKUY*ok&m6^C)nvIT;9+WI9p@^&*ecG=V8hu-u{yslyU--)!5MNSy0!0{ z>VTxJb|S;9mE>HBR?Q}hzZNunKGcA|EFA|(7M*m)4I-s)!aN^({1sPwJv^}?lAVH_ z&W=W}l4Y!ZQA?fuTN(48F)<3?3?wEB;0OW$d5x2>b#)bR5gGb;^R&1FBm2)&Br zKX4X#IJH9cfZ1`Pkh%_k zB%*m@09+VloQ88zZ%TTUVGvN}=2rVAO7w7o-Oclvf>*(Wi2sRgrA~hMqN``F0K-NXy7_l zn>2(%2E$X2LoKM4>RKtX2rL>s^Ldd!t5;+Mgav{i97Z}b%PmuaOJ2<@94lQxTf3zX{X((x$P$z<#_ocNj!M%EB4BH=s| z^2|H|RE-9zZz|Dhm)K<^ygxSaVCCoH+P|x(UsjHw7yySR{Qh}kU&;m^qJRuh51qA* zfPp4^Db1);^|d~zX&LLZ)xL#9#qV|R&$K!kb;9vPfH*B`Dqu1dV%49-z>mxuiJ%yr zmWZJSS2cCjlY57zxROEwX5ZiYrcdG^&TJx4mO;Fmv`K`ev_ceL21lGu$*jM!)bGq?ZL-8AY?6XQLOz8SR2Ub#}IIx@ssS7PpRF@IHU@kgKK%3vbBH z|26@kOetEO2yvc1GC+6Sr~HqvaOHsYE7rP|M?f%8bSNE4d6Kqic!cpHxKt^HtCio+ z+6N$t&MFSNK{8a=yI<CAR;XnackXzoQ6VQaqBd+TPAydFe~?Wt9`o3BK}#h z!cs%bXz#KuG;o()>%yPoNboocF^q%ro9D&Z<&v~IpzusIE`}Ix0YQ4GVdWZ>3N?*s ze%zGd`TiU5n6K+Wpd}Pdr~Mu7ZYnbu#mm#oC24J;R%Ah4=l+<4Wsk{pItF3BBp~5-4x>kf53DGn?^6C>H^hZP+ zj8Q=wWk4Dy5Mr~4M+i36w{=!h#j5C@IH?@ruryfQw$Adbh^Wip&{G)qQnG^H{D4b& zS$6`3m#IM~&2Z!Fid|$Qj!71x46?q-3gpfnkDC9TtiQ_RtOH+Fx-#wKkJee~TaBu# zjUt%`gIQNoWtAF_nYHjQU}^6FQC=!G4Yy8?PM5suOd*PL-tRhWavMtDwtvo`o#{Ea;C{EpL@cVD3$%s0XW1#;BpG^Wq-t*VpfvEN7DRTtz(?)#sr~(mkxdv(zqm+<`T1e%RQU7wj|=t1oZ{;2SFgVB9_e1s z_kh>%>f@WstsDNEmV)JomX*`Jreo^8_BMmC(%Q(sdDy0R?9K=Sq|LARRr)ty87WEr zTQ7>&9xcys6GnGxPmZ^bAZp+YGO*ZQ0?kzipwES6P)%ulQ5}okQ?eI~^l=PB^NRF! z9<{APNc0A=qjNu5t=45PsLjfZV^p?fTk#qaB6<^c?lT#uOxqcdsdU0D8edY}FCOAx zdQ9Cf;|JICsPx*b-J+fT;;ww1_XZRCnBG2xqh$^jB_#faZsciboGjR)G}r5llUk|&D*Ful{JSR#SSxkVpN!P%9$_OY znS^9?*W1e8gk=R9O_Tw%g5Qs*byI2nMv=|=Km#meo2`U1$Ue8JQ2W8o=If%8ciM4I z^Kgii6Wk5|nn&mb=3XhySIJXASHyptRq+A?#~&Oo6nw7~gZ={>cdoV6q9kq_DlPlx zaS-K6@MX8bDczoxllaH;skr~&C!epJ2O%e$d{Yj2H;Mv?72|hNp?Voq zO5cMR6*aZKaW?&?f%@euK={se1N3H_y%HFryT^QUl+yA`z+pNQAsj>@#L6a9r+ktV z?mt(e(i^jL+3bHyuZKQ&-_?=l4-snsDdEiYpR{~wjL0&eZ=A<_=H`ds-}~h!M%Y#E#nF1xNTZ{g(ivC zmI-;PP{gzHO9YJ4UN@J&>?=$to;baYINPPU?U{ZKKH_KlV2BSZWQGq5ZUB=^B=R9? z+^6csr({`(P*x;$FcQg5q~I05nHqzq=8CUGLjPXyqwJswLlrp(nvlmQrr9#%TkMrd zt5(lFYZ+7v)78+drcjNeaH6k|B(hx^1(m8Uq?>5fx6Wa!__g;AX#@~8JmpE4q9@f_ zCx_jHsum9+BD#NHciV5TZycl&UDl5~t<%D*dWX&d6B~IX|4Jpg?yPXzNeWggAK2wr zqyE*e239L@&Kz6(EFgV@xV@gEJ(hp?NF?CUJRgN#{me5+8_}TqQdO%*yIz%Wyb^=b zx8~Sk`0e2WQt{>>-VZQGM9p)I_j}XDkM2ibXmv_#=@XmCRaPMXELpY#v&!c1Ald!$ z4?#sozyQEOQSvHfoXCAl0y3Nqb#PJ|(9n1%>p8+!w|NL*DI%Qlf_8DG@(P_oCJwkB zo5iYV_mE_Ulfepy!Kf@-KDESgPR9!b zc9hzr-B}YExCP=Pn*mKxEApUqgk3kMeI&9$Wd-nTU}r=FR0sp4lobPPNZjI3ruZj?$e0hX zVn1_j>hM3t@I~;`&wsF~!s=sz2rUdIVp>q5*q*61Jxp+HX0QeeSo)XPC|69we5|#| zdN4a9Q9?#%)$8++^*+C#>@;8;86B0f;+iWiw>{iokz*r8U=Lv<=^eQ)&M#2T88?78uUZLQ4%z;#ho+%g>PH8l2_i zW)ZpWkOa0qVA4geuJ0p(D4Ls#{8u53P!48t2nT0Tni%Z}7F_BB-wJe&ba-e^WsTg8BnP3##XHY8 zo#jZuO%|+cLI?}h{eO~rjjQaw%`^lY);>Ic|3r187y-xh??^zWg;y@X)yD*QGUipz z0RA$Rq+ikJ8Zb|qEOZKmRn(Bf4K^^v^Yu2x#I9Z5yh}M0kmmfrrKE4XGjJFjBm1y6G&aS$>`|EUz*>r`-KP zuHD;K@;EG);9GM%Pwp7ZtK3#9zxFq(GWeXl9#cToy;Z~@-QZ?i3Vd20Kc^tpSHM1l zgysCy}Il@mWYZH#jrs>x4wBDT1vh|`Nbg7&cnQZO6LYv^TlGVtv7tfi3 z&oX)`ZYL|%(JUms%%MJi7^uGQB*O2`)Cx}PIw*fwo4E(8v)MHavmqpBsW>oS)ew#Z zI9rBNWc6`R`MEy(_i$@pQ5Gklmc^o)!y*E;*>6lfH;{GddmQd^<~d@ni8~ANawg{t zN~pW)140l}+=g+eZ2_w(wbAsnmMIjcg#~y;=FT=Wr-kx&viSZ7uFe!19>nzUlY#3@ zcD3vl@u&@<5GkdB!i@$fM8NzdlqPG>7IM$BEzk8QS7#0$PrKK6>BeFyq3rZiW~

XCa?*7Ecq?amm~f z-n&sLM$9!q-PqE9+7aAK9fAOUX&Fufx*jIp6 z(RFQul!TzPfPi!fA`;RqNO!kLN_QjD-3`*+N;lHo-F@gr@;}GtLC<-=_xrDJb8&HI z_Usk+T6@iAX3pH-7m1+;cxf3B_C_ak5+8@?XOKHt>uCseZ9-q;m^7Zo-o*|`aN|q( z_BYL%i`U1k-vjFJw-?VA=Q8y!{78rjkEQe?$3n~LwUS*TX5tl(Z6L?u6^}mF5(G}vlV6l7*x9_TE zfLGn@s?69C*;r$lZ!!vdr&!{{uRZ{tQsEot`S`17;^BAT!RZN!b}*-Pd@wHZ@RpmC znD0+aZ&vngo|?87K2*%DvmF&}uCC#o3N6o4*M|#|cpQ|s;v-=Fyw^kzX8M~)pMPI& z_)nx)MdQt@34Xg~3z%?S6tG=18L;NQwX>|ruwgpIB4Gz*Fl)60FD{7_sTjMIMtpc{ z9>4VHZN-}G@F{PvnHVWjrHkaRd)|Y$l;%HDq|IU$@JJv3k$%cE|0==xy+p}JJrXJQ zzcTX0q{wH@D~b97zIv#LcBd&6#7+vsR~#6?#{KAtQxLb-FZ@x;PNr(hdf*Seo@J*! zza##po*V$Y`-?7ZRxUpln*Sp!o#vakz&Fz;l+p;gj-O4bOAQ) zIPpC;1;%8E1Z8M$=@xmU!Cnc&W8voGPFlex*0;OUVOA@g-M~B4sX&|qF`~rsaSR{O zzu)y#KleXtp48sxZa(#8A0$|l_u)MBY{MmVCvUp%pcw7pmgo$;)7NBOG-t-OZ9fW4 z&B4spU58a?V`fl^3JCChY$Sk~I^$EtsgTD;^{Xv#g^c)WS)M?-JA7kge2zUt zcZ$gnmg0-)revj-+s(mB{RZ0N=;iH&UzD%3!j@>#GlgiPzzlLYp z&Q%NGM=d~4Mk!c9N!!3q@9W**S#qs$VQokF$x{H*9EV)0t6D+9J8WC#e?-8?I-2c| zjcboHha~xS-Fkcy7Lr$TrUMvT0+2WrUJbnI1)9)KZ9+hemX=={zIo>BR+Jw_5LnDE zYT1(jT~~oGP;bq!AgeXcV~K<>IubchI4iB<^p-}fj0Sn0^O@Hs_r*$)e2lIFOGu`# zw93}>H%5hXL#kH+Yu_EzEVOk$3VVj!?Nq z85!5to>r1>P6}6G|9-kiV;~I72`8#2@8nk;p*NcU^!JD#U9Io2$OGMA2w1CkfZrW3 z3$pUuw2^R^;4DLS09W_}+xH^fWF?R!zT|hLbi5dJ-)QX` z#xeMOY=jMyLbr@#sQlE38}aM6g)BWGgqb~rPfpsy`eLDuMM+Rk+9&#O1m0-6zHf73Bs(GZaEb71b14gcq9L+D8w0}ln-~VL`z)>me#eCA?-9%(aX2ulDii#) z$b|r+e~zJ0!0vWI+dc1<-fWYyL#)L5yvNLjzuMvI%u4T+; z?L&$6+9`a<3Y9ep$M`gAp;yMT`r%_ZeOBEfS%?!qaRkgy*k?5SkyPR7vs))f8x!P} z$&TPEN@*fmi;Wn3vKm5cnZEes6@=s$pNW)EjAMy9wQM=P5Y?EB!V!?ue4p5;|4reS zE{!QyPl2enkgR62oG1zPZo;vyl=pFYGyfLjc=TjOy@$05itV~c53=ysEE}K)7@J$Z{3mZESQyq%j21?B!~ebrkh%P6arNSMMey-eS(I%a}YrcDhlD#0x6= z^muK9WSnWu3Hxez5;5v&D5|{ z4W>f~day;T%Mq7fs3fT+c=V7h3AMm}~)=l_>ND&#sW$IUlTE!WV3Mt=bU`MT< zZIw~j;);gK7#6YM<>eA+VSFSI4SQi(i35iXNSC3tJ#xZ)Zjm0CUMUg~#2=_VEHYku znLx3+b*uAUEye2*vfXelR-FS^dZFkKA#{CA7K|Ygg}TcGn$@iry3Mc&EburU_-2M< zFIejdwI(trNPfLRI<+F*t64Vb2;zCs5%kE~&~S_lPcxLsMAj%g>G>s8fXIisPyK9_ zg1Xshs0Rt8No!awE-l73AI!JOI?Y4y7Q>lQxQo* zA2cvcB*lr}m62-tFy`v`Ik$f1`hZeh^g3WfcMu6?A3MOdXDgI6n8;HE(mXiFEklsGUEpiZk ze`J_#@#4`v(ipN`)`9yp1J_G0zF&tG4KM5zxyaDm-YpVdzk<^Qm>HD_n;x2mK36-n z@0+r5apsz~-q!9>l)~M>xt%>qmu_5`V19S@`6suzmGdoQ>MMrii%-8K7PQvdTB#k^ zf=hZC_!nlBY1x*7lNQ~{jTfdHEXEnHQAS-D@g?Xl)Kb>%?Xx#^W9|!1?qUnv-J72e z#Jr9{L+QDF>6S@Eq%dBQn<7~IEO~M5SLw-JRVoPz^+rQRH%V#@GW&t~W)~|@UfCR4 zBzzUFtn5t^79urK<4KclJ~}^K@RDDH)yg}b^XL=3L0ok~m|byAcUUid@h2p{ioE659WQ^B zE(o8NDeAQP&a(J>0VI@!l0;b3blUpktjQPmgUD(v;@yk9Mi%pTDTJS@Alu%F+E z#6smB?{`u@uRMhliQuC*>HNOnV|aWU$1ryQV`iwQgFo|S{R4s6lNj1OE+;yxdxh;r z8jb3a7sRZ@GOs?Cra*Iq=qw|d@z7yWn{r66b9SENg}{VPYliM5+Aj|2cOC7zo=*_% z_YH`nMbn2*#LVGb<}^IA9uszrUdE|?7Gh^Na<8uDw4p#eI(FX(K^+x zc*z~A4DVfqB5$I-77f6iE#04m_kJJi-EE==8*qT{1!ei}?e|*|zpbZgc(UO`+R(BJ z=$>eVrdwh9-5Cd-xi2{1Re$h6NkV;X@dP_#n%NxBoFRe3M0c_t&JTIvXo!6^CVvUp zfly54lsnvh^ka0A#w9-IO(KViHqH%2v&xj}Id4U~9Gx@&*G0`Us4AkWetI&}UoB;D zado+2cduMpVFzN3G|@OsvKW1HrjtUiu?JUPX_A~lt8h9~-Rlhc5w+St8N83_$r5ow z{YVl^qLRZ0)$y26yZ=RH4NPftb*{8=#LaFSR1i!sqZBi%_0*T7RqQo{WP;_-p-Y49 zN%4`?FkKeX^~+GgB(kt1iYtUu$>zRTqA^%cQjWEE*nQL|Jt<2u>z<*;iz{^oysbM~ zZUkQ7SEr9EMdavN(rLYcGK_A9j_4jr_~>mlWW@T}O0fJHkh}+hm!|)56{d7|B}cPY zS2L~^31=I8osfsIl~)h_H$kZFm``8Tkf@#z5GJEY4~5G4%H zHYnB5G`LEJc9(CAYjZV*#u#RL2j#|aT)(Y{HrR%pWFzOHTGgI>2vXovd@+@g7*{l@ z6xiN>vpzk2=)|(`o+3p#t(`D`XUCF&a>6j(Ki>bGR6bS4PWY!v8ePM1W>s~MbKBi+ zRmD>wRO%GsRaLXCiSygKOvc;moyCQu$-PWmtFT!0`}b7>TE@SKl9%^Z>@S*86%xcZ zUKl*)Xdi<2sZo=D0>d_}?X-IftD=sTFHayhMZH()VbOxV`PdsdCT=8;F>3fGUa@V1 zXf5aQz?8{R%R8NtaRi6^h#->|GAu<5k|Ix`O3{l24gWcFD%-&oi;Kp4=~~1eOqLPe zi%UP{P~w~hb=MY}+7Uacr}lC}v8GMmmEX^)ALD)(_VlxqPiqSloS=m>b3ei9XhxRg z`~dq}s#yaUhLP=v%!{N$!*R~fe~ouv^#aFxfJ8j^(W4$MTo`F21HpH%eP=-**^X}b z5tS2Zd(Mxl*p5(qTe>yud0+2Ye56`o}~istI#r@ zkKDfGCYJneZ1+RC=T}GXFL+9Wih>T=fhx^`Dzq-iu`hMhdE)9KoL8|rcT9d z#+Ve8^F<@T@Lxlj43ssP#gn3lf z)Z|o=^YQufjb^B1o*Meh=}uW-WwVO>N+>G2!KT(ItWZ})%@j^HX3X1Y3-!V%=lDVZ7AsJ`~D&=OH zlILszoBh1q?HqryD>gUv}j7~iamnl&G2T8=BvNs17M4ot2!spCUfZdN@6zM)gT z2L+GaYSPXtf3r>Xkfjh#)c^@safE&SaXs?b%gE>2m`REIZI&CWKVVlUl)t=ze#{XN zP=@lD)ZU1d3Ksb-tj?3As81;H!0oE@(`%0xk4-$PJ|b<2u?t{h*2Cd_DTQYf`1IIR zXcuMLAMeXV9JD*^6E~D#Mro!cdI@9rRy^Xb9tqI)MzTy#C!jV^7!TL9L88i!p+P$U zD2(CF=pzH@JabD8Co%N%-@3@N`qM&}EGL5K}o4vh}WGdTM4Gv|YTGY!}NNaju| z@vkR?RMKv3kqLFgM{xaQ-#bn-BUlv7_oGULGE5OATmexc-&Bz{;s?&uoZc|< z+9}FDrS@~#fL8*lN(|^faH?Iasf!HlS|5FC;C4@Kh~U0o|8Y7?pq00>n15MCe^>ADSmweG5|^ zJ$>LS3vH|!9+;o@k`dz+#=|?2Vbw{dE|?QdS^WIUzY0CW!ZhR!-=7TrM*QKay*jTD zNlWgqUv+v%vWNwxAZF}VPKHX~78zr=xGrua0|sR`Za)AWL-$RH4&WXz(eQb+pIVhR z>F(<6!d6Ks`C{RI?kd^-!Wl3HNSINPn;jO3NVOYF#Jl@$aw^7^N|C@8x>Y+MIBAv3 z)mdp;6Leo-cvwkzlaF|EtRz3Yexv1nSK=!A&6PTUs6JogoG@ge$ISQpQRV4g-6x{& z7)49>>@k8>(si`5Ij0qIT8!CagOcB`5l19$a>w^s)q`$qC$r|eqEFjW9aa#3&J5Go z)ea|J`B#J_z1!KU-Im)JOnRetmr4CzoHl^_J5J19$VOAL;cNHfgc?CK`<^|eU!(`l zBvpPpJF?qzX1AmJ0t>#i)#*BK2A1KJN75y2G6gC}hEq``gz@h!@~_JC=4ce;0wT|t z`*W_xm9(O57-sBhcRn;EDPE}|D$K<9uiiW>wRerOiK#7BU^if)9&s8y`|+}ztki7V zm=3sMd-qWVUAfAJ8QZ8~yA=Q{_Gl-jxPzpKRilJHn%vV?Oi3?%BC(jEFyik9>120A zb8S{ZSyzapQsrXh{B|q-u*P=r$omgUqu2UOiI_(7+p3yTzefm!Pb3$wQv5R_O=L%M za*DB>Os|tIy8yJMbaAUMsy#n4E}dzcV`8oKk1-$tGf`c?wSbPovGXteZ88)m~`98`Uf3ip;|3oVASZi@QAlHtulJU)lviykl%-Izr&jR2Pwet zcB+L+Wf@2bmBzkO89r!;&&aylaMQ!i3z4_j)HwxDvU+c0B7RFqv#DA1997OtDrSkI zzrc_Atp_y79f${9Rf2=6s|w>e4Zt2P*=XNl{{en2F3TE{KO z?AyyF$|^;6rQ)8x?&82^O+5Up9(IvP@H8?{F!F-*s#o6edl`c@hXva((G>TaFIx-S zf?mQHXSw96y{eHWYxGghQ%&I*H?Jz|N+)Ounu31RmfbfMGUo}W+^Y575nWa^we>o^ zao>pCy-(sfAva5HFY8I4vw!WehY9~KZnnt{M zfNtHUXgz_cIlFg7|6X14fNN1SZGUH(>pfliVEF(#Vdf8_0vX$%}R7Gr|y4`XqS!+(8*M@B6 zt2)*L3U=LBC@Gnq}L)Y{&yUAmW$JH^~iU!Zff z9bAWAzl#-eyfb`WY5g+e|Iz z<6bX|I)t=hvz@A%+UTPT%VIO7w*^$8u2LetCbgYa{hp*weY8;u4RwsEZ-( zb8**Z?Dr7b+xfchV|?Q=W(bLQD=DVv^D?tS__+)R{5BmaLt^1>-_ z@p=oc!jGp`A&e`OYv2yGi;v`y>;7`e&^AjtMUD3i%**fRN$+1rsW%Q$g${ z-UT$4{23G$dNBUjwCm;5yVvh-kLTNdy>2Rj=-atJa-O|-BM;sM*wzsY94SaL zAP_pev^ZkkhoQ*@K+}-nvF3)8`klwq$EIE6zr=eZ3qHvBmP!+CtjqOL)foCbt13iS zvu8bXGCivm=U~GglQ2Zr8J{4G$+~2sThfGDM*w&v6|6S1GpqPt3^W|L~V z*$|>2*UnnY!lTOk6Ns!l-=J%3-~Jgn*|ffJ&IMsPpcqb&0c(Bu5~up2Dwzyi-uM}2 z`n9qC{-Zv)p>=r((|4puRkcVX#lBV1NDMeAYIvuvO&q+a)p4(IY^c11^wxZ?+cgJ{$WC>aqbOn5jXxKj! z9A$v8!fCsrz9eDb%ca5oEiw*-Jc-(`R?4R=>t57XfRKr zyQV8{l<#*nG~BX7beR?Sq0;nxTqSjk+q3k7@HZddzhkW1^pMm6*?C+7!ir=wiWynj z(X;y(Mh!v`(2l72FBF73A-S(CxNWdNsA`&*e49%J$B=({eZT(#Lf&R^w(L5lJlS@8 z1J6kyL|0Q?AWya8hYp*Kg~m$A+R~z$-WDJ`YCwlpv3h_Fo)%ZrT!O2B%&igJ1%r7T z$l^SFVa*UAi?u^!YC`xFV$gz@;K8KGba{V$x|?M(I2Q6}PU*LSvPx#e?)RPUkRI3g z`km8%SZ->ZyUt;OsOT>aAIn^*=JhGJG`whpoK(y2{V6cBY&KqU_DyhsRTq1k@D2LN z5u3_fk(1+ed2<3Tp#Ye3B8DHE^thy1{VbtjJu1CEvU#sZV-dvWQ<*Np%ir`Oh>S-Z z9FPk!DxO9sAm?1fP~R#YP&0K%?NVbzG)-%4)VVb!isx_^CiYF#r4@+7J4-6Xo7hwp zzlmNqiGCxu(xliU=+aB0pHk&TtB+T9PuE4KfRFqJal{%fUT<^Yi}n_7i9wo{<0fNGJ63eP3{WwPSPlk&cHN|?oN;w}_IX~Us zBI(a6s~@+sbjXu>A%+#en8297Nc&-3_lzeut8o51e^W$d4L?sqX3e+LScH@F+$tN6 z=~J9vg?s3W!yGKL@P0k*%1?A`Pl4a!Gc3H5i=;NWfHqXTfHK4i#n-;Kx%S-aitP}~ z(mOjKdaqK4##>Q~z+2!j2PLzOi|@7zbz>AH&#QRf00S}Z<-A^7%Iq)FtT*LXy~5HH z`!GX=`_Mx)gsi;s!X3pdb~UVM(E;6nCZ4b5c0?YZY|%UvkE)^fqXTOGqVi4PIS#|Ex$5H%`BWp_@*)s^G$)%%p7-ij-v&ry!ygBgj;Y^ zmZrD2$&wTk6v?sT$4h1d!PTXcFb%-0xM*Ly$>yotYFx>J+1c#4ji{c~b!%&$w^Vp{ z;4A~y$188q5pSR5c`z(S!%?tkFV!>S3Y9R<1*O`)3a=ci`KBc+vg9HvU=YwWfe1}%_Atf%g})E zuj{lDYL2i}i#3m`=#H`u+y`FyC~nGzlGO{1tdZ|m2lC*3&CuJyY}GFf^p)~>i|)>% zqkddNy)AHt`X12vs4Y?0L3N|q1-^% zU~g%iC10+1fBQ&(j~l8VKnPW|VhOVwwqP2ND4Cicg)|9R{UrDKZlLl$SoDZlB85_L@!ERO`6Q;A9DA;h~QQV>hB4RzK>6#7is@|)|3LCxXo9m(@dOR zm5s#F)#HnC;9b1(D;)w}4i1M&U*xwG-9`Dq=Lu!Dl=Dj7pf_CJT+y8_WLWpzC|uR; z#rbHQ3V7(#!0pc%*(^9pI(rG1cUpA!6=IU4k6g@d!yFsnWIn3C+N1;zoW+5SFifAR zg)<+GS3wg6gv4BwkUXl_soC2O@TY?RI_J zp*6Q0IGGW3SJ9At_$@)Wj5vC6JT)5Jlx9hzUl>uq z7ew|WSFPPKLWrz?TcqZ7#jF6Ahgrg1TxJMXqGhNF;e4eX3@!L*x(awWA+m2XJ33vO z5$SzJE?o(C$fqCR)tfF9U5n~$JdGG@bA0g?#4zNM8PnWhMVYV$zgRm0U8g#Ci+PEloo@5IFW4_9jEAVbg$_*y7KWq=erkh(y_}u)MVQl<8F!D8gqmtYQQT^wG9&{i%}v9 zOUziZ>gT6inZndAQMG~`G!*-a)ZQ1`GEGmg^_z!6jAb3T0?=Dx06z|10v}V6F@mwg zH1!B|ykFM4a!~H$4;gIvIB}`o=Z``px3eEJFjT9?K1{h1$AFGKin?+e*MQ1R!PmC) zIu_eSy!)$N-!2Rh#?NNmlxIH~HQ-$OoS=x%2HQHBrY;FMdyoSu`E`!8U(Ou40H4|E zOwvC3juL~6rqR5zaQjSS4cp?fI?_SHNdS;=+9>RTY8KkIxk68SS*?P=z5mOjy$Hr{ z73nP<{l}HtYC?FTScOCOq-5XF$a=%h8vaYRb=W{A9KTg42RSDK?+YD!7r9y`U%Oob z(+^dQi6aVFE4W9Aty)Z&#^#et?pKFT=K&^_ng82`o_;fi_lJfl{S9A}TkwN#?FWiF z3=k?~r;%Lis$xH##h5W9-jSsK&{*Mc@->)+YN|(;!9ru5(wv)hw(l?MD0o!0Tt)C_ zWpV3xedMfp&~leB>D*vHSk$5OH?j@}2G2j0;<~ZgcNcZsHhMlRVz6mG3ZVr)Y2Ft# zW8SZN@^Df;#M?hQyFsptupchY?JMdiLF=O)wfejC8Iy2A3|w#@YBruj`=cM{EKIXC z{X)!nLvhvZ&$;xbH>Et?w;g_|NV8_VdXeQ}ZPi?H-BpZDXV)URlc*Maz32U~Cb@Lt z*(RrRx({ryDzN1FqjO+~;eA0VTR<`waTVSLM;PnB=l{d#hOF4qty?q%2(^XyWIKq; zg*@4NAn8T?52yB`!_v=<}S;XYr+gb%AA*`5&jG&>}%jg`xrR zy#2FoVzc^6P}I6YW!Kd;2c&`EUHEsw*nF0;=JJegfOMXf9b{$nr+*ixOCB)#aV@%+ zXU`)c)T+CNp)e_rM}qIBJlpw(yd!}~Hm&{<&Z^Y=+N=H0+1Uyx<>F7!Er#W%W!rR- z-8!CuA|)C0uw*qSpl(CKZaat_1>zhenTS=3c}!T%5pQQ{f-odHKa8e z8$BSe@SQ-|{PpTE4P(s^3;TT;T3;_v4(i%N#OItFPtma(4xB+&BK%3qI&2XJo#E?Q z$sTazr_L}{hTC*uhStY`0XN70WBpl9_wOuLOi2M+|2K=LEOlmjy+^KJL58GT#p<4o zZl6}m&JL*6-lX9HV|T9iAaI&0s35>uvGz~d5+}cYePOrkSToL6_;=CbZr%7BAFWrA zQ#z$uzH+Q|>dWA*4NyfDhTcO8N-;!%a*BZggu~xd)~op^m9KwWMXA_v4IU{}lu8xyz@_s9}Oa%!73ygF8@BNevptD-TkWvG3=~K^-3RM;Z7M)k^3;+8fxd=+s5%bYMgNp3rmA(I-q}Bs<=(FD zw^#NDT@YTS)Tx5JO8wOiOo+clu&GXL0;5#fy}ptHC|~VQRjbx6`YmSl09fR~z`5Hc zoL2H_=RVDL1{I#R#}EIqhBj(}(p;+_2GXwsE-G3lWm70kbx;BO{HK5wYBGQ(srii( z=YZoFJyYI^viYpJ%PE2E8Ej!+PaWl=aCvmFRJZ4Nk#lE>P z&$<_mavJ0~{%$gJO>H~=q~dTnJ7FUcfX~8LxihhC7*-z_$>O)CaZ6_8@T*I9W+wLT z{YB16ZZV>`VgAUA8?NIHQa+cTNEekwMmoMYiBh~$Mu9m zA2rdaK%HFo<(wr5Eb)^ty^f#xG%qxgH*Liu9bd=|m>?ft0ahI~(Tv}eAVilb!Ys#>))<$p^zKRT1QrqR{pTc+)M?uT-%d{Om@by0r4aXPz_53a#I62_fA2JLyJ z2^OOt41*L6(Hn=gvogkY0r=4Rg;MulVJ)O3fmhLJ&pD4()u=$dwjwzb_FbjHqceeq zi8()ZKKRE2Z_%o#F6?AuG;|~{NMqu&xpJBCyE#3-_m$KdG?f*UlAC0`b1-&DP~~pg zCK}v931!M#zfqi0bz2RXnKO2AtjosYzOsd{sO_|cFSrb;mCo!P(jSSz4!JlWq7^s{ z?R5QlqzT|>T5SN{sU}Dys4^vs0=wW){z*Ys*Urvl}r979^l~^}t?;5%6`uws_!rmpuSkdDg z0rCSVo~;-<2D+z;OAR*sY;xBra$gRu4dS_;yt%fbtS|S!a2LN0pJkI$YLGjTc#&WZ zm~xTJ*+{fAt2DAZkr=OgJ3Gb_!tr_lu*t$Rkl)QduwZDFq!8Pw;+T}Rx_c*=NMsPX zoe^Boy>59fg>MjS|2}`q>aK)mL9#(~P<6?mm2*A;c(vR{b9Dl~ZRMYv{JH1%(4dDN zN;N&i3W=ehl;@y<4?RFX!i4(EDd}zcE`T2RxC7|-HzOkO@89;>2*^k3Xrccw>VbDL zg}LubCy+QsT@nfkMEd%-$pb%H>)Y5EnHgBq1L=SsMm}&@KFZ);LN$hh0{wzGEdMqI z1SlwZL)*`~W;#Zu)(kceAL%X33?8IB=s)|)C`k$=uMq|EHv(B{!CJiK!=oF-j0_KC zs{c7WH*b<^3pl(B=mG}VU?`|-E3n~z`4|6F05}~k5KFK|W(y_&j5+5&C4e)+LLyl0 z14*L$pCiF}MIj@0;ro|A_F>cm?qDWDffzRbwjKZfNpK0yKnj^Jf5-i2{73`-r<}ix zr2khbD*r{wUxw2EtCZ0HA_YV{`VV92|1Jd*nfCu81)Rb3|0?J9zsLb+H2p`;LvDi* zexM}Z%z^(C;s1r<^#2!r5Tohe7W7|)Lh_sb?{0z^PXD%!|03ys@SQ^X3eGi# z^j8w_6HE{||LKD}4;X=ySAm$w{DoR*qF2!9I)O%L16)ZoZ+CLY;QnY{|`@CpgI5m literal 34905 zcmb5V1CV9U@-Euuv~AnAF>TvD?P=S#?P+V;wx*5Uwr$&ZJ->I)xi?;%|9Np&#EM;e z?^RiqUuIUV`m#zv8Wao_2nY%Y=p@NdHQ0(Pf*uG6=M)fBPT*8hA+{E^lsKx(FshpfsBa3j~_65rUQ^*Md9G0CsR~PscI+aTo1u5 z;5b+)H@Dij60_80ZO+Oe*P5kq!cA~4)1&+wGVPL<5T}6t$?zVK*N6&JRhj z$Me8O@DO5QUSCUp)Bq&S*jPGL>1I0yQ@DB{sx2rV#+eEBD&&)A2%6V+=6`~JxCj@u z*vXpF$V&oiY)^FurwuaJ`jW>eah}4S&-;=@RiBGQ9hRe!6{bBf8PhT6w1`Hh@EAo# zuV{Kb0MVLD5tuWtQm$t;*#ZV!;vv-kG}Rv91qZTb@VS&F7$)>o`(VA<1#0`|J%K4m zLqcgpbrcZ;0|7~b00I5a0Hge$06RLlTbVfig*Z!9I$@O%b?p$9-h@s7kgYP1biTX~ zq?`z95M)dZMJbrm>MbZx5MG&AmvrRQ;j{kdxUu}HIB$iKYC^JUoeYOMY%HB}a^&(+ z|9Eom{^p!CG5{&ULGmO5CKLO$r*xUKj`70QScRyot}Mh~BV!)np5i${Kfo zlq3Vwu_pHF(spZ6Nty>1Wd(6ODR5DXFdU9WVk-jM7&;*$qLc_VUy;G+sjsM{&ID%}w#pcUl2 zl_Bb^AzZ6>ydAI~>c2CmEMjG&Gn3UR*(@xYsI+BP;zs~thDh$pS&~?;54q;5CqmaK zm|voNW2!qt_oeIJQDH03A{qq*mB|`AL!p|liFzyBMKOB146W7slFoA>#{(@!k7`-> zQ6{}8^v_S2BH4|a&wD=CVy0cTS{JVuptAG59nv?lwp;xH(D(wPsoj4aB=WN0W9!R7 zh9Ul&gM7vM|BgT-TN|gpvHRsA=UJMLyDdmVo3~UtQu~Ui7kzBIbNI=r#wp~fq>TIi%EE)03 zvaP!8VvV}K+3)YRzG<1umzO@brJ9vxChpp-fFaq|^Fx5?YF{^p-Ca|=F8`kOChPd& zvdHG84MwD?y3LSCU9m0$W}Y$A87mL&v~!2R#mC)U65qAqV)whrc7wY<|6^p|#%UAf ziL@t+@Y9b@${YS1-_p^NP5c-)pCb;8*xZGcfn0po^D=cgS&Yx8V|ia){-jJVJ?nP< zO?p7O??{eW?D<2}NPAiP`ax!0`*~{f#R|f^)9JYvkJCj<-fLaggT@k?hi5(D^`wjb zJrULV!|7aet&6enWBa_y*SjN3r(wT}3oZ>O)-t-)JFxs*|K#T6)S2h?yX`1{_k%%m zweOjuuC=cZ9%u=^i!R$Dw)@4&bD6t|LdJY!iG`XSE)V>{&C=Mqkv2j!ZAxSv{W{Tl zfax74-o`HrQQE;t3K=(hKJt#MYSa0K%)|Gd?;@`(Nt5k$m1{ljaIyiQ2eP#R;WMe$ z_3eCFe+0Ift5YcwR_><@Dl&%XWQ?waQ{^t<+uZ{Tm|-Zuy&}O+KzM1yu{D_?z&|> z+EhJyG1yAOsk-|1jbe#?)q(L;(+z;@xw>{W!oeOwVPkyQUR?fSIQRNs&-fBUz9Cpq zw+T{iwC<<=a?a=KYr4BpURH(hiqC=TN|*fVSa;kDwYgR1KLSx}rv8pYt5kOyyXd~e z8w#CjtB?O4itNgynB9^CN{v|KxaMf8C{q}A7+6VK%%bJ!>=7*^SS+e!}qFeMtrI+m`*=vC$n&y>#SyMe49l zDy_`A6}R2WcNj5FxJG^AGB41F--aMdaI;G=@|I1;5i0($yzKmCI0F{-aHFVo!WMtk zWbS>ccmjv<=HSiUyY3Cd%+ISCvfa*o;OZ8?{sf~(3$(r%d+_$NFfXxKAfRD(U)OTM zi`Bya(G?Qy)_#_D&5&3Bv3lrjujp|`S+%7l*;t02BmO~wTOL1Bzu-_xDkdb{{;+GR z2k}6^T;v#mqxRo?N)1B+Kn?9Tue2lqFG(Q4DTPrzJ-J zutO|Ty&TAMyNgOMh9HD}-5dKF_XA(6mO|zzs*c16vKVVJ>cHkyA*<8d958>VnS#e3 zds|k%=0-5Azu>-j-oDlJWgvx$72iAZYUzH2@lvAlO>~YANP+pEE=KjZO6Xop^(Hen1jYlQyDyd{6RrenXq~$1FU_UMQ`oQ;;%7R78Zr; z(oindlF|7rTZRfp1IbKHbdZa$UYKQ^&5F+@9QJ3d{Y&xMaeZ7wvv2K0PDXS!$8U*FgL$++;{7GoX#g^$V zH(WLl5%FE#8aQ!Z<13T&SoRywdG&{|2|%4igVW$ie#35mCKI zB`1SK>+)F0v4*yh5C~bDh7|*2hRRC3i?jSphu-3}$^b#2EeV6)vZQ$mec@NnRTQNr zK(r-EP)(;AsNCWw-58`|RFnt|L&(3EXpjIkW$rCPUIEJ{U5YYJH&%nD$AaUPs&R+W zVV&5gb|O^X+Ij0F`h`d)v8722Y%wyMo~Nb>D1zgaA(iou78(Vc>X8Y6n~d6%k)jIP zn}f|T0*A&;SIlbw!cn3$;&mW4fpLoCUeE1ICyv>sHqv4)!ZHz8nv5QVLFVS2l`)@a zm{!4L;EYP9n#z;tw3$AZi6^#Cgzl?cNJ0U$a1i|f564RZP7Puzpq(Cd^sHru=FteJ z>N#Wq=L;~_QM4h{b$}l%$L@zw6EKay5hJq4#*Nx6&trz>(~xExpkNi7m#c|FmSw}x z3XHriuZR!W>tQiUHUn>sE{;#8nYY5{J}6e&&WFsoqzOCMWs;ZgyGQru5Gta>eKSu~a)H!3Sq znO=A`YvHZi^#0pn8Ex65rldBgJ|haHsU@N@2k|I>AZhTkr(V)CgZ(t*oJmrj? zto^EUP!rLp3p9THqhI1wDOve*sLFzR`AA3_YJMpXaw2R&qvF2O?g}$YCNXQ#oL6Ri zp~ok|#{2UMTGrkZ1d~ZgvWm7OK^tWXg;L@@O|d6b5>kB0T^1Iz{g4TrKr+nEh3(BS#W!5A@r>}K%plxGum)w zUD9abH>2G{vgDCa21}1}mqz0OYS)nIn3N%05zMbBm?LP^isA1lM9^#@fr;B8FY)rE zvb_jOY_U{c;)$XYg5h8UeXVSWMJI6t`aQJ*6#i)YhdzfZq+mqzG--n+tnrXL5wPX0 zp7_Kqt7uf_tvu?;6PMW#+aI?Uf30AyuhB!ns(4orR-0iVl4SZ7VU5Dzk@%)MeCgB{$+@%|EyNbcK5;j)a@Sb04Gx+hnoRZx0S?GjI{pET z4oK3J%@<4>Chs&cf1xNlv=5OtC%!bo3N1{7rSngT!`JV*;TNS5ThF2cZotHD$` zoE~UL%{Hj#YqW+@d4u5BSVdh#3PUI=$E!{D-~o&Llf0rN7HT&8TDg{pXBcdeF0^7% zKZQbBVMHa9SQ;-W6r{pNI}VE0IPQsFFYsu>&4_7MLdqF(BB;`~AvXS^a44l2C{rTs z!$EO0QGFqj;bstfb-{lGES$=G<1Z?u7V4)aYYiF$d>%Mg!ottYRS z$v+02$&zi4iEYCwREDxgZeGAc+yE$7MXtkysV(>SFHgX$h{*nP$P&JphgAdtqKrw2 z?lsA(R+Eo!L@By2SgICuvbCW8*NN5mQ&}pHK>?6)gW(|tGf{qasA`S~td&?}TWd#>-yZ=Bn z%IpV~l$cq6w+0Z&BRoN)vLfth=S^o-OMLaAq3!$=rU;7y;-EEdg&Aia!5e%W6kN@G z%U}2l;wk1W)W@QPD-o&!L^C@;02Ha()Dv_?5RC@qU;XasB%L|OS;EwMskia#;uXPA zDc#)D%!0wIV_FI2dJ)x%7$lQ9;Y31^!`N9#d09ImktoAjJIutUu88{o(eP)E#2<@{ z*Q**8Oqysz@mKl7K^QeLEgY%vTovVFQiO}X?=z%t2JW75)M#mGcFpBB`uFQQXD+nohYn+LymbtTYSDAWDY*6~L!ohQgRZ2}2Bn zY|;b%R!-u>3(f#@Q$cC+7CJ&-ny3)O<|zxJG}|wvB4q@fhj=53HXzs&5g3J-qyfuR zPq+eBtrAv9^`xj#zesMts)*aauDwB~nl$LWG zbj7+7lAcz*WpdMn_F0>&@pl@)PWv3^p%qs)xru@uHOR{ks0$;I?*Z(2)AaCoGlV5F z2B{q1Zt6u0Sd{VVX-X`x#gkWnyRS|=xXBU9o1s6|^hAzK6iO}$wb^`jerk9XkNycl zyjXC(Av5fw!@+g1u*FcOW)He=t5JJ^PLO|&T_N*rHr1RsSQRS@U z@1RiUc6@=xc%Bk@wfrI|1lS2ynfZCJAXOTm?jSmY`et63UQWwj@by^P@7k#Ll10_w z*e`{yv(b)+AP!e}pd*@{w#lCwyoOglT%K_F&oj}zJ8UCTXG`)m`E5q61@zfh@jp6W z5&m*BIMNGkT3_TRfA@dmX1;xKGuDPS7N#bSPV^4OrsIj@HebYO&?RrM$;6T%130(? zbGudIESoV6ndgSM!;y8rdsJABsK`bo%o~qs zxZCMli&nW;mYQe?GVrc{1{UQ|x4GI-bLpo9({X&Wnyk;Kbj!HqmG2<#ndh{Z_%_%O z0|kvAEo^bW=Y`ERu>)i6ou@HIS|&wzhXTY;ee4ZeQ} z>wj^|iL$o87!gA*-_eH0=P_|a|9}wRD>N#Vshx{TcI?4c6m`)Jl1fS z9=P+}#>igVaW(d^Xp}_JBf*iN`k{+cuO}T@JR+GL<$0*5?J7cV&4gWmt?Z&aabp&) zTn|buryP|$n#oCQJ{l5)4zdS{Dav3ITk7wcfxmk;P>S1!bCLmZTE96jE_~ot zP;#UO3%DRh0X&umv#3}hEHeiz&_^i+T&+2fRAlyyzINxLS6-R&S$pVEX^ zdftYpJv&n=?>e86mwY38?F#z<-8ktdm<}jUxFhfynY07NhG?vvFW}aA;!Y>DNDF!- zYWGxWYd`ZfKIWGVQ2+B`HO_#TO**|ED%XfyUo--64@UK3n!qeZ{qFSL5&!U)m|^eD zYn1pg<7&kCtyS#ob5G)}6qxeHVkmkIW7Y%N=+rONsV2*qK_=~)1ICFaG(^pb3tSD_ z?cc1!>A1EXqB-}|U>rZr*lD}qq{X8;+`0Bv^tIl>|5{u(=47Db%i_ELw)nqN6kjp& z?+f>w%iI64_RIsg9{h~P`^|JSq@)`n&# zjts&UPS%EYj{ij$n6JwJJNdD%ed@5IR^o5iU2r3Shq1Xf z?B<|~txr=>Bw@g8;ZonHNxow7R3K}Hk{DPon3Yobmwo#d2QL!&8yEYOeM4~U0x1g;|EU} zJRzQem^s`<#x@hGIs?ik{`2tW8G)Ut&^FZ2SSxb##BRV##hd_3+u$G&EswMIi`qUr zIjqIC-sjRp=#7+SV&lYjv@zLZR7X}-jVpGOYLo=;^T^U>9eO-TwhEkV^1h0IGEEZh z`Rr5TS`uzV0c=@S5t6uD*)>ojQ@uJ-Y%B~m#2p?vc$7*U6g#OEr>4oq zK=yYjTW=k>uzqNZ1NlREGLF&<&pFuecBjM`&mcpulL%3(;9zT=gI$jcrsr@H_)&ho zTy@dAq{xY>Ts2|q|B>KtwWhvMW1e-&b>V~LwDKZ@*~n1E=RjB9)H+RO$*=h-PWi_r zEU-``rz|aVJKY04B}>y5Q)ueYV0!u+m88wdb@A`qC8h}=_hJ7Z(h>Os@hJE}bLvpW zVlr9RrJE?jx>$3|W=Uv#;%|NqT5m0VB@hUfD>%bVV1Q%O?oV!W%}nbELk{qvV{clX zl3!)en;#F&%ZJT@P3O$~b98hR0(g_Ae8O%kL=ZsQ`)?8i8exsJ#HVX+D4jbs9*mMMLiMaFKwCVXn;<`pyMEqorka-4-y@tws`lL@lJLlaD#*z=kOJc<}Al^V7x(T0@?6 zenrU+X{K6HYnL@^(M&0{?PPn#06A^XV!I(5H=MVKOC$D7^&#^wKKl%0UT6+znZ2?@ z^q9CnaI?Ge8j8G{6aj7APG0fv3o>U~27SPW3rXr9%rL~^$LK1{tm=okQ7b#G$W-i< zh1xhum4DVCDAt}qgqOc}O!rG_5-R~E|3c?NrVRx;dsl`{*l9iDFi&N|_2w^vqg}4$zA_v#}uejTUP& z?-Izh)OFQ~*iq6}JRTDpKOPA_nXz4yDmlU!`z_Uzu>Hw?fpgwLY4FV_P;t%G?? z^El>v2hxHgLA8$L8?VgAdJDYiR2H1*JWDca8|x=eR|{QfS|%=R7Z=ajcnYxabw0ry zFF}@QOw{7>xkM5Z?y(&s7kvjLbY}FQE(U^q9e}k-T(%h)8eBM8fwnkYBHbT!YC186 zq--&eSeoBlaQ30sr;>BlbrXD5&J*U++M*{YrOQzwa5sJ++did(SCXv3!rI~j*)nW~ z)@n>}i@W z;zU!z1aOy87GvEl0QwIq);+&PgDy`6sf`NT^PNxV3yDp@WW?D$ob{s#^r^!}6VmUM zpol*D3B9E&W%GSo>hn=Wl(~+yON!e9mb!;HQ@HGgKT7+}wZ_*N174AbG9$kd#uhu4 z2Ao0%JuuMK5{J2k$^26vO2Dxnu%m$X0b8`K*G5zSPOSrRvVA;;m3W89 z%%YnSW5J~6pNjqJbojX`eT8DP$v|Sl{lJ7S%RD6nKm7OZ7Xtlmbh#c-jFR|#zE3T} zHVY$OD4AdO@=Yv@1==_dzE7WPRbf2q%!{wu1df;(i z18w3tv&}(m8lbd_915XJ$~lhl>IGN1YmkN>crA2AF35ZxS_b8aUa3~4!bJezN+eY7 zUiZYbd$8oNHrP+S%P@X+FxWd&SWo1YF%a+4%hdy>Ae}f1NRjCPj|povHRc?W@JqK( z6YWmH^KJYVwj8jjqDC^APTqjL#633J+Vhtj zD!J(7r+$RqZNO9x0o^HV@1Rz4fx^3wACGG%3f;V;mHj}he@Ty!l3!UO6`uMhtkN+L z_}N2;3&g9cn1rICN*cWy9E_~dLwVg-%&z?e6PYRk%?dR{eyC}Y`cb&lu#Tp|5gU4O0 zR+fpS z(ZtEg!p7{MJtpTmo60-fs6O4k!F{KAsUYR*m?kBJQ0)OqK4Be2)_&$0P6}3Gq<+VkOXvuyI~2DI@^&cJOTar?8NnvYj#t-`~dIr z4@bEYW#gU*S-MGoQCvsLAllwoW9MEPbTVa98n-AF%B?VUC_BPxsB&RliKcn#?v$#a z-i2;jCgNC()>cFtvYbcn&q)0j;bmHV%2dGf)kPC!(Os*7U*`fXtNWTSvk(VhYtNTEx2?Z8{9qOOq_5 zo#B23GsJJdGgmiIY1B7FMfb`~DxI=@;cn@VEIH8>msWa)U5C+cDUW(bG`H;rv=PS- z;>$1^W|l{~o`&GMlwLgjDp18p4hntm;&w~hK&iG zz)EX`d#5`QXegBw3Ueihps7%i*{gp9PmaJeof*c%6P)o8d2$e-qF^@?gNZ@qWFbaC z2U2|%5)q4`V23}P#3|oOFol=`et;!&CA00wqPWd5D!B&Hx(aQBDD2-!g0(qd{; z%TZ52jdDyT)9pg?(gQn^y~)iO>ls1?4M3?Gp72b+Hv|5J(Bcria|H==XMCGleH%o< zL|W44A|L2La6RTjBgz|SHoo!;1QO;Rf`EQAt>D6A9A4>=_v6Wh*y`cWy#ufC-)E^r zcrhXG@zcRp_sW-)K9O?WE-=G@?0w{$6^HFt*5h{v&pvpLWY_OImF|C~!3t-Tno{ma z{#ZAhI1sgP0)`jc^WwT$x_W`M^=3chI}F8%fiAj5P^yu}IwVwQH5p6v&x8d(#syC3 zOtW16MnHHFvNazk@+yl(#hsVb*&m81U%Wxw_h(-OB=TFqfR#~-Hdi}L8ybu)hou1s zynOcpBCu6*Isa3o@9j#*ELKBmR&mkiv2SPZZw@bq)j+^t8O!hJO-hfFQ@pJA4x6Kw z4g#;08~sfxlHIb>^~ZaYtU0x<3(?G|)qS5neHVP}k6~Ag6cUQ${}e!6-SC)xz^dl%}8k<0rso&bz_^Rww4RjgA}9oVxuu&EWTxR@bFn0*OM+^ z@6y{C{Dt@@h$bzFPP_)T(!NA@Hfp(xx4Mo?`6}|*#OwY}2KZ*?-kYXC0&>vt>Rqi# zb0-WXfp0keG0_Bl<>cv6ZS+ndC7nX@iv8P`8kh)mu*g))-Thnt8>CxJmzpq_Tm_hUB&M%V~h5IW@4Z5?#w0Vci4@F(Cf zqJ0OC5{sbZeQoPohMzBNoFaQ#5X9j~*!@2BA9>f$ww@s%t8t1T$w-C}ecF#;m7rGc zoJ*wfvW%Ikh^vsWoN1mhl0dz_%4$|9yH5LJV}6#p7!ly-7lcpRTfz&*bKy}%O6H$t zpC~dnn0x-7vV7aLkejFcu#2nZMm>Whd1;>P2}2Lk${KV($I@Zn*x zQ8A#f=n!$}uyIK6@Tu@gXfa9ID9~UjFp-(?z7Y}-Q4!*^lHrh(l97_pP!bW*P*ao9 zanLifvog@~(G#;U((yA>a4}IcGc&Vt@Nly+^Kr6rbMp|>3sSI4Pzk7Ta0&A83NZ+( z@QX+?iD|J&=<*M@vXRO1(g|_1$_g+m@KA{f@k+x3RJ9P%u#r~%A**2|t7|Q# z@2H_;EN$SdZD^rs=3=NQY^W@4sv&2tCS1=Cf zYi{S}Xl3T=VCCZCqG=tVV;8LF7-neeZ{!$ccSVB;Qa=@Dt^9q;HBYVDum z9uVaenCKjw?i`Wr6B_3kmf{hS=^mZ$6<_RcqZ?pr5b9zc6sVpUXtJw8WtWHof?*qkq{Z08WWx#6P%t9nUb0snpzT;RhyDs z7?t0Yom-etP@nX>J*~JQ{dZGFSzB3JXmM6jZDwd)c0^fzdUJksePL2fNoLLO?AF4# z_TPz}l^G=^#YL4(#dTd(_4VcT?adYWt+l0%4fQGIf6}Y^vg(Ek8~gK{0fnt&6-|Gt zTKj%?&D3`cRQArd^#PhkmfI$_yQX*kl&ADmW(?G24KxeLY>{Jq=?69pfWi^SzbRy){zGA1> z89?v!IACdFU~y(_VPSE2`EY)1XL)@auyrxMeLc5(v9`6pwsX96c(Zx^y1z8MyE?nG zv2wC8bGE zeDVBobAI@6b9Q@sd-L%8@N{$k{Pz5C^ZI=M@$vB$2%n#y9G7=IKtS*llA=N?ZY$?m zo^~qpctefc`PTDV$w&lSPlIZejm|Po8YS>3C}(L0LK5htxoLjzxsqX8dPHi8QG(!m ziUd87Pxu7>)cN0FN51`D^R5Z)JP9$y=xzFzjfXM8H?nb?dCVof(#?HgixFGA+sYEj zGT~~=|9H{bz4)BvC?D{7x=R{pdj;3k!|?>MxrO8P?=PReySCof-JiSR!{NW=?0_a> zKQY|YxCjzJabEWP8M*~*xm&25Ls+A{ZQ~AQyLSW14FFt+9rjxtwQSB=QoXq zdc4+;%ZC-54jaEMe)Rq>kKOn0KB|k{a5H|1QEc9rpg`4G@?4MP=t$@AF<(!%yDcZs zsW%wSQ+AC)GPC(`c@DUacKs+ddM%>4e0$m33cFjX;=S^1OHcS-yt%fWz06LGCl|wK zISTzP8=)%TbYq#nxHdbh<3Q?J+Na&aMCf^MlkU^2KY(iIZi2~f_w;$o6?`+B%q#vYJK@t9_sTVX5LprXU27>3S+uy|VLQsH4TeYpODHc;Vxa`d&uxt(A^SVs# z1>LUmvjWZTt8H_SmkQ?s@9r9TpU;DlKA=xF`}592*EgT%f^MrDpTL&az!Klq8`f&q zRjOm~KHyL1$HFGqVwtoK6Hzm0)2lF+t~Ez3GbWza`^7#WUlE2gErQc1I$39iXwy&O z9TN>l)BtIcn&x`@n;LhgTQ0Q;MjOD^E_|vf9JK0$c_?%2vJ!)99?iYa zHb|*he={Ep-nC~$-p35Nx9nDhS^*xP`wtQ_noZ!x{W%pn6yK^0i*F@g9?yOCspd!3 z@1=HRZI(Rm8!XOnpsAz1=RI9ob?i4pK5sFA@uBqXW9U4H!RLF@#joUbyHTSL9(G;| zxWf%Cw+89Ut!slTfl|*p%+sd5S`W$FsOQWs$YyZAw$~^Ti&=#!zbg?`(QOu6Va><) z8VSp(EnfaQGEe$%?U9+5M%f2kHtqn?{{5(+H zZa?mF5`ywVm^8@fZPuqgRDl*&zj$j6>~XIEYabbimF4sxYiSZh@sZmh0oM4n0Et7F zW=L3p_#9EZR=V&SURx`=XDi9ye(obJ?;en`fj8Q<2g^4s-?3LDF`uh4(BjOqp)d5Q z8xPvzev6|`yVp>Tw@1zdB>4v0JFXUwR=dRjmoAl{xX~w`zF(G)R>b>*EsjdYd}& z63X=JYnZXD;Z0^!W;$rBb8wOOZ`PfgTMK1+P2LAQ>TFvlx9+QSJ}H{v_9pcc_udM( z4+wYz9T0spV;mWL9II7=Ea~y;9NCE<)k?YuFL52yQN9DA*92>}pDl5=gb{FuTIec6 z5kU73Me?&=X=YvdI#P4jm0O7 zOJj%aKVFV~g`gjhL5(U4zNO-6MsT$d^#WoDKUy3ZRe2iYR!+ZCfzoZ85c|W+@zd?e zpcn@8#niAubCY$+CkF--e!cwuf*`H+8eIl&j4nV7p{|#eHTSGV#vvzNi0^xzeYcv| z0xVn_ahY-j`OnX%aU`A0!v4<|kE4{C*BpK`}jDSbSPX39d5_9`U|?_(e$L zhWFwQdRaj~NJa#thF>Tn9T{!h0V|T-o6hAHu#M4ZT$S}|x|##_&=#XM-!{m5Ax^vP zRkyE~5S5J+d}X%a=sXMznVHJ%QVFV&J6LTYWYLU_V$%9I$Uo2umQzwEK29httEFoQ zkFb%sdsSs3u+pOi8Q1YP(@I@o0Hz>E?U`){VmOK;XYJ8*;_KGJLrH&!89Bj?SJoCq zLKOynYEq{#lSh<%Vc;=ygMI=NDI1hkOt~*$oVQTc`if>K?hSumVb4jvCiA8(L@?aK z%<1ROASiHwl1nJA5qel5am#8AeCU$&?Bz?hLc;3#|U`{9oVq?2Y>V1(1KOvY#l$kdl6P@Xm}I#o#eU@CQ(gY*%`k~FDju}adGbw`N$ zDnV8OyyXRD>rIp52l7=Whs2Y+7ww?LCHHVjCk_~NsZxtbS(p?MAD_DOi{S3W> z!@tfEraqAn#gkSYu5}gYP5yP1sp<6Ix>ZgXg8av&1vO89vYfFFPsH&xl_7l6Vkb^x+tBmMDEWsdxTsj6Qg?Z~s)|wx`S32#= zSZ{mfhl*(})XUHP$rDj$EqtR#xQ|#={lSLegN7&!vYBzF1}eiKjjgBq?e;kYUg)P3 z98=Koo(mqNPBdcli=G9rjqj2|-y zOkbJ1@n1ZGtmI?`AFer{lh2ol-iatQSsb&UrB z*OR8G={-HSrm<*c%Txxf7Sg`wbT9;$8?INK`5`%n%is54S|48GEZFd^oPpn7Ueh;j z;&Kh!8ts$pHeLh) zIQJ{wAqd#WJ_(|i!wJitA>6>xE&_q_Q$AsiS4ARVJc_5q#tz~V&HP^K=f!^6izZa_ z^O=zyo_+yEm4l@Vlb~Yq@-0E6ELem$gi6?p9j%GAuMnT#!wE>ra^P0CdsQIa1x8A@ zdGnYwQB~X(Vw|;ExhX3V+KekaY=ZR<&!os#VT^8}#jOPT&V}^eUqfi4=xRjV zTh8Wmlb%E?J75=|Uh$Ihx1f#C?PFWh2L77oJc~L{BVFC0&sq%aGhBAHG$B*fz51}M zLN7xw>|7*F{H6P)HbIw3`9kxGv{$-kDzWc|9mCqZw*V{g<|7SgQ+uzn!X?`nL3i~6 zm1r3TgKL!mehZp=<78tb00P#amt)Wl+bi%BZ+A$7x&%7tgS6k;%Z#Lm8Ew89g;Cl; z`u^3h56JYM8yM>AlL8gVr*c(7nWy$Ic!=pMp;PZH>8|p7gp+szHswudXFr}8pcok90vcj3e%!%`K8*a-vnj1_A%`XekD!odg~Z--@K?9+M9~8MrqH zk|I8c&Ukfs+hNwGLIOZ z4VBWyV-BH6lQA8bP+}<#w(!kJ+k?>&|1Z|3=c}TgsdGd2fT4o;Xgd@UtS&`$HCM!v z(vp!0(~;MCgQAlo4_%*uprl-)d6 zzUEH`x5a*!SYeCFUb{l{QykJVG`5 ze~87Dj3v`4b2SPpVw&jc@S(SiiQ_5apH6*!I&qu$4-=0Kcs)MCO!pLN2<`{pdyd|o z3w%GWi;eK#4>v!C^oPvM0`)#B1TJ*-4+=C(IPk~UHtAd+&0o%bCdjYbt}nk4EhbLa z+q-h7@#8o%+#c6!UM@dT6VP3HH+fIiKYK<_dd^bi@cBA1W$NV_kp$)Z%DHQ=w>i8e{Jcrs$J1vpP35`#Is}{e+{}?G z2T2xNO!4`CyXkwWyO+8?VZfT@{Jrz77#L%HHm3t!r!V%!D5vMZC8|Pr_;U#kM zvTMvt_h|fM<=(R!9-+e_MYdVB2pH`7b;vGUUq zJ9^`8*8~JdU%*|E>76*<;O;dZ358t5raI%cH>Zr7ZztQA+WU(G;~WfHyhNWK$?^~% z2NzS}dp1W=uV!+Jrq9M+_g43QaGjU%o;0Mg6Sh|4%TFG23}gKjkF(`DZZRKNCte z7HO6TsXV~!KVP_t0eC`of&6GaoX??g$P0PeDQiIv_RJpl z-(qtiT~8aS|Y{AEq-?;+BLQj2Ee7i-_@n;|(MCc1sJ96))fy7b1K-e1ky zzE_IrSGB^PO>d@j2@zf0?HvTN9FjOqv~_mOPI~qoZ+>X~IX&1_yKcV>I*iqmfzai+ zZsW3c@7Xtavc7+Qsr<;CaOZajJlkS_XHJvjsA|C;^RQ77a9!`j=cjP(ez^oKA$NRh z@VSgB<_UVrSuel7E74v=BPd2Y4+ec;%;~PWpZwZgZ>$FZQ`R)E9!JoBdp|4l_4_1zt$ZTfJ{NW(Y_&Cz0GNAs^7wKi^!*t7lF@vzW4x%rA zJMr|Bz2yv^=hStQe347%LesT*4S>L~4#S({RMmFqUedCw>a);^KVif6v+|1v>J~`T zgKOVJzoE@OSUd=vc0Y7jbCt)I)u{I1^x3|N0?6R>l+R-YnFK(aj2WdRyRQ2o5VQ4_RF^Q$R#$?sk>UdFgU zo1Yh~rdIM9GIZ=f3mKx#z!kjQ5R^s#MkbW_^o$ z@2a)tycI9w#dncB{`?w#%Ji$V9mvk)j^$FXLw#l-nsS!#{TTKBm!Vjpse%k!#?>1n zsp>i(Fsh~Fa5k*=uMf^5?nPPUiu z5oU5~us$pLXg%9}cYXdY?U&+iZ^8Yf?#`tjCD`unK1;b>6{U;c<8^i~`CKlT2gcZy z7<%UHs`2!pl3cla$x91%bX`xiYox7Yz+z zw>B6FmQ5};pBsd_`zr1F%Bcc*Typ0cNb{+OCZ#4~OG-UDKE#oco zN~V?Rlx*E^B(Ky`-_PxHpZYL1p2zml%livZ=vqpFG>r9KMj+;!>j>zl81s@Uvk7Q;R5Pi6g3XrCp}+j9-8VO) z0qPaggAS?PQC{G4H*nr)>a1>XYm{tR&OqSFyQfFDqrKj}w3RJo+=~Q)M^XFx$oS96 z*j0IrKCSK<{kHW^-M}u*Ymb>2QMbCYHeRELZeFkUw^AhHBfTPfSA^y^p~R_*G}Dtq z`SwxE=PBxM&qXUN*B+zpfOiX7RbL-M4|nnurViTVJ>RWqp`AGJ+|9FgxvZ~FcXJxi z({{s87^~G;KvSj+$b*O{VPU`Cd50a6iDJi%T-M|bsK%g6#5PHP)L}VY_4%@^uSt?- zvXnbgHvWKdB!ymb=&P2nN!sV6&_nE+C5)NP<^MHW_v`I=ISFz7FfHzR6!j?%^X^df zWe^d*Wir1sdnW5{=#9&M8c|W6HM0qJMyY~KjsknBAGfC+B4pK>il}qOEYgCM>o`-| z&~V#glO}YotUyFI1$($Teh6nEKty)Al~{!^(Y3I?wtSU`y4W>Q$TroxULM9eONG5O z(zMXQ8GD(7Iy-Z73dP9&12V#9(4~8cg;;!r!&q!H>z6Q#^`_aVUAh53@)G=hs5B`& z>XQvYKK4!L27J;8PN`ZYe2gXQ47ddr<~^gJkQxpeZHk~4xe6TBVHW26$sv+1If#nmjO9d@ z{+5Cvm~_*`1bS2zGdtG9cV6jqCY=E~)HYk^t5oSe8<(EcaU*(Fo#6@-z^`c*aw|FV z$>fzEX*v{9OLtQ^!e~#zxK;tQq{!Ix>cKLwi;@a)f>U7Gninl{$p%Gx zmYTRJa!J;398kIC>`DyL#6MsxBs}3%*6dfiMw8LU`qx-yu#!@phN)DUbVoK*R*~G< zPuQB7btSnb#WXT)n+{0DSq35j>UGd=Hi+mL-6IbIZSzz$iK3<%)%oA-gEr&b^Q;W?EeCbBl3^Ee|wrMIy zuWI_^KyB!3i_H{H*8w?{MrI1u4U`e?`9NBj;v$?}Vd&W}z@=Y{>!sGjYz%`y+3)**4Ce_MJCWVMGo7F_ z8=sD(Guc?R1U?(lD8q#pG?f8jA#%?QTyKXuA{qN?I2LSd?i~(Tc7D1GH4@OEhjN0N zkB?_#?J|;F7;RNZ&ROqck4(k@4axn2jF_@@W~o zP|@OQB8*SG|EFIjhXs?T)8xFkaHnCGHj{eKM`5puuu_gRfe60V$1poilse4AxnEvM z!|6JU7*)7HOX^tJlcno+#(Nz7+%))2N<_fdx1$!TZY-Jub{~wH?Rrb9+@=ui91(4N zJ)XYB`9J;`$-9@{`PMh8dd&j+akUVHb#kdaiRl>THp>sPkDys@odm7G01gieLH{G@ zxUvBSU7_~H>Z#zUvSEW-rWR*X^xJuTQV&+o%l5W>#g7@+GgPlTV(;fQdOlss8cOT0 z{I>H+l+_Q$mu*jqehy!_XLktk{0|D68L!3B$+s+EZD=g5v0EP?x3(5zPI4uzYf%Pt z1~QP79f|g(H)MwDdEMEbmL;4&4_h-7$f1@-vqys5r|3INYXuUuchqrazVhfemni92 zS{0|RAW>CZ+{jdQ*>hyRQs_8`q{_ieP#Z@3yRL4hxVh}wu|3%~pE%W|r(=RP>Nuwd ziEn(=7Ek-O&2PESXHIf0CsT!)eN_ir)A_O}aH^h+sHz&xIniA?cVFq~HmqY-GuNkb z==X^oIg(v0U9(|YU4==sV`T`6+R$^st}wwcWTSAl>g+^CrW`HPK{_Rv`J}dktzi`` z2aq;J<+&*|`vE-=FaQ%1O1@| zJsu#ckyUA1J2qZAMwm#^Ti`bIzG#w`YPdLOopTA70rnL~y_uk-L+qG1@eb}KIoA2V zlTWmyBGp8%K%`rqZHo~Q%Mlow4Y#%y_@TO(4RdfNkk034ptqZqz-u?-90BQ&gu9Js zdIf>)CJTuL$D|>fY1lZ&2-->VQ0A%HLD>`)I+Jbb$E0h|Pn!);-9jlSj_-Ybu>gfm1IAWai@AxQEi`X0 zO$@Y#N#p@M*!@&e?51VFH2X-Iw)BjRj+DjHb>^pS&7?qhS_-o!!IgEKEZJKOtdDMF zE-6LI0C~w#B&u+fb)iQ(3jJCID9+A~X*g-2-&auS7jW5HRxBYXN2C=HnSplgo?MNQ5$ZPj9QjANrf+a>A-`})}KA9l#-(_<;hI< zV~lU7a5!S{4H|7;2JOl4x+dHC_s@&EYXVmK{Z7cSSOoNYk0Os%#jk>0nho2wCuArS9@uKYW`8%pQz`t30RE8q zYFxE;1M!H$dT~|9K0-~i+9&Gov8j)@NP|C7&Pf#IINCnx?^u@6Z1-#&g6jkGz+thu z`vYmz_ZK$@?}$El4DanLO-9Ox4LEFd)#@WBf1jg05OBBgmTOY`HXYX5X0|{!hQ*@zq+9AJH3j3tdo zdMu4sc!#_@;~TNoN5D28Ycb6#FwNu{r&JiHWPWcQcpEZHF)Kowgj?NU-%EwQQqS2V zot^G^Mu*K9d#?g@Edh0v0=rcJyOsRjw!@>sFZ!DZo7BUA_}XHeSlbuuVw|;sztBUx z_Z`C`>VCb#ea@zf|MuY+UJfocBg}9_P#1O85?V3e6e0@TNoKveVxeyobWIphZx4zb1f4#g5_;$H5d`EroBBP_oGA8w6ndv9{(4*mJR&?^ zUmx0D%=~q~o6}#O_r>>P%ys-N=pigu@$5i#7Zz9Y;r*mH!%yJLqv>3petYS8G|P4u zoj~^(w8za=6n}=pC9yp zxsI6UNV~h_DssM%c=zYpzExUUzdxmAeg4}0@WAh$XXob|+4Z`7vRd~Bb9U*#^m4yG zOeX04LB;#rV)1?YG^#qQ|9F|{vmZwf))&tYD);mYM!}0mv(;MJY=-C$ftyGDC-_n4 zbT6NphoeGW$h7TypVO>g37_G+dfr$Fr%M}xI}!|I8*x1tf!hn%jz^_;^0^O?Q=SBy z(l*N_KSiq9%XtP<-0dwzQk!n?FN?QX*#Rb;Ss%b5GhMd6wBnp$H}R_SJ~@-I*aSwq z=eiwoJnbW=)Q+|WzKAmsqR#@m>2`VD`IMlhJO#NKn6Sj`!c`+tTJEjcaL*8a$<8~M z7ZmUn;f?z8@tqV!nMbwns?|*^CCawp)K7pWI1d-qmJ;-=Nm;hJpWa4y-5_ zu?*6g-kyGRJ`4V){C6*Z>CdG+tV=Z39nEvk{uiy#Jhf;3?7*#(=pDi>6`2OI?7%75 zv#g}tNh&bJ+#xk5-1cE9WIJ|2n&5H_tSQ*&GO%BWg8Xw2WK0dNe)+osudqr5tt>4A zlb1~R%Oq=JFD6n?Q?VuvP-xU{7%n@wvkh2-4wG@J=2}v)nQvIoz}ZMkdFl!_R{)!3 zClCjhhDK7+t3#r7wvPwzYgkW2nD#Xtw_8SWy(ks)s>*dX?VgIG8%|NlHiMkpG(Pf= zmf+_J0HK!Ed#c7PLuu3^WBEWQIvyj!r%Wg!*8?>ZUDrEDyEhxvB9}K?e9oSsvI5Uj zMD8=RugYitMQjZg&#NF#uqszP@AQZj!r0Fu!?vtvaZLeDBtN;a=0|5ctVhapls6r1 z8v2|xvxP@&D3D|umXqc+1_PYKA16jUQDKVK4fbT37qM&?LX$%XGOgQw%H-lfdjb=% z50;ZA8BH$Nsot`?52?%DI*8!PXA`JrmAscMf`#wXt?B9ae`ZWJZBZtpEC^G$U0sN3KTketxFIYRJZf ztzPb?HUEZRlYR*rDu9+@Ag6UQt6+?`_N|bfKrbFzi=7jhQ@=|jIcbs0<2;%>ZtFLd zmXPn9Ee5%45XKKS@r3XUH(7#Y|nfCd@eg?iyG$tXKsx)zzHF{s~JfwC-#X ztO_#CRMqhS$X(tLCS@%sjqgiv!>Zx~J0m&S0=N>;{fJMnTR(mQ~!DuNf zmA|7<6m29qZK;U*YYPvE$($L3D^DwlxtgV)9-%QWiI83+G8Q3f!H}zyCvQ-OJ7wa8 zFq9!?D={=XXF#zTFP)2K$Jp64)L@H}CjKVla%MYX@QSK6|-b0oq}5`DA*s5J=#53xk)%d|x~8qNw~OKy}gQi4VaAWosOgNn@c4ch@+WAkj&^;VXBcFTSdU|pOTfOOPVv0hPfamHVJn=K|#Rvm5@|gpAc(tE5`(; z)!V7dS6~RRN};CrxaJaa_~~!Jyg@73soTOO2|Pt=8{DMmYK-!Oe+NPX=`4GbG7EPq z1=Ld+(jPQl6Y?m1*nri28jyIeVl48(WJ_2#6wn9)8E^aaRfa+1u_n0ev}KjbKksme zx6%4L*zIayqBhz!#CT9C_o&Jd*5kE+gN*S}sP2g`!ECtJ6Xuqyvuh=Yr_RAN^|Ypa zRoL0`#w9jL(VT>mcS(wila|YBocC%=A=|~n`@_kv-1bUqNjwDA$g3=o8Z zLhOcMr^}~a8rUoM)oVvzieAQaP*`((XvpbrRD&@G`S?@Vrz2%F(vTiZQ-@d|{-|YA z63r@0Wi-%`9-v#OS3Cl>$Gf<50qkvYCWvf|r>TyRDcoCFhRyjRFwicTCRA2#%|-|= zaL$#Ohk=J z4KB{(2`HTJ;}L4kJjtXODLs|jZ}+k3eL}G@RY(U|9izR3@0`fSyBHegncC)w`Q=Vd zi18)1??@{`rY9?O(d>LoKH>CN?+4<-npYMowRPv)afj+UOMOmTqi4>Siu2JK+W~;f zc;m%(Qk|Lr&^dCnO32bu9SyE!?H7wY7z_7Iz(d(dv73h0oyo&BAhb9lnj4OcRSgm7 zDjmtttHrsOBSEJ5VsH3AJslQ+x-A&D7$G=sjTn@rZa_U9y8jeaeK|a_zJWakF_Rxo zP%FVWexrtA6}TvDKFMa55ZhM9RQ_HDGMcADS?wfZeW`}3RlZFxc4&rn1%KN2{3vB5 zALXQh7J?Jze3R;&$!f>8$~3DuQrXJ@qoKB^rA(e~AVWKW2D-{0k*(dqnGV_phc69o zsXP-|D0FC03)QB_I!*^V!hkoWUac`@7`v5Wjjk*QE6>J68{gWn=%K2N0QX*6C>CHs z5*KpEi$EfwQmFC#kgYNRBKtTtcZl!KW`y9hwBLZe@7F)oL59|8mCW3rDFUr>9pfuDi{ugvqaq+Rv>zFjP^l5(r0XfnA0%p^oTNPE#+T1&twsp0c%qf?C1OgI)T$_2uK~>u343^4Vv;I+ zTB)qXR7)P5+f;?+WMWWzeqP^t#P(xM`r~EjL^~-OgqVs)g>j{^L}}tozLwLOO96V@ zOw>Jiu^?fFHMsky13D?#P_;rVl}=)2;8dXeKZ?9Qy^;^bPc8Mnh-z&rpq8#F5cY|) z9<>O#XHSwmkx)NPiyCyxYaTkK(^CslA`sBB>SzGSW4AX^$s;HtIduq>vGrkHX{`gs z`uiNwIK)&9EA+D9^L=Q>3mlRnxc%LbSB{SIE$5ZlMB;#9EBm#m3qZmqGN_4gs>}kc zCLB*gCJYFuUdEGB?PP>lzdt{8n_y9^I1RO!Y74D9;Xur(Xz$GeZ$k?Gtz6``C<_D? zY%Xrl6_}x<-t`{$vNQR2!k?^p^9WaHyv=RZvBH))0+YP^TDxX?>ls7RP)2Tweh* zUJB?fd_UVzY3>Uy!EK}(p7Z5nK20D(VLDlO4& zCu61GD}ag%xU~4sH?N_<#pEzdSn8mSomlmQo6bZP-Y`1Q7S1(S=3An-Hh$|InpH0q<0zIy%^qpzQQ-$|AJY}(wIiX>0Q#JOEpSd{!p1-z=tL2>{`A^ZUk$P~j% z1rnN%?0L==r1frKWx?jkM0p0Ni0Q;t1Zsb_v<*W?a@kruS)ZcPz#rWy?kaSs?`lDz zd@8IKlSwp;zoE9lOIZP{Na^IkDA%};Z)(pn$FVY?JdjC4YQRI(2xP|>o#J<%>Bwqg zgY{O9k62(#1u)2cWk42pSb^=?;!q2g!8@uQj~X#lq^mNX-pCx3pOpaoszi}IC^R~0 zs`FIQ)2v14?5U1l2XWJ*v1nO!Fx8qRROT;HWbUVZ@)7!v}I!1jF7d>%ub!++F`gS~< z44qp6#E!;b;T61;VuW4XjC@kbDA-Vc;BTa?`!WS2fawCI=q+lI@c_ElF@khE(urdftPFkk( z3g$^zMe6$TyW8TPUy%#n_;ONV?;CSG8`9xd}TDg2N&hyO@xkfR{52W{B!K&1(hFsD-R_a2J_k6p!<-7lu z^73=({ju}swd?I>)XkQNX`Z-F3SY@fatIdw&l9*(*)rrvBk^gA>fug28Oc%BMfr;UIc@`1j$yqD?iH+JOY(~_Wegc(j)5g zc%37L9Wy)Om%CF@zqftLUs?A8j@G%NY;x|kZ_B{dZ|r>NiD6Xg(%%Li5`9N5dK(|NrHybRg(9~Rk8^F zuvOrC*GE<}$c5i_dZcCtPt$+?A_l#9iZ3PqK-Hea)wF?{J!Q3TNjgHH) z5{+3N*+#vBc&doHc+=Scjd7z%@F7XI3EL_ABMY62sl*?em|?>mR^zKlRXSqDrHI3b zagztM;vuJo1G`XCE@TH?d&*Rk4wou5FOmlGVtsQ2QWOP_gB}tO;1d@mhUgx#uEd~BAXSr5#G!8At|#_n6&CMi zKi1c3mVlD$80TWc*_W=jCwM?wBCrR}pC8p>R9o!+njD57kB&s~dJ_Nx0q5;ZSFcFm zy@rE|j#ViJMoZfN$?!1cd$_62+98;^kYL&~%H=fhdyaGhF}u65d6XJiYs0#*x_hlY zqIuByum^@vk>)fmK_83eo+x!3yHL9&@Rb7W_n{0R2?4h^UO$WXHLn4as>MXXW`!-m zZS$M7gQ~KcZSGy(EC@bunh|0+`;U*yx?dg#s+$_OeqxDjz_9@l*&SHi};Ys@yN=W%JwmL@PIny#hj0b{d&_Lau+hu8J$eX`IoY?B(-3q=uZ) zf5)3Mp4T-%8q`l$v-kAr0nd$JjqMYwT}p+lpa-v{k11;HR?-cPC|FGo?Lb1RYVT8k zyBvh5v@@#2ukIx2y%6C)hJsF@T}BtKoHT6gyezZ`rBcI`e5F{5+6F1~HPxaFeJ})7 z$4vir$tYLZ$pY}mF-2B&HI2UjiE{$PGY_X-UTYEg=?VksnqiQKxf&6-yfXsTx399w zsEWa18M2cty5^{cKD1Kqh}RfwVd$;E^R2YL07DM6Ao1+Ai0G5nB2`xPBzn4}qGlnJLgHOH%v2CcDi-q` zo*t?=ua7&X%z_p;gP2&|7wme<_mI(ofr>VIe|0V|H+1lYHAiOzPbDYxARQG93+c8W zMN)MK@H&7=dCWw?DXl*g?$xw{I2*||_Rk_T&|2Y29asDLsAeNk_wJ@YV~Gn!%F)J- zH_LPnZPAlxgkXcpoHO#lpx1T;PU6$x@t?x9|H=^04GD6p1v7)xKzrZwK(Fm;-4Yq| z*K*moFz}{IA#{@3F-(5)dw;dhZuz*qCsdXc*;3wjzvoIQ1bT?*S1$-%U5MDI&YWej zTI(GcKdz?2h`Cht=;SFxdb>L)AH?k%^BDf<&5SlTrZj!9{MwmuhA6-(94&nVVatIP zr$Z=VNS%xr%o))J-GU=me!#Z^CXREO4p`E$Np5)UBbeFThHiqHbA!q!6O=lcG zJl5$$efFkj)5cX#_(iXsfNB(ULbuRT)?>o3Cxq;C2Se7=QiLE6Y)i}JJ75|v=q2aV z;>p>t`fsVe=|`KNse^OD z0K*E4H3U}|l7pEE7~kjOHiB$52HMCcG4mc{C7T*sq{0(}s0C6lsKi~0#bl8J!nbvS3$&TQ)U7g_=ezDpk$4#qM_?@W9V^mC|5(Z zYtxSCJyWz7aY2@bJi``gv3=5NevdYDrrVI0ZX9g0IH;$`ssE{o=1sA(gw|BYvR)wN zO;ChljjYDb_8UQ>Ea!Ry7m6{M1^Vp{J@k zWwzL9;zE0gDz+~I&VMfnp8MQndMd&jFM>J6HWT?=6CaYZ85cj@N%P=sSPJ%W_U?4^r22#2+&v@G3r!kgX9l@+WY zM@>*PAJ({)kT}bHY7H5S^?o~DI3`1TQFSWyx(A1~oDrbvz-zEM6ZdzLuRjNpDWuDQ-zL zMP9a;CwsP=_s5uaU5=@gnWE@prMDb z5nVI*h=4a?C-Vbgq*7?C3^)sow)e;B&FYG}0Cx22!`IDk0(@CVzpQ>wBDt`yU2N!r z9P_6Arn*V?AfjeOT*An8B+o@4cz>~yL}6=-JMMo5f`U|>*XYpA8}PPUw6#b~%90K+Y)*UD2H(idv?i<=5&Md^gT!~Y(&<=H z&HBF_p#~!>(nG1f&*q6( zu7DS?C~M=a?32N(6?m}G>aN?HP*qD`WfEVJ_oBTywQ#U z*IbNO5p{Y>u$;aFyuj}&4f*2qWn#kaSd5IZHEnQh)$G{lp`8^PP}b;lp(FBH66=Xy z{lQi2rAyH_go$kNO{{Rr+CW-s(Umb;E3NwRETr$a(8oX15EQp*P=g3CJ2^Vq^c++5 zn6jW|Bg(7b*JUCd3q;+7X&ho?4mu&KKB)cDcsqA| ze=_mr4w z|Gn<0ZCA^jVdeDJVTFITiej7|vZ#{l<90E5Aeqk7pEmb}JW3_hfpFfGYEor=1>+Qc zIu7aMr;DEi!u!u4&YrqAL8L`-U+zzK^b*X;=xll@!mX;5o3o4`-hPR@jkSm2 zwZIXo9gDXh)vPDY^@P<4VARM_DutrZDJx#pcKp>z{_yNWAE9Wv5Zi#My-?b#taku+ zGvkGz_pG%0eGo%>DzFCwC$5QTUb9z;2wsZ=t7%}~Y77Rc_yDiRj#2A)v<0abz^ho< zWJ%keDH>4KKY+fLagDWgRMGMKfIT+*Rcgckgh4#GiXW|A2Tw16GJaUwY#p~=2T$uz zsspinKq6O@@C99snj5W}N7~U~+J76n)$@=;ug3$nyYtHd5U%t>kwS!S5FJ0(mzNBKY0O1(tp$DLQHzmuC*s(^ z9bgg@Upf020bGlC!WPD?|Mq%pf~K2{<`IT?qizG)Hl#^_=R7kts29@@4DmZ%Jgn$b zM=8+G!gi{}!$|UT0V3EaqWkn_V1BI3C=yQw1h}z#t|4H3WPP-8m*GS}2;drpQPTKx zK?R*uiI)d``FwGr1as)?;t4jIbka}2+FE4XbjY1>fFRwK$##bW0z3)Agz{+zqDZjo zv=VPX8|x?JyS<>Jf5&(%o{)|CM~uoUN6EEPjFBcifPBQ!$5FHHPZD`EHDik3ed?1j zbM#;Yd|jkUe2)C1o^aplRZ>m6;1EKejigRwfu6~b!=8<3Poju3{&f#!w;x2r-b_=N zqAZ_5#wa7WrZ9DxKf{gt{E>waWFlma?mFQ|gLKAFlU}fRMHI($M!<|M*#AiY-pR(C z!Ke}QT-l(+{!a+z{1j98=EgpipZ}8vwGj}5kRnF-picw({cwF(7t0w(1w^Jc7AT34 z%I9jF2X#ubgrzhf6d4kPhhdE+Oi}y0Ku-wxU>jfb%~c}Zn3qZb#s#cdhe`1Q80|q+ zL7ON3Rjn8}{Uru!{c?C64{#J&uK|XJILKAxqmkCer4lEp1(u$xTDt~m&qXk8;*Y@j z2ObnX&16hb)lZOFy{7rgPevfEILGRg1_{ZS->KrI{PmSTT@@&O!qBSZqtNqYpMd2` z8}_G_RQbeIxHA$!`x*HYUf1%a(slq(+@?0D4f{ZqLLUhZVnni;bVoa@L z#P!K8#hs2mH50^%2)tU1X&{O`g~+iyIzcwQQl6aBE#lbQa%bpk} zc&k)ov`Cb}ywq~%&q#{Hvv)&C-P|A$Shx9d^ezwf!fJ&zk_Z-tSJnc+lkj9ibD^oaqIBuS_h%~*-m2&tIw-5-+Xee2>XmCLili5X8Id9Sjc12Y`ykNpKkSsCr35_;WjxXCQ0xi8) zs_29<(n?G_Cw~~lR7e(=I!L)1A11Um-J|~al!_@LL1NknZ8^PxM>xuK#K1C|O@*LH z>(&TFpY_(D%9HtQ|5TY3Tb%t6#)`?B?ZelXZs>f5VS9dmhrCdvV|w?|l}I zIksc3aEu}*zt2nZ6qy($e6LHd3|0~*`YZzoY zM)IGxi-~q4?1^2%?$%jF4p>QeN&Uqr9v=#2pc%=9L*f8f0D?2{)q3nB)Ky=UOh?pc zxe>%F0&XFZ2R?QYC1NpAt@?c4IsyctDzcg!=9_g0SdxYAFjwJu3Csfa1gA9x5U0U+ zw_sND90clOgn1-OYMqUuRcb_=GZ2be4a;tgez2GVQ67hP6-{==GG?4IW=k4w4kn~vJ1nHKkY`PTB`9|GvV)^6N72+;$H%DENRrsFc zdv3!xn@|^pOGJxX$lJjE+Efi!-b6S6gk%aYPbnp7Z4*EPgM z6vT8kQ-FYo-_Q3L1dbLfh7zM2-frUEg`-WRyTs-@?q6|*>!<})G@y3Fy<*L%jYO|- zHy^a69RO+9qcTKwQ|sRjekN#E2Z5BLSX?2aP3}?XvP>dPjKPvB@$i=^!QKz0QER-0 z`%c8d6q+rSXBf0i6jJF7I?*Z;0kl9r4aKQez~F8!s5%Cpqvg7(o&31&gz#QR~mf2Fh;?imhc`E}*^lb4~393|0v2 zb|T^=%0wR`&_(DV0cJf4w{7$Z{9AAqZ@(eFmQtB8P@TKv6~_>0qBTk?y(K|ue@5JR zqu~=WbY=LtOAf?Du5R!-<8Ir+`on(@>XQ|9{EaGq!1P4ct})*Co1@tz zB%&}h#dt@0P8aLf8~ug!kN5Ys)iQNn>QKBR?A2fMcJt;sACgsH_+L*5)(T~k=9`b< z46Oy40}O8<)CPeR*wP7JnM5!YLjog>Be_bpF_0Lv4)6*b`cYJQ&_h`=1s5$^o`brh z_M&@G2y;*$WqgC57{}W&eYei` zhy-rB){b_0<>cDu0{4FT(c75;uX*xyHH{I1UY5uxTj1=6IEidLhR z6MLUNmZ-7Afi%Q27GOH6wH#TV<7TH|jxmOz2)a_iTL?3n%ziW#WU1{dtT7du0^e5- zkkrG-M&mbl1-?PUi=*1jB9`mB4059C#j}Yh>S~CX1GcP=MLFmR>y-hifok+%l`2EY zxP(@PWKpUDlbmSLm1luL4`6oQhdRfDRsCrNwj;BcG)u*@@Kc(b!N0Jx`<~+t=YH&HYO#Rus`)% z+$u2`!Vqe8u-sd3T=%V`9Uf|7b`8X;+~t2_kZufWAxlW5bYAWy#sL3(&l429rDcAc zdM6ejnW?FCZ_yfi-k5yGp)~KQ8WEY{{4~SK#eJ#IkKxq&;M6-j?pa&NRVPy4PIWZ?^F**Yufxl0_Yk4#x26a-#Dq*6^|RPO%osNI+O<{3E|J ziI|b)P#nlUi!(w|(|brmCu)#-+Y5-@fhu!sxMxiAtSe`9B&YD^k5-WvsmLjir{?u8 z%_I2L`wNr1*qzcLs%Q#m2LD&~?GR?Ri2XBs33ep7n*prM;N;mSPA1;Pp?Wd<_&aG} zJ{kEX*UCQPTZtJ5etM?OB_t8%QO-;Ui3v^K{pjRxN}}Mq_pGQV<}WCjAB=JsoZ~CL zwh*)#;v36~wFPRZ$#Fm}79^?Shg^ifR`XhPQK3nI8#u5djTi18B|)?Y4C1kPV-kBh zOWMl@s*pp-2bv`$=3_g3GnhtC`9ykL)uwQVTkO&>V`|w>LhjU>16UM5)H*)HVdt** zKEvVUYzi?+LL+(ljNL?sE-m0`O!x ztKa)iUq(?mVs}Ro3sZN!2D?LM=6k+&*_o2ke^C%HDbw?LoBtI;EbU5BF%OVX4KAn` zFR+`;7vJ9x*h{C3#qJ1a58s=*%OUC=FZK0#NjJVob&_ELuhzJzg*SdQr1!vI0iiL4 zh{5cz-V{Fm1~`Gb0#d^Y9!+kU;{>(?VKim}f%S4y>g(?C>_FdN&zzbs^`k$w(1;GS zGNG#(AI%*cC$JvEL53AVmDJs`>+Xkl7(SG>f13%+nGlaC23o@(*SSQ^plqQnZ-jUKf7FZ-QA7i++ETY>i2w5Ksd)7(m(x7z7On4d_qe;1C<` zNCp6t$nU>DXkh^Rzs%Os*3`w*!SqkBKb4iVqPt5-01PG40EHxg?%Ur?4e-CBk(H^j z3lTFTz}b)iz!&pR_o#10AA11zF#y2{>ffA!fLZ`&|JVI5jiY}8{9SMkke9#N3V_J? ze*ykRkMd81ziY1jruzTOoB*^p|4ViQkg5Bp=ijxv{=LK+FaVza$nu}%{1-kD&~II@ zzpMzr*ZfZ@sDD@a`u7_9%ky7U&wm|n{|BVMRKNbOD5%)~e>{Jwfc;-lr2hlTU#ejL z9p$&6)?fDJKcM_wCG7u-)Ak>5{;nGKFPuMH_wW1vDgE_ZH|#I__8)Nmi;CEv&E`Kf zfPc@V1mN(W4dCw@V*fppzg5KkGQa #include "Filter.h" #include "dsps_fft4r.h" #include "dsps_wind_hann.h" @@ -11,6 +12,15 @@ namespace Espfc { namespace Math { +class Peak +{ +public: + Peak(): freq(0), value(0) {} + Peak(float f, float v): freq(f), value(v) {} + float freq; + float value; +}; + template class FFTAnalyzer { @@ -35,6 +45,8 @@ class FFTAnalyzer // Generate hann window dsps_wind_hann_f32(_wind, Size); + clearPeaks(); + return 1; } @@ -67,24 +79,59 @@ class FFTAnalyzer { size_t k = j * 2; _samples[j] = _samples[k] * _samples[k] + _samples[k + 1] * _samples[k + 1]; + //_samples[j] = sqrt(_samples[j]); } - // find highest noise peak freq - float maxAmt = 0; - float maxFreq = 0; - for (size_t j = 1; j < (Size >> 1) - 1; j++) + clearPeaks(); + const size_t begin = std::max((size_t)1, (size_t)(_freq_min / _bin_width)); + const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)); + + float noise = 0; + size_t noiseCount = 0; + float valueMax = 0; + for(size_t b = begin; b <= end; b++) + { + noiseCount++; + float value = _samples[b]; + if (value > valueMax) valueMax = value; + noise += value; + } + noise -= valueMax; + noise /= noiseCount; + + size_t peakCount = 0; + for(size_t b = begin; b <= end; b++) { - const float freq = _bin_width * j + _bin_offset; - if(freq < _freq_min) continue; - if(freq > _freq_max) break; - const float amt = _samples[j]; - if(amt > maxAmt) + if(_samples[b] > noise && _samples[b] > _samples[b - 1] && _samples[b] > _samples[b + 1]) { - maxAmt = amt; - maxFreq = freq; + float f0 = b * _bin_width + _bin_offset; + float k0 = _samples[b]; + + float fl = f0 - _bin_width; + float kl = _samples[b - 1]; + + float fh = f0 + _bin_width; + float kh = _samples[b + 1]; + + // weighted average + float centerFreq = (k0 * f0 + kl * fl + kh * fh) / (k0 + kl + kh); + + _peaks[peakCount] = Peak(centerFreq, _samples[b]); + + peakCount++; + b++; // next bin can't be peak } } - if(maxFreq > 0) freq = maxFreq; + + if(peakCount > 1) + { + // sort peaks by value + std::sort(_peaks, _peaks + peakCount, [](const Peak& a, const Peak& b) -> bool { + return a.value < b.value; + }); + } + + freq = _peaks[0].freq; return 1; } @@ -92,6 +139,13 @@ class FFTAnalyzer float freq; private: + void clearPeaks() + { + for(size_t i = 0; i < PEAKS_COUNT; i++) _peaks[i] = Peak(); + } + + static const size_t BINS = Size >> 1; + int16_t _rate; int16_t _freq_min; int16_t _freq_max; @@ -101,6 +155,9 @@ class FFTAnalyzer float _bin_width; float _bin_offset; + static const size_t PEAKS_COUNT = BINS >> 1; + Peak _peaks[PEAKS_COUNT]; + // fft input and output __attribute__((aligned(16))) float _samples[Size]; // Window coefficients diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 4c833d03..4b0ff321 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -410,6 +410,7 @@ class Model state.magTimer.setRate(state.magRate); } + const uint32_t gyroPreFilterRate = state.gyroTimer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; const uint32_t inputFilterRate = state.loopTimer.rate; const uint32_t pidFilterRate = state.loopTimer.rate; @@ -431,7 +432,7 @@ class Model } else { state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate); } - state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); + state.gyroFilter2[i].begin(config.gyroFilter2, gyroPreFilterRate); state.gyroFilter3[i].begin(config.gyroFilter3, gyroFilterRate); state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), gyroFilterRate); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 3a9a605c..95f32085 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -737,6 +737,7 @@ class ModelConfig gyroFilter = FilterConfig(FILTER_PT1, 100); gyroFilter2 = FilterConfig(FILTER_PT1, 213); gyroFilter3 = FilterConfig(FILTER_FIR2, 250); // 0 to off + //gyroFilter3 = FilterConfig(FILTER_PT1, 100); // 0 to off gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170); gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 20170f7d..0d19e862 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -139,6 +139,8 @@ struct ModelState VectorInt16 gyroRaw; VectorFloat gyroSampled; + VectorFloat gyroImu; + VectorFloat gyroDynNotch; VectorInt16 accelRaw; VectorInt16 magRaw; @@ -146,7 +148,6 @@ struct ModelState VectorFloat accel; VectorFloat mag; - VectorFloat gyroImu; VectorFloat gyroPose; Quaternion gyroPoseQ; diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 7303fa2f..763cfac5 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -19,7 +19,7 @@ namespace Sensor { class GyroSensor: public BaseSensor { public: - GyroSensor(Model& model): _model(model) {} + GyroSensor(Model& model): _dyn_notch_denom(1), _model(model) {} int begin() { @@ -42,9 +42,9 @@ class GyroSensor: public BaseSensor _model.state.gyroCalibrationRate = _model.state.loopTimer.rate; _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroCalibrationRate; - _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); - _sma.begin(_model.config.loopSync); + _dyn_notch_denom = std::min(1u, _model.state.loopTimer.rate / 1000); + _dyn_notch_sma.begin(_dyn_notch_denom); #ifdef ESPFC_DSP for(size_t i = 0; i < 3; i++) @@ -53,6 +53,8 @@ class GyroSensor: public BaseSensor } #endif + _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); + return 1; } @@ -77,8 +79,16 @@ class GyroSensor: public BaseSensor VectorFloat input = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale; - // moving average filter - _model.state.gyroSampled = _sma.update(input); + if(_model.config.gyroFilter2.freq) + { + for(size_t i = 0; i < 3; ++i) + { + _model.state.gyroSampled.set(i, _model.state.gyroFilter2[i].update(input[i])); + } + } else { + // moving average filter + _model.state.gyroSampled = _sma.update(input); + } return 1; } @@ -94,6 +104,7 @@ class GyroSensor: public BaseSensor calibrate(); bool dynamicFilterEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER); + bool dynamicFilterFeed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ; bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check(); @@ -124,36 +135,48 @@ class GyroSensor: public BaseSensor _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroFilter2[i].update(_model.state.gyro[i])); if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) { _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); } + } + + _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + for(size_t i = 0; i < 3; ++i) + { if(dynamicFilterEnabled || dynamicFilterDebug) { + float freq = 0; + if(dynamicFilterFeed) + { #ifdef ESPFC_DSP - int status = _fft[i].update(_model.state.gyro[i]); - dynamicFilterUpdate = dynamicFilterEnabled && status; - const float freq = _fft[i].freq; + int status = _fft[i].update(_model.state.gyroDynNotch[i]); + dynamicFilterUpdate = dynamicFilterEnabled && status; + freq = _fft[i].freq; #else - _model.state.gyroAnalyzer[i].update(_model.state.gyro[i]); - const float freq = _model.state.gyroAnalyzer[i].freq; + _model.state.gyroAnalyzer[i].update(_model.state.gyroDynNotch[i]); + freq = _model.state.gyroAnalyzer[i].freq; #endif - if (dynamicFilterDebug) - { - _model.state.debug[i] = lrintf(freq); - if (i == debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); - } - if(dynamicFilterUpdate) dynamicFilterApply((Axis)i, freq); - if(dynamicFilterEnabled) - { - _model.state.gyro.set(i, _model.state.gyroDynamicFilter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); + if (dynamicFilterDebug) + { + _model.state.debug[i] = lrintf(freq); + if (i == debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + if(dynamicFilterEnabled && dynamicFilterUpdate) + { + dynamicFilterApply((Axis)i, freq); + } } } + if(dynamicFilterEnabled) + { + _model.state.gyro.set(i, _model.state.gyroDynamicFilter[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); + } + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) { _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); @@ -226,6 +249,8 @@ class GyroSensor: public BaseSensor } Math::Sma _sma; + Math::Sma _dyn_notch_sma; + size_t _dyn_notch_denom; Model& _model; Device::GyroDevice * _gyro; From 7304d1c113a2519e6238ddf56da908a1ff443db6 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 14 Jul 2023 01:10:35 +0200 Subject: [PATCH 12/14] dyn notch - filter more peaks --- lib/Espfc/src/Cli.h | 1 + lib/Espfc/src/Math/FFTAnalyzer.h | 103 ++++++++++------------ lib/Espfc/src/Model.h | 9 +- lib/Espfc/src/ModelConfig.h | 18 ++-- lib/Espfc/src/ModelState.h | 7 +- lib/Espfc/src/Sensor/GyroSensor.h | 136 +++++++++++++++++------------ lib/Espfc/src/Target/TargetESP32.h | 4 +- 7 files changed, 146 insertions(+), 132 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index ba13cf86..38a93356 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -380,6 +380,7 @@ class Cli Param(PSTR("features"), &c.featureMask), Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), + Param(PSTR("debug_axis"), &c.debugAxis), Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices), Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices), diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index e3e3771d..d2b9142c 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -21,7 +21,7 @@ class Peak float value; }; -template +template class FFTAnalyzer { public: @@ -32,18 +32,18 @@ class FFTAnalyzer _rate = rate; _freq_min = config.min_freq; _freq_max = config.max_freq; - _peak_count = config.width; + _peak_count = std::min((size_t)config.width, PEAKS_MAX); freq = (_freq_min + _freq_max) * 0.5f; _idx = 0; - _bin_width = (float)_rate / Size; // no need to dived by 2 as we next process `Size / 2` results + _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results _bin_offset = _bin_width * 0.5f; // center of bin - dsps_fft4r_init_fc32(NULL, Size >> 1); + dsps_fft4r_init_fc32(NULL, BINS); // Generate hann window - dsps_wind_hann_f32(_wind, Size); + dsps_wind_hann_f32(_wind, SAMPLES); clearPeaks(); @@ -55,27 +55,27 @@ class FFTAnalyzer { _samples[_idx] = v; - if(++_idx < Size) return 0; // not enough samples + if(++_idx < SAMPLES) return 0; // not enough samples _idx = 0; // apply window function - for (size_t j = 0; j < Size; j++) + for (size_t j = 0; j < SAMPLES; j++) { _samples[j] *= _wind[j]; // real } // FFT Radix-4 - dsps_fft4r_fc32(_samples, Size >> 1); + dsps_fft4r_fc32(_samples, BINS); // Bit reverse - dsps_bit_rev4r_fc32(_samples, Size >> 1); + dsps_bit_rev4r_fc32(_samples, BINS); - // Convert one complex vector with length Size/2 to one real spectrum vector with length Size/2 - dsps_cplx2real_fc32(_samples, Size >> 1); + // Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2 + dsps_cplx2real_fc32(_samples, BINS); - // calculate magnitude squared - for (size_t j = 0; j < Size >> 1; j++) + // calculate magnitude + for (size_t j = 0; j < BINS; j++) { size_t k = j * 2; _samples[j] = _samples[k] * _samples[k] + _samples[k + 1] * _samples[k + 1]; @@ -86,65 +86,59 @@ class FFTAnalyzer const size_t begin = std::max((size_t)1, (size_t)(_freq_min / _bin_width)); const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)); - float noise = 0; - size_t noiseCount = 0; - float valueMax = 0; for(size_t b = begin; b <= end; b++) { - noiseCount++; - float value = _samples[b]; - if (value > valueMax) valueMax = value; - noise += value; - } - noise -= valueMax; - noise /= noiseCount; - - size_t peakCount = 0; - for(size_t b = begin; b <= end; b++) - { - if(_samples[b] > noise && _samples[b] > _samples[b - 1] && _samples[b] > _samples[b + 1]) - { - float f0 = b * _bin_width + _bin_offset; - float k0 = _samples[b]; + if(!(_samples[b] > _samples[b - 1] && _samples[b] > _samples[b + 1])) continue; - float fl = f0 - _bin_width; - float kl = _samples[b - 1]; + float f0 = b * _bin_width; + float k0 = _samples[b]; - float fh = f0 + _bin_width; - float kh = _samples[b + 1]; + float fl = f0 - _bin_width; + float kl = _samples[b - 1]; - // weighted average - float centerFreq = (k0 * f0 + kl * fl + kh * fh) / (k0 + kl + kh); + float fh = f0 + _bin_width; + float kh = _samples[b + 1]; - _peaks[peakCount] = Peak(centerFreq, _samples[b]); + // weighted average + float centerFreq = (k0 * f0 + kl * fl + kh * fh) / (k0 + kl + kh); - peakCount++; - b++; // next bin can't be peak + for(size_t p = 0; p < _peak_count; p++) + { + if(!(_samples[b] > peaks[p].value)) continue; + for(size_t k = _peak_count - 1; k > p; k--) + { + peaks[k] = peaks[k - 1]; + } + peaks[p] = Peak(centerFreq, _samples[b]); } + b++; // next bin can't be peak } - if(peakCount > 1) - { - // sort peaks by value - std::sort(_peaks, _peaks + peakCount, [](const Peak& a, const Peak& b) -> bool { - return a.value < b.value; - }); - } - - freq = _peaks[0].freq; + // max peak freq + freq = peaks[0].freq; + // sort peaks by freq + std::sort(peaks, peaks + _peak_count, [](const Peak& a, const Peak& b) -> bool { + if (a.freq == 0.f) return false; + if (b.freq == 0.f) return true; + return a.freq > b.freq; + }); + return 1; } float freq; + static const size_t PEAKS_MAX = 8; + Peak peaks[PEAKS_MAX]; + private: void clearPeaks() { - for(size_t i = 0; i < PEAKS_COUNT; i++) _peaks[i] = Peak(); + for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Peak(); } - static const size_t BINS = Size >> 1; + static const size_t BINS = SAMPLES >> 1; int16_t _rate; int16_t _freq_min; @@ -155,13 +149,10 @@ class FFTAnalyzer float _bin_width; float _bin_offset; - static const size_t PEAKS_COUNT = BINS >> 1; - Peak _peaks[PEAKS_COUNT]; - // fft input and output - __attribute__((aligned(16))) float _samples[Size]; + __attribute__((aligned(16))) float _samples[SAMPLES]; // Window coefficients - __attribute__((aligned(16))) float _wind[Size]; + __attribute__((aligned(16))) float _wind[SAMPLES]; }; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 4b0ff321..d54cf483 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -419,10 +419,11 @@ class Model for(size_t i = 0; i <= AXIS_YAW; i++) { state.gyroAnalyzer[i].begin(gyroFilterRate, config.dynamicFilter); - if(isActive(FEATURE_DYNAMIC_FILTER)) { - state.gyroDynamicFilter[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), gyroFilterRate); - if(config.dynamicFilter.width > 0) { - state.gyroDynamicFilter2[i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 300), gyroFilterRate); + if(isActive(FEATURE_DYNAMIC_FILTER)) + { + for(size_t p = 0; p < (size_t)config.dynamicFilter.width; p++) + { + state.gyroDynNotchFilter[i][p].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } } state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 95f32085..24ec1e5f 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -634,6 +634,7 @@ class ModelConfig uint8_t ibatSource; int8_t debugMode; + uint8_t debugAxis; BuzzerConfig buzzer; @@ -736,8 +737,8 @@ class ModelConfig gyroFilter = FilterConfig(FILTER_PT1, 100); gyroFilter2 = FilterConfig(FILTER_PT1, 213); - gyroFilter3 = FilterConfig(FILTER_FIR2, 250); // 0 to off - //gyroFilter3 = FilterConfig(FILTER_PT1, 100); // 0 to off + //gyroFilter3 = FilterConfig(FILTER_FIR2, 250); // 0 to off + gyroFilter3 = FilterConfig(FILTER_PT1, 120); // 0 to off gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170); gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off @@ -857,19 +858,19 @@ class ModelConfig input.rateType = 3; // actual - input.rate[AXIS_ROLL] = 15; + input.rate[AXIS_ROLL] = 18; input.expo[AXIS_ROLL] = 0; - input.superRate[AXIS_ROLL] = 47; + input.superRate[AXIS_ROLL] = 36; input.rateLimit[AXIS_ROLL] = 1998; - input.rate[AXIS_PITCH] = 15; + input.rate[AXIS_PITCH] = 18; input.expo[AXIS_PITCH] = 0; - input.superRate[AXIS_PITCH] = 47; + input.superRate[AXIS_PITCH] = 36; input.rateLimit[AXIS_PITCH] = 1998; - input.rate[AXIS_YAW] = 15; + input.rate[AXIS_YAW] = 20; input.expo[AXIS_YAW] = 0; - input.superRate[AXIS_YAW] = 47; + input.superRate[AXIS_YAW] = 36; input.rateLimit[AXIS_YAW] = 1998; input.filterType = INPUT_FILTER; @@ -970,6 +971,7 @@ class ModelConfig modelName[0] = 0; debugMode = DEBUG_NONE; + debugAxis = 1; blackboxDev = 0; blackboxPdenom = 32; // 1kHz blackboxFieldsDisabledMask = 0; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 0d19e862..1547b9fc 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -139,8 +139,9 @@ struct ModelState VectorInt16 gyroRaw; VectorFloat gyroSampled; - VectorFloat gyroImu; VectorFloat gyroDynNotch; + VectorFloat gyroImu; + VectorInt16 accelRaw; VectorInt16 magRaw; @@ -148,7 +149,6 @@ struct ModelState VectorFloat accel; VectorFloat mag; - VectorFloat gyroPose; Quaternion gyroPoseQ; VectorFloat accelPose; @@ -169,8 +169,7 @@ struct ModelState Filter gyroFilter3[3]; Filter gyroNotch1Filter[3]; Filter gyroNotch2Filter[3]; - Filter gyroDynamicFilter[3]; - Filter gyroDynamicFilter2[3]; + Filter gyroDynNotchFilter[3][8]; Filter gyroImuFilter[3]; Math::FreqAnalyzer gyroAnalyzer[3]; diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 763cfac5..b73e1d73 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -43,13 +43,13 @@ class GyroSensor: public BaseSensor _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroCalibrationRate; _sma.begin(_model.config.loopSync); - _dyn_notch_denom = std::min(1u, _model.state.loopTimer.rate / 1000); + _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); #ifdef ESPFC_DSP for(size_t i = 0; i < 3; i++) { - _fft[i].begin(_model.state.loopTimer.rate, _model.config.dynamicFilter); + _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter); } #endif @@ -103,12 +103,6 @@ class GyroSensor: public BaseSensor calibrate(); - bool dynamicFilterEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER); - bool dynamicFilterFeed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; - bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ; - bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check(); - - const int debugAxis = 1; // filtering for(size_t i = 0; i < 3; ++i) @@ -121,67 +115,38 @@ class GyroSensor: public BaseSensor { _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); } - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { _model.state.debug[0] = lrintf(degrees(_model.state.gyro[i])); } _model.state.gyro.set(i, _model.state.gyroFilter3[i].update(_model.state.gyro[i])); - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { _model.state.debug[1] = lrintf(degrees(_model.state.gyro[i])); } _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); + + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) + { + _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) + if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); } } - _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + filterDynNotch(); for(size_t i = 0; i < 3; ++i) { - if(dynamicFilterEnabled || dynamicFilterDebug) - { - float freq = 0; - if(dynamicFilterFeed) - { -#ifdef ESPFC_DSP - int status = _fft[i].update(_model.state.gyroDynNotch[i]); - dynamicFilterUpdate = dynamicFilterEnabled && status; - freq = _fft[i].freq; -#else - _model.state.gyroAnalyzer[i].update(_model.state.gyroDynNotch[i]); - freq = _model.state.gyroAnalyzer[i].freq; -#endif - if (dynamicFilterDebug) - { - _model.state.debug[i] = lrintf(freq); - if (i == debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); - } - if(dynamicFilterEnabled && dynamicFilterUpdate) - { - dynamicFilterApply((Axis)i, freq); - } - } - } - - if(dynamicFilterEnabled) - { - _model.state.gyro.set(i, _model.state.gyroDynamicFilter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroDynamicFilter2[i].update(_model.state.gyro[i])); - } - - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == debugAxis) - { - _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); - } - if(_model.config.debugMode == DEBUG_GYRO_FILTERED) { _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); @@ -196,18 +161,73 @@ class GyroSensor: public BaseSensor } private: - void dynamicFilterApply(Axis i, const float freq) + void filterDynNotch() { + bool dynamicFilterEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER); + bool dynamicFilterFeed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; + bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ; + bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check(); const float q = _model.config.dynamicFilter.q * 0.01; - //const float bw = 0.5f * (freq / q)); // half bandwidth - if(_model.config.dynamicFilter.width > 0 && _model.config.dynamicFilter.width < 30) { - const float w = 0.005f * _model.config.dynamicFilter.width; // half witdh - const float freq1 = freq * (1.0f - w); - const float freq2 = freq * (1.0f + w); - _model.state.gyroDynamicFilter[i].reconfigure(freq1, freq1, q); - _model.state.gyroDynamicFilter2[i].reconfigure(freq2, freq2, q); - } else { - _model.state.gyroDynamicFilter[i].reconfigure(freq, freq, q); + + if(dynamicFilterEnabled || dynamicFilterDebug) + { + _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + + for(size_t i = 0; i < 3; ++i) + { +#ifdef ESPFC_DSP + const size_t peakCount = _model.config.dynamicFilter.width; + if(dynamicFilterFeed) + { + int status = _fft[i].update(_model.state.gyroDynNotch[i]); + dynamicFilterUpdate = dynamicFilterEnabled && status; + if(dynamicFilterDebug) + { + if(i == _model.config.debugAxis) + { + _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq); + _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq); + _model.state.debug[2] = lrintf(_fft[i].peaks[2].freq); + _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + } + if(dynamicFilterEnabled && dynamicFilterUpdate) + { + for(size_t p = 0; p < peakCount; p++) + { + float freq = _fft[i].peaks[p].freq; + _model.state.gyroDynNotchFilter[i][p].reconfigure(freq, freq, q); + } + } + } + if(dynamicFilterEnabled) + { + for(size_t p = 0; p < peakCount; p++) + { + _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[i][p].update(_model.state.gyro[i])); + } + } +#else + if(dynamicFilterFeed) + { + _model.state.gyroAnalyzer[i].update(_model.state.gyroDynNotch[i]); + float freq = _model.state.gyroAnalyzer[i].freq; + if(dynamicFilterDebug) + { + _model.state.debug[i] = lrintf(freq); + if(i == _model.config.debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + } + if(dynamicFilterEnabled && dynamicFilterUpdate) + { + _model.state.gyroDynNotchFilter[i][0].reconfigure(freq, freq, q); + } + } + if(dynamicFilterEnabled) + { + _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[i][0].update(_model.state.gyro[i])); + } +#endif + } } } diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index b6dadfa7..9a6614ae 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -9,8 +9,8 @@ #define ESPFC_OUTPUT_COUNT 8 #define ESPFC_OUTPUT_0 27 #define ESPFC_OUTPUT_1 25 -#define ESPFC_OUTPUT_2 12 -#define ESPFC_OUTPUT_3 4 +#define ESPFC_OUTPUT_2 4 +#define ESPFC_OUTPUT_3 12 #define ESPFC_OUTPUT_4 -1 #define ESPFC_OUTPUT_5 -1 #define ESPFC_OUTPUT_6 -1 From e60ccb57dfa71b031a3d61d1c5e0ed4f5fd1e88c Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 15 Jul 2023 20:20:39 +0200 Subject: [PATCH 13/14] peak detection fixes + unit tests --- lib/Espfc/src/Math/FFTAnalyzer.h | 45 ++---------------------- lib/Espfc/src/Math/Utils.h | 56 +++++++++++++++++++++++++++++ lib/Espfc/src/Sensor/GyroSensor.h | 4 +-- test/test_math/test_math.cpp | 58 +++++++++++++++++++++++++++++++ 4 files changed, 119 insertions(+), 44 deletions(-) diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index d2b9142c..8b68fc95 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -3,7 +3,7 @@ // https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c -#include +#include "Math/Utils.h" #include "Filter.h" #include "dsps_fft4r.h" #include "dsps_wind_hann.h" @@ -12,15 +12,6 @@ namespace Espfc { namespace Math { -class Peak -{ -public: - Peak(): freq(0), value(0) {} - Peak(float f, float v): freq(f), value(v) {} - float freq; - float value; -}; - template class FFTAnalyzer { @@ -86,43 +77,13 @@ class FFTAnalyzer const size_t begin = std::max((size_t)1, (size_t)(_freq_min / _bin_width)); const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)); - for(size_t b = begin; b <= end; b++) - { - if(!(_samples[b] > _samples[b - 1] && _samples[b] > _samples[b + 1])) continue; - - float f0 = b * _bin_width; - float k0 = _samples[b]; - - float fl = f0 - _bin_width; - float kl = _samples[b - 1]; - - float fh = f0 + _bin_width; - float kh = _samples[b + 1]; - - // weighted average - float centerFreq = (k0 * f0 + kl * fl + kh * fh) / (k0 + kl + kh); - - for(size_t p = 0; p < _peak_count; p++) - { - if(!(_samples[b] > peaks[p].value)) continue; - for(size_t k = _peak_count - 1; k > p; k--) - { - peaks[k] = peaks[k - 1]; - } - peaks[p] = Peak(centerFreq, _samples[b]); - } - b++; // next bin can't be peak - } + Math::peakDetect(_samples, begin, end, _bin_width, peaks, _peak_count); // max peak freq freq = peaks[0].freq; // sort peaks by freq - std::sort(peaks, peaks + _peak_count, [](const Peak& a, const Peak& b) -> bool { - if (a.freq == 0.f) return false; - if (b.freq == 0.f) return true; - return a.freq > b.freq; - }); + Math::peakSort(peaks, _peak_count); return 1; } diff --git a/lib/Espfc/src/Math/Utils.h b/lib/Espfc/src/Math/Utils.h index 14e3d692..8648ef61 100644 --- a/lib/Espfc/src/Math/Utils.h +++ b/lib/Espfc/src/Math/Utils.h @@ -1,10 +1,21 @@ #ifndef _ESPFC_MATH_UTILS_H_ #define _ESPFC_MATH_UTILS_H_ +#include + namespace Espfc { namespace Math { +class Peak +{ +public: + Peak(): freq(0), value(0) {} + Peak(float f, float v): freq(f), value(v) {} + float freq; + float value; +}; + inline long mapi(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; @@ -64,6 +75,51 @@ namespace Math { { return rad * (180.0f * invPi()); } + + void peakDetect(float * samples, size_t begin_bin, size_t end_bin, float bin_width, Peak * peaks, size_t peak_count) + { + for(size_t b = begin_bin; b <= end_bin; b++) + { + if(samples[b] > samples[b - 1] && samples[b] > samples[b + 1]) + { + float f0 = b * bin_width; + float k0 = samples[b]; + + float fl = f0 - bin_width; + float kl = samples[b - 1]; + + float fh = f0 + bin_width; + float kh = samples[b + 1]; + + // weighted average + float centerFreq = (k0 * f0 + kl * fl + kh * fh) / (k0 + kl + kh); + + for(size_t p = 0; p < peak_count; p++) + { + if(samples[b] > peaks[p].value) + { + for(size_t k = (peak_count - 1); k > p; k--) + { + peaks[k] = peaks[k - 1]; + } + peaks[p] = Peak(centerFreq, samples[b]); + break; + } + } + b++; // next bin can't be peak + } + } + } + + // sort peaks by freq, move zero to end + void peakSort(Peak * peaks, size_t peak_count) + { + std::sort(peaks, peaks + peak_count, [](const Peak& a, const Peak& b) -> bool { + if (a.freq == 0.f) return false; + if (b.freq == 0.f) return true; + return a.freq < b.freq; + }); + } } } diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index b73e1d73..31dddfa4 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -196,7 +196,7 @@ class GyroSensor: public BaseSensor for(size_t p = 0; p < peakCount; p++) { float freq = _fft[i].peaks[p].freq; - _model.state.gyroDynNotchFilter[i][p].reconfigure(freq, freq, q); + if(freq > 0) _model.state.gyroDynNotchFilter[i][p].reconfigure(freq, freq, q); } } } @@ -219,7 +219,7 @@ class GyroSensor: public BaseSensor } if(dynamicFilterEnabled && dynamicFilterUpdate) { - _model.state.gyroDynNotchFilter[i][0].reconfigure(freq, freq, q); + if(freq > 0) _model.state.gyroDynNotchFilter[i][0].reconfigure(freq, freq, q); } } if(dynamicFilterEnabled) diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index c098b160..e2e537f8 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -44,6 +44,61 @@ void test_math_deadband() TEST_ASSERT_EQUAL_INT32(10, Math::deadband( 20, 10)); } +void test_math_peak_detect_full() +{ + using Math::Peak; + + float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; + Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; + + Math::peakDetect(samples, 1, 14, 1, peaks, 4); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[0].value); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 13.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 6.f, peaks[1].value); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[2].value); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].value); +} + +void test_math_peak_detect_partial() +{ + using Math::Peak; + + float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; + Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; + + Math::peakDetect(samples, 3, 12, 1, peaks, 3); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].value); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].value); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.8f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[2].value); +} + +void test_math_peak_sort() +{ + using Math::Peak; + + Peak peaks[8] = { Peak(20, 10), Peak(10, 10), Peak(0, 10), Peak(5, 5) }; + + Math::peakSort(peaks, 4); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, peaks[3].freq); +} + void test_vector_int16_access() { VectorInt16 v; @@ -757,6 +812,9 @@ int main(int argc, char **argv) RUN_TEST(test_math_map); RUN_TEST(test_math_map3); RUN_TEST(test_math_deadband); + RUN_TEST(test_math_peak_detect_full); + RUN_TEST(test_math_peak_detect_partial); + RUN_TEST(test_math_peak_sort); RUN_TEST(test_vector_int16_access); RUN_TEST(test_vector_int16_math); From e1ee40eab156e3d414a5ab9ff490cf985a119971 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 16 Jul 2023 14:15:56 +0200 Subject: [PATCH 14/14] dyn notch tidy + update defaults --- lib/Espfc/src/Math/FFTAnalyzer.h | 16 +++------ lib/Espfc/src/ModelConfig.h | 56 ++++++++++++++++++++----------- lib/Espfc/src/Sensor/GyroSensor.h | 4 +-- 3 files changed, 43 insertions(+), 33 deletions(-) diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index 8b68fc95..613b39d4 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -20,16 +20,14 @@ class FFTAnalyzer int begin(int16_t rate, const DynamicFilterConfig& config) { + int16_t nyquistLimit = rate / 2; _rate = rate; _freq_min = config.min_freq; - _freq_max = config.max_freq; + _freq_max = std::min(config.max_freq, nyquistLimit); _peak_count = std::min((size_t)config.width, PEAKS_MAX); - freq = (_freq_min + _freq_max) * 0.5f; - _idx = 0; _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results - _bin_offset = _bin_width * 0.5f; // center of bin dsps_fft4r_init_fc32(NULL, BINS); @@ -74,22 +72,17 @@ class FFTAnalyzer } clearPeaks(); - const size_t begin = std::max((size_t)1, (size_t)(_freq_min / _bin_width)); - const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)); + const size_t begin = (_freq_min / _bin_width) + 1; + const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1; Math::peakDetect(_samples, begin, end, _bin_width, peaks, _peak_count); - // max peak freq - freq = peaks[0].freq; - // sort peaks by freq Math::peakSort(peaks, _peak_count); return 1; } - float freq; - static const size_t PEAKS_MAX = 8; Peak peaks[PEAKS_MAX]; @@ -108,7 +101,6 @@ class FFTAnalyzer size_t _idx; float _bin_width; - float _bin_offset; // fft input and output __attribute__((aligned(16))) float _samples[SAMPLES]; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 24ec1e5f..2d3573e8 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -735,19 +735,30 @@ class ModelConfig fusion.gain = 50; fusion.gainI = 5; + // BF x 0.85 + gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170); gyroFilter = FilterConfig(FILTER_PT1, 100); gyroFilter2 = FilterConfig(FILTER_PT1, 213); - //gyroFilter3 = FilterConfig(FILTER_FIR2, 250); // 0 to off - gyroFilter3 = FilterConfig(FILTER_PT1, 120); // 0 to off - gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170); - gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off - gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off - dynamicFilter = DynamicFilterConfig(0, 300, 80, 400); // 8%. q:1.2, 80-400 Hz + dynamicFilter = DynamicFilterConfig(0, 300, 80, 400); // 8%. q:3.0, 80-400 Hz + dtermDynLpfFilter = FilterConfig(FILTER_PT1, 145, 60); dtermFilter = FilterConfig(FILTER_PT1, 128); dtermFilter2 = FilterConfig(FILTER_PT1, 128); - dtermDynLpfFilter = FilterConfig(FILTER_PT1, 145, 60); - dtermNotchFilter = FilterConfig(FILTER_NOTCH, 0, 0); + + // ESPFC defaults => BF x 0.75 + //gyroDynLpfFilter = FilterConfig(FILTER_PT1, 375, 150); + //gyroFilter = FilterConfig(FILTER_PT1, 100); + //gyroFilter2 = FilterConfig(FILTER_PT1, 188); + //dynamicFilter = DynamicFilterConfig(5, 200, 80, 400); // 8%. q:2.0, 80-400 Hz + + //dtermDynLpfFilter = FilterConfig(FILTER_PT1, 128, 53); + //dtermFilter = FilterConfig(FILTER_PT1, 113); + //dtermFilter2 = FilterConfig(FILTER_PT1, 113); + + gyroFilter3 = FilterConfig(FILTER_PT1, 150); + gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off + gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off + dtermNotchFilter = FilterConfig(FILTER_NOTCH, 0, 0); // off accelFilter = FilterConfig(FILTER_BIQUAD, 15); magFilter = FilterConfig(FILTER_BIQUAD, 10); @@ -858,25 +869,25 @@ class ModelConfig input.rateType = 3; // actual - input.rate[AXIS_ROLL] = 18; + input.rate[AXIS_ROLL] = 20; input.expo[AXIS_ROLL] = 0; - input.superRate[AXIS_ROLL] = 36; + input.superRate[AXIS_ROLL] = 40; input.rateLimit[AXIS_ROLL] = 1998; - input.rate[AXIS_PITCH] = 18; + input.rate[AXIS_PITCH] = 20; input.expo[AXIS_PITCH] = 0; - input.superRate[AXIS_PITCH] = 36; + input.superRate[AXIS_PITCH] = 40; input.rateLimit[AXIS_PITCH] = 1998; - input.rate[AXIS_YAW] = 20; + input.rate[AXIS_YAW] = 30; input.expo[AXIS_YAW] = 0; input.superRate[AXIS_YAW] = 36; input.rateLimit[AXIS_YAW] = 1998; input.filterType = INPUT_FILTER; input.filterAutoFactor = 50; - input.filter = FilterConfig(FILTER_PT3, 0); - input.filterDerivative = FilterConfig(FILTER_PT3, 0); + input.filter = FilterConfig(FILTER_PT3, 0); // 0: auto + input.filterDerivative = FilterConfig(FILTER_PT3, 0); // 0: auto input.interpolationMode = INPUT_INTERPOLATION_AUTO; // mode input.interpolationInterval = 26; @@ -885,10 +896,16 @@ class ModelConfig failsafe.delay = 4; failsafe.killSwitch = 0; - // PID controller config - pid[PID_ROLL] = { .P = 42, .I = 85, .D = 30, .F = 90 }; - pid[PID_PITCH] = { .P = 46, .I = 90, .D = 32, .F = 95 }; - pid[PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 90 }; + // PID controller config (BF default) + //pid[PID_ROLL] = { .P = 42, .I = 85, .D = 30, .F = 90 }; + //pid[PID_PITCH] = { .P = 46, .I = 90, .D = 32, .F = 95 }; + //pid[PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 90 }; + //pid[PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; + + // PID controller config (ESPFC default) + pid[PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }; + pid[PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }; + pid[PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }; pid[PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; pid[PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }; @@ -989,6 +1006,7 @@ class ModelConfig debugMode = DEBUG_GYRO_SCALED; serial[ESPFC_DEV_PRESET_BLACKBOX].functionMask |= SERIAL_FUNCTION_BLACKBOX; serial[ESPFC_DEV_PRESET_BLACKBOX].blackboxBaud = SERIAL_SPEED_250000; + serial[ESPFC_DEV_PRESET_BLACKBOX].baud = SERIAL_SPEED_250000; #endif #ifdef ESPFC_DEV_PRESET_MODES diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 31dddfa4..fb810e32 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -132,14 +132,14 @@ class GyroSensor: public BaseSensor if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { - _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); + _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); } _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { - _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); + _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); } }