130 lines
3 KiB
C++
130 lines
3 KiB
C++
#include <fstream>
|
|
#include <iostream>
|
|
|
|
#include "utils.hpp"
|
|
#include "FlyByWire.hpp"
|
|
|
|
using std::cout;
|
|
using std::endl;
|
|
|
|
using FlyByWire::Real;
|
|
using FlyByWire::Complex;
|
|
|
|
void test_Filter1_step_response();
|
|
void test_Filter2_random_noise();
|
|
void test_PID_simple_system();
|
|
|
|
int main()
|
|
{
|
|
cout.precision(16);
|
|
|
|
// test_Filter1_step_response();
|
|
// test_Filter2_random_noise();
|
|
test_PID_simple_system();
|
|
|
|
return 0;
|
|
}
|
|
|
|
void test_Filter1_step_response()
|
|
{
|
|
// step response of simple 1st order filter
|
|
double tend = 10.,
|
|
dt = 0.1,
|
|
t, y = 0.;
|
|
FlyByWire::Filter1 filt1(1, 2, 3, 4, dt, y);
|
|
|
|
for(int i = 0 ; i <= (tend/dt) ; i++)
|
|
{
|
|
t = i*dt;
|
|
cout << t << " " << y << "\n";
|
|
y = filt1.Filter(1.);
|
|
}
|
|
}
|
|
|
|
void test_Filter2_random_noise()
|
|
{
|
|
// Generate some uniform noise and filter it to see if the filter class works as expected
|
|
uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};
|
|
|
|
double tend = 10.,
|
|
dt = 0.025,
|
|
omega = 2*M_PI*10,
|
|
Q = 20.,
|
|
t, y = 0., y2;
|
|
FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);
|
|
|
|
std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
|
|
|
|
if(!out.is_open())
|
|
{
|
|
std::cerr << "Could not open the file in writing mode.\n";
|
|
return;
|
|
}
|
|
|
|
out << "a = [";
|
|
for(int i = 0 ; i <= (tend/dt) ; i++)
|
|
{
|
|
t = i*dt;
|
|
y = RAND_XORWOW_A_B(state, -1., 1.);
|
|
y2 = filt1.Filter(y);
|
|
out << t << " " << y << " " << y2 << "\n";
|
|
}
|
|
out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
|
|
}
|
|
|
|
void test_PID_simple_system()
|
|
{
|
|
// Simple mass system with gravity g and perturbation w
|
|
// xdotdot = -g + u + w
|
|
const Real tf = 10., dt = 1./50.;
|
|
const Real g = 9.81, wMag = 1.;
|
|
const Real Kp = 1., Ki = 0.1, Kd = 0.1;
|
|
Real x = 0., xdot = 0., xdotdot = 0., u, w, t, xd, e;
|
|
|
|
uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};// noise generator state
|
|
|
|
FlyByWire::PID pid(
|
|
Kp,
|
|
Ki,
|
|
Kd,
|
|
dt,
|
|
-HUGE_VALUE_REAL,//output_lower_bound_
|
|
HUGE_VALUE_REAL,//output_upper_bound_
|
|
Real(0.01),//tau_filtered_derivative
|
|
-HUGE_VALUE_REAL,//integrator_lower_bound_
|
|
HUGE_VALUE_REAL,//integrator_upper_bound_
|
|
true,// auto_anti_windup_
|
|
1e-3);// anti_windup margin
|
|
|
|
// Output to file
|
|
std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
|
|
|
|
if(!out.is_open())
|
|
{
|
|
std::cerr << "Could not open the file in writing mode.\n";
|
|
return;
|
|
}
|
|
|
|
out << "a = [";
|
|
for(t = 0. ; t <= tf ; t += dt)
|
|
{
|
|
// compute system target
|
|
xd = sin(2*M_PI*t/2);
|
|
e = xd-x;
|
|
|
|
// compute system Input
|
|
// u = pid.Filter(e);
|
|
|
|
// simulate system
|
|
w = RAND_XORWOW_A_B(state, -wMag, wMag);
|
|
xdotdot = -g + u + w;
|
|
xdot += xdotdot*dt; // dirty Forward Euler
|
|
x += xdot*dt; // dirty Forward Euler
|
|
|
|
// write to file
|
|
out << t << " " << xd << " " << x << " " << xdot << " " << xdotdot << " " << e << " " << u << " " << w << "\n";
|
|
}
|
|
|
|
out << "];\nfigure(1), plot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
|
|
out << "figure(2), plot(a(:,1), a(:,6), a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
|
|
}
|