tau_damp.c

一个simulink中调用m函数的例子

/*
 * MATLAB Compiler: 2.0.1
 * Date: Wed Oct 17 15:45:44 2001
 * Arguments: "-h" "-x" "vxdot.m" 
 */
#include "tau_damp.h"
#include "a2clcd.h"
#include "a2clcdxc.h"
#include "vp.h"

static double __Array0_r[3] = { 1.0, 0.0, 0.0 };

static double __Array1_r[1] = { 0.0 };

static double __Array2_r[3] = { 0.0, 0.0, 1.0 };

static double __Array3_r[1] = { 0.0 };

static double __Array4_r[1] = { 0.0 };

static double __Array5_r[3] = { 0.0, 1.0, 0.0 };

static double __Array6_r[1] = { 0.0 };

static double __Array7_r[1] = { 0.0 };

static double __Array8_r[1] = { 0.0 };

static double __Array9_r[3] = { 0.0, 1.0, 0.0 };

static double __Array10_r[3] = { 0.0, 1.0, 0.0 };

static double __Array11_r[1] = { 0.0 };

static double __Array12_r[3] = { 0.0, 1.0, 0.0 };

static double __Array13_r[3] = { 0.0, 1.0, 0.0 };

static double __Array14_r[1] = { 0.0 };

static double __Array15_r[3] = { 0.0, -1.0, 0.0 };

static double __Array16_r[3] = { 0.0, 1.0, 0.0 };

static double __Array17_r[1] = { 0.0 };

static double __Array18_r[3] = { 0.0, -1.0, 0.0 };

static double __Array19_r[3] = { 0.0, 1.0, 0.0 };

static double __Array20_r[1] = { 0.0 };

static double __Array21_r[3] = { 0.0, 1.0, 0.0 };

static double __Array22_r[3] = { 0.0, 1.0, 0.0 };

static double __Array23_r[1] = { 0.0 };

static double __Array24_r[3] = { 0.0, 1.0, 0.0 };

static double __Array25_r[3] = { 0.0, 1.0, 0.0 };

static double __Array26_r[1] = { 0.0 };

static double __Array27_r[3] = { 0.0, -1.0, 0.0 };

static double __Array28_r[3] = { 0.0, 1.0, 0.0 };

static double __Array29_r[1] = { 0.0 };

static double __Array30_r[3] = { 0.0, -1.0, 0.0 };

static double __Array31_r[3] = { 0.0, 1.0, 0.0 };

static double __Array32_r[1] = { 0.0 };

/*
 * The function "Mtau_damp" is the implementation version of the "tau_damp"
 * M-function from file "E:\RTW\NEW3\TAU_DAMP.M" (lines 1-307). It contains the
 * actual compiled code for that M-function. It is a static function and must
 * only be called from one of the interface functions, appearing below.
 */
/*
 * function td=tau_damp(veh,vr,de)
 */
static mxArray * Mtau_damp(int nargout_,
                           mxArray * veh,
                           mxArray * vr,
                           mxArray * de) {
    mxArray * td = mclGetUninitializedArray();
    mxArray * F_1b = mclGetUninitializedArray();
    mxArray * F_1w = mclGetUninitializedArray();
    mxArray * F_2b = mclGetUninitializedArray();
    mxArray * F_2w = mclGetUninitializedArray();
    mxArray * F_3b = mclGetUninitializedArray();
    mxArray * F_3w = mclGetUninitializedArray();
    mxArray * F_4b = mclGetUninitializedArray();
    mxArray * F_4w = mclGetUninitializedArray();
    mxArray * F_5b = mclGetUninitializedArray();
    mxArray * F_5w = mclGetUninitializedArray();
    mxArray * F_6b = mclGetUninitializedArray();
    mxArray * F_6w = mclGetUninitializedArray();
    mxArray * F_7b = mclGetUninitializedArray();
    mxArray * F_7w = mclGetUninitializedArray();
    mxArray * F_8b = mclGetUninitializedArray();
    mxArray * F_8w = mclGetUninitializedArray();
    mxArray * F_Bb = mclGetUninitializedArray();
    mxArray * F_Bw = mclGetUninitializedArray();
    mxArray * M_1bb = mclGetUninitializedArray();
    mxArray * M_2bb = mclGetUninitializedArray();
    mxArray * M_3bb = mclGetUninitializedArray();
    mxArray * M_4bb = mclGetUninitializedArray();
    mxArray * M_5bb = mclGetUninitializedArray();
    mxArray * M_6bb = mclGetUninitializedArray();
    mxArray * M_7bb = mclGetUninitializedArray();
    mxArray * M_8bb = mclGetUninitializedArray();
    mxArray * M_Bbb = mclGetUninitializedArray();
    mxArray * Pf_b = mclGetUninitializedArray();
    mxArray * R_1b = mclGetUninitializedArray();
    mxArray * R_2b = mclGetUninitializedArray();
    mxArray * R_3b = mclGetUninitializedArray();
    mxArray * R_4b = mclGetUninitializedArray();
    mxArray * R_5b = mclGetUninitializedArray();
    mxArray * R_6b = mclGetUninitializedArray();
    mxArray * R_7b = mclGetUninitializedArray();
    mxArray * R_8b = mclGetUninitializedArray();
    mxArray * R_fb = mclGetUninitializedArray();
    mxArray * R_w1 = mclGetUninitializedArray();
    mxArray * R_w2 = mclGetUninitializedArray();
    mxArray * R_w3 = mclGetUninitializedArray();
    mxArray * R_w4 = mclGetUninitializedArray();
    mxArray * R_w5 = mclGetUninitializedArray();
    mxArray * R_w6 = mclGetUninitializedArray();
    mxArray * R_w7 = mclGetUninitializedArray();
    mxArray * R_w8 = mclGetUninitializedArray();
    mxArray * R_wf = mclGetUninitializedArray();
    mxArray * a1 = mclGetUninitializedArray();
    mxArray * a2 = mclGetUninitializedArray();
    mxArray * a3 = mclGetUninitializedArray();
    mxArray * a4 = mclGetUninitializedArray();
    mxArray * a5 = mclGetUninitializedArray();
    mxArray * a6 = mclGetUninitializedArray();
    mxArray * a7 = mclGetUninitializedArray();
    mxArray * a8 = mclGetUninitializedArray();
    mxArray * af = mclGetUninitializedArray();
    mxArray * cd = mclGetUninitializedArray();
    mxArray * cl = mclGetUninitializedArray();
    mxArray * i_fb = mclGetUninitializedArray();
    mxArray * k_fb = mclGetUninitializedArray();
    mxArray * sf = mclGetUninitializedArray();
    mxArray * t1 = mclGetUninitializedArray();
    mxArray * t2 = mclGetUninitializedArray();
    mxArray * t3 = mclGetUninitializedArray();
    mxArray * t4 = mclGetUninitializedArray();
    mxArray * t5 = mclGetUninitializedArray();
    mxArray * t6 = mclGetUninitializedArray();
    mxArray * t7 = mclGetUninitializedArray();
    mxArray * t8 = mclGetUninitializedArray();
    mxArray * tf = mclGetUninitializedArray();
    mxArray * v_1c1 = mclGetUninitializedArray();
    mxArray * v_1cb = mclGetUninitializedArray();
    mxArray * v_2c2 = mclGetUninitializedArray();
    mxArray * v_2cb = mclGetUninitializedArray();
    mxArray * v_3c3 = mclGetUninitializedArray();
    mxArray * v_3cb = mclGetUninitializedArray();
    mxArray * v_4c4 = mclGetUninitializedArray();
    mxArray * v_4cb = mclGetUninitializedArray();
    mxArray * v_5c5 = mclGetUninitializedArray();
    mxArray * v_5cb = mclGetUninitializedArray();
    mxArray * v_6c6 = mclGetUninitializedArray();
    mxArray * v_6cb = mclGetUninitializedArray();
    mxArray * v_7c7 = mclGetUninitializedArray();
    mxArray * v_7cb = mclGetUninitializedArray();
    mxArray * v_8c8 = mclGetUninitializedArray();
    mxArray * v_8cb = mclGetUninitializedArray();
    mxArray * v_Bcb = mclGetUninitializedArray();
    mxArray * v_Bcf = mclGetUninitializedArray();
    mxArray * xcp = mclGetUninitializedArray();
    mclValidateInputs("tau_damp", 3, &veh, &vr, &de);
    /*
     * 
     * % td=tau_damp(veh,vr,de); calculates damping forces from 
     * % vehicle variables ,generalized velocity vr and delta angles de
     * 
     * % --------------------------------------------------------------
     * % forces on FUSELAGE 
     * 
     * % Fuselage reference surface
     * sf=pi/4*veh.d^2;
     */
    mlfAssign(
      &sf,
      mlfMtimes(
        mlfMrdivide(mlfPi(), mlfScalar(4.0)),
        mlfFeval(
          mclValueVarargout(),
          mlxMpower,
          mlfIndexRef(veh, ".d"),
          mlfScalar(2.0),
          NULL)));
    /*
     * 
     * % relative velocity of B_b wrt c in b
     * v_Bcb=vr(1:3)+vp(vr(4:6),veh.B_b);
     */
    mlfAssign(
      &v_Bcb,
      mlfPlus(
        mlfIndexRef(vr, "(?)", mlfColon(mlfScalar(1.0), mlfScalar(3.0), NULL)),
        mlfFeval(
          mclValueVarargout(),
          mlxVp,
          mlfIndexRef(
            vr, "(?)", mlfColon(mlfScalar(4.0), mlfScalar(6.0), NULL)),
          mlfIndexRef(veh, ".B_b"),
          NULL)));
    /*
     * 
     * % fuselage rotation matrix
     * i_fb=[1; 0; 0];
     */
    mlfAssign(&i_fb, mlfDoubleMatrix(3, 1, __Array0_r, NULL));
    /*
     * if   norm([0; v_Bcb(2); v_Bcb(3)])<1e-12, k_fb=[0; 0; 1];
     */
    if (mlfTobool(
          mlfLt(
            mlfNorm(
              mlfVertcat(
                mlfDoubleMatrix(1, 1, __Array1_r, NULL),
                mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(2.0)), NULL),
                mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(3.0)), NULL),
                NULL),
              NULL),
            mlfScalar(1e-12)))) {
        mlfAssign(&k_fb, mlfDoubleMatrix(3, 1, __Array2_r, NULL));
    /*
     * else k_fb=[0; v_Bcb(2); v_Bcb(3)]/norm([0; v_Bcb(2); v_Bcb(3)]);end
     */
    } else {
        mlfAssign(
          &k_fb,
          mlfMrdivide(
            mlfVertcat(
              mlfDoubleMatrix(1, 1, __Array3_r, NULL),
              mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(2.0)), NULL),
              mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(3.0)), NULL),
              NULL),
            mlfNorm(
              mlfVertcat(
                mlfDoubleMatrix(1, 1, __Array4_r, NULL),
                mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(2.0)), NULL),
                mlfHorzcat(mlfIndexRef(v_Bcb, "(?)", mlfScalar(3.0)), NULL),
                NULL),
              NULL)));
    }
    /*
     * R_fb=[i_fb, vp(k_fb,i_fb), k_fb];
     */
    mlfAssign(&R_fb, mlfHorzcat(i_fb, mlfVp(k_fb, i_fb), k_fb, NULL));
    /*
     * 
     * % relative velocity of B_b wrt c in f
     * v_Bcf=R_fb'*v_Bcb;
     */
    mlfAssign(&v_Bcf, mlfMtimes(mlfCtranspose(R_fb), v_Bcb));
    /*
     * 
     * % attack angle
     * af=atan2(v_Bcf(3),v_Bcf(1));
     */
    mlfAssign(
      &af,
      mlfAtan2(
        mlfIndexRef(v_Bcf, "(?)", mlfScalar(3.0)),
        mlfIndexRef(v_Bcf, "(?)", mlfScalar(1.0))));
    /*
     * 
     * % wind frame rotation matrix
     * R_wf=[cos(af) 0 -sin(af); 0 1 0; sin(af) 0 cos(af)];
     */
    mlfAssign(
      &R_wf,
      mlfVertcat(
        mlfHorzcat(mlfCos(af), mlfScalar(0.0), mlfUminus(mlfSin(af)), NULL),
        mlfDoubleMatrix(1, 3, __Array5_r, NULL),
        mlfHorzcat(mlfSin(af), mlfScalar(0.0), mlfCos(af), NULL),
        NULL));
    /*
     * 
     * % cl cd xcp computation
     * [cl,cd,xcp]=a2clcdxc(af);
     */
    mlfAssign(&cl, mlfA2clcdxc(&cd, &xcp, af));
    /*
     * 
     * % damping forces on B wrt w 
     * F_Bw=-0.5*veh.rho*sf*v_Bcf'*v_Bcf*[cd; 0; cl];
     */
    mlfAssign(
      &F_Bw,
      mlfMtimes(
        mlfMtimes(
          mlfMtimes(
            mlfMtimes(
              mlfFeval(
                mclValueVarargout(),
                mlxMtimes,
                mlfScalar(-0.5),
                mlfIndexRef(veh, ".rho"),
                NULL),
              sf),
            mlfCtranspose(v_Bcf)),
          v_Bcf),
        mlfVertcat(
          mlfHorzcat(cd, NULL),
          mlfDoubleMatrix(1, 1, __Array6_r, NULL),
          mlfHorzcat(cl, NULL),
          NULL)));
    /*
     * 
     * % damping forces on B wrt b 
     * F_Bb=R_fb*R_wf*F_Bw;
     */
    mlfAssign(&F_Bb, mlfMtimes(mlfMtimes(R_fb, R_wf), F_Bw));
    /*
     * 
     * % force application point
     * Pf_b=[-xcp*veh.l; 0; 0];
     */
    mlfAssign(
      &Pf_b,
      mlfVertcat(
        mlfHorzcat(
          mlfFeval(
            mclValueVarargout(),
            mlxMtimes,
            mlfUminus(xcp),
            mlfIndexRef(veh, ".l"),
            NULL),
          NULL),
        mlfDoubleMatrix(1, 1, __Array7_r, NULL),
        mlfDoubleMatrix(1, 1, __Array8_r, NULL),
        NULL));
    /*
     * 
     * % moments on B with pole in b wrt b
     * M_Bbb=vp(Pf_b,F_Bb);
     */
    mlfAssign(&M_Bbb, mlfVp(Pf_b, F_Bb));
    /*
     * 
     * tf=[F_Bb;M_Bbb];
     */
    mlfAssign(
      &tf, mlfVertcat(mlfHorzcat(F_Bb, NULL), mlfHorzcat(M_Bbb, NULL), NULL));
    /*
     * 
     * % --------------------------------------------------------------
     * % forces on FIN 1 
     * 
     * % fin1 rotation matrix
     * R_1b=[cos(de(1)) 0 sin(de(1)); 0 1 0; -sin(de(1)) 0 cos(de(1))];
     */
    mlfAssign(
      &R_1b,
      mlfVertcat(
        mlfHorzcat(
          mlfCos(mlfIndexRef(de, "(?)", mlfScalar(1.0))),
          mlfScalar(0.0),
          mlfSin(mlfIndexRef(de, "(?)", mlfScalar(1.0))),
          NULL),
        mlfDoubleMatrix(1, 3, __Array9_r, NULL),
        mlfHorzcat(
          mlfUminus(mlfSin(mlfIndexRef(de, "(?)", mlfScalar(1.0)))),
          mlfScalar(0.0),
          mlfCos(mlfIndexRef(de, "(?)", mlfScalar(1.0))),
          NULL),
        NULL));
    /*
     * 
     * % relative velocity of fin1 middle point wrt c in b
     * v_1cb=vr(1:3)+vp(vr(4:6),veh.P1_b);
     */
    mlfAssign(
      &v_1cb,
      mlfPlus(
        mlfIndexRef(vr, "(?)", mlfColon(mlfScalar(1.0), mlfScalar(3.0), NULL)),
        mlfFeval(
          mclValueVarargout(),
          mlxVp,
          mlfIndexRef(
            vr, "(?)", mlfColon(mlfScalar(4.0), mlfScalar(6.0), NULL)),
          mlfIndexRef(veh, ".P1_b"),
          NULL)));
    /*
     * 
     * % relative velocity of fin1 middle point wrt c in 1
     * v_1c1=R_1b'*v_1cb;
     */
    mlfAssign(&v_1c1, mlfMtimes(mlfCtranspose(R_1b), v_1cb));
    /*
     * 
     * % attack angle
     * a1=atan2(v_1c1(3),v_1c1(1));
     */
    mlfAssign(
      &a1,
      mlfAtan2(
        mlfIndexRef(v_1c1, "(?)", mlfScalar(3.0)),
        mlfIndexRef(v_1c1, "(?)", mlfScalar(1.0))));
    /*
     * 
     * % fin1 wind frame rotation matrix
     * R_w1=[cos(a1) 0 -sin(a1); 0 1 0; sin(a1) 0 cos(a1)];
     */
    mlfAssign(
      &R_w1,
      mlfVertcat(
        mlfHorzcat(mlfCos(a1), mlfScalar(0.0), mlfUminus(mlfSin(a1)), NULL),
        mlfDoubleMatrix(1, 3, __Array10_r, NULL),
        mlfHorzcat(mlfSin(a1), mlfScalar(0.0), mlfCos(a1), NULL),
        NULL));
    /*
     * 
     * % cl and cd computation
     * [cl,cd]=a2clcd(a1);
     */
    mlfAssign(&cl, mlfA2clcd(&cd, a1));
    /*
     * 
     * % damping forces on 1 wrt w 
     * F_1w=-0.5*veh.rho*veh.sw*(v_1c1(1)^2+v_1c1(3)^2)*[cd; 0; cl];
     */
    mlfAssign(
      &F_1w,
      mlfMtimes(
        mlfMtimes(
          mlfFeval(
            mclValueVarargout(),
            mlxMtimes,
            mlfFeval(
              mclValueVarargout(),
              mlxMtimes,
              mlfScalar(-0.5),
              mlfIndexRef(veh, ".rho"),
              NULL),
            mlfIndexRef(veh, ".sw"),
            NULL),
          mlfPlus(
            mlfMpower(
              mlfIndexRef(v_1c1, "(?)", mlfScalar(1.0)), mlfScalar(2.0)),
            mlfMpower(
              mlfIndexRef(v_1c1, "(?)", mlfScalar(3.0)), mlfScalar(2.0)))),
        mlfVertcat(
          mlfHorzcat(cd, NULL),
          mlfDoubleMatrix(1, 1, __Array11_r, NULL),
          mlfHorzcat(cl, NULL),
          NULL)));
    /*
     * 
     * % damping forces on 1 wrt b 
     * F_1b=R_1b*R_w1*F_1w;
     */
    mlfAssign(&F_1b, mlfMtimes(mlfMtimes(R_1b, R_w1), F_1w));
    /*
     * 
     * % moments on 1 with pole in b wrt b
     * M_1bb=vp(veh.P1_b,F_1b);
     */
    mlfAssign(
      &M_1bb,
      mlfFeval(
        mclValueVarargout(), mlxVp, mlfIndexRef(veh, ".P1_b"), F_1b, NULL));
    /*
     * 
     * t1=[F_1b;M_1bb];
     */
    mlfAssign(
      &t1, mlfVertcat(mlfHorzcat(F_1b, NULL), mlfHorzcat(M_1bb, NULL), NULL));
    /*
     * 
     * % --------------------------------------------------------------
     * % forces on FIN 2 
     * 
     * % fin2 rotation matrix
     * R_2b=[cos(de(2)) 0 sin(de(2)); sin(de(2)) 0 -cos(de(2)); 0 1 0];
     */
    mlfAssign(
      &R_2b,
      mlfVertcat(
        mlfHorzcat(
          mlfCos(mlfIndexRef(de, "(?)", mlfScalar(2.0))),
          mlfScalar(0.0),
          mlfSin(mlfIndexRef(de, "(?)", mlfScalar(2.0))),
          NULL),
        mlfHorzcat(
          mlfSin(mlfIndexRef(de, "(?)", mlfScalar(2.0))),
          mlfScalar(0.0),
          mlfUminus(mlfCos(mlfIndexRef(de, "(?)", mlfScalar(2.0)))),
          NULL),
        mlfDoubleMatrix(1, 3, __Array12_r, NULL),
        NULL));
    /*