├── .github ├── cpp-plot.gif └── dynnotch.gif ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── adaptnotch-config.cmake.in └── plot.cmake.in ├── data ├── rawimu-hover.txt └── rawimu-motion.txt ├── include └── adaptnotch │ └── adaptnotch.h ├── matlab └── dynnotch.m └── src ├── adaptnotch.cpp ├── csv.h └── main.cpp /.github/cpp-plot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusk01/adaptive-gyro-filtering/6e2565694a6b9cba3007958670fc2b85975b2868/.github/cpp-plot.gif -------------------------------------------------------------------------------- /.github/dynnotch.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusk01/adaptive-gyro-filtering/6e2565694a6b9cba3007958670fc2b85975b2868/.github/dynnotch.gif -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | *.m~ 3 | *.pyc 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(adaptnotch VERSION 0.1) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | if(NOT CMAKE_BUILD_TYPE) 6 | # Options: Debug, Release, MinSizeRel, RelWithDebInfo 7 | message(STATUS "No build type selected, default to Release") 8 | set(CMAKE_BUILD_TYPE "Release") 9 | endif() 10 | 11 | find_package(Eigen3 REQUIRED) 12 | message(STATUS "Eigen Version: " ${EIGEN3_VERSION_STRING}) 13 | 14 | add_library(adaptnotch SHARED src/adaptnotch.cpp) 15 | target_include_directories(adaptnotch PUBLIC 16 | $ 17 | $ 18 | ${EIGEN3_INCLUDE_DIRS}) 19 | 20 | option(BUILD_EXAPPS "Build adaptnotch example" ON) 21 | 22 | if(BUILD_EXAPPS) 23 | # Setup cmake paths 24 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 25 | 26 | # Download plot library 27 | set(PLOT_DIR ${CMAKE_CURRENT_BINARY_DIR}/plot-download) 28 | set(BUILD_EXAMPLES OFF CACHE INTERNAL "") # don't build plot examples 29 | configure_file("${CMAKE_MODULE_PATH}/plot.cmake.in" "${PLOT_DIR}/CMakeLists.txt" IMMEDIATE @ONLY) 30 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . WORKING_DIRECTORY ${PLOT_DIR} ) 31 | execute_process(COMMAND ${CMAKE_COMMAND} --build . WORKING_DIRECTORY ${PLOT_DIR}) 32 | add_subdirectory(${PLOT_DIR}/src ${PLOT_DIR}/build) 33 | 34 | add_executable(main src/main.cpp) 35 | target_include_directories(main PUBLIC ${EIGEN3_INCLUDE_DIRS}) 36 | target_link_libraries(main adaptnotch pthread plot) 37 | endif() 38 | 39 | 40 | include(GNUInstallDirs) 41 | set(INSTALL_CONFIGDIR ${CMAKE_INSTALL_LIBDIR}/cmake/adaptnotch) 42 | 43 | install(TARGETS adaptnotch 44 | EXPORT adaptnotch-targets 45 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 46 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) 47 | 48 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 49 | 50 | install(EXPORT adaptnotch-targets 51 | FILE adaptnotch-targets.cmake 52 | DESTINATION ${INSTALL_CONFIGDIR}) 53 | 54 | include(CMakePackageConfigHelpers) 55 | write_basic_package_version_file( 56 | ${CMAKE_CURRENT_BINARY_DIR}/adaptnotch-config-version.cmake 57 | VERSION ${PROJECT_VERSION} 58 | COMPATIBILITY AnyNewerVersion) 59 | 60 | configure_package_config_file(${CMAKE_CURRENT_LIST_DIR}/cmake/adaptnotch-config.cmake.in 61 | ${CMAKE_CURRENT_BINARY_DIR}/adaptnotch-config.cmake 62 | INSTALL_DESTINATION ${INSTALL_CONFIGDIR}) 63 | 64 | install(FILES 65 | ${CMAKE_CURRENT_BINARY_DIR}/adaptnotch-config.cmake 66 | ${CMAKE_CURRENT_BINARY_DIR}/adaptnotch-config-version.cmake 67 | DESTINATION ${INSTALL_CONFIGDIR}) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Parker Lusk 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Adaptive Gyro Notch Filtering 2 | ============================= 3 | 4 | Signal conditioning is an important part of a control strategy. In the context of multirotor control, the on-board gyro provides angular rate measurements which are directly used to drive the actual angular rate to the desired angular rate. In this setting, gyro noise is fed into the actuators, creating visible attitude fluctuations (i.e., "wiggles") even when the vehicle is meant to be hovering. Additionally, noisy gyro signals are directly related to hot motors (which can lead to motor failure, but is also an indication of performance). The majority of gyro noise comes from vibrations, caused by the spinning of the motors. 5 | 6 | An easy way to reduce noise is with a low-pass filter. A commonly employed strategy in control is an [RC-type low-pass filter](https://en.wikipedia.org/wiki/Low-pass_filter), which is equivalent to a first-order IIR filter (e.g., see [here](http://www.olliw.eu/2016/digital-filters/#chapter22)). The tradeoff in filtering is always attenuation vs sample delay. A low cutoff frequency will more strongly attenuate high frequency noise at the cost of increased sample delay. Therefore, instead of designing a low-pass filter with a low cutoff frequency to attenuate high-frequency motor noise, a more targeted approach should be used. 7 | 8 | This repo provides an implementation of an adaptive notch filter for gyro noise suppresion. It is largely inspired by [Betaflight](https://github.com/betaflight/betaflight), where the algorithm is meant to run on a resource-constrained microprocessor. While still efficient, this implementation enjoys the benefit of easy debugging and visualizations to help tune such an algorithm. 9 | 10 | ## Example Application 11 | 12 | The [IMU data](data/) provided in this repo was collected at 500 Hz on a large hexarotor with 12" props. At hover, each motor spins around 4400 RPM. This corresponds to a frequency of roughly 73 Hz. Therefore, ignoring DC, we would expect the largest frequency component of gyro data to be at 73 Hz with harmonics at integer multiples. In the gif below, we can see this frequency component and its associated higher frequency harmonics. Note that on a smaller vehicle (like popular FPV drones that Betaflight is primarily used for), we would expect the motors to spin much faster. 13 | 14 | After the dynamic notch algorithm converges to roughly 73 Hz (black vertical line), we can see that the post-filtering spectrum does not have this component and the time series gyro data is much cleaner. 15 | 16 |

17 | 18 | ### C++ Library 19 | 20 | This repo also provides a C++ adaptive notch filtering library. An example application is provided that reads the provided IMU data and produces the same results as the MATLAB gif above. The excellent terminal plotting library [`plot`](https://github.com/fbbdev/plot) is included to produce plots (only in the example application) of the spectrum before and after filtering. 21 | 22 |

23 | 24 | ## Technical Description 25 | 26 | Gyro samples are stored in a ring buffer, the size of which corresponds to the FFT length. In this example, a 128-length FFT is used. To analyze the spectrum in real-time, a Hann window is used to taper the values on the edges of the buffer (cf. [short-time Fourier transform](https://en.wikipedia.org/wiki/Short-time_Fourier_transform)). The peak finding algorithm to adaptively determine the center frequency of the notch filter searches within a user-defined range. The center frequency is estimated using a weighted mean and smoothing center frequency estimates through an alpha/RCLPF filter. 27 | 28 | ### Resources 29 | 30 | - [Betaflight's `gyroanalyse.c`](https://github.com/betaflight/betaflight/blob/master/src/main/flight/gyroanalyse.c) 31 | - Biquads: [OlliW](http://www.olliw.eu/2016/digital-filters) 32 | - Biquads: [Nigel Redmon](https://www.earlevel.com/main/2012/11/26/biquad-c-source-code/) 33 | - Betaflight filtering: [rav's Python code](https://github.com/rav-rav/betaflightCalc/tree/master/src) 34 | - Other improvements: [Pawel S.'s "Matrix Filter" in iNav](https://quadmeup.com/emuflight-and-inav-matrix-filter/) 35 | - Excellent notch filter theory and basic implementation video: https://www.youtube.com/watch?v=ysS4bIXFAsU 36 | -------------------------------------------------------------------------------- /cmake/adaptnotch-config.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the adaptnotch package 2 | # It defines the following variables 3 | # ADAPTNOTCH_LIBRARIES - libraries to link against 4 | 5 | # Compute paths 6 | get_filename_component(ADAPTNOTCH_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 7 | 8 | # Our library dependencies (contains definitions for IMPORTED targets) 9 | if(NOT TARGET adaptnotch) 10 | include("${ADAPTNOTCH_CMAKE_DIR}/adaptnotch-targets.cmake") 11 | endif() 12 | 13 | # These are IMPORTED targets created by adaptnotch-targets.cmake 14 | set(ADAPTNOTCH_LIBRARIES adaptnotch) 15 | -------------------------------------------------------------------------------- /cmake/plot.cmake.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(plot-download NONE) 3 | 4 | include(ExternalProject) 5 | ExternalProject_Add(plot 6 | GIT_REPOSITORY https://github.com/fbbdev/plot 7 | GIT_TAG master 8 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/src" 9 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/build" 10 | CONFIGURE_COMMAND "" 11 | BUILD_COMMAND "" 12 | INSTALL_COMMAND "" 13 | TEST_COMMAND "" 14 | ) -------------------------------------------------------------------------------- /include/adaptnotch/adaptnotch.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file adaptnotch.h 3 | * @brief Adaptive notch filtering algorithm 4 | * @author Parker Lusk 5 | * @date 8 Dec 2020 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace adaptnotch { 19 | 20 | /** 21 | * @brief Ring buffer implementation for applying FFT to chunks of data 22 | */ 23 | class RingBuffer 24 | { 25 | public: 26 | /** 27 | * @brief RingBuffer constructor. Values initialize to zero. 28 | * 29 | * @param[in] N Fixed size of buffer 30 | */ 31 | RingBuffer(size_t N) { data_ = Eigen::VectorXd::Zero(N); } 32 | ~RingBuffer() = default; 33 | 34 | /** 35 | * @brief Push a new element onto the buffer. 36 | * 37 | * @param[in] x Element to add 38 | */ 39 | void add(double x) 40 | { 41 | data_(idx)= x; 42 | idx = (idx + 1) % data_.size(); 43 | } 44 | 45 | /** 46 | * @brief Copies the internal buffer into a sequential buffer. 47 | * 48 | * @return Vector of elements ordered from last (0) to first (N). 49 | */ 50 | Eigen::VectorXd sequentialView() const 51 | { 52 | Eigen::VectorXd v = Eigen::VectorXd::Zero(data_.size()); 53 | const size_t m = data_.size() - idx; 54 | // push the first m elements onto v 55 | for (size_t i=0; i 0) { 60 | for (size_t i=m; i fft_; ///< fft object 243 | std::unique_ptr notch1_, notch2_; ///< dual notch filters 244 | 245 | // \brief Parameter initialization 246 | int fft_bin_count_; ///< num useful bins for real input, excl. Nyquist bin 247 | bool dual_notch_; ///< use two notch filters or one 248 | double notch1_ctr_, notch2_ctr_; ///< dual notch scale factors 249 | double Q_; ///< Filter quality factor 250 | int min_hz_, max_hz_; ///< search range for peaks 251 | int fftFs_; ///< sample rate of downsampled input data 252 | int max_samples_; ///< number of samples to accumulate before downsampling 253 | double fres_; ///< frequency resolution 254 | int start_bin_; ///< start peak search at this bin 255 | Eigen::VectorXd window_; ///< window for tapering data for FFT 256 | 257 | // \brief State 258 | Eigen::VectorXd Y_; ///< most recent spectrum 259 | double peakFreq_; ///< estimated frequency of noise peak in specified range 260 | 261 | double input_accumulator_; ///< accumulator for downsampling to fft Fs 262 | double input_samples_; ///< num samples in accumulator 263 | 264 | RingBuffer buffer_; 265 | 266 | void reset(); 267 | Eigen::VectorXd windowHann(int N); 268 | double findFreqPeak(); 269 | 270 | }; 271 | 272 | namespace utils { 273 | template 274 | T clamp(T v, T lb, T ub) { 275 | if (v < lb) v = lb; 276 | if (v > ub) v = ub; 277 | return v; 278 | } 279 | } // ns utils 280 | 281 | } // ns adaptnotch -------------------------------------------------------------------------------- /matlab/dynnotch.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Dynamic Notch Filtering via FFT 3 | % 4 | % Uses a windowed FFT to find the largest frequency component in a 5 | % user-defined range. Peak detection is implemented using a weighted 6 | % average and is smoothed using an alpha filter. A biquad (2nd order IIR) 7 | % notch filter is updated with the detected center frequency. In fact, two 8 | % biquad notch filters are designed with a user-defined separation to 9 | % better notch out the motor noise while keeping phase lag small. 10 | % 11 | % Inspired by Betaflight gyroanalyse.c 12 | % 13 | % CSV data captured in flight from Snapdragon Flight Pro using the 14 | % sensor_imu_tester command, with IMU sampled at 500 Hz. 15 | % 16 | % Parker Lusk 17 | % 6 Dec 2020 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | clear, clc; 20 | %% Load raw IMU Data 21 | f = '../data/rawimu-hover.txt'; 22 | f = '../data/rawimu-motion.txt'; 23 | D = csvread(f,2); 24 | 25 | % unpack for convenience 26 | % sequence_number, timestamp(us), updated_timestamp(us), 27 | % acc_x, acc_y, acc_z, ang_x, ang_y, ang_z, temperature_C 28 | t = D(:,2); 29 | acc = D(:,4:6); 30 | gyr = D(:,7:9); 31 | 32 | % which axis will we inspect? 33 | axis = 1; 34 | 35 | % sampling parameters 36 | Fs = 1e6/mean(diff(t)); 37 | Ts = 1/Fs; 38 | N = length(t); 39 | n = 0:N-1; 40 | t = Ts*n; 41 | 42 | %% Initial LPF'ing 43 | 44 | lpfFc = 90; 45 | [lpf.b,lpf.a] = butter(1,lpfFc/(Fs/2)); 46 | gyrlpfd = filter(lpf.b,lpf.a,gyr(:,axis)); 47 | 48 | %% Parameter Setup 49 | 50 | % Perform filtering at gyro looprate (i.e., pidDenom = 1) 51 | filterFs = Fs; 52 | 53 | FFT_WINDOW_SIZE = 128; 54 | FFT_BIN_COUNT = FFT_WINDOW_SIZE / 2; 55 | DYN_NOTCH_SMOOTH_HZ = 4; 56 | DYN_NOTCH_CALC_TICKS = 3 * 4; 57 | 58 | dyn_notch_width_percent = 2; 59 | dyn_notch_q = 360; 60 | dyn_notch_min_hz = 60; 61 | dyn_notch_max_hz = 200; 62 | 63 | %% gyroDataAnalyseInit 64 | 65 | dynNotch1Ctr = 1 - dyn_notch_width_percent / 100; 66 | dynNotch2Ctr = 1 + dyn_notch_width_percent / 100; 67 | dynNotchQ = dyn_notch_q / 100; 68 | dynNotchMinHz = dyn_notch_min_hz; 69 | dynNotchMaxHz = max(2 * dynNotchMinHz, dyn_notch_max_hz); 70 | 71 | if dyn_notch_width_percent == 0 72 | dualNotch = false; 73 | else 74 | dualNotch = true; 75 | end 76 | 77 | % Notice how fftSamplingRateHz >= 2 * dynNotchMaxHz (i.e., Nyquist if 78 | % dynNotchMaxHz is the highest freq we care about). 79 | gyroLoopRateHz = round(filterFs); 80 | samples = fix(max(1, gyroLoopRateHz / (2 * dynNotchMaxHz))); 81 | fftSamplingRateHz = fix(gyroLoopRateHz / samples); 82 | 83 | fftResolution = fftSamplingRateHz / FFT_WINDOW_SIZE; 84 | fftStartBin = fix(max(2, dynNotchMinHz / round(fftResolution))); 85 | % if fftStartBin == Inf, fftStartBin = 2; end 86 | smoothFactor = 2 * pi * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); 87 | 88 | hannWindow = zeros(FFT_WINDOW_SIZE,1); 89 | for i = 0:(FFT_WINDOW_SIZE-1) 90 | hannWindow(i+1) = (0.5 - 0.5 * cos(2 * pi * i / (FFT_WINDOW_SIZE - 1))); 91 | end 92 | 93 | %% gyroDataAnalyseStateInit 94 | maxSampleCount = samples; 95 | state.centerFreq = repmat(dynNotchMaxHz,3,1); % any init value 96 | 97 | %% Initialize other state vars 98 | 99 | state.sampleCount = 0; 100 | state.oversampledGyroAccumulator = zeros(3,1); 101 | state.downsampledGyroData = zeros(3, FFT_WINDOW_SIZE); 102 | 103 | state.circularBufferIdx = 0; 104 | step.updateTicks = 0; 105 | state.updateStep = 0; 106 | 107 | state.fftData = zeros(3,FFT_WINDOW_SIZE); 108 | state.rfftData = zeros(3,FFT_WINDOW_SIZE); 109 | 110 | %% Initialize filters 111 | 112 | notch1 = biquadNotchInit(state.centerFreq(axis) * dynNotch1Ctr, 1/filterFs, dynNotchQ); 113 | notch2 = biquadNotchInit(state.centerFreq(axis) * dynNotch2Ctr, 1/filterFs, dynNotchQ); 114 | 115 | %% Plotting setup 116 | 117 | % calculate phyiscal frequencies 118 | % f = (0:FFT_BIN_COUNT)*fftResolution; 119 | f = fftSamplingRateHz/2 * linspace(0, 1, FFT_BIN_COUNT+1); 120 | 121 | figure(1), clf; 122 | subplot(311); hold on; grid on; title('Gyro'); 123 | hP1 = plot(1:maxSampleCount,1:maxSampleCount); 124 | ylabel('rad/s'); xlabel('Sample idx in ring buffer'); 125 | subplot(312); hold on; grid on; title('Spectrum'); 126 | hP2 = plot(1:maxSampleCount,1:maxSampleCount,'-*'); 127 | hP2max = scatter([],[],'*'); 128 | hP2cntr = xline(100,'LineWidth',2); 129 | xlabel('Hz'); 130 | subplot(313); hold on; grid on; title('After filtering'); 131 | hP3 = plot(1:maxSampleCount,1:maxSampleCount,'-*'); 132 | xlabel('Hz'); 133 | 134 | figure(2), clf; hold on; 135 | h2P = plot(t,gyr(:,axis)); 136 | h2Pfilt = plot(0,0); 137 | h2Pline = xline(0,'LineWidth',2); 138 | xlim([0 t(end)]); 139 | xlabel('Time (s)'); ylabel('Gyro (rad/s)'); 140 | title(['Raw vs Filtered Gyro (axis ' num2str(axis) ')']); 141 | 142 | [Hlpf,~] = freqz(lpf.b,lpf.a,1024,fftSamplingRateHz); 143 | [H1,F] = freqz(notch1.b, notch1.a,1024,fftSamplingRateHz); 144 | [H2,~] = freqz(notch2.b, notch2.a,1024,fftSamplingRateHz); 145 | H = Hlpf .* H1 .* H2; 146 | figure(3), clf; 147 | subplot(311); hold on; grid on; box on; title('Filter Design'); 148 | h3P1 = plot(F, 20*log10(abs(H))); 149 | xlabel('Frequency (Hz)'); ylabel('Magnitude (dB)'); 150 | subplot(312); hold on; grid on; box on; 151 | h3P2 = plot(F, rad2deg(angle(H))); 152 | xlabel('Frequency (Hz)'); ylabel('Phase (degrees)'); 153 | subplot(313); hold on; grid on; box on; 154 | h3P3 = plot(F, -unwrap(angle(H)) ./ (2*pi*F/fftSamplingRateHz)); 155 | xlabel('Frequency (Hz)'); ylabel('Sample lag'); 156 | 157 | gyrofiltered = zeros(1,N); 158 | 159 | %% Main loop 160 | for i = 1:N 161 | k = n(i); 162 | 163 | % 164 | % gyro.c / gyro_filter_impl.c : filterGyro() 165 | % 166 | 167 | % gyroDataAnalysePush 168 | state.oversampledGyroAccumulator = state.oversampledGyroAccumulator + gyr(i,:)'; 169 | 170 | % perform notch filtering 171 | y = gyrlpfd(i); 172 | [notch1, y] = biquadApplyDF1(notch1, y); 173 | [notch2, y] = biquadApplyDF1(notch2, y); 174 | gyrofiltered(i) = y; 175 | 176 | % 177 | % gyro.c / gyroanalyse.c : gyroDataAnalyse() 178 | % 179 | 180 | state.sampleCount = state.sampleCount + 1; 181 | 182 | % Downsample to FFT rate via averaging 183 | if state.sampleCount == maxSampleCount 184 | state.sampleCount = 0; 185 | sample = state.oversampledGyroAccumulator / maxSampleCount; 186 | state.downsampledGyroData(:,state.circularBufferIdx+1) = sample; 187 | state.oversampledGyroAccumulator = zeros(3,1); 188 | 189 | state.circularBufferIdx = mod(state.circularBufferIdx + 1, FFT_WINDOW_SIZE); 190 | 191 | % state.updateTicks = DYN_NOTCH_CALC_TICKS; 192 | state.updateTicks = 1; 193 | end 194 | 195 | % CPU util management by breaking gyro analysis into smaller steps 196 | if state.updateTicks == 0, continue; end 197 | state.updateTicks = 0; %state.updateTicks - 1; 198 | 199 | % 200 | % gyroanalyse.c : gyroDataAnalyseUpdate() 201 | % 202 | 203 | updateFFTPlots = false; 204 | 205 | STEP_ARM_CFFT_F32 = 0; 206 | STEP_BITREVERSAL = 1; 207 | STEP_STAGE_RFFT_F32 = 2; 208 | STEP_ARM_CMPLX_MAG_F32 = 3; 209 | STEP_CALC_FREQUENCIES = 4; 210 | STEP_UPDATE_FILTERS = 5; 211 | STEP_HANNING = 6; 212 | STEP_COUNT = 7; 213 | 214 | if state.updateStep == STEP_ARM_CFFT_F32 215 | state.fftData(1,:) = fft(state.fftData(1,:)) / N; 216 | state.fftData(2,:) = fft(state.fftData(2,:)) / N; 217 | state.fftData(3,:) = fft(state.fftData(3,:)) / N; 218 | 219 | elseif state.updateStep == STEP_BITREVERSAL 220 | state.updateStep = state.updateStep + 1; 221 | % elseif state.updateStep == STEP_STAGE_RFFT_F32 222 | % state.rfftData(1,:) = fftshift(state.fftData(1,:)); 223 | % state.rfftData(2,:) = fftshift(state.fftData(2,:)); 224 | % state.rfftData(3,:) = fftshift(state.fftData(3,:)); 225 | state.rfftData = state.fftData; 226 | 227 | elseif state.updateStep == STEP_ARM_CMPLX_MAG_F32 228 | state.fftData = abs(state.rfftData); 229 | % state.fftData = state.fftData(:,FFT_WINDOW_SIZE/2:end); 230 | state.updateStep = state.updateStep + 1; 231 | % elseif state.updateStep == STEP_CALC_FREQUENCIES 232 | 233 | dataMax = 0; 234 | dataMin = 1; 235 | binMax = 0; 236 | dataMinHi = 1; 237 | for ii = fftStartBin:FFT_BIN_COUNT 238 | if state.fftData(axis,ii+1) > state.fftData(axis,ii) % bin height increased 239 | if state.fftData(axis,ii+1) > dataMax 240 | dataMax = state.fftData(axis,ii+1); 241 | binMax = ii; 242 | end 243 | end 244 | end 245 | if binMax == 0 % no bin increase, hold prev max bin 246 | binMax = fix(state.centerFreq(axis) / fftResolution); 247 | else % there was a max, find min 248 | for ii = binMax-1:-1:1 % look for min below max 249 | dataMin = state.fftData(axis,ii+1); 250 | if state.fftData(axis,ii) > state.fftData(axis,ii+1), break; end 251 | end 252 | for ii = binMax+1:(FFT_BIN_COUNT-1) % look for min above max 253 | dataMinHi = state.fftData(axis,ii+1); 254 | if state.fftData(axis,ii+1) < state.fftData(axis,ii), break; end 255 | end 256 | end 257 | dataMin = min(dataMin, dataMinHi); 258 | 259 | % accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak 260 | squaredData = state.fftData(axis,binMax+1) ^ 2; 261 | fftSum = squaredData; 262 | fftWeightedSum = squaredData * binMax; 263 | 264 | % accumulate upper shoulder unless it would be FFT_BIN_COUNT 265 | shoulderBin = binMax + 1; 266 | if shoulderBin < FFT_BIN_COUNT 267 | squaredData = state.fftData(axis,shoulderBin+1) ^ 2; 268 | fftSum = fftSum + squaredData; 269 | fftWeightedSum = fftWeightedSum + squaredData * shoulderBin; 270 | end 271 | 272 | % accumulate lower shoulder unless lower shoulder would be bin 0 (DC) 273 | if binMax > 1 274 | shoulderBin = binMax - 1; 275 | squaredData = state.fftData(axis,shoulderBin+1) ^ 2; 276 | fftSum = fftSum + squaredData; 277 | fftWeightedSum = fftWeightedSum + squaredData * shoulderBin; 278 | end 279 | 280 | % get centerFreq in hz from weighted bins (weighted mean) 281 | centerFreq = 0; 282 | fftMeanIndex = 0; 283 | if fftSum > 0 284 | fftMeanIndex = fftWeightedSum / fftSum; 285 | centerFreq = fftMeanIndex * fftResolution; 286 | else 287 | centerFreq = state.centerFreq(axis); 288 | end 289 | centerFreq = constrain(centerFreq, dynNotchMinHz, dynNotchMaxHz); 290 | 291 | % LPF-style dynamic smoothing 292 | % dynamicFactor = constrain(dataMax / dataMin, 1, 2); 293 | dynamicFactor = 1; 294 | smoothFactor = 0.1; 295 | state.centerFreq(axis) = state.centerFreq(axis) + smoothFactor * dynamicFactor * (centerFreq - state.centerFreq(axis)); 296 | 297 | updateFFTPlots = true; 298 | 299 | elseif state.updateStep == STEP_UPDATE_FILTERS 300 | notch1 = biquadNotchUpdate(notch1, state.centerFreq(axis) * dynNotch1Ctr, 1/filterFs, dynNotchQ); 301 | notch2 = biquadNotchUpdate(notch2, state.centerFreq(axis) * dynNotch2Ctr, 1/filterFs, dynNotchQ); 302 | state.updateStep = state.updateStep + 1; 303 | % elseif state.updateStep == STEP_HANNING 304 | ringBufIdx = FFT_WINDOW_SIZE - state.circularBufferIdx; 305 | state.fftData(:,1:ringBufIdx) = state.downsampledGyroData(:,(state.circularBufferIdx+1):FFT_WINDOW_SIZE) .* repmat(hannWindow(1:ringBufIdx)',3,1); 306 | if state.circularBufferIdx > 0 307 | state.fftData(:,(ringBufIdx+1):FFT_WINDOW_SIZE) = state.downsampledGyroData(:,1:state.circularBufferIdx) .* repmat(hannWindow(ringBufIdx+1:FFT_WINDOW_SIZE)',3,1); 308 | end 309 | 310 | % make copy of buffer data just for extra analysis 311 | state.data = state.fftData; 312 | end 313 | 314 | state.updateStep = mod(state.updateStep + 1, STEP_COUNT); 315 | 316 | % 317 | % Visualization 318 | % 319 | 320 | if updateFFTPlots 321 | set(hP1,'XData',1:FFT_WINDOW_SIZE,'YData',state.downsampledGyroData(axis,:)); 322 | set(hP2,'XData',f,'YData',state.fftData(axis,1:FFT_BIN_COUNT+1)); 323 | set(hP2max,'XData',f(binMax+1),'YData',state.fftData(axis,binMax+1)); 324 | set(hP2cntr,'Value',state.centerFreq(axis)); 325 | figure(1); subplot(312); title(['centerFreq = ' num2str(round(state.centerFreq(axis))) ' Hz']) 326 | 327 | s = constrain(i-FFT_WINDOW_SIZE, 1, i); 328 | Y = fft(gyrofiltered(s:i),FFT_WINDOW_SIZE)/N; 329 | set(hP3,'XData',f,'YData',abs(Y(1:FFT_BIN_COUNT+1))); 330 | drawnow 331 | end 332 | 333 | set(h2Pfilt,'XData',t(1:i),'YData',gyrofiltered(1:i)); 334 | set(h2Pline,'Value',t(i)); 335 | 336 | [H1,F] = freqz(notch1.b, notch1.a,1024,fftSamplingRateHz); 337 | [H2,~] = freqz(notch2.b, notch2.a,1024,fftSamplingRateHz); 338 | H = Hlpf .* H1 .* H2; 339 | set(h3P1,'YData',20*log10(abs(H))); 340 | set(h3P2,'YData',rad2deg(angle(H))); 341 | set(h3P3,'YData',-unwrap(angle(H)) ./ (2*pi*F/fftSamplingRateHz)); 342 | end 343 | 344 | %% Helpers 345 | function x = constrain(x, low, high) 346 | if x < low, x = low; end 347 | if x > high, x = high; end 348 | end 349 | 350 | function filter = biquadNotchInit(fc, Ts, Q) 351 | 352 | % normalized frequency in [0, 2pi] 353 | omega = 2 * pi * fc * Ts; 354 | sn = sin(omega); 355 | cs = cos(omega); 356 | alpha = sn / (2 * Q); 357 | 358 | filter.fc = fc; 359 | filter.Ts = Ts; 360 | filter.Q = Q; 361 | 362 | % notch (b num, a denom) 363 | b0 = 1; 364 | b1 = -2 * cs; 365 | b2 = 1; 366 | a0 = 1 + alpha; 367 | a1 = -2 * cs; 368 | a2 = 1 - alpha; 369 | 370 | % normalize into biquad form (a0 == 1) 371 | filter.b0 = b0 / a0; 372 | filter.b1 = b1 / a0; 373 | filter.b2 = b2 / a0; 374 | filter.a1 = a1 / a0; 375 | filter.a2 = a2 / a0; 376 | 377 | % convenience: 378 | filter.b = [filter.b0 filter.b1 filter.b2]; 379 | filter.a = [1 filter.a1 filter.a2]; 380 | 381 | % initialize delay elements 382 | filter.x1 = 0; 383 | filter.x2 = 0; 384 | filter.y1 = 0; 385 | filter.y2 = 0; 386 | end 387 | 388 | % requires a direct form 1 (DF1) apply implementation to allow changing coeffs 389 | function filter = biquadNotchUpdate(filter, fc, Ts, Q) 390 | % backup state 391 | x1 = filter.x1; 392 | x2 = filter.x2; 393 | y1 = filter.y1; 394 | y2 = filter.y2; 395 | 396 | filter = biquadNotchInit(fc, Ts, Q); 397 | 398 | % restore state 399 | filter.x1 = x1; 400 | filter.x2 = x2; 401 | filter.y1 = y1; 402 | filter.y2 = y2; 403 | end 404 | 405 | % slightly less precise than DF2, but works in dynamic mode 406 | function [filter, y] = biquadApplyDF1(filter, x) 407 | y = filter.b0 * x + filter.b1 * filter.x1 + filter.b2 * filter.x2 - (filter.a1 * filter.y1 + filter.a2 * filter.y2); 408 | 409 | % shift feedback delay lines 410 | filter.x2 = filter.x1; 411 | filter.x1 = x; 412 | 413 | % shift feedforward delay lines 414 | filter.y2 = filter.y1; 415 | filter.y1 = y; 416 | end -------------------------------------------------------------------------------- /src/adaptnotch.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file adaptnotch.cpp 3 | * @brief Adaptive notch filtering algorithm 4 | * @author Parker Lusk 5 | * @date 8 Dec 2020 6 | */ 7 | 8 | #include 9 | 10 | namespace adaptnotch { 11 | 12 | constexpr double pi = 3.14159265358979323846; 13 | 14 | AdaptiveNotch::AdaptiveNotch(const Params& params) 15 | : params_(params), buffer_(params.NFFT) 16 | { 17 | fft_bin_count_ = params_.NFFT / 2; 18 | 19 | // two notch filters can be composed into a dual notch filter to increase 20 | // the stopband without increasing sample delay too much. 21 | dual_notch_ = (params_.dual_notch_width_percent != 0); 22 | if (dual_notch_) { 23 | notch1_ctr_ = 1 - params_.dual_notch_width_percent / 100.0; 24 | notch2_ctr_ = 1 + params_.dual_notch_width_percent / 100.0; 25 | } 26 | 27 | Q_ = params_.Q / 100.0; 28 | 29 | // calculate frequency range to search for peaks over 30 | min_hz_ = params_.min_hz; 31 | max_hz_ = std::max(2 * min_hz_, params_.max_hz); 32 | 33 | // compute the maximum sample rate required to analyze the spectrum with a 34 | // max frequency component at the max peak searching range. We will use this 35 | // frequency to downsample the input data for a more efficient FFT. 36 | const double nyquist = 2. * max_hz_; 37 | max_samples_ = std::max(1, static_cast(params_.Fs / nyquist)); 38 | fftFs_ = params_.Fs / max_samples_; 39 | 40 | fres_ = fftFs_ / static_cast(params_.NFFT); 41 | start_bin_ = std::max(params_.start_bin, static_cast(min_hz_ / fres_)); 42 | 43 | // construct a window for performing FFT on real-time data 44 | window_ = windowHann(params_.NFFT); 45 | 46 | fft_.SetFlag(Eigen::FFT::Unscaled); 47 | fft_.SetFlag(Eigen::FFT::HalfSpectrum); 48 | 49 | // initialize state variables 50 | reset(); 51 | } 52 | 53 | // ---------------------------------------------------------------------------- 54 | 55 | double AdaptiveNotch::apply(double x) 56 | { 57 | // accumulate input samples to downsample and respect FFT rate 58 | input_accumulator_ += x; 59 | input_samples_++; 60 | 61 | if (input_samples_ == max_samples_) { 62 | // downsample to FFT rate 63 | const double sample = input_accumulator_ / max_samples_; 64 | 65 | // reset accumulator 66 | input_accumulator_ = 0; 67 | input_samples_ = 0; 68 | 69 | buffer_.add(sample); 70 | 71 | peakFreq_ = findFreqPeak(); 72 | 73 | // update notch filters 74 | if (dual_notch_) { 75 | notch1_->update(peakFreq_ * notch1_ctr_, params_.Fs, Q_); 76 | notch2_->update(peakFreq_ * notch2_ctr_, params_.Fs, Q_); 77 | } else { 78 | notch1_->update(peakFreq_, params_.Fs, Q_); 79 | } 80 | } 81 | 82 | // 83 | // Apply notch filter 84 | // 85 | 86 | double y = notch1_->apply(x); 87 | if (dual_notch_) y = notch2_->apply(y); 88 | 89 | return y; 90 | } 91 | 92 | // ---------------------------------------------------------------------------- 93 | // Private Methods 94 | // ---------------------------------------------------------------------------- 95 | 96 | void AdaptiveNotch::reset() 97 | { 98 | // initialize peak frequency estimator 99 | peakFreq_ = max_hz_; 100 | 101 | // reset downsample accumulator 102 | input_accumulator_ = 0; 103 | input_samples_ = 0; 104 | 105 | // initialize notch filters 106 | if (dual_notch_) { 107 | notch1_.reset(new BiquadNotch(peakFreq_ * notch1_ctr_, params_.Fs, Q_)); 108 | notch2_.reset(new BiquadNotch(peakFreq_ * notch2_ctr_, params_.Fs, Q_)); 109 | } else { 110 | notch1_.reset(new BiquadNotch(peakFreq_, params_.Fs, Q_)); 111 | } 112 | } 113 | 114 | // ---------------------------------------------------------------------------- 115 | 116 | Eigen::VectorXd AdaptiveNotch::windowHann(int N) 117 | { 118 | Eigen::VectorXd w = Eigen::VectorXd::Zero(N); 119 | for (size_t i=0; i dataMax) { 148 | dataMax = Y_(i); 149 | binMax = i; 150 | } 151 | } 152 | 153 | if (binMax == 0) { 154 | // edge case, don't do anything 155 | binMax = static_cast(peakFreq_ / fres_); 156 | } else { 157 | // look for the min below the max peak 158 | for (size_t i=binMax-1; i>1; i--) { 159 | dataMin = Y_(i); 160 | // break if the bin below will increase 161 | if (Y_(i-1) > Y_(i)) break; 162 | } 163 | // look for the min above the max peak 164 | for (size_t i=binMax+1; i 1) { 191 | shoulderBin = binMax - 1; 192 | sq = Y_(shoulderBin) * Y_(shoulderBin); 193 | fftSum += sq; 194 | fftWeightedSum += sq * shoulderBin; 195 | } 196 | 197 | // calculate peak freq from weighted bins 198 | double centerFreq = peakFreq_; 199 | if (fftSum > 0) { 200 | const double meanIdx = fftWeightedSum / fftSum; 201 | centerFreq = meanIdx * fres_; 202 | } 203 | centerFreq = utils::clamp(centerFreq, 204 | static_cast(min_hz_), static_cast(max_hz_)); 205 | 206 | // 207 | // Dynamic smoothing of peak freq estimate 208 | // 209 | 210 | // move to big peaks fast 211 | const double alpha = 0.1; 212 | const double gamma = utils::clamp(dataMax / dataMin, 1., 0.8/alpha); 213 | centerFreq = peakFreq_ + alpha * gamma * (centerFreq - peakFreq_); 214 | 215 | return centerFreq; 216 | } 217 | 218 | } // ns adaptnotch 219 | -------------------------------------------------------------------------------- /src/csv.h: -------------------------------------------------------------------------------- 1 | // Copyright: (2012-2015) Ben Strasser 2 | // License: BSD-3 3 | // 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // 3. Neither the name of the copyright holder nor the names of its contributors 17 | // may be used to endorse or promote products derived from this software 18 | // without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | // POSSIBILITY OF SUCH DAMAGE. 31 | 32 | #ifndef CSV_H 33 | #define CSV_H 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #ifndef CSV_IO_NO_THREAD 43 | #include 44 | #include 45 | #include 46 | #endif 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace io{ 53 | //////////////////////////////////////////////////////////////////////////// 54 | // LineReader // 55 | //////////////////////////////////////////////////////////////////////////// 56 | 57 | namespace error{ 58 | struct base : std::exception{ 59 | virtual void format_error_message()const = 0; 60 | 61 | const char*what()const noexcept override{ 62 | format_error_message(); 63 | return error_message_buffer; 64 | } 65 | 66 | mutable char error_message_buffer[512]; 67 | }; 68 | 69 | const int max_file_name_length = 255; 70 | 71 | struct with_file_name{ 72 | with_file_name(){ 73 | std::memset(file_name, 0, sizeof(file_name)); 74 | } 75 | 76 | void set_file_name(const char*file_name){ 77 | if(file_name != nullptr){ 78 | // This call to strncpy has parenthesis around it 79 | // to silence the GCC -Wstringop-truncation warning 80 | (strncpy(this->file_name, file_name, sizeof(this->file_name))); 81 | this->file_name[sizeof(this->file_name)-1] = '\0'; 82 | }else{ 83 | this->file_name[0] = '\0'; 84 | } 85 | } 86 | 87 | char file_name[max_file_name_length+1]; 88 | }; 89 | 90 | struct with_file_line{ 91 | with_file_line(){ 92 | file_line = -1; 93 | } 94 | 95 | void set_file_line(int file_line){ 96 | this->file_line = file_line; 97 | } 98 | 99 | int file_line; 100 | }; 101 | 102 | struct with_errno{ 103 | with_errno(){ 104 | errno_value = 0; 105 | } 106 | 107 | void set_errno(int errno_value){ 108 | this->errno_value = errno_value; 109 | } 110 | 111 | int errno_value; 112 | }; 113 | 114 | struct can_not_open_file : 115 | base, 116 | with_file_name, 117 | with_errno{ 118 | void format_error_message()const override{ 119 | if(errno_value != 0) 120 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 121 | "Can not open file \"%s\" because \"%s\"." 122 | , file_name, std::strerror(errno_value)); 123 | else 124 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 125 | "Can not open file \"%s\"." 126 | , file_name); 127 | } 128 | }; 129 | 130 | struct line_length_limit_exceeded : 131 | base, 132 | with_file_name, 133 | with_file_line{ 134 | void format_error_message()const override{ 135 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 136 | "Line number %d in file \"%s\" exceeds the maximum length of 2^24-1." 137 | , file_line, file_name); 138 | } 139 | }; 140 | } 141 | 142 | class ByteSourceBase{ 143 | public: 144 | virtual int read(char*buffer, int size)=0; 145 | virtual ~ByteSourceBase(){} 146 | }; 147 | 148 | namespace detail{ 149 | 150 | class OwningStdIOByteSourceBase : public ByteSourceBase{ 151 | public: 152 | explicit OwningStdIOByteSourceBase(FILE*file):file(file){ 153 | // Tell the std library that we want to do the buffering ourself. 154 | std::setvbuf(file, 0, _IONBF, 0); 155 | } 156 | 157 | int read(char*buffer, int size){ 158 | return std::fread(buffer, 1, size, file); 159 | } 160 | 161 | ~OwningStdIOByteSourceBase(){ 162 | std::fclose(file); 163 | } 164 | 165 | private: 166 | FILE*file; 167 | }; 168 | 169 | class NonOwningIStreamByteSource : public ByteSourceBase{ 170 | public: 171 | explicit NonOwningIStreamByteSource(std::istream&in):in(in){} 172 | 173 | int read(char*buffer, int size){ 174 | in.read(buffer, size); 175 | return in.gcount(); 176 | } 177 | 178 | ~NonOwningIStreamByteSource(){} 179 | 180 | private: 181 | std::istream∈ 182 | }; 183 | 184 | class NonOwningStringByteSource : public ByteSourceBase{ 185 | public: 186 | NonOwningStringByteSource(const char*str, long long size):str(str), remaining_byte_count(size){} 187 | 188 | int read(char*buffer, int desired_byte_count){ 189 | int to_copy_byte_count = desired_byte_count; 190 | if(remaining_byte_count < to_copy_byte_count) 191 | to_copy_byte_count = remaining_byte_count; 192 | std::memcpy(buffer, str, to_copy_byte_count); 193 | remaining_byte_count -= to_copy_byte_count; 194 | str += to_copy_byte_count; 195 | return to_copy_byte_count; 196 | } 197 | 198 | ~NonOwningStringByteSource(){} 199 | 200 | private: 201 | const char*str; 202 | long long remaining_byte_count; 203 | }; 204 | 205 | #ifndef CSV_IO_NO_THREAD 206 | class AsynchronousReader{ 207 | public: 208 | void init(std::unique_ptrarg_byte_source){ 209 | std::unique_lockguard(lock); 210 | byte_source = std::move(arg_byte_source); 211 | desired_byte_count = -1; 212 | termination_requested = false; 213 | worker = std::thread( 214 | [&]{ 215 | std::unique_lockguard(lock); 216 | try{ 217 | for(;;){ 218 | read_requested_condition.wait( 219 | guard, 220 | [&]{ 221 | return desired_byte_count != -1 || termination_requested; 222 | } 223 | ); 224 | if(termination_requested) 225 | return; 226 | 227 | read_byte_count = byte_source->read(buffer, desired_byte_count); 228 | desired_byte_count = -1; 229 | if(read_byte_count == 0) 230 | break; 231 | read_finished_condition.notify_one(); 232 | } 233 | }catch(...){ 234 | read_error = std::current_exception(); 235 | } 236 | read_finished_condition.notify_one(); 237 | } 238 | ); 239 | } 240 | 241 | bool is_valid()const{ 242 | return byte_source != nullptr; 243 | } 244 | 245 | void start_read(char*arg_buffer, int arg_desired_byte_count){ 246 | std::unique_lockguard(lock); 247 | buffer = arg_buffer; 248 | desired_byte_count = arg_desired_byte_count; 249 | read_byte_count = -1; 250 | read_requested_condition.notify_one(); 251 | } 252 | 253 | int finish_read(){ 254 | std::unique_lockguard(lock); 255 | read_finished_condition.wait( 256 | guard, 257 | [&]{ 258 | return read_byte_count != -1 || read_error; 259 | } 260 | ); 261 | if(read_error) 262 | std::rethrow_exception(read_error); 263 | else 264 | return read_byte_count; 265 | } 266 | 267 | ~AsynchronousReader(){ 268 | if(byte_source != nullptr){ 269 | { 270 | std::unique_lockguard(lock); 271 | termination_requested = true; 272 | } 273 | read_requested_condition.notify_one(); 274 | worker.join(); 275 | } 276 | } 277 | 278 | private: 279 | std::unique_ptrbyte_source; 280 | 281 | std::thread worker; 282 | 283 | bool termination_requested; 284 | std::exception_ptr read_error; 285 | char*buffer; 286 | int desired_byte_count; 287 | int read_byte_count; 288 | 289 | std::mutex lock; 290 | std::condition_variable read_finished_condition; 291 | std::condition_variable read_requested_condition; 292 | }; 293 | #endif 294 | 295 | class SynchronousReader{ 296 | public: 297 | void init(std::unique_ptrarg_byte_source){ 298 | byte_source = std::move(arg_byte_source); 299 | } 300 | 301 | bool is_valid()const{ 302 | return byte_source != nullptr; 303 | } 304 | 305 | void start_read(char*arg_buffer, int arg_desired_byte_count){ 306 | buffer = arg_buffer; 307 | desired_byte_count = arg_desired_byte_count; 308 | } 309 | 310 | int finish_read(){ 311 | return byte_source->read(buffer, desired_byte_count); 312 | } 313 | private: 314 | std::unique_ptrbyte_source; 315 | char*buffer; 316 | int desired_byte_count; 317 | }; 318 | } 319 | 320 | class LineReader{ 321 | private: 322 | static const int block_len = 1<<20; 323 | std::unique_ptrbuffer; // must be constructed before (and thus destructed after) the reader! 324 | #ifdef CSV_IO_NO_THREAD 325 | detail::SynchronousReader reader; 326 | #else 327 | detail::AsynchronousReader reader; 328 | #endif 329 | int data_begin; 330 | int data_end; 331 | 332 | char file_name[error::max_file_name_length+1]; 333 | unsigned file_line; 334 | 335 | static std::unique_ptr open_file(const char*file_name){ 336 | // We open the file in binary mode as it makes no difference under *nix 337 | // and under Windows we handle \r\n newlines ourself. 338 | FILE*file = std::fopen(file_name, "rb"); 339 | if(file == 0){ 340 | int x = errno; // store errno as soon as possible, doing it after constructor call can fail. 341 | error::can_not_open_file err; 342 | err.set_errno(x); 343 | err.set_file_name(file_name); 344 | throw err; 345 | } 346 | return std::unique_ptr(new detail::OwningStdIOByteSourceBase(file)); 347 | } 348 | 349 | void init(std::unique_ptrbyte_source){ 350 | file_line = 0; 351 | 352 | buffer = std::unique_ptr(new char[3*block_len]); 353 | data_begin = 0; 354 | data_end = byte_source->read(buffer.get(), 2*block_len); 355 | 356 | // Ignore UTF-8 BOM 357 | if(data_end >= 3 && buffer[0] == '\xEF' && buffer[1] == '\xBB' && buffer[2] == '\xBF') 358 | data_begin = 3; 359 | 360 | if(data_end == 2*block_len){ 361 | reader.init(std::move(byte_source)); 362 | reader.start_read(buffer.get() + 2*block_len, block_len); 363 | } 364 | } 365 | 366 | public: 367 | LineReader() = delete; 368 | LineReader(const LineReader&) = delete; 369 | LineReader&operator=(const LineReader&) = delete; 370 | 371 | explicit LineReader(const char*file_name){ 372 | set_file_name(file_name); 373 | init(open_file(file_name)); 374 | } 375 | 376 | explicit LineReader(const std::string&file_name){ 377 | set_file_name(file_name.c_str()); 378 | init(open_file(file_name.c_str())); 379 | } 380 | 381 | LineReader(const char*file_name, std::unique_ptrbyte_source){ 382 | set_file_name(file_name); 383 | init(std::move(byte_source)); 384 | } 385 | 386 | LineReader(const std::string&file_name, std::unique_ptrbyte_source){ 387 | set_file_name(file_name.c_str()); 388 | init(std::move(byte_source)); 389 | } 390 | 391 | LineReader(const char*file_name, const char*data_begin, const char*data_end){ 392 | set_file_name(file_name); 393 | init(std::unique_ptr(new detail::NonOwningStringByteSource(data_begin, data_end-data_begin))); 394 | } 395 | 396 | LineReader(const std::string&file_name, const char*data_begin, const char*data_end){ 397 | set_file_name(file_name.c_str()); 398 | init(std::unique_ptr(new detail::NonOwningStringByteSource(data_begin, data_end-data_begin))); 399 | } 400 | 401 | LineReader(const char*file_name, FILE*file){ 402 | set_file_name(file_name); 403 | init(std::unique_ptr(new detail::OwningStdIOByteSourceBase(file))); 404 | } 405 | 406 | LineReader(const std::string&file_name, FILE*file){ 407 | set_file_name(file_name.c_str()); 408 | init(std::unique_ptr(new detail::OwningStdIOByteSourceBase(file))); 409 | } 410 | 411 | LineReader(const char*file_name, std::istream&in){ 412 | set_file_name(file_name); 413 | init(std::unique_ptr(new detail::NonOwningIStreamByteSource(in))); 414 | } 415 | 416 | LineReader(const std::string&file_name, std::istream&in){ 417 | set_file_name(file_name.c_str()); 418 | init(std::unique_ptr(new detail::NonOwningIStreamByteSource(in))); 419 | } 420 | 421 | void set_file_name(const std::string&file_name){ 422 | set_file_name(file_name.c_str()); 423 | } 424 | 425 | void set_file_name(const char*file_name){ 426 | if(file_name != nullptr){ 427 | strncpy(this->file_name, file_name, sizeof(this->file_name)); 428 | this->file_name[sizeof(this->file_name)-1] = '\0'; 429 | }else{ 430 | this->file_name[0] = '\0'; 431 | } 432 | } 433 | 434 | const char*get_truncated_file_name()const{ 435 | return file_name; 436 | } 437 | 438 | void set_file_line(unsigned file_line){ 439 | this->file_line = file_line; 440 | } 441 | 442 | unsigned get_file_line()const{ 443 | return file_line; 444 | } 445 | 446 | char*next_line(){ 447 | if(data_begin == data_end) 448 | return nullptr; 449 | 450 | ++file_line; 451 | 452 | assert(data_begin < data_end); 453 | assert(data_end <= block_len*2); 454 | 455 | if(data_begin >= block_len){ 456 | std::memcpy(buffer.get(), buffer.get()+block_len, block_len); 457 | data_begin -= block_len; 458 | data_end -= block_len; 459 | if(reader.is_valid()) 460 | { 461 | data_end += reader.finish_read(); 462 | std::memcpy(buffer.get()+block_len, buffer.get()+2*block_len, block_len); 463 | reader.start_read(buffer.get() + 2*block_len, block_len); 464 | } 465 | } 466 | 467 | int line_end = data_begin; 468 | while(buffer[line_end] != '\n' && line_end != data_end){ 469 | ++line_end; 470 | } 471 | 472 | if(line_end - data_begin + 1 > block_len){ 473 | error::line_length_limit_exceeded err; 474 | err.set_file_name(file_name); 475 | err.set_file_line(file_line); 476 | throw err; 477 | } 478 | 479 | if(buffer[line_end] == '\n' && line_end != data_end){ 480 | buffer[line_end] = '\0'; 481 | }else{ 482 | // some files are missing the newline at the end of the 483 | // last line 484 | ++data_end; 485 | buffer[line_end] = '\0'; 486 | } 487 | 488 | // handle windows \r\n-line breaks 489 | if(line_end != data_begin && buffer[line_end-1] == '\r') 490 | buffer[line_end-1] = '\0'; 491 | 492 | char*ret = buffer.get() + data_begin; 493 | data_begin = line_end+1; 494 | return ret; 495 | } 496 | }; 497 | 498 | 499 | //////////////////////////////////////////////////////////////////////////// 500 | // CSV // 501 | //////////////////////////////////////////////////////////////////////////// 502 | 503 | namespace error{ 504 | const int max_column_name_length = 63; 505 | struct with_column_name{ 506 | with_column_name(){ 507 | std::memset(column_name, 0, max_column_name_length+1); 508 | } 509 | 510 | void set_column_name(const char*column_name){ 511 | if(column_name != nullptr){ 512 | std::strncpy(this->column_name, column_name, max_column_name_length); 513 | this->column_name[max_column_name_length] = '\0'; 514 | }else{ 515 | this->column_name[0] = '\0'; 516 | } 517 | } 518 | 519 | char column_name[max_column_name_length+1]; 520 | }; 521 | 522 | 523 | const int max_column_content_length = 63; 524 | 525 | struct with_column_content{ 526 | with_column_content(){ 527 | std::memset(column_content, 0, max_column_content_length+1); 528 | } 529 | 530 | void set_column_content(const char*column_content){ 531 | if(column_content != nullptr){ 532 | std::strncpy(this->column_content, column_content, max_column_content_length); 533 | this->column_content[max_column_content_length] = '\0'; 534 | }else{ 535 | this->column_content[0] = '\0'; 536 | } 537 | } 538 | 539 | char column_content[max_column_content_length+1]; 540 | }; 541 | 542 | 543 | struct extra_column_in_header : 544 | base, 545 | with_file_name, 546 | with_column_name{ 547 | void format_error_message()const override{ 548 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 549 | R"(Extra column "%s" in header of file "%s".)" 550 | , column_name, file_name); 551 | } 552 | }; 553 | 554 | struct missing_column_in_header : 555 | base, 556 | with_file_name, 557 | with_column_name{ 558 | void format_error_message()const override{ 559 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 560 | R"(Missing column "%s" in header of file "%s".)" 561 | , column_name, file_name); 562 | } 563 | }; 564 | 565 | struct duplicated_column_in_header : 566 | base, 567 | with_file_name, 568 | with_column_name{ 569 | void format_error_message()const override{ 570 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 571 | R"(Duplicated column "%s" in header of file "%s".)" 572 | , column_name, file_name); 573 | } 574 | }; 575 | 576 | struct header_missing : 577 | base, 578 | with_file_name{ 579 | void format_error_message()const override{ 580 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 581 | "Header missing in file \"%s\"." 582 | , file_name); 583 | } 584 | }; 585 | 586 | struct too_few_columns : 587 | base, 588 | with_file_name, 589 | with_file_line{ 590 | void format_error_message()const override{ 591 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 592 | "Too few columns in line %d in file \"%s\"." 593 | , file_line, file_name); 594 | } 595 | }; 596 | 597 | struct too_many_columns : 598 | base, 599 | with_file_name, 600 | with_file_line{ 601 | void format_error_message()const override{ 602 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 603 | "Too many columns in line %d in file \"%s\"." 604 | , file_line, file_name); 605 | } 606 | }; 607 | 608 | struct escaped_string_not_closed : 609 | base, 610 | with_file_name, 611 | with_file_line{ 612 | void format_error_message()const override{ 613 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 614 | "Escaped string was not closed in line %d in file \"%s\"." 615 | , file_line, file_name); 616 | } 617 | }; 618 | 619 | struct integer_must_be_positive : 620 | base, 621 | with_file_name, 622 | with_file_line, 623 | with_column_name, 624 | with_column_content{ 625 | void format_error_message()const override{ 626 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 627 | R"(The integer "%s" must be positive or 0 in column "%s" in file "%s" in line "%d".)" 628 | , column_content, column_name, file_name, file_line); 629 | } 630 | }; 631 | 632 | struct no_digit : 633 | base, 634 | with_file_name, 635 | with_file_line, 636 | with_column_name, 637 | with_column_content{ 638 | void format_error_message()const override{ 639 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 640 | R"(The integer "%s" contains an invalid digit in column "%s" in file "%s" in line "%d".)" 641 | , column_content, column_name, file_name, file_line); 642 | } 643 | }; 644 | 645 | struct integer_overflow : 646 | base, 647 | with_file_name, 648 | with_file_line, 649 | with_column_name, 650 | with_column_content{ 651 | void format_error_message()const override{ 652 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 653 | R"(The integer "%s" overflows in column "%s" in file "%s" in line "%d".)" 654 | , column_content, column_name, file_name, file_line); 655 | } 656 | }; 657 | 658 | struct integer_underflow : 659 | base, 660 | with_file_name, 661 | with_file_line, 662 | with_column_name, 663 | with_column_content{ 664 | void format_error_message()const override{ 665 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 666 | R"(The integer "%s" underflows in column "%s" in file "%s" in line "%d".)" 667 | , column_content, column_name, file_name, file_line); 668 | } 669 | }; 670 | 671 | struct invalid_single_character : 672 | base, 673 | with_file_name, 674 | with_file_line, 675 | with_column_name, 676 | with_column_content{ 677 | void format_error_message()const override{ 678 | std::snprintf(error_message_buffer, sizeof(error_message_buffer), 679 | R"(The content "%s" of column "%s" in file "%s" in line "%d" is not a single character.)" 680 | , column_content, column_name, file_name, file_line); 681 | } 682 | }; 683 | } 684 | 685 | using ignore_column = unsigned int; 686 | static const ignore_column ignore_no_column = 0; 687 | static const ignore_column ignore_extra_column = 1; 688 | static const ignore_column ignore_missing_column = 2; 689 | 690 | template 691 | struct trim_chars{ 692 | private: 693 | constexpr static bool is_trim_char(char){ 694 | return false; 695 | } 696 | 697 | template 698 | constexpr static bool is_trim_char(char c, char trim_char, OtherTrimChars...other_trim_chars){ 699 | return c == trim_char || is_trim_char(c, other_trim_chars...); 700 | } 701 | 702 | public: 703 | static void trim(char*&str_begin, char*&str_end){ 704 | while(str_begin != str_end && is_trim_char(*str_begin, trim_char_list...)) 705 | ++str_begin; 706 | while(str_begin != str_end && is_trim_char(*(str_end-1), trim_char_list...)) 707 | --str_end; 708 | *str_end = '\0'; 709 | } 710 | }; 711 | 712 | 713 | struct no_comment{ 714 | static bool is_comment(const char*){ 715 | return false; 716 | } 717 | }; 718 | 719 | template 720 | struct single_line_comment{ 721 | private: 722 | constexpr static bool is_comment_start_char(char){ 723 | return false; 724 | } 725 | 726 | template 727 | constexpr static bool is_comment_start_char(char c, char comment_start_char, OtherCommentStartChars...other_comment_start_chars){ 728 | return c == comment_start_char || is_comment_start_char(c, other_comment_start_chars...); 729 | } 730 | 731 | public: 732 | 733 | static bool is_comment(const char*line){ 734 | return is_comment_start_char(*line, comment_start_char_list...); 735 | } 736 | }; 737 | 738 | struct empty_line_comment{ 739 | static bool is_comment(const char*line){ 740 | if(*line == '\0') 741 | return true; 742 | while(*line == ' ' || *line == '\t'){ 743 | ++line; 744 | if(*line == 0) 745 | return true; 746 | } 747 | return false; 748 | } 749 | }; 750 | 751 | template 752 | struct single_and_empty_line_comment{ 753 | static bool is_comment(const char*line){ 754 | return single_line_comment::is_comment(line) || empty_line_comment::is_comment(line); 755 | } 756 | }; 757 | 758 | template 759 | struct no_quote_escape{ 760 | static const char*find_next_column_end(const char*col_begin){ 761 | while(*col_begin != sep && *col_begin != '\0') 762 | ++col_begin; 763 | return col_begin; 764 | } 765 | 766 | static void unescape(char*&, char*&){ 767 | 768 | } 769 | }; 770 | 771 | template 772 | struct double_quote_escape{ 773 | static const char*find_next_column_end(const char*col_begin){ 774 | while(*col_begin != sep && *col_begin != '\0') 775 | if(*col_begin != quote) 776 | ++col_begin; 777 | else{ 778 | do{ 779 | ++col_begin; 780 | while(*col_begin != quote){ 781 | if(*col_begin == '\0') 782 | throw error::escaped_string_not_closed(); 783 | ++col_begin; 784 | } 785 | ++col_begin; 786 | }while(*col_begin == quote); 787 | } 788 | return col_begin; 789 | } 790 | 791 | static void unescape(char*&col_begin, char*&col_end){ 792 | if(col_end - col_begin >= 2){ 793 | if(*col_begin == quote && *(col_end-1) == quote){ 794 | ++col_begin; 795 | --col_end; 796 | char*out = col_begin; 797 | for(char*in = col_begin; in!=col_end; ++in){ 798 | if(*in == quote && (in+1) != col_end && *(in+1) == quote){ 799 | ++in; 800 | } 801 | *out = *in; 802 | ++out; 803 | } 804 | col_end = out; 805 | *col_end = '\0'; 806 | } 807 | } 808 | 809 | } 810 | }; 811 | 812 | struct throw_on_overflow{ 813 | template 814 | static void on_overflow(T&){ 815 | throw error::integer_overflow(); 816 | } 817 | 818 | template 819 | static void on_underflow(T&){ 820 | throw error::integer_underflow(); 821 | } 822 | }; 823 | 824 | struct ignore_overflow{ 825 | template 826 | static void on_overflow(T&){} 827 | 828 | template 829 | static void on_underflow(T&){} 830 | }; 831 | 832 | struct set_to_max_on_overflow{ 833 | template 834 | static void on_overflow(T&x){ 835 | // using (std::numeric_limits::max) instead of std::numeric_limits::max 836 | // to make code including windows.h with its max macro happy 837 | x = (std::numeric_limits::max)(); 838 | } 839 | 840 | template 841 | static void on_underflow(T&x){ 842 | x = (std::numeric_limits::min)(); 843 | } 844 | }; 845 | 846 | 847 | namespace detail{ 848 | template 849 | void chop_next_column( 850 | char*&line, char*&col_begin, char*&col_end 851 | ){ 852 | assert(line != nullptr); 853 | 854 | col_begin = line; 855 | // the col_begin + (... - col_begin) removes the constness 856 | col_end = col_begin + (quote_policy::find_next_column_end(col_begin) - col_begin); 857 | 858 | if(*col_end == '\0'){ 859 | line = nullptr; 860 | }else{ 861 | *col_end = '\0'; 862 | line = col_end + 1; 863 | } 864 | } 865 | 866 | template 867 | void parse_line( 868 | char*line, 869 | char**sorted_col, 870 | const std::vector&col_order 871 | ){ 872 | for (int i : col_order) { 873 | if(line == nullptr) 874 | throw ::io::error::too_few_columns(); 875 | char*col_begin, *col_end; 876 | chop_next_column(line, col_begin, col_end); 877 | 878 | if (i != -1) { 879 | trim_policy::trim(col_begin, col_end); 880 | quote_policy::unescape(col_begin, col_end); 881 | 882 | sorted_col[i] = col_begin; 883 | } 884 | } 885 | if(line != nullptr) 886 | throw ::io::error::too_many_columns(); 887 | } 888 | 889 | template 890 | void parse_header_line( 891 | char*line, 892 | std::vector&col_order, 893 | const std::string*col_name, 894 | ignore_column ignore_policy 895 | ){ 896 | col_order.clear(); 897 | 898 | bool found[column_count]; 899 | std::fill(found, found + column_count, false); 900 | while(line){ 901 | char*col_begin,*col_end; 902 | chop_next_column(line, col_begin, col_end); 903 | 904 | trim_policy::trim(col_begin, col_end); 905 | quote_policy::unescape(col_begin, col_end); 906 | 907 | for(unsigned i=0; i 941 | void parse(char*col, char &x){ 942 | if(!*col) 943 | throw error::invalid_single_character(); 944 | x = *col; 945 | ++col; 946 | if(*col) 947 | throw error::invalid_single_character(); 948 | } 949 | 950 | template 951 | void parse(char*col, std::string&x){ 952 | x = col; 953 | } 954 | 955 | template 956 | void parse(char*col, const char*&x){ 957 | x = col; 958 | } 959 | 960 | template 961 | void parse(char*col, char*&x){ 962 | x = col; 963 | } 964 | 965 | template 966 | void parse_unsigned_integer(const char*col, T&x){ 967 | x = 0; 968 | while(*col != '\0'){ 969 | if('0' <= *col && *col <= '9'){ 970 | T y = *col - '0'; 971 | if(x > ((std::numeric_limits::max)()-y)/10){ 972 | overflow_policy::on_overflow(x); 973 | return; 974 | } 975 | x = 10*x+y; 976 | }else 977 | throw error::no_digit(); 978 | ++col; 979 | } 980 | } 981 | 982 | templatevoid parse(char*col, unsigned char &x) 983 | {parse_unsigned_integer(col, x);} 984 | templatevoid parse(char*col, unsigned short &x) 985 | {parse_unsigned_integer(col, x);} 986 | templatevoid parse(char*col, unsigned int &x) 987 | {parse_unsigned_integer(col, x);} 988 | templatevoid parse(char*col, unsigned long &x) 989 | {parse_unsigned_integer(col, x);} 990 | templatevoid parse(char*col, unsigned long long &x) 991 | {parse_unsigned_integer(col, x);} 992 | 993 | template 994 | void parse_signed_integer(const char*col, T&x){ 995 | if(*col == '-'){ 996 | ++col; 997 | 998 | x = 0; 999 | while(*col != '\0'){ 1000 | if('0' <= *col && *col <= '9'){ 1001 | T y = *col - '0'; 1002 | if(x < ((std::numeric_limits::min)()+y)/10){ 1003 | overflow_policy::on_underflow(x); 1004 | return; 1005 | } 1006 | x = 10*x-y; 1007 | }else 1008 | throw error::no_digit(); 1009 | ++col; 1010 | } 1011 | return; 1012 | }else if(*col == '+') 1013 | ++col; 1014 | parse_unsigned_integer(col, x); 1015 | } 1016 | 1017 | templatevoid parse(char*col, signed char &x) 1018 | {parse_signed_integer(col, x);} 1019 | templatevoid parse(char*col, signed short &x) 1020 | {parse_signed_integer(col, x);} 1021 | templatevoid parse(char*col, signed int &x) 1022 | {parse_signed_integer(col, x);} 1023 | templatevoid parse(char*col, signed long &x) 1024 | {parse_signed_integer(col, x);} 1025 | templatevoid parse(char*col, signed long long &x) 1026 | {parse_signed_integer(col, x);} 1027 | 1028 | template 1029 | void parse_float(const char*col, T&x){ 1030 | bool is_neg = false; 1031 | if(*col == '-'){ 1032 | is_neg = true; 1033 | ++col; 1034 | }else if(*col == '+') 1035 | ++col; 1036 | 1037 | x = 0; 1038 | while('0' <= *col && *col <= '9'){ 1039 | int y = *col - '0'; 1040 | x *= 10; 1041 | x += y; 1042 | ++col; 1043 | } 1044 | 1045 | if(*col == '.'|| *col == ','){ 1046 | ++col; 1047 | T pos = 1; 1048 | while('0' <= *col && *col <= '9'){ 1049 | pos /= 10; 1050 | int y = *col - '0'; 1051 | ++col; 1052 | x += y*pos; 1053 | } 1054 | } 1055 | 1056 | if(*col == 'e' || *col == 'E'){ 1057 | ++col; 1058 | int e; 1059 | 1060 | parse_signed_integer(col, e); 1061 | 1062 | if(e != 0){ 1063 | T base; 1064 | if(e < 0){ 1065 | base = T(0.1); 1066 | e = -e; 1067 | }else{ 1068 | base = T(10); 1069 | } 1070 | 1071 | while(e != 1){ 1072 | if((e & 1) == 0){ 1073 | base = base*base; 1074 | e >>= 1; 1075 | }else{ 1076 | x *= base; 1077 | --e; 1078 | } 1079 | } 1080 | x *= base; 1081 | } 1082 | }else{ 1083 | if(*col != '\0') 1084 | throw error::no_digit(); 1085 | } 1086 | 1087 | if(is_neg) 1088 | x = -x; 1089 | } 1090 | 1091 | template void parse(char*col, float&x) { parse_float(col, x); } 1092 | template void parse(char*col, double&x) { parse_float(col, x); } 1093 | template void parse(char*col, long double&x) { parse_float(col, x); } 1094 | 1095 | template 1096 | void parse(char*col, T&x){ 1097 | // Mute unused variable compiler warning 1098 | (void)col; 1099 | (void)x; 1100 | // GCC evalutes "false" when reading the template and 1101 | // "sizeof(T)!=sizeof(T)" only when instantiating it. This is why 1102 | // this strange construct is used. 1103 | static_assert(sizeof(T)!=sizeof(T), 1104 | "Can not parse this type. Only buildin integrals, floats, char, char*, const char* and std::string are supported"); 1105 | } 1106 | 1107 | } 1108 | 1109 | template, 1111 | class quote_policy = no_quote_escape<','>, 1112 | class overflow_policy = throw_on_overflow, 1113 | class comment_policy = no_comment 1114 | > 1115 | class CSVReader{ 1116 | private: 1117 | LineReader in; 1118 | 1119 | char*row[column_count]; 1120 | std::string column_names[column_count]; 1121 | 1122 | std::vectorcol_order; 1123 | 1124 | template 1125 | void set_column_names(std::string s, ColNames...cols){ 1126 | column_names[column_count-sizeof...(ColNames)-1] = std::move(s); 1127 | set_column_names(std::forward(cols)...); 1128 | } 1129 | 1130 | void set_column_names(){} 1131 | 1132 | 1133 | public: 1134 | CSVReader() = delete; 1135 | CSVReader(const CSVReader&) = delete; 1136 | CSVReader&operator=(const CSVReader&); 1137 | 1138 | template 1139 | explicit CSVReader(Args&&...args):in(std::forward(args)...){ 1140 | std::fill(row, row+column_count, nullptr); 1141 | col_order.resize(column_count); 1142 | for(unsigned i=0; i 1153 | void read_header(ignore_column ignore_policy, ColNames...cols){ 1154 | static_assert(sizeof...(ColNames)>=column_count, "not enough column names specified"); 1155 | static_assert(sizeof...(ColNames)<=column_count, "too many column names specified"); 1156 | try{ 1157 | set_column_names(std::forward(cols)...); 1158 | 1159 | char*line; 1160 | do{ 1161 | line = in.next_line(); 1162 | if(!line) 1163 | throw error::header_missing(); 1164 | }while(comment_policy::is_comment(line)); 1165 | 1166 | detail::parse_header_line 1167 | 1168 | (line, col_order, column_names, ignore_policy); 1169 | }catch(error::with_file_name&err){ 1170 | err.set_file_name(in.get_truncated_file_name()); 1171 | throw; 1172 | } 1173 | } 1174 | 1175 | template 1176 | void set_header(ColNames...cols){ 1177 | static_assert(sizeof...(ColNames)>=column_count, 1178 | "not enough column names specified"); 1179 | static_assert(sizeof...(ColNames)<=column_count, 1180 | "too many column names specified"); 1181 | set_column_names(std::forward(cols)...); 1182 | std::fill(row, row+column_count, nullptr); 1183 | col_order.resize(column_count); 1184 | for(unsigned i=0; i 1219 | void parse_helper(std::size_t r, T&t, ColType&...cols){ 1220 | if(row[r]){ 1221 | try{ 1222 | try{ 1223 | ::io::detail::parse(row[r], t); 1224 | }catch(error::with_column_content&err){ 1225 | err.set_column_content(row[r]); 1226 | throw; 1227 | } 1228 | }catch(error::with_column_name&err){ 1229 | err.set_column_name(column_names[r].c_str()); 1230 | throw; 1231 | } 1232 | } 1233 | parse_helper(r+1, cols...); 1234 | } 1235 | 1236 | 1237 | public: 1238 | template 1239 | bool read_row(ColType& ...cols){ 1240 | static_assert(sizeof...(ColType)>=column_count, 1241 | "not enough columns specified"); 1242 | static_assert(sizeof...(ColType)<=column_count, 1243 | "too many columns specified"); 1244 | try{ 1245 | try{ 1246 | 1247 | char*line; 1248 | do{ 1249 | line = in.next_line(); 1250 | if(!line) 1251 | return false; 1252 | }while(comment_policy::is_comment(line)); 1253 | 1254 | detail::parse_line 1255 | (line, row, col_order); 1256 | 1257 | parse_helper(0, cols...); 1258 | }catch(error::with_file_name&err){ 1259 | err.set_file_name(in.get_truncated_file_name()); 1260 | throw; 1261 | } 1262 | }catch(error::with_file_line&err){ 1263 | err.set_file_line(in.get_file_line()); 1264 | throw; 1265 | } 1266 | 1267 | return true; 1268 | } 1269 | }; 1270 | } 1271 | #endif 1272 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @brief Entry-point for dynamic notch filter example 4 | * @author Parker Lusk 5 | * @date 8 Dec 2020 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "csv.h" 22 | 23 | static constexpr int DATUMS = 4; // number of columns to be extracted from CSV 24 | using Data = Eigen::Matrix; 25 | 26 | void usage(int argc, char const *argv[]) 27 | { 28 | std::cout << argv[0] << " [axis] [plot]" << std::endl << std::endl; 29 | std::cout << "\tRun adaptive notching algorithm on gyro data stored in CSV."; 30 | std::cout << std::endl << "\t"; 31 | std::cout << "CSV file expected to have been generated from sfpro or to be "; 32 | std::cout << std::endl << "\t"; 33 | std::cout << "in same format. A gyro axis to analyze may be specified as"; 34 | std::cout << std::endl << "\t"; 35 | std::cout << "(1, 2, 3) which corresponds to axes (x, y, z)."; 36 | std::cout << std::endl << std::endl; 37 | std::cout << "\tIf a 3rd argument is specified (e.g., '1'), an ASCII plot"; 38 | std::cout << std::endl << "\t"; 39 | std::cout << "of the pre- and post-spectrum is shown in the terminal."; 40 | std::cout << std::endl << std::endl; 41 | } 42 | 43 | // ---------------------------------------------------------------------------- 44 | 45 | Data parseCSV(const std::string& file) 46 | { 47 | 48 | // count number of entries (estimate) 49 | std::ifstream ifile(file); 50 | const int N = std::count(std::istreambuf_iterator(ifile), 51 | std::istreambuf_iterator(), '\n'); 52 | ifile.close(); 53 | 54 | io::CSVReader in(file); 55 | in.next_line(); // ignore "dsp clock" message 56 | 57 | // we only care about these four (DATUMS) columns 58 | in.read_header(io::ignore_extra_column, 59 | "timestamp(us)", "ang_x", "ang_y", "ang_z"); 60 | 61 | Data D = Data::Zero(N, DATUMS); 62 | int i = 0; 63 | int time_us; 64 | double wx, wy, wz; 65 | while (in.read_row(time_us, wx, wy, wz)) { 66 | D.row(i++) << time_us*1e-6, wx, wy, wz; 67 | } 68 | 69 | // resize to however many valid entries there were 70 | D.conservativeResize(i, DATUMS); 71 | 72 | return D; 73 | } 74 | 75 | // ---------------------------------------------------------------------------- 76 | 77 | void plotResults(const adaptnotch::AdaptiveNotch& filter, 78 | const Eigen::VectorXd& gyro, const Eigen::VectorXd& gyrof, int n) 79 | { 80 | static const int N = filter.params().NFFT; 81 | static Eigen::FFT fft; 82 | static plot::TerminalInfo term; 83 | static bool init = false; 84 | if (!init) { 85 | term.detect(); 86 | fft.SetFlag(Eigen::FFT::Unscaled); 87 | fft.SetFlag(Eigen::FFT::HalfSpectrum); 88 | init = true; 89 | } 90 | constexpr float ymax = 0.2f; // arbitrary FFT mag scaling 91 | static plot::RealCanvas prefilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term); 92 | static plot::RealCanvas postfilter({ { 0.0f, ymax }, { N/2.0f, 0.0f } }, plot::Size(60, 10), term); 93 | 94 | // Build block layout 95 | auto layout = 96 | plot::alignment( 97 | { term.size().x, 0 }, 98 | plot::margin( 99 | plot::vbox( 100 | plot::frame(u8"Original Spectrum", plot::Align::Center, &prefilter), 101 | plot::frame(u8"Filtered Spectrum", plot::Align::Center, &postfilter)))); 102 | 103 | // pre-filter spectrum 104 | const Eigen::VectorXd Y = filter.spectrum() / N; 105 | 106 | // select N most recent filtered measurements 107 | const size_t s = (n-N<0) ? 0 : n-N; 108 | const Eigen::VectorXd yf = gyrof.segment(s,N); 109 | 110 | // post-filter spectrum 111 | Eigen::VectorXcd Yfc; 112 | fft.fwd(Yfc, yf); 113 | const Eigen::VectorXd Yf = Yfc.array().abs() / N; 114 | 115 | // Plot spectrum pre-filtering 116 | prefilter.clear(); 117 | for (size_t i=1; i(i-1), static_cast(Y(i-1))}, 119 | {static_cast(i), static_cast(Y(i))}}); 120 | } 121 | 122 | // Plot spectrum post-filtering 123 | postfilter.clear(); 124 | for (size_t i=1; i(i-1), static_cast(Yf(i-1))}, 126 | {static_cast(i), static_cast(Yf(i))}}); 127 | } 128 | 129 | for (auto const& line: layout) 130 | std::cout << term.clear_line() << line << std::endl; 131 | std::cout << term.move_up(layout.size().y) << std::flush; 132 | } 133 | 134 | // ---------------------------------------------------------------------------- 135 | 136 | int main(int argc, char const *argv[]) 137 | { 138 | 139 | int axis = 1; ///< x, y, or z axis of gyro to analyze 140 | std::string file; ///< input data from IMU 141 | bool shouldPlot = false; ///< show FFT plots in terminal 142 | 143 | if (argc < 2) { 144 | std::cerr << "Not enough input arguments." << std::endl << std::endl; 145 | usage(argc, argv); 146 | return -1; 147 | } else if (argc >= 2) { 148 | file = std::string(argv[1]); 149 | } 150 | 151 | if (argc >= 3) { 152 | axis = std::stoi(argv[2]); 153 | if (axis < 1 || axis > 3) axis = 1; 154 | } 155 | 156 | if (argc >= 4) shouldPlot = true; 157 | 158 | // 159 | // Process raw IMU data 160 | // 161 | 162 | Data D = parseCSV(file); 163 | 164 | const Eigen::VectorXd diff = D.col(0).bottomRows(D.rows()-1) - D.col(0).topRows(D.rows()-1); 165 | const double Ts = diff.mean(); 166 | const double Fs = 1./Ts; 167 | const int N = D.rows(); 168 | 169 | // 170 | // Adaptive notch filter setup 171 | // 172 | 173 | adaptnotch::AdaptiveNotch::Params params; 174 | adaptnotch::AdaptiveNotch filter(params); 175 | 176 | // 177 | // Main loop - simulated gyro sampling 178 | // 179 | 180 | Eigen::VectorXd gyrof = Eigen::VectorXd::Zero(N); 181 | const auto start = std::chrono::steady_clock::now(); 182 | 183 | 184 | for (size_t n=0; n(end - start).count() * 1e-6; 197 | 198 | const double timu = D(N-1,0) - D(0,0); 199 | 200 | std::cout << "Processed " << timu << " seconds (" << N << " samples) of IMU"; 201 | std::cout << " data in " << duration << " seconds" << std::endl; 202 | std::cout << "Real-time factor: " << timu / duration << std::endl; 203 | std::cout << "Estimated peak freq: " << filter.peakFreq() << std::endl; 204 | 205 | // 206 | // Write data to file 207 | // 208 | 209 | Eigen::MatrixXd out(N, 3); 210 | out << Eigen::VectorXd::LinSpaced(N, 0, N*Ts), D.col(axis), gyrof; 211 | 212 | std::ofstream of("data_processed.txt"); 213 | of << out << std::endl; 214 | of.close(); 215 | 216 | return 0; 217 | } 218 | --------------------------------------------------------------------------------