From fd02b48d045e08ea1d9f928b964e81e1babc5495 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 10 Mar 2009 18:41:32 +0000 Subject: [PATCH] *** empty log message *** --- sw/simulator/scilab/q1d/q1d_ctl_common.sci | 156 +++++++++ sw/simulator/scilab/q1d/q1d_ctl_miac.sci | 70 +++++ sw/simulator/scilab/q1d/q1d_ctl_mrac.sci | 45 +++ sw/simulator/scilab/q1d/q1d_fdm.sci | 67 ++++ sw/simulator/scilab/q1d/q1d_ins.sci | 110 +++++++ sw/simulator/scilab/q1d/q1d_run.sce | 215 +++++++++++++ sw/simulator/scilab/q1d/q1d_sensors.sci | 51 +++ sw/simulator/scilab/q1d/q1d_utils.sci | 18 ++ sw/simulator/scilab/q3d/q3d_ctl.sci | 172 ++++++++++ sw/simulator/scilab/q3d/q3d_fdm.sci | 64 ++++ sw/simulator/scilab/q3d/q3d_utils.sci | 11 + sw/simulator/scilab/q3d/test_2.sce | 27 ++ sw/simulator/scilab/q6d/q6d_ahrs.sci | 107 +++++++ sw/simulator/scilab/q6d/q6d_algebra.sci | 223 +++++++++++++ sw/simulator/scilab/q6d/q6d_fdm.sci | 144 +++++++++ sw/simulator/scilab/q6d/q6d_guidance.sci | 295 ++++++++++++++++++ sw/simulator/scilab/q6d/q6d_ins.sci | 91 ++++++ sw/simulator/scilab/q6d/q6d_sensors.sci | 54 ++++ sw/simulator/scilab/q6d/q6d_stabilization.sci | 207 ++++++++++++ sw/simulator/scilab/q6d/test1.sce | 67 ++++ 20 files changed, 2194 insertions(+) create mode 100644 sw/simulator/scilab/q1d/q1d_ctl_common.sci create mode 100644 sw/simulator/scilab/q1d/q1d_ctl_miac.sci create mode 100644 sw/simulator/scilab/q1d/q1d_ctl_mrac.sci create mode 100644 sw/simulator/scilab/q1d/q1d_fdm.sci create mode 100644 sw/simulator/scilab/q1d/q1d_ins.sci create mode 100644 sw/simulator/scilab/q1d/q1d_run.sce create mode 100644 sw/simulator/scilab/q1d/q1d_sensors.sci create mode 100644 sw/simulator/scilab/q1d/q1d_utils.sci create mode 100644 sw/simulator/scilab/q3d/q3d_ctl.sci create mode 100644 sw/simulator/scilab/q3d/q3d_fdm.sci create mode 100644 sw/simulator/scilab/q3d/q3d_utils.sci create mode 100644 sw/simulator/scilab/q3d/test_2.sce create mode 100644 sw/simulator/scilab/q6d/q6d_ahrs.sci create mode 100644 sw/simulator/scilab/q6d/q6d_algebra.sci create mode 100644 sw/simulator/scilab/q6d/q6d_fdm.sci create mode 100644 sw/simulator/scilab/q6d/q6d_guidance.sci create mode 100644 sw/simulator/scilab/q6d/q6d_ins.sci create mode 100644 sw/simulator/scilab/q6d/q6d_sensors.sci create mode 100644 sw/simulator/scilab/q6d/q6d_stabilization.sci create mode 100644 sw/simulator/scilab/q6d/test1.sce diff --git a/sw/simulator/scilab/q1d/q1d_ctl_common.sci b/sw/simulator/scilab/q1d/q1d_ctl_common.sci new file mode 100644 index 0000000000..db257da0b8 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_ctl_common.sci @@ -0,0 +1,156 @@ +REF_Z = 1; +REF_ZD = 2; +REF_ZDD = 3; +REF_SIZE = 3; + +// +// Parameters +// + +ctl_max_err_z = 5; // m +ctl_kp = -50/20; +ctl_kd = -10/20; + +ctl_max_cmd = 0.99; // actuators high saturation +ctl_min_cmd = 0.01; // actuators low saturation + +ref_omega = rad_of_deg(200.);// second order linear dynamics +ref_zeta = 0.85; +ref_saturate = 1; // with saturations +ref_max_accel = 2. * fdm_g; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g +ref_min_accel = -0.9 * fdm_g;// m/s2 +ref_max_speed = 3.; // m/s + + +// +// propagation of a second order linear model with saturations +// +function [Xrefi1] = ctl_update_ref(Xrefi, Xspi, dt) + + Xrefi1 = zeros(FDM_SIZE, 1); + + // second order ref model + Xrefi1(REF_Z) = Xrefi(REF_Z) + dt * Xrefi(REF_ZD); + Xrefi1(REF_ZD) = Xrefi(REF_ZD) + dt * Xrefi(REF_ZDD); + Xrefi1(REF_ZDD) = -2*ref_zeta*ref_omega * Xrefi(REF_ZD) ... + -ref_omega^2 * ( Xrefi(REF_Z) - Xspi); + + if ref_saturate + // saturate acceleration + if Xrefi1(REF_ZDD) > ref_max_accel + Xrefi1(REF_ZDD) = ref_max_accel; + elseif Xrefi1(REF_ZDD) < ref_min_accel + Xrefi1(REF_ZDD) = ref_min_accel; + end + // saturate speed + if Xrefi1(REF_ZD) >= ref_max_speed & Xrefi1(REF_ZDD) >= 0 + Xrefi1(REF_ZD) = ref_max_speed; + Xrefi1(REF_ZDD) = 0; + elseif Xrefi1(REF_ZD) <= -ref_max_speed & Xrefi1(REF_ZDD) <= 0 + Xrefi1(REF_ZD) = -ref_max_speed; + Xrefi1(REF_ZDD) = 0; + end + end + +endfunction + +// +// +// Display +// +// + +function ctl_display_ref(ctl_sp, ctl_ref_state, time) +nr = 3; +nc = 1; + +subplot(nr,nc,1); +//plot2d(time, ctl_sp, 5); +//plot2d(time, ctl_ref_state(REF_Z,:),2); +_x = [time; time]'; +_y = [ ctl_sp; ctl_ref_state(REF_Z,:)]'; +plot2d(_x, _y, leg="input@ref", style=[3,2]); +xtitle('Z');//, 'time in s', 'm'); + +subplot(nr,nc,2); +plot2d(time, ctl_ref_state(REF_ZD,:),2); +plot2d(time, ref_max_speed * ones(1, length(time)), 5); +plot2d(time, -ref_max_speed * ones(1, length(time)), 5); +xtitle('ZD'); + +subplot(nr,nc,3); +plot2d(time, ctl_ref_state(REF_ZDD,:),2); +plot2d(time, ref_max_accel * ones(1, length(time)), 5); +plot2d(time, ref_min_accel * ones(1, length(time)), 5); +xtitle('ZDD'); + + +endfunction + + +function ctl_display(ctl_type, ctl_list, ctl_sp, ctl_ref_state, ins_state, ctl_adp_state, ctl_adp_cov, ctl_adp_state, time) + + nr = 3; + nc = 2; + + subplot(nr,nc,1); + _rect = [0, -0.2, 5, .7]; + plot2d(time, ins_state(INS_Z,:),3); + plot2d(time, ctl_ref_state(REF_Z,:),2, rect=_rect); + plot2d(time, ctl_sp, 5); + legends(["setpoint", "reference", "achieved"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Altitude'); + + subplot(nr,nc,3); + plot2d(time, ins_state(INS_ZD,:),3); + plot2d(time, ctl_ref_state(REF_ZD,:),2); + legends(["reference", "achieved"],[2 3], with_box=%f, opt="ur"); + xtitle('Vertical Speed'); + + subplot(nr,nc,5); + plot2d(time, ins_state(INS_ZDD,:),3); + plot2d(time, ctl_ref_state(REF_ZDD,:),2); + legends(["reference", "achieved"],[2 3], with_box=%f, opt="ur"); + xtitle('Vertical Acceleration'); + + select ctl_type + case CTL_MIAC + subplot(nr,nc,2); +// _rect = [time(1), 20, time($), 500]; +// plot2d(time, ctl_adp_meas,3, rect=_rect); +// plot2d(time, ctl_adp_state,2, rect=_rect); + plot2d(time, ctl_adp_meas,3); + plot2d(time, ctl_adp_state,2); + legends(["estimation", "measure"],[2 3], with_box=%f, opt="ur"); + xtitle('Parameter adaptation'); + + subplot(nr,nc,4); + _rect = [time(1), 0., time($), 0.003]; + plot2d(time, ctl_adp_cov,2, rect=_rect); +// plot2d(time, ctl_adp_cov,2); + xtitle('Parameter adaptation covariance'); + case CTL_MRAC + subplot(nr,nc,2); + _rect = [time(1), 1, time($), 4]; + plot2d(time, ctl_adp_state,2, rect=_rect); + legends(["estimation"],[2 ], with_box=%f, opt="ur"); + xtitle('Parameter adaptation'); + end + + subplot(nr,nc,6); + xset("color",5); + xfpoly(ctl_list(5),ctl_list(3)); + xset("color",8); + xfpoly(ctl_list(5),ctl_list(4)); + xset("color",1); + //plot2d(time, ctl_list(1), 1); + + _rect = [time(1), 0, time($), 1.0]; + plot2d(time, ctl_list(2), 2, rect=_rect); +// plot2d(time, ctl_list(2), 2); + + legends(["feedforward", "feedback"],[2 5], with_box=%f, opt="ur"); + xtitle('Controler Output'); + +endfunction + diff --git a/sw/simulator/scilab/q1d/q1d_ctl_miac.sci b/sw/simulator/scilab/q1d/q1d_ctl_miac.sci new file mode 100644 index 0000000000..71a9edd082 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_ctl_miac.sci @@ -0,0 +1,70 @@ +// +// Adaptation of the invert dynamic model parameters : +// dimension one kalman filter estimating invert +// of the mass +// +adp_sys_noise = 0.0000005; +adp_accel_noise = 0.5; + +adp_min_cmd = 0.01; + +MIAC_USE_VNULL = 1; +miac_vnull = 7.; + + +function [Xadpi1p, Padpi1p, Madpi1] = ctl_adapt_model(Xadpip, Padpip, Xinsi, Ui) + + if (Ui < adp_min_cmd) + Xadpi1p = Xadpip + // so propagate covariance only + Padpi1p = Padpip; // + adp_sys_noise; + Madpi1 = 0; + else + // Propagate + // we're estimating a constant + Xadpi1m = Xadpip + // so propagate covariance only + Padpi1m = Padpip + adp_sys_noise; + + // Update + // our measurement + Madpi1 = (Xinsi(INS_ZDD) + fdm_g) / ( Ui ); + if MIAC_USE_VNULL + Madpi1 = Madpi1 / (1. - Xinsi(INS_ZD)/miac_vnull); + end + // compute residual + res = Madpi1 - Xadpi1m; + // kalman gain + adp_meas_noise = adp_accel_noise/Ui; + E = Padpi1m + adp_meas_noise; + K = Padpi1m / E; + // update state and covariance + Padpi1p = Padpi1m - K * Padpi1m; + Xadpi1p = Xadpi1m + K * res; + end + +endfunction + + +function [Xrefi, Ui, Xadpi, Padpi, Madpi, feed_fwd] = ctl_miac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Padpim1, Uim1) + // adapt model parameters + [Xadpi, Padpi, Madpi] = ctl_adapt_model(Xadpim1, Padpim1, Xinsi, Uim1) + // update reference + Xrefi = ctl_update_ref(Xrefim1, Xspi, dt); + // error control + err_z = Xinsi(INS_Z) - Xrefi(REF_Z); + err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z); + err_zd = Xinsi(INS_ZD) - Xrefi(REF_ZD); + // invert model + feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1); + if MIAC_USE_VNULL + feed_fwd = feed_fwd / (1 - Xrefi(REF_ZD) / miac_vnull); + end + // compute command + feed_bck = ctl_kp * err_z + ctl_kd * err_zd + feed_fwd = trim(feed_fwd, ctl_min_cmd, ctl_max_cmd); + Ui = feed_fwd + feed_bck; + Ui = trim(Ui, ctl_min_cmd, ctl_max_cmd); + +endfunction + diff --git a/sw/simulator/scilab/q1d/q1d_ctl_mrac.sci b/sw/simulator/scilab/q1d/q1d_ctl_mrac.sci new file mode 100644 index 0000000000..b0013f35ea --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_ctl_mrac.sci @@ -0,0 +1,45 @@ +// +// MRAC Adaptive control parameter +// + +mrac_gma = 0.001; +mrac_lbd = (-ctl_kd + sqrt(ctl_kd^2-4*ctl_kp))/2; +mrac_c = (-ctl_kd - sqrt(ctl_kd^2-4*ctl_kp))/2; + +function [Xadpi1p] = ctl_mrac(Xadpim1, Xinsi, Xrefim1, Uim1) + + // error control + err_z = Xinsi(INS_Z) - Xrefim1(REF_Z); + err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z); + err_zd = Xinsi(INS_ZD) - Xrefim1(REF_ZD); + // generalized error + s = err_zd + mrac_lbd*err_z; + Xadpi1p = Xadpim1 + mrac_gma*s*(Uim1- ... + Xinsi(INS_ZD) * abs(Xinsi(INS_ZD)) * fdm_Cd); + +endfunction + + +function [Xrefi, Ui, Xadpi, feed_fwd] = ctl_mrac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Uim1) + // adapt model parameters + [Xadpi] = ctl_mrac(Xadpim1, Xinsi, Xrefim1, Uim1); + // update reference + Xrefi = ctl_update_ref(Xrefim1, Xspi, dt); + // error control + err_z = Xinsi(INS_Z) - Xrefi(REF_Z); + err_z = trim(err_z, -ctl_max_err_z, ctl_max_err_z); + err_zd = Xinsi(INS_ZD) - Xrefi(REF_ZD); + // invert model + if 1 + feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1); + else +// feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi + ... +// Xinsi(INS_ZD) * abs(Xinsi(INS_ZD)) * fdm_Cd; + feed_fwd = ( Xrefi(REF_ZDD) + fdm_g ) / Xadpi(1) + ... + Xrefi(REF_ZD) * abs(Xrefi(REF_ZD)) * fdm_Cd; + end + // compute command + feed_bck = 1/Xadpi(1)*(ctl_kp * err_z + ctl_kd * err_zd); + Ui = feed_fwd + feed_bck; + Ui = trim(Ui, ctl_min_thrust, ctl_max_thrust); +endfunction diff --git a/sw/simulator/scilab/q1d/q1d_fdm.sci b/sw/simulator/scilab/q1d/q1d_fdm.sci new file mode 100644 index 0000000000..bc1ae1bc54 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_fdm.sci @@ -0,0 +1,67 @@ +// +// Flight dynamic model +// + +// State Vector +FDM_Z = 1; +FDM_ZD = 2; +FDM_ZDD = 3; +FDM_SIZE = 3; + +// u + +// Parameters +fdm_mass = 0.5; // mass in kg +fdm_g = 9.81; // gravity acceleration m/s2 +fdm_max_thrust = 4. * fdm_mass * fdm_g; // actuators high saturation +fdm_min_thrust = 0.05 * fdm_mass * fdm_g; // actuators low saturation +fdm_Cd = 0.01; // non dimentional drag coefficient +fdm_Ct0 = fdm_max_thrust; // non dimentional lift coefficient +fdm_Ctzd = -fdm_max_thrust/7; // non dimensional lift coefficient + +function [Xdot] = fdm_get_derivatives(t, X, U, perturb, param) + + mass = param(1); + Xdot = zeros(FDM_ZD, 1); + Xdot(FDM_Z) = X(FDM_ZD); + u_trim = trim(U(1), 0.01, 1.); + thrust = u_trim * (fdm_Ct0 + fdm_Ctzd * X(FDM_ZD)); + Xdot(FDM_ZD) = 1/mass*(thrust - fdm_Cd * X(FDM_ZD) * abs(X(FDM_ZD))) - fdm_g + perturb; + +endfunction + +// +// Integrate the dynamic model +// +function [Xfdmi1] = fdm_run(Xfdmi, Ui, ti, ti1, perturbi, parami) + + Xfdmi1 = zeros(FDM_SIZE,1); + Xfdmi1(FDM_Z:FDM_ZD) = ode(Xfdmi(FDM_Z:FDM_ZD), ti, ti1, list(fdm_get_derivatives, Ui, perturbi, parami)); + xd = fdm_get_derivatives(ti1, Xfdmi1(FDM_Z:FDM_ZD), Ui, perturbi, parami); + Xfdmi1(FDM_ZDD) = xd(FDM_ZD); + Xfdmi1 = Xfdmi1; + +endfunction + +// +// Simple display +// +function fdm_display_simple(Xfdm, time) + +nr = 3; +nc = 1; + +subplot(nr,nc,1); +plot2d(time, Xfdm(FDM_Z,:)); +xtitle('Z'); + +subplot(nr,nc,2); +plot2d(time, Xfdm(FDM_ZD,:)); +xtitle('ZD'); + +subplot(nr,nc,3); +plot2d(time, Xfdm(FDM_ZDD,:)); +xtitle('ZDD'); + +endfunction + diff --git a/sw/simulator/scilab/q1d/q1d_ins.sci b/sw/simulator/scilab/q1d/q1d_ins.sci new file mode 100644 index 0000000000..4dd61d1563 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_ins.sci @@ -0,0 +1,110 @@ + + + +INS_Z = 1; +INS_ZD = 2; +INS_BIAS = 3; +INS_SIZE = 3; +INS_ZDD = 4; // WARNING : hack - byproduct + // keeping size to 3 for covariance + +function [Xi1, Pi1] = ins_run(Xi, Pi, sensors_i, sensors_i1, dt) + + // + // propagate + // + F = [ 1 dt -dt^2/2 + 0 1 -dt + 0 0 1 ]; + B = [ dt^2/2 dt 0]'; + + Qz = 0.01*dt; + Qzd = 0.01 * dt^2/2; + Qbias = 0.0001 * dt; + Q = [ Qz 0 0 + 0 Qzd 0 + 0 0 Qbias ]; + + accel = sensors_i( SENSORS_ACCEL ) - 9.81; + + Xi1m = F * Xi + B * accel; + + Pi1m = F * Pi * F' + Q; + + // + // Update + // + H = [1 0 0]; + R = 0.1; + // state residual + y = sensors_i1( SENSORS_BARO ) - H * Xi1m; + // covariance residual + S = H*Pi1m*H' + R; + // kalman gain + K = Pi1m*H'*inv(S); + // update state + Xi1 = Xi1m + K*y; + // update covariance + Pi1 = Pi1m - K*H*Pi1m; + Xi1(4) = accel - Xi1(INS_BIAS); + +endfunction + +function [Pi] = getP(n, P,i) + Pi = P(:,(i-1)*n+1:i*n); +endfunction + + +// +// Simple display +// +function ins_display_simple(Xins, Pins, Xfdm, Xsensors, time) + +nr = 3; +nc = 2; + +subplot(nr,nc,1); +plot2d(time, Xsensors(SENSORS_BARO,:),3); +plot2d(time, Xfdm(FDM_Z,:),2); +plot2d(time, Xins(INS_Z,:), 5); +legends(["Estimation", "Truth", "Measurement"],[5 2 3], with_box=%f, opt="ur"); +xtitle('Altitude'); + +subplot(nr,nc,3); +plot2d(time, Xfdm(FDM_ZD,:), 2); +plot2d(time, Xins(INS_ZD,:), 5); +legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur"); +xtitle('Vertical Speed'); + +subplot(nr,nc,5); +plot2d(time, Xfdm(FDM_ZDD,:),2); +plot2d(time, Xins(4,:), 5); +legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur"); +xtitle('Vertical Acceleration'); + +subplot(nr,nc,2); +plot2d(time, 1.0*Xsensors(SENSORS_ACCEL_BIAS,:),2); +plot2d(time, Xins(INS_BIAS,:), 5); +legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur"); +xtitle('Accelerometer Bias'); + +subplot(nr,nc,4); +Pzz = zeros(1,length(time)); +Pzdzd = zeros(1,length(time)); +for i=1:length(time) + Pi = getP(INS_SIZE, Pins, i); + Pzz(i) = Pi(INS_Z, INS_Z); + Pzdzd(i) = Pi(INS_ZD, INS_ZD); + Pbb(i) = Pi(INS_BIAS, INS_BIAS); +end + +plot2d(time, Pzz, 5); +plot2d(time, Pzdzd, 2); +plot2d(time, Pbb, 3); +legends(["Altitude", "Speed", "Bias"],[5 2 3], with_box=%f, opt="ur"); +xtitle('Estimation Covariance'); + + + +endfunction + diff --git a/sw/simulator/scilab/q1d/q1d_run.sce b/sw/simulator/scilab/q1d/q1d_run.sce new file mode 100644 index 0000000000..c0682e1597 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_run.sce @@ -0,0 +1,215 @@ +clear(); + +exec("tvc_utils.sci"); +exec("tvc_fdm.sci"); +exec("tvc_sensors.sci"); +exec("tvc_ins.sci"); +exec("tvc_ctl_common.sci"); +exec("tvc_ctl_miac.sci"); +exec("tvc_ctl_mrac.sci"); + + +// +// Duration of the simulation +// +ts = 0; +te = 14; +dt = 1./512.; +time = ts:dt:te; + +// +// type of control +// +CTL_MIAC = 0; +CTL_MRAC = 1; +ctl_type = CTL_MIAC; + + +// +// input trajectory +// +// single input - height +ctl_sp = zeros(1, length(time)); +if 0 + // step 1m 5s + k = find ( time > 3. & time < 6.); + ctl_sp(k) = 1.; +end +if 1 + // step 15m 14s + k = find ( time > 1. & time < 7); + ctl_sp(k) = 15.; +end +if 0 + // 1rad/s sine input + omega = 2.; + ctl_sp = 3.5 * sin(omega * time); +end + + +// +// Flight dynamic model +// aka true state +// +fdm_state = zeros(FDM_SIZE, length(time)); +fdm_perturb = zeros(1, length(time)); +fdm_param = fdm_mass * ones(1, length(time)); +fdm_state(:,1) = [ 0; 0; 0]; +if 0 + k = find(time >= 5 & time < 5.25); + fdm_perturb(k) = 20.; +end +if 0 + k = find(time >= 5); + fdm_param(k) = 0.5*fdm_mass; +end + + +// +// Sensor Model state +// +sensors_state = zeros(SENSORS_SIZE, length(time)); + +// +// State Estimation ( INS ) +// +BYPASS_INS = 0; +ins_state = zeros(INS_SIZE+1, length(time)); +ins_state(:,1) = [ 0; 0; 0; 0]; +ins_cov = zeros(INS_SIZE, INS_SIZE * length(time)); +ins_cov(1:INS_SIZE,1:INS_SIZE) = [ 1 0 0 + 0 1 0 + 0 0 1 ]; + + +// +// reference model +// +ctl_ref_state = zeros(REF_SIZE, length(time)); +// parameter of the inverted model +ctl_adp_state = zeros(1, length(time)); +ctl_adp_state(1) = 35; +ctl_adp_cov = zeros(1, length(time)); +ctl_adp_cov(1) = 0.05; +ctl_adp_meas = zeros(1, length(time)); + +// output of the controller +ctl_command = zeros(1, length(time)); +ctl_command(1) = 1; +ctl_feed_fwd = zeros(1, length(time)); +ctl_feed_fwd(1) = 1; +ctl_max = zeros(1, length(time)); +ctl_max(1) = 0; +ctl_min = zeros(1, length(time)); +ctl_min(1) = 0; + +for i = 1:length(time)-1 + + ti = time(i); + ti1 = time(i+1); + // run control + Xinsi = ins_state(:,i); + if i==1 + im1 = i; + else + im1 = i - 1; + end + Xspi = ctl_sp(:,i); + Xrefim1 = ctl_ref_state(:,im1); + Xadpim1 = ctl_adp_state(:,im1); + Padpim1 = ctl_adp_cov(:,im1); + Uim1 = ctl_command(:,im1); + select ctl_type + case CTL_MIAC + [Xrefi, Ui, Xadpi, Padpi, Madpi, feed_fwd] = ctl_miac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Padpim1, Uim1); + case CTL_MRAC + [Xrefi, Ui, Xadpi, feed_fwd] = ctl_mrac_run(Xinsi, Xrefim1, Xspi, dt, Xadpim1, Uim1); + end + ctl_ref_state(:,i) = Xrefi; + ctl_command(:,i) = Ui; + ctl_feed_fwd(:,i) = feed_fwd; + ctl_max(:,i) = max([Ui feed_fwd]); + ctl_min(:,i) = min([Ui feed_fwd]); + ctl_adp_state(:,i) = Xadpi; + if ctl_type == CTL_MIAC + ctl_adp_cov(:,i) = Padpi; + ctl_adp_meas(:,i) = Madpi; + end + // run_fdm + Xfdmi = fdm_state(:,i); + Xfdmi1 = fdm_run(Xfdmi, Ui, ti, ti1, fdm_perturb(:,i), fdm_param(i)); + fdm_state(:,i+1) = Xfdmi1; + // run sensor model + Xsensorsi1 = sensors_run(ti, Xfdmi); + sensors_state(:,i+1) = Xsensorsi1; + // run ins + Pinsi = getP(INS_SIZE, ins_cov, i); + Xsensorsi = sensors_state(:,i); + [Xinsi1, Pinsi1] = ins_run(Xinsi(1:INS_SIZE), Pinsi, Xsensorsi, Xsensorsi1, dt); + ins_state(:,i+1) = Xinsi1; + ins_cov(:,(i)*INS_SIZE+1:(i+1)*INS_SIZE) = Pinsi1; + // + if BYPASS_INS + ins_state(INS_Z,i+1) = fdm_state(FDM_Z,i+1); + ins_state(INS_ZD,i+1) = fdm_state(FDM_ZD,i+1); + ins_state(INS_ZDD,i+1) = fdm_state(FDM_ZDD,i+1); + end +end + +ctl_max = [0 ctl_max 0]; +ctl_min = [0 ctl_min 0]; +time_exp = [0 time te]; +ctl_list = list(ctl_command, ctl_feed_fwd, ctl_max, ctl_min, time_exp); + +if 0 + + set("current_figure",0); + clf(); + f=get("current_figure"); + f.figure_name="FDM"; + drawlater(); + fdm_display_simple(fdm_state, time); + drawnow(); + + set("current_figure",1); + clf(); + f=get("current_figure"); + f.figure_name="Sensors"; + drawlater(); + sensors_display_simple(sensors_state, time); + drawnow(); + +end + +if 1 + + set("current_figure",2); + clf(); + f=get("current_figure"); + f.figure_name="INS"; + drawlater(); + ins_display_simple(ins_state, ins_cov, fdm_state, sensors_state, time); + drawnow(); + + +set("current_figure",3); +clf(); +f=get("current_figure"); +f.figure_name="Control"; +drawlater(); +ctl_display(ctl_type, ctl_list, ctl_sp, ctl_ref_state, ins_state, ctl_adp_state, ctl_adp_cov, ctl_adp_state, time); +drawnow(); + +end + +if 0 + +set("current_figure",4); +clf(); +f=get("current_figure"); +f.figure_name="Control - Reference"; +drawlater(); +ctl_display_ref(ctl_sp, ctl_ref_state, time); +drawnow(); + +end diff --git a/sw/simulator/scilab/q1d/q1d_sensors.sci b/sw/simulator/scilab/q1d/q1d_sensors.sci new file mode 100644 index 0000000000..7779602bf7 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_sensors.sci @@ -0,0 +1,51 @@ +// +// +// +// +// + + + +SENSORS_ACCEL = 1; +SENSORS_BARO = 2; +SENSORS_ACCEL_BIAS = 3; +SENSORS_SIZE = 3; + + +accel_noise_std_dev = 0.20; +accel_bias = 0.4; +baro_noise_std_dev = 0.1; +baro_period = 10; +baro_step = 0.06; + + +function [Xsensorsi] = sensors_run(ti, Xfdmi) + Xsensorsi = zeros(SENSORS_SIZE, 1); + Xsensorsi(SENSORS_ACCEL) = Xfdmi(FDM_ZDD) + 9.81 +... + accel_bias +... + accel_noise_std_dev * rand(1,1,'normal'); + Xsensorsi(SENSORS_ACCEL_BIAS) = accel_bias; + + baro_real = Xfdmi(FDM_Z) + baro_noise_std_dev * rand(1,1,'normal'); + baro_disc = round(baro_real/baro_step) * baro_step; + Xsensorsi(SENSORS_BARO) = baro_disc; +endfunction + +// +// Simple display +// +function sensors_display_simple(Xsensors, time) + +nr = 2; +nc = 1; + +subplot(nr,nc,1); +plot2d(time, Xsensors(SENSORS_ACCEL,:)); +xtitle('ACCEL'); + +subplot(nr,nc,2); +plot2d(time, Xsensors(SENSORS_BARO,:)); +xtitle('BARO'); + +endfunction + diff --git a/sw/simulator/scilab/q1d/q1d_utils.sci b/sw/simulator/scilab/q1d/q1d_utils.sci new file mode 100644 index 0000000000..0262601235 --- /dev/null +++ b/sw/simulator/scilab/q1d/q1d_utils.sci @@ -0,0 +1,18 @@ + +function [rad] = rad_of_deg(deg) + rad = deg / 180 * %pi; +endfunction + +function [deg] = deg_of_rad(rad) + deg = rad * 180 / %pi; +endfunction + +function [o] = trim(i,_min, _max) + o = i; + if i > _max + o = _max + elseif i < _min + o = _min + end +endfunction + diff --git a/sw/simulator/scilab/q3d/q3d_ctl.sci b/sw/simulator/scilab/q3d/q3d_ctl.sci new file mode 100644 index 0000000000..bd3fb5d4e5 --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_ctl.sci @@ -0,0 +1,172 @@ + + + +global ctl_sp_pos; +global ctl_ref_0; +global ctl_ref_1; +global ctl_ref_2; +global ctl_ref_3; +global ctl_ref_4; + + + + + + +function ctl_init() + + global fdm_time; + + global ctl_sp_pos; + ctl_sp_pos = zeros(AXIS_NB, length(fdm_time)); + global ctl_ref_0; + ctl_ref_0 = zeros(AXIS_NB, length(fdm_time)); + global ctl_ref_1; + ctl_ref_1 = zeros(AXIS_NB, length(fdm_time)); + global ctl_ref_2; + ctl_ref_2 = zeros(AXIS_NB, length(fdm_time)); + global ctl_ref_3; + ctl_ref_3 = zeros(AXIS_NB, length(fdm_time)); + global ctl_ref_4; + ctl_ref_4 = zeros(AXIS_NB, length(fdm_time)); + +endfunction + + + + + +function ctl_run(i) + + global ctl_sp_pos; + if fdm_time(i) < 10 + ctl_sp_pos(:,i)= [ 5; 0]; + else + ctl_sp_pos(:,i)= [ 0; 1]; + end + + ctl_update_ref(i); + + +endfunction + + +ctl_omega_ref1 = [ rad_of_deg(90); rad_of_deg(90)]; +ctl_zeta_ref1 = [ 0.85; 0.85 ]; + +ctl_omega_ref2 = [ rad_of_deg(1440); rad_of_deg(1440)]; +ctl_zeta_ref2 = [ 0.85; 0.85 ]; + +a0 = ctl_omega_ref1^2 .* ctl_omega_ref2^2; +a1 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_omega_ref2^2 + ... + 2 * ctl_zeta_ref2 .* ctl_omega_ref2 .* ctl_omega_ref1^2; +a2 = ctl_omega_ref1^2 + ... + 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_zeta_ref2 .* ctl_omega_ref2 + ... + ctl_omega_ref2^2; +a3 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 + 2 * ctl_zeta_ref2 .* ctl_omega_ref2; +a4 = [1;1]; + + + + +function ctl_update_ref(i) + global ctl_sp_pos; + global ctl_ref_0; + global ctl_ref_1; + global ctl_ref_2; + global ctl_ref_3; + global ctl_ref_4; + + err_pos = ctl_ref_0(:,i-1) - ctl_sp_pos(:,i-1); + ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i-1) -a2 .* ctl_ref_2(:,i-1) -a1 .* ctl_ref_1(:,i-1) -a0.*err_pos; + + dt = 1/512; + ctl_ref_3(:,i) = ctl_ref_3(:,i-1) + dt * ctl_ref_4(:,i-1); + ctl_ref_2(:,i) = ctl_ref_2(:,i-1) + dt * ctl_ref_3(:,i-1); + ctl_ref_1(:,i) = ctl_ref_1(:,i-1) + dt * ctl_ref_2(:,i-1); + ctl_ref_0(:,i) = ctl_ref_0(:,i-1) + dt * ctl_ref_1(:,i-1); + +endfunction + + +function ctl_display() + nr = 5; + nc = 3; + subplot(nr,nc,1); + plot2d(fdm_time, fdm_state(FDM_SX,:),2); + plot2d(fdm_time, ctl_ref_0(AXIS_X,:),3); + plot2d(fdm_time, ctl_sp_pos(AXIS_X,:),5); + legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X(0)'); + + subplot(nr,nc,2); + plot2d(fdm_time, fdm_state(FDM_SZ,:),2); + plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3); + plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); + legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z(0)'); + + subplot(nr,nc,3); + plot2d(fdm_time, fdm_state(FDM_STHETA,:),2); +// plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3); +// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Theta(0)'); + + subplot(nr,nc,4); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_1(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X(1)'); + + subplot(nr,nc,5); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_1(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z(1)'); + + subplot(nr,nc,6); + plot2d(fdm_time, fdm_state(FDM_STHETAD,:),2); +// plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3); +// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Theta(1)'); + + subplot(nr,nc,7); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_2(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X(2)'); + + subplot(nr,nc,8); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_2(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z(2)'); + + subplot(nr,nc,10); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_3(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X(3)'); + + subplot(nr,nc,11); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_3(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z(3)'); + + subplot(nr,nc,13); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_4(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X(4)'); + + subplot(nr,nc,14); +// plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, ctl_ref_4(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z(4)'); + +endfunction + diff --git a/sw/simulator/scilab/q3d/q3d_fdm.sci b/sw/simulator/scilab/q3d/q3d_fdm.sci new file mode 100644 index 0000000000..f11a6434ae --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_fdm.sci @@ -0,0 +1,64 @@ +// +// Flight Dynamic Model of a planar quadrotor +// + +// +// State Vector +// +FDM_SX = 1; +FDM_SZ = 2; +FDM_STHETA = 3; +FDM_SXD = 4; +FDM_SZD = 5; +FDM_STHETAD = 6; +FDM_SSIZE = 6; + +// +// Actuators +// +FDM_MOTOR_RIGHT = 1; +FDM_MOTOR_LEFT = 2; +FDM_MOTOR_NB = 2; + +fdm_g = 9.81; +fdm_mass = 0.25; +fdm_inertia = 0.0078; + +fdm_dt = 1./512.; + +global fdm_time; +global fdm_state; +global fdm_accel; + + +function fdm_init(t_start, t_stop) + + global fdm_time; + fdm_time = t_start:fdm_dt:t_stop; + global fdm_state; + fdm_state = zeros(FDM_SSIZE, length(fdm_time)); + +endfunction + +function fdm_run(i, cmd) + + global fdm_state; + global fdm_time; + fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd)); + +endfunction + +function [Xdot] = fdm_get_derivatives(t, X, U) + + Xdot = zeros(length(X),1); + Xdot(FDM_SX) = X(FDM_SXD); + Xdot(FDM_SZ) = X(FDM_SZD); + Xdot(FDM_STHETA) = X(FDM_STHETAD); + Xdot(FDM_SXD) = sum(U)/fdm_mass*sin(X(FDM_STHETA)); + Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g); + Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT)); + + + +endfunction + diff --git a/sw/simulator/scilab/q3d/q3d_utils.sci b/sw/simulator/scilab/q3d/q3d_utils.sci new file mode 100644 index 0000000000..93156bc86c --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_utils.sci @@ -0,0 +1,11 @@ +function [rad] = rad_of_deg(deg) + rad = deg / 180 * %pi; +endfunction + +function [deg] = deg_of_rad(rad) + deg = rad * 180 / %pi; +endfunction + +AXIS_X = 1; +AXIS_Z = 2; +AXIS_NB = 2; diff --git a/sw/simulator/scilab/q3d/test_2.sce b/sw/simulator/scilab/q3d/test_2.sce new file mode 100644 index 0000000000..b756a3a6af --- /dev/null +++ b/sw/simulator/scilab/q3d/test_2.sce @@ -0,0 +1,27 @@ +clear(); +clearglobal(); + + +exec('q3d_utils.sci'); +exec('q3d_fdm.sci'); +exec('q3d_ctl.sci'); + + + +fdm_init(0,20.); +ctl_init(); + +for i=1:length(fdm_time)-1 + fdm_run(i+1, [1.227; 1.227]); + ctl_run(i+1); + + +end + +drawlater(); +set("current_figure",0); +clf(); +f=get("current_figure"); +f.figure_name="CTL"; +ctl_display(); +drawnow(); \ No newline at end of file diff --git a/sw/simulator/scilab/q6d/q6d_ahrs.sci b/sw/simulator/scilab/q6d/q6d_ahrs.sci new file mode 100644 index 0000000000..8c560a8fd5 --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_ahrs.sci @@ -0,0 +1,107 @@ + + +AHRS_QI = 1; +AHRS_QX = 2; +AHRS_QY = 3; +AHRS_QZ = 4; +AHRS_BP = 5; +AHRS_BQ = 6; +AHRS_BR = 7; +AHRS_SSIZE = 7; + +global ahrs_state; +global ahrs_euler; +global ahrs_rate; + +function ahrs_init() + + global fdm_time; + + global ahrs_state; + ahrs_state = zeros(AHRS_SSIZE, length(fdm_time)); + ahrs_state(AHRS_QI, 1) = 1; + global ahr_euler; + ahrs_euler = zeros(AXIS_NB, length(fdm_time)); + global ahrs_rate; + ahrs_rate = zeros(AXIS_NB, length(fdm_time)); + +endfunction + + + +function ahrs_propagate(i) + + global sensor_gyro; + global ahrs_state; + global ahrs_euler; + global ahrs_rate; + + ahrs_state(AHRS_BP:AHRS_BR, i) = ahrs_state(AHRS_BP:AHRS_BR, i-1); + ahrs_rate(:,i) = sensor_gyro(:,i) - ahrs_state(AHRS_BP:AHRS_BR, i-1); + + W = get_omega_quat(ahrs_rate(:,i-1)); + K_lagrange = 1.; + quat = fdm_state(FDM_SQI:FDM_SQZ); + quat_dot = -1/2 * W * quat; + quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat; + dt = 1/512; + ahrs_state(AHRS_QI:AHRS_QZ, i) = ahrs_state(AHRS_QI:AHRS_QZ, i-1) + dt * quat_dot; + + ahrs_euler(:,i) = euler_of_quat(ahrs_state(AHRS_QI:AHRS_QZ,i)); + +endfunction + +function ahrs_display() + nr = 3; + nc = 3; + global fdm_state; + global fdm_euler; + global fdm_raccel; + + global fdm_time; + + subplot(nr,nc,1); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PHI,:)),3); + plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_PHI,:)),2); + legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Phi'); + subplot(nr,nc,2); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_THETA,:)),3); + plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_THETA,:)),2); + legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Theta'); + subplot(nr,nc,3); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PSI,:)),3); + plot2d(fdm_time, deg_of_rad(ahrs_euler(EULER_PSI,:)),2); + legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Psi'); + + subplot(nr,nc,4); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SP,:)),3); +legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('P'); + subplot(nr,nc,5); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SQ,:)),3); +legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Q'); + subplot(nr,nc,6); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SR,:)),3); +legends(["setpoint", "ahrs", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('R'); + + subplot(nr,nc,7); + plot2d(fdm_time, fdm_raccel(AXIS_X,:),3); +legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Pd'); + subplot(nr,nc,8); + plot2d(fdm_time, fdm_raccel(AXIS_Y,:),3); +legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Qd'); + subplot(nr,nc,9); + plot2d(fdm_time, fdm_raccel(AXIS_Z,:),3); +legends(["setpoint", "reference", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Rd'); + + + +endfunction diff --git a/sw/simulator/scilab/q6d/q6d_algebra.sci b/sw/simulator/scilab/q6d/q6d_algebra.sci new file mode 100644 index 0000000000..af7588595f --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_algebra.sci @@ -0,0 +1,223 @@ +function [rad] = rad_of_deg(deg) + rad = deg / 180 * %pi; +endfunction + +function [deg] = deg_of_rad(rad) + deg = rad * 180 / %pi; +endfunction + + +EULER_PHI = 1; +EULER_THETA = 2; +EULER_PSI = 3; +EULER_NB = 3; + +AXIS_X = 1; +AXIS_Y = 2; +AXIS_Z = 3; +AXIS_NB = 3; + + +Q_QI = 1; +Q_QX = 2; +Q_QY = 3; +Q_QZ = 4; +Q_SIZE = 4; + +// +// +// +function [dcm] = dcm_of_quat(q) + + qi2 = q(Q_QI)*q(Q_QI); + qiqx = q(Q_QI)*q(Q_QX); + qiqy = q(Q_QI)*q(Q_QY); + qiqz = q(Q_QI)*q(Q_QZ); + qx2 = q(Q_QX)*q(Q_QX); + qxqy = q(Q_QX)*q(Q_QY); + qxqz = q(Q_QX)*q(Q_QZ); + qy2 = q(Q_QY)*q(Q_QY); + qyqz = q(Q_QY)*q(Q_QZ); + qz2 = q(Q_QZ)*q(Q_QZ); + m00 = qi2 + qx2 - qy2 - qz2; + m01 = 2 * ( qxqy + qiqz ); + m02 = 2 * ( qxqz - qiqy ); + m10 = 2 * ( qxqy - qiqz ); + m11 = qi2 - qx2 + qy2 - qz2; + m12 = 2 * ( qyqz + qiqx ); + m20 = 2 * ( qxqz + qiqy ); + m21 = 2 * ( qyqz - qiqx ); + m22 = qi2 - qx2 - qy2 + qz2; + dcm = [ m00 m01 m02 + m10 m11 m12 + m20 m21 m22 ]; + +endfunction + + +// +// +// +function [quat] = quat_of_euler(euler) + + phi2 = euler(EULER_PHI) / 2.0; + theta2 = euler(EULER_THETA) / 2.0; + psi2 = euler(EULER_PSI) / 2.0; + + sinphi2 = sin( phi2 ); + cosphi2 = cos( phi2 ); + sintheta2 = sin( theta2 ); + costheta2 = cos( theta2 ); + sinpsi2 = sin( psi2 ); + cospsi2 = cos( psi2 ); + + q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; + q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; + q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; + q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; + + quat = [q0 q1 q2 q3]'; + +endfunction + +// +// +// +function [euler] = euler_of_quat(quat) + dcm00 = 1.0 - 2*(quat(Q_QY)*quat(Q_QY) + quat(Q_QZ)*quat(Q_QZ)); + dcm01 = 2*(quat(Q_QX)*quat(Q_QY) + quat(Q_QI)*quat(Q_QZ)); + dcm02 = 2*(quat(Q_QX)*quat(Q_QZ) - quat(Q_QI)*quat(Q_QY)); + dcm12 = 2*(quat(Q_QY)*quat(Q_QZ) + quat(Q_QI)*quat(Q_QX)); + dcm22 = 1.0 - 2*(quat(Q_QX)*quat(Q_QX) + quat(Q_QY)*quat(Q_QY)); + + phi = atan( dcm12, dcm22 ); + theta = -asin( dcm02 ); + psi = atan( dcm01, dcm00 ); + + euler = [phi; theta; psi]; +endfunction + + +// +// +// +function [vo] = quat_vect_mult(q, vi) + dcm = dcm_of_quat(q); + vo = dcm * vi; +endfunction + +// +// +// +function [vo] = quat_vect_inv_mult(q, vi) + dcm = dcm_of_quat(q); + vo = dcm' * vi; +endfunction + + +// +// +// q_a2c = q_a2b comp q_b2c , aka q_a2c = q_b2c * q_a2b +// +function [q_a2c] = quat_comp(q_a2b, q_b2c) + M = [ q_b2c(Q_QI) -q_b2c(Q_QX) -q_b2c(Q_QY) -q_b2c(Q_QZ) + q_b2c(Q_QX) q_b2c(Q_QI) q_b2c(Q_QZ) -q_b2c(Q_QY) + q_b2c(Q_QY) -q_b2c(Q_QZ) q_b2c(Q_QI) q_b2c(Q_QX) + q_b2c(Q_QZ) q_b2c(Q_QY) -q_b2c(Q_QX) q_b2c(Q_QI) ]; + q_a2c = M * q_a2b; +endfunction + +// +// +// q_a2b = q_a2b comp_inv q_b2c , aka q_a2b = qinv(_b2c) * q_a2c +// +function [q_a2b] = quat_comp_inv(q_a2c, q_b2c) + q_c2b = quat_inv(q_b2c); + q_a2b = quat_comp(q_a2c, q_c2b); +endfunction + +// +// +// q_a2c = q_b2a inv_comp q_b2c , aka q_a2b = qinv(_b2c) * q_a2c +// +function [q_a2c] = quat_inv_comp(q_b2a, q_b2c) + q_a2b = quat_inv(q_b2a); + q_a2c = quat_comp(q_a2b, q_b2c); +endfunction + + +function [b] = quat_div(a, c) + M = [ c(Q_QI) +c(Q_QX) +c(Q_QY) +c(Q_QZ) + c(Q_QX) -c(Q_QI) -c(Q_QZ) +c(Q_QY) + c(Q_QY) +c(Q_QZ) -c(Q_QI) -c(Q_QX) + c(Q_QZ) -c(Q_QY) +c(Q_QX) -c(Q_QI) ]; + b = M * a; +endfunction + + + +// +// +// +function [b] = quat_inv(a) + b(Q_QI) = a(Q_QI); + b(Q_QX) = -a(Q_QX); + b(Q_QY) = -a(Q_QY); + b(Q_QZ) = -a(Q_QZ); +endfunction + + +// +// +// +function [b] = quat_wrap_shortest(a) + if a(Q_QI) < 0 + b = quat_explementary(a); + else + b = a; + end +endfunction + + +// +// +// +function [b] = quat_explementary(a) + b(Q_QI) = -a(Q_QI); + b(Q_QX) = -a(Q_QX); + b(Q_QY) = -a(Q_QY); + b(Q_QZ) = -a(Q_QZ); +endfunction + +// +// +// +function [b] = quat_wrap_shortest(a) + if a(Q_QI) < 0 + b = quat_explementary(a); + else + b = a; + end +endfunction + +// +// +// +function [omega] = get_omega_quat(rate) + p = rate(1); + q = rate(2); + r = rate(3); + omega = [ 0 p q r + -p 0 -r q + -q r 0 -p + -r -q p 0 ]; +endfunction + +// +// +// +function [ c ] = cross_product(a, b) + c = [ a(2)*b(3) - a(3)*b(2) + a(3)*b(1) - a(1)*b(3) + a(1)*b(2) - a(2)*b(1) ]; +endfunction diff --git a/sw/simulator/scilab/q6d/q6d_fdm.sci b/sw/simulator/scilab/q6d/q6d_fdm.sci new file mode 100644 index 0000000000..c2c153c18d --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_fdm.sci @@ -0,0 +1,144 @@ +// +// Flight Dynamic Model of a quadrotor +// + + +// +// State Vector +// +FDM_SX = 1; +FDM_SY = 2; +FDM_SZ = 3; +FDM_SXD = 4; +FDM_SYD = 5; +FDM_SZD = 6; +FDM_SQI = 7; +FDM_SQX = 8; +FDM_SQY = 9; +FDM_SQZ = 10; +FDM_SP = 11; +FDM_SQ = 12; +FDM_SR = 13; +FDM_SSIZE = 13; + +// +// Actuators +// +FDM_MOTOR_FRONT = 1; +FDM_MOTOR_RIGHT = 2; +FDM_MOTOR_BACK = 3; +FDM_MOTOR_LEFT = 4; +FDM_MOTOR_NB = 4; + +fdm_g = 9.81; // gravitational acceleration +fdm_mass = 0.5; // mass in kg +fdm_inertia = [0.0078 0. 0. // inertia tensor + 0. 0.0078 0. + 0. 0. 0.0137 ]; +fdm_Ct0 = 4. * fdm_mass * fdm_g / 4; // thrust coefficient +fdm_V0 = 7.; // +fdm_Cd = 0.01; // drag coefficient +fdm_la = 0.25; // arm length +fdm_Cm = fdm_Ct0 / 10; // torque coefficient + +fdm_wind = [ 0; 0; 0]; + +fdm_dt = 1/512; + +global fdm_time; +global fdm_state; +global fdm_euler; +global fdm_accel; +global fdm_raccel; + + +function fdm_init(t_start, t_stop) + global fdm_time; + fdm_time = t_start:fdm_dt:t_stop; + global fdm_state; + fdm_state = zeros(FDM_SSIZE, length(fdm_time)); + fdm_state(FDM_SQI, 1) = 1.; + global fdm_euler; + fdm_euler = zeros(AXIS_NB, length(fdm_time)); + global fdm_accel; + fdm_accel = zeros(AXIS_NB, length(fdm_time)); + global fdm_raccel; + fdm_raccel = zeros(AXIS_NB, length(fdm_time)); + +endfunction + + + +function fdm_run(i, cmd) + + global fdm_state; + global fdm_time; + fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd)); + xd = fdm_get_derivatives(fdm_time(i), fdm_state(:, i), cmd); + global fdm_accel; + fdm_accel(:,i) = xd(FDM_SXD:FDM_SZD); + global fdm_raccel; + fdm_raccel(:,i) = xd(FDM_SP:FDM_SR); + global fdm_euler; + fdm_euler(:,i) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,i)); + +endfunction + + + +function [Xdot] = fdm_get_derivatives(t, X, U) + + Xdot = zeros(length(X),1); + // position + Xdot(FDM_SX:FDM_SZ) = X(FDM_SXD:FDM_SZD); + // speed + Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass * fdm_get_forces_ltp(X, U); + // orientation quaternion + rates_body = X(FDM_SP:FDM_SR); + W = get_omega_quat(rates_body); + K_lagrange = 1.; + quat = X(FDM_SQI:FDM_SQZ); + quat_dot = -1/2 * W * quat; + quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat; + Xdot(FDM_SQI:FDM_SQZ) = quat_dot; + // body rates + moments_body = fdm_get_moments_body(X, U); + i_omega = fdm_inertia * rates_body; + omega_i_omega = cross_product(rates_body, i_omega); + Xdot(FDM_SP:FDM_SR) = inv(fdm_inertia) * (moments_body - omega_i_omega); + +endfunction + + +function [F_ltp] = fdm_get_forces_ltp(X, U) + + gspeed_ltp = X(FDM_SXD:FDM_SZD); + airspeed_ltp = gspeed_ltp - fdm_wind; + quat = X(FDM_SQI:FDM_SQZ); + airspeed_body = quat_vect_mult(quat, airspeed_ltp); + + lift_body = [0; 0; -sum(U) * fdm_Ct0 * ( 1 + 1/fdm_V0 * airspeed_body(AXIS_Z))]; + lift_ltp = quat_vect_inv_mult(quat, lift_body); + + weight_ltp = [0; 0; fdm_g * fdm_mass]; + + drag_ltp = -fdm_Cd * norm(airspeed_ltp) * airspeed_ltp; + + F_ltp = lift_ltp + weight_ltp + drag_ltp; + +endfunction + + +function [M_body] = fdm_get_moments_body(X, U) + + k1 = fdm_la * fdm_Ct0; + k2 = fdm_Cm; + moments_of_u = [ 0 -k1 0 k1 + k1 0 -k1 0 + -k2 k2 -k2 k2 ]; + + M_body = moments_of_u * U; + +endfunction + + diff --git a/sw/simulator/scilab/q6d/q6d_guidance.sci b/sw/simulator/scilab/q6d/q6d_guidance.sci new file mode 100644 index 0000000000..064db6af49 --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_guidance.sci @@ -0,0 +1,295 @@ + + + + +global guidance_sp_psi; +global guidance_sp_pos; +global guidance_ref_pos; +global guidance_ref_speed; +global guidance_ref_accel; + +global guidance_cmd_ff; +global guidance_cmd_fb; +global guidance_output_quat; +global guidance_output_thrust; + +guidance_omega_ref = [ rad_of_deg(90); rad_of_deg(90); rad_of_deg(270) ]; +guidance_zeta_ref = [ 0.8; 0.8; 0.8 ]; + +guidance_mass = 0.5; +guidance_Ct0 = 0.5*9.81*4; + +guidance_Kp = 0.5* [ -0.1; -0.1; -0.1]; +guidance_Kd = 1.5* [ -0.1; -0.1; -0.1]; + + +function guidance_init() + + global fdm_time; + + global guidance_sp_psi; + guidance_sp_psi = zeros(1, length(fdm_time)); + global guidance_sp_pos; + guidance_sp_pos = zeros(AXIS_NB, length(fdm_time)); + global guidance_ref_pos; + guidance_ref_pos = zeros(AXIS_NB, length(fdm_time)); + global guidance_ref_speed; + guidance_ref_speed = zeros(AXIS_NB, length(fdm_time)); + global guidance_ref_accel; + guidance_ref_accel = zeros(AXIS_NB, length(fdm_time)); + + global guidance_output_quat; + guidance_output_quat = zeros(Q_SIZE, length(fdm_time)); + global guidance_output_thrust; + guidance_output_thrust = zeros(1, length(fdm_time)); + +endfunction + +function guidance_run(i) + +// guidance_step_phi(i); +// guidance_step_theta(i); +// guidance_step_psi(i); +// global guidance_output_thrust; +// guidance_output_thrust(i) = 0.29; + +// guidance_step_x(i); + guidance_step_y(i); +// guidance_step_z(i); + guidance_update_ref(i); + guidance_hover(i); + + + global stabilization_sp_thrust; + global stabilization_sp_quat; + global guidance_output_quat; + global guidance_output_thrust; + stabilization_sp_thrust(i) = guidance_output_thrust(i); + stabilization_sp_quat(:,i) = guidance_output_quat(:, i); + +endfunction + + + + +// +// +// + +function guidance_hover(i) + + global guidance_ref_pos; + global guidance_ref_speed; + global guidance_ref_accel; + global ins_state; + global ahrs_state; + + pos_err = ins_state(INS_SX:INS_SZ,i) - guidance_ref_pos(:,i); + speed_err = ins_state(INS_SXD:INS_SZD,i) - guidance_ref_speed(:,i); + + corr_thrust_ltp = guidance_Kp .* pos_err + guidance_Kd .* speed_err; + + nli_thrust_ltp = -guidance_mass / guidance_Ct0 * ( [ 0; 0; 9.81] - guidance_ref_accel(:,i)); + + thrust_ltp = nli_thrust_ltp + corr_thrust_ltp; + + axis_foo = cross_product(thrust_ltp/norm(thrust_ltp), [0; 0; -1]); + + angle_foo = asin(norm(axis_foo)); + + quat_foo = [ cos(angle_foo); -axis_foo]; + +// printf("%f %f %f\n", nli_thrust_ltp(1), nli_thrust_ltp(2), nli_thrust_ltp(3)); +// printf("%f %f %f %f\n", quat_foo(1), quat_foo(2), quat_foo(3), quat_foo(4)); + +// thrust_body = quat_vect_mult(ahrs_state(AHRS_QI:AHRS_QZ, i), thrust_ltp); + + global guidance_output_thrust; + guidance_output_thrust(i) = norm(thrust_ltp); + + global guidance_output_quat; + guidance_output_quat(:, i) = quat_foo; //[1; 0; 0; 0]; +endfunction + + +// +// +// +function guidance_update_ref(i) + + global guidance_sp_pos; + global guidance_ref_pos; + global guidance_ref_speed; + global guidance_ref_accel; + + ref_err_pos = guidance_ref_pos(:,i-1) - guidance_sp_pos(:,i-1); + + guidance_ref_accel(:,i) = -guidance_omega_ref^2 .* ref_err_pos ... + -2*guidance_zeta_ref .* guidance_omega_ref .* guidance_ref_speed(:,i-1); + dt = 1/512; + guidance_ref_speed(:,i) = guidance_ref_speed(:,i-1) + dt * guidance_ref_accel(:,i-1); + guidance_ref_pos(:,i) = guidance_ref_pos(:,i-1) + dt * guidance_ref_speed(:,i-1); + +endfunction + + + +// +// +// +function guidance_step_x(i) + global fdm_time; + if modulo(i,2048) < 1024 + pos_sp = [ 1; 0; 0]; + else + pos_sp = [ -1; 0; 0]; + end + global guidance_sp_pos; + guidance_sp_pos(:,i) = pos_sp; +endfunction + +// +// +// +function guidance_step_y(i) + global fdm_time; + if modulo(i,2048) < 1024 + pos_sp = [ 0; 1; 0]; + else + pos_sp = [ 0; -1; 0]; + end + global guidance_sp_pos; + guidance_sp_pos(:,i) = pos_sp; +endfunction + +// +// +// +function guidance_step_z(i) + global fdm_time; + if modulo(i,1024) < 512 + pos_sp = [ 0; 0; -1]; + else + pos_sp = [ 0; 0; 0]; + end + global guidance_sp_pos; + guidance_sp_pos(:,i) = pos_sp; +endfunction + + + +// +// +// +function guidance_step_phi(i) + global fdm_time; + if modulo(i,512) < 256 + euler_sp = [ rad_of_deg(10); 0; 0]; + else + euler_sp = [ rad_of_deg(-10); 0; 0]; + end + global guidance_output_quat; + guidance_output_quat(:, i) = quat_of_euler(euler_sp); +endfunction + +// +// +// +function guidance_step_theta(i) + global fdm_time; + if modulo(i,512) < 256 + euler_sp = [ 0; rad_of_deg(10); 0]; + else + euler_sp = [ 0; rad_of_deg(-10); 0]; + end + global guidance_output_quat; + guidance_output_quat(:, i) = quat_of_euler(euler_sp); +endfunction + +// +// +// +function guidance_step_psi(i) + global fdm_time; + if modulo(i,512) < 256 + euler_sp = [ 0; 0; rad_of_deg(10)]; + else + euler_sp = [ 0; 0; rad_of_deg(-10)]; + end + global guidance_output_quat; + guidance_output_quat(:, i) = quat_of_euler(euler_sp); +endfunction + + +// +// +// +function guidance_display() + + nr = 4; + nc = 3; + global ins_state; + global ins_accel; + global guidance_ref_pos; + global guidance_ref_speed; + global guidance_ref_accel; + + subplot(nr,nc,1); + plot2d(fdm_time, ins_state(INS_SX,:),2); + plot2d(fdm_time, guidance_ref_pos(AXIS_X,:),3); + plot2d(fdm_time, guidance_sp_pos(AXIS_X,:),5); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X'); + subplot(nr,nc,2); + plot2d(fdm_time, ins_state(INS_SY,:),2); + plot2d(fdm_time, guidance_ref_pos(AXIS_Y,:),3); + plot2d(fdm_time, guidance_sp_pos(AXIS_Y,:),5); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Y'); + subplot(nr,nc,3); + plot2d(fdm_time, ins_state(INS_SZ,:),2); + plot2d(fdm_time, guidance_ref_pos(AXIS_Z,:),3); + plot2d(fdm_time, guidance_sp_pos(AXIS_Z,:),5); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z'); + + subplot(nr,nc,4); + plot2d(fdm_time, ins_state(INS_SXD,:),2); + plot2d(fdm_time, guidance_ref_speed(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Xdot'); + subplot(nr,nc,5); + plot2d(fdm_time, ins_state(INS_SYD,:),2); + plot2d(fdm_time, guidance_ref_speed(AXIS_Y,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Ydot'); + subplot(nr,nc,6); + plot2d(fdm_time, ins_state(INS_SZD,:),2); + plot2d(fdm_time, guidance_ref_speed(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Zdot'); + + subplot(nr,nc,7); + plot2d(fdm_time, ins_accel(AXIS_X,:),2); + plot2d(fdm_time, guidance_ref_accel(AXIS_X,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Xdotdot'); + subplot(nr,nc,8); + plot2d(fdm_time, ins_accel(AXIS_Y,:),2); + plot2d(fdm_time, guidance_ref_accel(AXIS_Y,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Ydotdot'); + subplot(nr,nc,9); + plot2d(fdm_time, ins_accel(AXIS_Z,:),2); + plot2d(fdm_time, guidance_ref_accel(AXIS_Z,:),3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Zdotdot'); + + subplot(nr,nc,12); + plot2d(fdm_time, guidance_output_thrust,3); + legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Thrust'); + +endfunction + + diff --git a/sw/simulator/scilab/q6d/q6d_ins.sci b/sw/simulator/scilab/q6d/q6d_ins.sci new file mode 100644 index 0000000000..45e0bb0a2f --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_ins.sci @@ -0,0 +1,91 @@ + +INS_SX = 1; +INS_SY = 2; +INS_SZ = 3; +INS_SXD = 4; +INS_SYD = 5; +INS_SZD = 6; +INS_SBX = 7; +INS_SBY = 8; +INS_SBZ = 9; +INS_SSIZE = 9; + + +global ins_state; +global ins_accel; + + +function ins_init() + + global ins_state; + ins_state = zeros(INS_SSIZE, length(fdm_time)); + global ins_accel; + ins_accel = zeros(AXIS_NB, length(fdm_time)); + +endfunction + +// propagate from i-1 to i +function ins_propagate(i) + + global ins_state; + global ins_accel; + global ahrs_state; + global sensor_accel; + + // convert to LTP + accel_ltp = quat_vect_inv_mult(ahrs_state(Q_QI:Q_QZ, i), sensor_accel(:,i-1)); + // correct for gravity + accel_ltp = accel_ltp + [ 0; 0; 9.81]; + // store unbiased value + ins_accel(:,i) = accel_ltp - ins_state(INS_SBX:INS_SBZ,i-1); + // compute derivative + state_dot = [ ins_state(INS_SXD:INS_SZD, i-1); ins_accel(:,i); 0; 0; 0]; + // propagate + dt = fdm_time(i) - fdm_time(i-1); + ins_state(:,i) = ins_state(:,i-1) + state_dot * dt; + +endfunction + + +function ins_display() + + nr = 3; + nc = 3; + global ins_state; + global fdm_state; + global fdm_time; + + subplot(nr,nc,1); + plot2d(fdm_time, fdm_state(FDM_SX,:),3); + plot2d(fdm_time, ins_state(INS_SX,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('X'); + subplot(nr,nc,2); + plot2d(fdm_time, fdm_state(FDM_SY,:),3); + plot2d(fdm_time, ins_state(INS_SY,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Y'); + subplot(nr,nc,3); + plot2d(fdm_time, fdm_state(FDM_SZ,:),3); + plot2d(fdm_time, ins_state(INS_SZ,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Z'); + + subplot(nr,nc,4); + plot2d(fdm_time, fdm_state(FDM_SXD,:),3); + plot2d(fdm_time, ins_state(INS_SXD,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Xdot'); + subplot(nr,nc,5); + plot2d(fdm_time, fdm_state(FDM_SYD,:),3); + plot2d(fdm_time, ins_state(INS_SYD,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Ydot'); + subplot(nr,nc,6); + plot2d(fdm_time, fdm_state(FDM_SZD,:),3); + plot2d(fdm_time, ins_state(INS_SZD,:),2); + legends(["setpoint", "INS", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Zdot'); + +endfunction + diff --git a/sw/simulator/scilab/q6d/q6d_sensors.sci b/sw/simulator/scilab/q6d/q6d_sensors.sci new file mode 100644 index 0000000000..7c6ef93964 --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_sensors.sci @@ -0,0 +1,54 @@ +// +// Sensors model +// + + + + +global sensor_gyro; +global sensor_accel; +global sensor_baro; +global sensor_gps_pos; +global sensor_gps_speed; + + +function sensors_init() + + global fdm_time; + global sensor_accel; + sensor_accel = zeros(AXIS_NB, length(fdm_time)); + global sensor_gyro; + sensor_gyro = zeros(AXIS_NB, length(fdm_time)); + global sensor_baro; + sensor_baro = zeros(1, length(fdm_time)); + global sensor_gps_pos; + sensor_gps_pos = zeros(AXIS_NB, length(fdm_time)); + global sensor_gps_speed; + sensor_gps_speed = zeros(AXIS_NB, length(fdm_time)); + +endfunction + + + +function sensors_run(i) + + global fdm_state; + global fdm_accel; + global sensor_gyro; + global sensor_accel; + global sensor_baro; + global sensor_gps_pos; + global sensor_gps_speed; + + sensor_gyro(:,i) = fdm_state(FDM_SP:FDM_SR, i); + + sensor_accel(:,i) = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), (fdm_accel(:,i)-[0; 0; 9.81])); + + sensor_gps_pos(:,i) = fdm_state(FDM_SX:FDM_SZ, i); + + sensor_gps_speed(:,i) = fdm_state(FDM_SXD:FDM_SZD, i); + +// sensor_baro(:,i) = fdm_state(FDM_SZ, i); + +endfunction + diff --git a/sw/simulator/scilab/q6d/q6d_stabilization.sci b/sw/simulator/scilab/q6d/q6d_stabilization.sci new file mode 100644 index 0000000000..8ce570212e --- /dev/null +++ b/sw/simulator/scilab/q6d/q6d_stabilization.sci @@ -0,0 +1,207 @@ + + + + +global stabilization_sp_thrust; +global stabilization_sp_quat; +global stabilization_sp_euler; +global stabilization_ref_quat; +global stabilization_ref_euler; +global stabilization_ref_rate; +global stabilization_ref_raccel; + +global stabilization_cmd_ff; +global stabilization_cmd_fb; +global stabilization_cmd_axis; +global stabilization_cmd_motors; + + +stab_omega_ref = [ rad_of_deg(1440); rad_of_deg(1440); rad_of_deg(720) ]; +stab_zeta_ref = [ 0.8; 0.8; 0.8 ]; + + +function stabilization_init() + + global fdm_time; + + global stabilization_sp_thrust; + global stabilization_sp_quat; + global stabilization_sp_euler; + global stabilization_ref_quat; + global stabilization_ref_euler; + global stabilization_ref_rate; + global stabilization_ref_raccel; + + stabilization_sp_thrust = zeros(1, length(fdm_time)); + stabilization_sp_quat = zeros(Q_SIZE, length(fdm_time)); + stabilization_sp_euler = zeros(EULER_NB, length(fdm_time)); + stabilization_ref_quat = zeros(Q_SIZE, length(fdm_time)); + stabilization_ref_quat(Q_QI, 1) = 1.; + stabilization_ref_euler = zeros(EULER_NB, length(fdm_time)); + stabilization_ref_rate = zeros(AXIS_NB, length(fdm_time)); + stabilization_ref_raccel = zeros(AXIS_NB, length(fdm_time)); + + global stabilization_cmd_ff; + global stabilization_cmd_fb; + global stabilization_cmd_axis; + global stabilization_cmd_motors; + stabilization_cmd_ff = zeros(AXIS_NB, length(fdm_time)); + stabilization_cmd_fb = zeros(AXIS_NB, length(fdm_time)); + stabilization_cmd_axis = zeros(4, length(fdm_time)); + stabilization_cmd_motors = zeros(4, length(fdm_time)); + +endfunction + +Kp = [ -0.5; -0.5; -0.5 ]; +Kd = [ -0.5; -0.5; -0.5 ]; +Kdd = [ 0.007; 0.007; 0.007 ]; + +function stabilization_run(i) + + global ahrs_state; + global stabilization_sp_euler; + global stabilization_sp_quat; + stabilization_sp_euler(:,i) = euler_of_quat(stabilization_sp_quat(:,i)); + + stabilization_update_ref(i); + + err_angle = quat_div(stabilization_ref_quat(:,i), ahrs_state(AHRS_QI:AHRS_QZ,i)); + err_rate = ahrs_rate(:,i) - stabilization_ref_rate(:,i); + + global stabilization_cmd_fb; + stabilization_cmd_fb(:,i) = Kp .* err_angle(Q_QX:Q_QZ) + Kd .* err_rate ; + global stabilization_cmd_ff; + stabilization_cmd_ff(:,i) = Kdd .* stabilization_ref_raccel(:,i); + global stabilization_cmd_axis; + global stabilization_sp_thrust; + stabilization_cmd_axis(:,i) = [stabilization_cmd_ff(:,i)+stabilization_cmd_fb(:,i) + stabilization_sp_thrust(i)]; + stabilization_mix(i); + +endfunction + + +function stabilization_update_ref(i) + + global stabilization_sp_quat; + global stabilization_ref_quat; + global stabilization_ref_rate; + global stabilization_ref_raccel; + +// pause + +// error_quat = quat_mult_inv(stabilization_sp_quat(:,i-1), stabilization_ref_quat(:,i-1)); +// error_quat = quat_inv_comp(stabilization_ref_quat(:,i-1), stabilization_sp_quat(:,i-1)); + error_quat = quat_div(stabilization_sp_quat(:,i-1), stabilization_ref_quat(:,i-1)); + error_quat = quat_wrap_shortest(error_quat); + + ref_accel_0 = -stab_omega_ref^2 .* error_quat(Q_QX:Q_QZ); + ref_accel_1 = -2. * stab_zeta_ref .* stab_omega_ref .* stabilization_ref_rate(:,i-1); + + stabilization_ref_raccel(:,i) = ref_accel_0 + ref_accel_1; + + W = get_omega_quat(stabilization_ref_rate(:,i-1)); + K_lagrange = 1.; + quat = stabilization_ref_quat(:,i-1); + quat_dot = -1/2 * W * quat; + quat_dot = quat_dot + K_lagrange * ( 1 - norm(quat)) * quat; + dt = 1/512; + stabilization_ref_quat(:,i) = stabilization_ref_quat(:,i-1) + dt * quat_dot; + + stabilization_ref_rate(:,i) = stabilization_ref_rate(:,i-1) + dt * stabilization_ref_raccel(:,i-1); + + global stabilization_ref_euler; + stabilization_ref_euler(:,i) = euler_of_quat(stabilization_ref_quat(:,i)); + +endfunction + + + +function stabilization_mix(i) + + global stabilization_cmd_axis; + global stabilization_cmd_motors; + + motors_of_axis = [ 0 1 -1 1 + -1 0 1 1 + 0 -1 -1 1 + 1 0 1 1 ]; + + stabilization_cmd_motors(:,i) = motors_of_axis * stabilization_cmd_axis(:,i); + +endfunction + + + + +function stabilization_display() + + nr = 3; + nc = 3; + global stabilization_ref_euler; + global stabilization_sp_euler; + + global fdm_state; + global fdm_euler; + global fdm_time; + + subplot(nr,nc,1); + plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_PHI,:)),2); + plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_PHI,:)),5); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PHI,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('PHI'); + subplot(nr,nc,2); + plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_THETA,:)),2); + plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_THETA,:)),5); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_THETA,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('THETA'); + subplot(nr,nc,3); + plot2d(fdm_time, deg_of_rad(stabilization_ref_euler(EULER_PSI,:)),2); + plot2d(fdm_time, deg_of_rad(stabilization_sp_euler(EULER_PSI,:)),5); + plot2d(fdm_time, deg_of_rad(fdm_euler(EULER_PSI,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('PSI'); + + global stabilization_ref_rate; + subplot(nr,nc,4); + plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_X,:)),2); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SP,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('P'); + subplot(nr,nc,5); + plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_Y,:)),2); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SQ,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('Q'); + subplot(nr,nc,6); + plot2d(fdm_time, deg_of_rad(stabilization_ref_rate(AXIS_Z,:)),2); + plot2d(fdm_time, deg_of_rad(fdm_state(FDM_SR,:)),3); + legends(["setpoint", "Ref", "fdm"],[5 2 3], with_box=%f, opt="ur"); + xtitle('R'); + + + + + + global stabilization_cmd_axis; + global stabilization_cmd_motors; + + subplot(nr,nc,7); + plot2d(fdm_time, stabilization_cmd_axis(AXIS_X,:),1); + plot2d(fdm_time, stabilization_cmd_axis(AXIS_Y,:),2); + plot2d(fdm_time, stabilization_cmd_axis(AXIS_Z,:),3); + legends(["p", "q", "r"],[1 2 3], with_box=%f, opt="ur"); + xtitle('Cmd axis'); + + subplot(nr,nc,8); + plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_FRONT,:),1); + plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_RIGHT,:),2); + plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_BACK,:),3); + plot2d(fdm_time, stabilization_cmd_motors(FDM_MOTOR_LEFT,:),4); + legends(["f", "r", "b", "l"],[1 2 3 4], with_box=%f, opt="ur"); + xtitle('Cmd motor'); + +endfunction + diff --git a/sw/simulator/scilab/q6d/test1.sce b/sw/simulator/scilab/q6d/test1.sce new file mode 100644 index 0000000000..ad89b60483 --- /dev/null +++ b/sw/simulator/scilab/q6d/test1.sce @@ -0,0 +1,67 @@ +clear(); +clearglobal(); + +exec('tins_algebra.sci'); +exec('tins_fdm.sci'); +exec('tins_sensors.sci'); +exec('tins_ahrs.sci'); +exec('tins_ins.sci'); +exec('tins_guidance.sci'); +exec('tins_stabilization.sci'); + + + + +fdm_init(0.,4.); +sensors_init(); +ahrs_init(); +ins_init(); +stabilization_init(); +guidance_init(); + +sensors_run(1); + +for i=1:length(fdm_time)-1 + + fdm_run(i+1, stabilization_cmd_motors(:,i)); + sensors_run(i+1); + ahrs_propagate(i+1); + ins_propagate(i+1); + guidance_run(i+1); + stabilization_run(i+1); + +end + +if 0 + set("current_figure",0); + clf(); + f=get("current_figure"); + f.figure_name="AHRS"; + drawlater(); + ahrs_display(); + drawnow(); + + set("current_figure",1); + clf(); + f=get("current_figure"); + f.figure_name="INS"; + drawlater(); + ins_display(); + drawnow(); +end + +set("current_figure",2); +clf(); +f=get("current_figure"); +f.figure_name="STABILIZATION"; +drawlater(); +stabilization_display(); +drawnow(); + +set("current_figure",3); +clf(); +f=get("current_figure"); +f.figure_name="GUIDANCE"; +drawlater(); +guidance_display(); +drawnow();