heli.cpp

UAV 自动驾驶的

	old_ma1	= m->a1;
	if( isnan( m->a1 ) )
		abort();

	Vector<2>		U;
	U[0]			= c->A1;
	U[1]			= c->B1;

	Vector<13>		args;
	args[ 0]		= cg->uvw[0];
	args[ 1]		= cg->uvw[1];
	args[ 2]		= cg->pqr[0];
	args[ 3]		= cg->pqr[1];
	args[ 4]		= m->db1dv;
	args[ 5]		= m->da1du;
	args[ 6]		= m->w_in;
	args[ 7]		= m->w_off;
	args[ 8]		= m->kc;
	args[ 9]		= m->dir;
	args[10]		= fb->tau;
	args[11]		= fb->Kc;
	args[12]		= fb->Kd;
	
	RK4( X, Xdot, cg->time, U, args, dt, &RotorFlapDynamics );

	// Extract out our new state
	m->a1		= X[0];
	m->b1		= X[1];
	fb->d		= X[2];
	fb->c		= X[3];
	m->a1dot	= Xdot[0];
	m->b1dot	= Xdot[1];
	fb->d_dot	= Xdot[2];
	fb->c_dot	= Xdot[3];

	if( isnan( m->a1 ) )
		abort();

	// Sum Up Total Forces and Moments At CG of main and tail rotors
	cg->F += m->F;
	cg->F += t->F;

	cg->M += m->M;
	cg->M += t->M;
}


/*
 *  Static CG information for the XCell
 */
void
Heli::setup_cg()
{
	Forces *		cg = &this->cg;

	cg->fs_cg	=  0.0;			// in
	cg->wl_cg	= 10.91;		// in
	cg->wt		= 19.5;			// lbs
	cg->ix		=  0.2184;		// slug-ft^2
	cg->iy		=  0.3214;		// slug-ft^2
	cg->iz		=  0.4608;		// slug-ft^2
	cg->ixz		=  0.0337;		// slug-ft^2
	cg->hp_loss	=  0.1;			// HP
	cg->m		= cg->wt / 32.2;	// slugs
	cg->altitude	=  0.0;			// initial DA ft
}
	

/*
 *  Setup the initial control inputs
 */
void
Heli::setup_controls()
{
	control_def *		c = &this->c;

	c->A1		= 0.0;			// roll (rad + right wing down)
	c->B1		= 0.0;			// pitch (rad + nose down)
	c->mr_col	= 2.5*C_DEG2RAD;	// mr col (rad)
	c->tr_col	= 4.5*C_DEG2RAD;	// tr col (rad)
	c->mr_rev	= 1500.0;		// mr RPM
	c->tr_rev	= 4.6 * c->mr_rev;	// tr RPM
	c->gyro_gain	= 0.08;			// Basic rate gyro is 0.08
}


/*
 *  Setup the main rotor parameters
 */
void
Heli::setup_main_rotor()
{
	mainrotor_def *		m	= &this->m;
	Forces *		cg	= &this->cg;

	/* Parameters */
	m->fs		=  0.0;			// in
	m->wl		=  0.0;			// in
	m->is		=  0.0;			// longitudinal shaft tilt (rad)
	m->e		=  0.0225;		// ft
	m->i_b		=  0.0847;		// slug-ft^2
	m->r		=  2.25;		// ft
	m->ro		=  0.6;			// ft
	m->a		=  6.0;			// */rad
	m->cd0		=  0.01;		// nondimentional
	m->b		=  2;			// # of blades
	m->c		=  0.1979;		// ft
	m->twst		=  0.0;			// rad
	m->k1		=  0;			// delta-3 hinge	
	m->dir		= -1.0;			// MR direction of rotation viewed from top (1 = ccw; -1 = cw)
	m->ib		=  0.0;			// laterial shaft tilt (rad)


	/* Dynamics */
	double			rho;
	double			temp;
	double			pres;
	double			sp_sound;


	atmosphere(
		cg->altitude - cg->NED[2],
		&rho,
		&pres,
		&temp,
		&sp_sound
	);

	m->omega	= this->c.mr_rev * C_TWOPI / 60.0;	// rad/s
	m->v_tip	= m->r * this->m.omega;	// ft/s

	// mr lock number
	m->lock		= rho*m->a*m->c*pow(m->r, 4.0) / m->i_b;

	// natural freq shift
	m->omega_f	= ( m->lock*m->omega/16.0 )
		* (1.0 + (8.0/3.0)*(m->e/m->r));

	// cross-couple coef.
	m->k2		= 0.75 * (m->e/m->r) * (m->omega/m->omega_f);

	// total cross-couple coef.
	m->kc		= m->k1 + m->k2;

	// time constant of rotor
	m->tau		= 16.0 / (m->omega * m->lock);

	// off-axis flap
	m->w_off	= m->omega / (1.0 + sqr(m->omega / m->omega_f));

	// on-axis flap
	m->w_in		= m->omega / m->omega_f * m->w_off;

	// moment coef
	m->dl_db1	= 0.75 * m->b * m->c * sqr(m->r) * rho
		* sqr(m->v_tip) * m->a * m->e / ( m->lock*m->r );
	m->dm_da1	= m->dl_db1;

	// thrust coef
	m->ct		= cg->wt / ( rho * C_PI * sqr(m->r) * sqr(m->v_tip) );

	// solidity
	m->sigma	= m->b * m->c / (C_PI * m->r);

	// flap back coef
	m->db1dv	= -( 2.0 / m->v_tip )
		* (8.0 * m->ct / (m->a * m->sigma) + sqrt(m->ct/2.0) );

	// flab back coef
	m->da1du	= -m->db1dv;

	m->vi		= 15.0;
	m->a1		= 0.0;
	m->b1		= 0.0;
	m->a1dot	= 0.0;
	m->b1dot	= 0.0;
	m->thrust	= 0.0;
	m->F.fill();
	m->M.fill();
}


/*
 *  Flybar Information
 *
 * (Tischler and Mettler, System Identification Modeling
 * Of a Model-Scale Helicopter)
 */
void
Heli::setup_flybar()
{
	flybar_def *		fb = &this->fb;

	/* Parameters */
	fb->tau		= 0.36;			// sec
	fb->Kd		= 0.3;
	fb->Kc		= 0.3;

	/* Dyanmics */
	fb->c		= 0.0;
	fb->c_dot	= 0.0;
	fb->d		= 0.0;
	fb->d_dot	= 0.0;
}


void
Heli::setup_fins()
{
	Forces *		cg	= &this->cg;
	std::vector<Fin> &	fins	= this->fins;

	fins.clear();

	// Fuselage
	fins.push_back( Fin( cg,
		  3.0000,		// fs
		 12.0000,		// waterline
		 -0.4240,		// xuu
		 -1.2518,		// yvv
		 -0.8861		// zww
	));

	// Horizontal fin
	fins.push_back( Fin( cg,
		  0.0000,		// fs
		  0.0000,		// waterline
		 -1.0000,		// xuu
		  0.0000,		// yvv
		  0.0000		// zww
	));

	// Vertical fin
	fins.push_back( Fin( cg,
		-41.5000,		// fs
		  7.2500,		// wl
		  0.0000,		// xuu
		 -1.4339,		// yvv
		  0.0000		// zww
	));
}


/*
 * Tailrotor parameters
 */
void
Heli::setup_tail_rotor()
{
	tailrotor_def *		t	= &this->t;

	/* Parameters */
	t->fs		= -41.5;		// in
	t->wl		=   7.25;		// in
	t->r		=   0.5417;		// ft
	t->r0		=   0.083;		// ft
	t->a		=   3.0;		// */rad
	t->b		=   2;			// # of TR blades
	t->c		=   0.099;		// ft
	t->twst		=   0.0;		// rad
	t->cd0		=   0.01;		// nondimentional
	t->duct		=   0.0;		// duct augmetation (duct*thrust; power/duct)


	/* Dyanmics */
	t->omega	= this->c.tr_rev * C_TWOPI / 60.0;
	t->fr		= t->cd0 * t->r * t->b * t->c;
	t->vi		= 10.0;
	t->thrust	= 0.0;
	t->F.fill();
	t->M.fill();
}


/*
 * Compute CG relative positions for the different components
 */
void
Heli::compute_cg()
{
	Forces *		cg = &this->cg;

	// Waterline of the center of gravity
	double			wl = cg->wl_cg;

	// Fuselage station of the center of gravity
	double			fs = cg->fs_cg;


	mainrotor_def *		m = &this->m;
	m->h		= (wl - m->wl) / 12.0;
	m->d		= (fs - m->fs) / 12.0;

	tailrotor_def *		t = &this->t;
	t->h		= (wl - t->wl) / 12.0;
	t->d		= (fs - t->fs) / 12.0;
}


void
Heli::setup_sixdof()
{
	sixdof_fe_init_inputs_def sixinit;
	Forces *		cg = &this->cg;

	sixinit.Ixx	= cg->ix;
	sixinit.Iyy	= cg->iy;
	sixinit.Izz	= cg->iz;
	sixinit.Ixz	= cg->ixz;
	sixinit.m	= cg->m;

	sixinit.THETA.fill();
	sixinit.pqr.fill();
	sixinit.uvw.fill();
	sixinit.NED.fill();

	/* Start in low hover */
	sixinit.NED[2]	= -1.00;

	sixdof_fe_init(
		&sixinit,
		&this->sixdofIn,
		&this->sixdofX
	);
}



/*
 * This will perform the entire calculations of everything that needs
 * to happen to make the math model of the vehicle work.  This is the
 * function to call to propogate the helicopter model, 6-DOF,
 * landing gear, servos, and wind.
 */
void
Heli::step(
	double			model_dt,
	const double		U[4]
)
{
	Forces *		cg = &this->cg;
	const Force<Frame::Body>	g( cg->compute_gravity() );

	if( isnan( cg->F[0] ) )
		abort();

	this->do_forces( model_dt, g );
	if( isnan( cg->F[0] ) )
		abort();

	this->do_gear( model_dt );
	if( isnan( cg->F[0] ) )
		abort();

	this->do_servos( model_dt, U );
	if( isnan( cg->F[0] ) )
		abort();


	this->sixdofIn.F = cg->F;
	this->sixdofIn.M = cg->M;

	sixdof_fe(
		&this->sixdofX,
		&this->sixdofIn,
		model_dt
	);

	cg->NED		= this->sixdofX.NED;
	cg->uvw		= this->sixdofX.Vb;
	cg->V		= this->sixdofX.Ve;
	cg->THETA	= this->sixdofX.THETA;
	cg->pqr		= this->sixdofX.rate;

	/*
	 * Aaron says to do this after sixdof_fe() and memcpy().
	 * It is turned off right now because it costs 10 usec
	 * and we have zero wind all the time now.
	 */
	//this->do_wind( model_dt );

	/* Advance the time clock */
	cg->time += model_dt;

	// Remove the force of gravity to show what an accelerometer
	// would read at the cg.
	cg->F -= g;

	if( isnan( cg->NED[0] ) )
		abort();
}


/*
 * This will perform the initialization of the entire model.
 * It is also the function to call to reset the model.  For the case
 * of resetting, more than the minimum calculations are done, but this
 * miminimizes the number of functions to deal with.
 */
void
Heli::reset()
{
	/*
	 *  CG and controls must be setup before anything else.
	 */
	this->setup_cg();
	this->setup_controls();
	this->setup_main_rotor();
	this->setup_tail_rotor();
	this->setup_flybar();
	this->setup_fins();
	this->setup_wind();
	this->setup_gear();
	this->setup_servos();

	/*
	 * Now that everything is setup, compute the CG for the system
	 */
	this->compute_cg();

	/*
 	 * 6-DOF Initializations must happen after
	 * CG is computed.
	 */
	this->setup_sixdof();

	// CG Initialization
	this->cg.time		= 0.0;
	this->cg.altitude	= 0.0;

	this->cg.NED =   this->sixdofX.NED;
	this->cg.uvw =   this->sixdofX.Vb;
	this->cg.V =     this->sixdofX.Ve;
	this->cg.THETA = this->sixdofX.THETA;
	this->cg.pqr =   this->sixdofX.rate;

	this->cg.F.fill();
	this->cg.M.fill();

	// set/clear the holds
	this->sixdofIn.hold_p	= 0;
	this->sixdofIn.hold_q	= 0;
	this->sixdofIn.hold_r	= 0;
	this->sixdofIn.hold_u	= 0;
	this->sixdofIn.hold_v	= 0;
	this->sixdofIn.hold_w	= 0;
}

}