Filter: examples: add SlewLimiter example to test functionality

libraries/Filter/examples/SlewLimiter/SlewLimiter.cpp

@@ -0,0 +1,75 @@
+// Example to test functionality of SlewLimiter
+
+#include <AP_HAL/AP_HAL.h>
+#include <Filter/SlewLimiter.h>
+#include <AP_Math/chirp.h>
+
+/* on Linux run with
+    ./waf configure --board linux
+    ./waf --targets examples/SlewLimiter
+    ./build/linux/examples/SlewLimiter
+*/
+
+void loop();
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+const float slew_rate_max = 50.0;
+const float slew_rate_tau = 1.0;
+
+// 400 hz loop
+const uint16_t dt_us = 2500;
+const float dt_s = dt_us * 1.0e-6;
+
+// Slew limiter object to be tested
+SlewLimiter slew_limiter{slew_rate_max, slew_rate_tau};
+
+// Chirp object to generate test signal
+Chirp chirp;
+
+// Chirp setup params
+const float magnitude = 5.0;
+const float duration = 60.0;
+const float frequency_start = 5.0;
+const float frequency_stop = 100;
+const float time_fade_in = 15;
+const float time_fade_out = 10;
+const float time_const_freq = 2.0 / frequency_start;
+
+uint64_t waveform_time_us = 0;
+
+static void setup()
+{
+    hal.console->printf("SlewLimiter - rate max: %f, tau: %f, loop rate: %fHz\n", slew_rate_max, slew_rate_tau, 1 / dt_s);
+
+    chirp.init(duration, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
+    hal.console->printf("Chirp - duration: %fs, magnitude: %f, start: %fHz, stop: %fHz, fade in: %fs, fade out:%fs, hold: %fs\n", duration, magnitude, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
+
+    hal.console->printf("Time (s), input, slew rate, mod\n");
+
+    hal.scheduler->stop_clock(waveform_time_us);
+
+}
+
+void loop()
+{
+    const float waveform_time_s = waveform_time_us * 1.0e-6;
+
+    const float input = chirp.update(waveform_time_s, magnitude);
+
+    const float mod = slew_limiter.modifier(input, dt_s);
+
+    const float slew_rate = slew_limiter.get_slew_rate();
+
+    hal.console->printf("%f, %f, %f, %f\n", waveform_time_s, input, slew_rate, mod);
+
+    if (chirp.completed()) {
+        exit(1);
+    }
+
+    // Force clock times so we can run faster than real time
+    waveform_time_us += dt_us;
+    hal.scheduler->stop_clock(waveform_time_us);
+}
+
+AP_HAL_MAIN();

libraries/Filter/examples/SlewLimiter/SlewLimiterPlot.m

@@ -0,0 +1,43 @@
+close all
+clear
+clc
+
+% Script for plotting outputs of SlewLimiter examples
+
+files = dir('*.csv');
+for i = numel(files):-1:1
+    data = readmatrix(files(i).name);
+
+    run{i}.time = data(:,1);
+    run{i}.input = data(:,2);
+    run{i}.slew = data(:,3);
+    run{i}.mod = data(:,4);
+
+    legend_val{i} = strrep(files(i).name, '_' ,'\_');
+end
+
+figure
+tiledlayout(3,1)
+
+nexttile
+hold all
+for i = 1:numel(run)
+    plot(run{i}.time, run{i}.input)
+end
+ylabel('Input')
+
+nexttile
+hold all
+for i = 1:numel(run)
+    plot(run{i}.time, run{i}.slew)
+end
+legend(legend_val,'Location','eastoutside')
+ylabel('Slew rate')
+
+nexttile
+hold all
+for i = 1:numel(run)
+    plot(run{i}.time, run{i}.mod)
+end
+xlabel('Time (s)')
+ylabel('mod')

libraries/Filter/examples/SlewLimiter/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_example(
+        use='ap',
+    )