//----------------------------------------------------------
// Module Diag_CapteursT1
//----------------------------------------------------------
#ifndef __orc_Mod_Diag_CapteursT1_h
#define __orc_Mod_Diag_CapteursT1_h

#include "Exec/mt.h"
#include "Exec/rt.h"
#include "Exec/module.h"

#include "module_Diag_CapteursT1_Inc.h"

#define LRW (22 + NEQ*MAX(16,NEQ + 9))
#define LIW (20 + NEQ)

class orc_RT_X4_Diags_T1;

class orc_Mod_Diag_CapteursT1: public ModuleAlgo { 
protected: 
	// Internal Variables 
// Orccad Version: 3.2
// Module : Diag_CapteursT1
// Variables declaration File
// Date of creation : Thu Oct  2 09:27:47 2008

  double q_hat[4], vel_hat[3], DtQ;
  double q_gx[4],vel_gx[3];
  double x_model[NEQ];
  double dx_model[NEQ];
  double TZERO;
  int cpt,neq;
  double /* xinit[NEQ], */ qinit[4];
  double qmodel[4]; //quaternion calcule par le model mecanique
  double velmodel[3];//vitesse angulaire calcule par le model mecanique
  double b_model[3];
  double acc_model[3];
  double pos_model[3];
  double vit_model[3];
  double vitmot_model[4]; 
  double extended_state_model[20];
  double dot_xinit[17];
  double v_l[4];
  double q_mod[4];
  double Vmot_l[4], s_ref_diag[4];
  double xp0[3], xp1[3], xp2[3], xp3[3];
  double dt, rtol, abstol, rwork[LRW], tint, tout, tol[2];
 int itol, itask, istate, iopt, jt, liw, lrw, iwork[LIW];
 double Time;
 orc_RT_X4_Diags_T1 *pRT;

 FILE *q1mod, *q2mod, *q3mod, *q4mod, *q1obs, *q2obs, *q3obs, *q4obs, *res;
 int jsave;
 char qf1mod[10];
 char qf2mod[10];
 char qf3mod[10];
 char qf4mod[10];
 char qf1obs[10];
 char qf2obs[10];
 char qf3obs[10];
 char qf4obs[10];
 char residu[10];

public: 
	orc_Mod_Diag_CapteursT1(ModuleTask *mt,double period):
	  ModuleAlgo("orc_Mod_Diag_CapteursT1",mt,period) {};
	~orc_Mod_Diag_CapteursT1() {};
	 // Output Ports declaration
	 // Output Event Ports declaration
	int Sensor_FailE, SensorValue;
	 // Output param Ports declaration

	 // Methods of computation 
	 void init(const double AccI[3], const double MagI[3], const double GyroI[3], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const double PosI[6]); 
	 void param(); 
	 void reparam(); 
	 void compute(const double AccI[3], const double MagI[3], const double GyroI[3], const double VmotI[4], const double VrelI[3], const double D_PosI[6], const double PosI[6] ); 
	 void end(); 
 }; 
#endif 
// End class  orc_Mod_Diag_CapteursT1