HydroSDL/hydro_source/compute_state_dot.cpp

Go to the documentation of this file.

#include "math.h"
#include "mex.h"   //--This one is required
#include "string.h"

#include "hydro_includes.h"



#ifndef PI_DEF 
#define pi 3.141592653589793 
#define PI 3.141592653589793 
#endif 




void compute_state_dot(struct datas *d, struct parameters *P, double *state)
{
        int k;

//      data_init(d);
//      data_update_param(d, P);
        data_update_state(d, state);

        //d->tanCal_yaw = d->tanCal_yaw0;

        EE_flat_compute(d);
        


        d->EE_left = d->EE_left_flat;
        d->EE_right = d->EE_right_flat;
        d->EE_yaw = d->EE_yaw_flat;

        
        d->Fac_update = 1.0;

        for (k=0; k<2; k++)
        {

                equ_compute(d);
           
                d->EE_left = d->EE_left - d->Fac_update*d->Equ_left/d->dEqu_left;
                d->EE_right = d->EE_right - d->Fac_update*d->Equ_right/d->dEqu_right;
                d->EE_yaw = d->EE_yaw - d->Fac_update*d->Equ_yaw/d->dEqu_yaw;
        }


        if (d->EE_left+d->epsilon < 0) d->EE_left = d->epsilon;
        if (d->EE_right+d->epsilon < 0) d->EE_right = d->epsilon;
        if (d->EE_left > d->E_foils-d->epsilon) d->EE_left = d->E_foils-d->epsilon;
        if (d->EE_right > d->E_foils-d->epsilon) d->EE_right = d->E_foils-d->epsilon;
        if (d->EE_yaw > d->E_yaw-d->epsilon) d->EE_yaw = d->E_yaw-d->epsilon;
        if (d->EE_yaw+d->epsilon < 0) d->EE_yaw = d->epsilon;


        angles_compute(d);

        
//      if (d->Tan_AoA_sail < 0)  d->Tan_AoA_sail = 0;


        aero_coeff_control(d);

        forces_compute(d);

        acceleration_compute(d);


/*      //% System definition //unused camera stuff

        if (d->x+P->x_rel < P->x0)
        {
                d->dd_cam[0] = 0.0;
                d->dd_cam[1] = 0.0; 
                d->dd_cam[2] = 0.0;
                
                d->d_cam[0] = 0.0;
                d->d_cam[1] = 0.0; 
                d->d_cam[2] = 0.0;
        }
        else
        {
                if (d->t < P->time_stop)
                {
                d->dd_cam[0] = P->Gain * (P->alpha_cam*(d->x_cam - (d->x + P->x_rel)) + P->beta_cam*(d->dx_cam - d->dx));
                d->dd_cam[1] = P->Gain * (P->alpha_cam*(d->y_cam - (d->y + P->y_rel)) + P->beta_cam*(d->dy_cam - d->dy));
                d->dd_cam[2] = P->Gain * (P->alpha_cam*(d->z_cam - (d->z + P->z_rel)) + P->beta_cam*(d->dz_cam - d->dz));
                }
                else
                {
    
                d->dd_cam[0] = P->Gain * (P->beta_cam*d->dx_cam/3.0);
        d->dd_cam[1] = P->Gain * (P->beta_cam*d->dy_cam/3.0);
        d->dd_cam[2] = P->Gain * (P->beta_cam*d->dz_cam/3.0 + 1.0);
                }
        }

*/

        

        return;
}

---