heli.cpp

UAV 自动驾驶的

/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: Heli.cpp,v 2.1 2003/03/08 05:15:30 tramm Exp $
 *
 * (c) Aaron Kahn
 * (c) Trammell Hudson
 *
 * This is the math model for the XCell model helicopter.  The
 * forces and momement are computed in this model.  The 6-DOF rigid body
 * motion, wind, servos, landing gear, and IMU are all take care of inside 
 * the simulation library.
 *
 *************
 *
 *  This file is part of the autopilot simulation package.
 *
 *  For more details:
 *
 *	http://autopilot.sourceforge.net/
 *
 *  Autopilot is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Autopilot is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Autopilot; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <ctime>

#include "macros.h"
#include "Heli.h"
#include "Servo.h"
#include "wind_model.h"
#include "Gear.h"
#include "Fin.h"

#include <mat/rk4.h>
#include <mat/Conversions.h>
#include <mat/Matrix.h>
#include <mat/Vector.h>
#include <mat/Vector_Rotate.h>
#include <mat/Quat.h>
#include <mat/Nav.h>

namespace sim {

using std::vector;
using namespace libmat;
using namespace util;

/*
 * this is for the rotor flapping
 *              .  .  . .
 *	Xdot = [a1 b1 d c]
 *	X = [a1 b1 d c]
 *	A1B1 = [A1 B1]
 *	args = [u v p q db1dv da1du w_in w_off kc dir tau Kc Kd]
 */
static void
RotorFlapDynamics(
	Vector<4> &		Xdot,
	const Vector<4> &	X,
	const double 		UNUSED( t ),
	const Vector<2> &	A1B1,
	const Vector<13> &	args
)
{
	double	u	= args[0];
	double	v	= args[1];
	double	p	= args[2];
	double	q	= args[3];
	double	db1dv	= args[4];
	double	da1du	= args[5];
	double	w_in	= args[6];
	double	w_off	= args[7];
	double	kc	= args[8];
	double	dir	= args[9];
	double	tau	= args[10];
	double	Kc	= args[11];
	double	Kd	= args[12];

	double	a1	= X[0];
	double	b1	= X[1];
	double	d	= X[2];
	double	c	= X[3];

	// flybar mixing (lateral and longitudinal)
	double	A1	= A1B1[0] + Kd*d;
	double	B1	= A1B1[1] + Kc*c;

	double	a_sum	= b1 - A1 + dir*kc*a1 - db1dv*v*0.3;
	double	b_sum	= a1 + B1 - dir*kc*b1 - da1du*u*0.3;
	double	a1dot	= -w_in*b_sum - w_off*a_sum - q;
	double	b1dot	= -w_in*a_sum + w_off*b_sum - p;

	double	d_dot = -d/tau - p + 0.2731*A1B1[0]/tau;
	double	c_dot = -c/tau - q - 0.2587*A1B1[1]/tau;

	Xdot[0] = a1dot;
	Xdot[1] = b1dot;
	Xdot[2] = d_dot;
	Xdot[3] = c_dot;

	// If this is unchecked, the program dies eventually
	if( isnan( Xdot[0] ) )
		abort();

#ifdef CHECK_NAN
	if( Xdot.isnan() )
		abort();
#endif
}


/*
 *  Initializes and runs the wind simulation
 */
void
Heli::setup_wind()
{
	wind_inputs_def *	params	= &this->wind_params;
	wind_state_def *	state	= &this->wind_state;

	params->seed		= time(0);
	params->wind_max	= Velocity<Frame::NED>(
		0,			// N
		0,			// E
		0			// D
	);

	wind_init(
		params,
		state
	);
}




void
Heli::do_wind(
	double			dt
)
{
	wind_inputs_def *	params	= &this->wind_params;
	wind_state_def *	state	= &this->wind_state;

	/* Update the wind state */
	wind_model(
		params,
		state,
		dt
	);

	/* Transform the NED wind velocities into body velocities */
	this->cg.uvw += rotate<Frame::Body>(
		state->Ve,
		this->cg.THETA
	);
}




/*
 * Servo Information (all servos are generic)
 *
 */
void
Heli::setup_servos()
{
	this->servos.clear();

	Servo		pitch( 	 -8.0*C_DEG2RAD,  8.0*C_DEG2RAD );
	Servo		roll(	 -8.0*C_DEG2RAD,  8.0*C_DEG2RAD );
	Servo		coll(	-12.5*C_DEG2RAD, 18.0*C_DEG2RAD );
	Servo		tr(	-20.0*C_DEG2RAD, 20.0*C_DEG2RAD );

	this->servos.push_back( pitch );
	this->servos.push_back( roll );
	this->servos.push_back( coll );
	this->servos.push_back( tr );
}


/*
 * This will perform the time stepping of the servo model.
 * The values of U[7] are as follows...
 *	U[0] = B1 swashplate tilt (+ nose down) (rad)
 *	U[1] = A1 swashplate tilt (+ right roll) (rad)
 *	U[2] = main rotor collective (rad)
 *	U[3] = tail rotor collective (rad)
 *
 *  The svIn[3] and svX[3] are as follows...
 *	sv[0] = B1 (pitch servo)
 *	sv[1] = A1 (roll servo)
 *	sv[2] = main rotor collective
 *	sv[3] = tail rotor collective
 */
void
Heli::do_servos(
	double			dt,
	const double		U[4]
)
{
	control_def *		c = &this->c;

	double *		controls[] = {
		&c->B1,
		&c->A1,
		&c->mr_col,
		&c->tr_col,
	};

	for( int i=0 ; i < 4 ; i++ )
		*controls[i] = this->servos[i].step( dt, U[i] );
}


/*
 *  Configure the landing gear with four points plus the
 * tail skid.
 */
void
Heli::setup_gear()
{
	Forces *		cg		= &this->cg;

	// Remove any that we had last time
	this->gear.clear();


	/*
	 *  Landing skids
	 */
	const double		skid_strength	= 5000; // lb/ft?
	const bool		training_gear	= 0;
	double			length		= 12.0;
	double			width		=  5.6;
	double			offset		=  2.0;
	double			height		= 15.0;

	if( training_gear )
	{
		length		= 30.0;
		width		= 30.0;
	 	offset		=  0.0;
		height		= 20.0;
	}

	this->gear = Gear::skids(
		cg,
		skid_strength,
		length,
		width,
		offset,
		height
	);

	/*
	 *  Skid on the tail is not as strong as the landing skids.
	 * It also has more friction.
	 */
	this->gear.push_back( Gear(
		"tail skid",
		skid_strength / 3.0,
		(-41.5 - cg->fs_cg) / 12.0,
		(  0.0                 ) / 12.0,
		( 15.0 - cg->wl_cg) / 12.0,
		140.0
	));


	/**
	 *  Add contact points for the main rotor.
	 * HTF do you append to a std::vector?
	 */
	const std::vector<Gear>	rotors = Gear::rotor(
		cg,
		this->m.r,
		this->m.fs,
		this->m.wl,
		140.0
	);

	FOR_ALL_CONST( std::vector<Gear>, gear, rotors,
		this->gear.push_back( *gear );
	);
}


/*
 * This will perform the landing gear calculations with the landing gear
 * model provided in the simulation library.
 */
void
Heli::do_gear(
	double			UNUSED( dt )
)
{
	Forces *		cg = &this->cg;

	// body -> earth transformation matrix
	const Matrix<3,3> 	cBE( eulerDC( cg->THETA.v ) );

	// make the cEB matrix
	// earth -> body transformation matrix
	const Matrix<3,3> 	cEB( cBE.transpose() );

	// make the wx matrix (omega-cross matrix)
	const Matrix<3,3>	wx( eulerWx( cg->pqr.v ) );


	FOR_ALL( vector<Gear>, g, this->gear,
		g->step( cg, cBE, cEB, wx );
	);
}


/*
 * This will perform the forces and moments calculations on the aircraft
 * before the calculations of the 6-DOF.  The landing gear calculations are done
 * after this function.  The servo and wind models are run after this as well.
 */
void
Heli::do_forces(
	double			dt,
	const Force<Frame::Body> &	g
)
{
	mainrotor_def *		m	= &this->m;
	flybar_def *		fb	= &this->fb;
	tailrotor_def *		t	= &this->t;
	Forces *		cg	= &this->cg;
	Blade *			mb	= &this->main_rotor_blade;
	Blade *			tb	= &this->tail_rotor_blade;
	control_def *		c	= &this->c;

	// Zero our moments, and compute our local gravitational vector.
	cg->M.fill();
	cg->F = g;

	// compute the current atmospheric conditions
	// density of air (slug/ft^3)
	double			rho	= cg->rho();


	// Main Rotor Calculations
	mb->a		= m->a;
	mb->b		= m->b;
	mb->c		= m->c;
	mb->Cd0		= m->cd0;
	mb->collective	= c->mr_col;
	mb->e		= 0.7;			// oswalds efficency factor
	mb->omega	= c->mr_rev*C_TWOPI/60.0;
	mb->R		= m->r;
	mb->R0		= m->ro;
	mb->rho		= rho;
	mb->twst	= m->twst;
	mb->Vperp	= cg->uvw[0] * (m->is + m->a1)
			- cg->uvw[1] * (m->ib + m->b1)
			- cg->uvw[2];

	mb->step();

	m->thrust	= mb->T;
	m->power	= mb->P;
	m->torque	= mb->Q;
	m->vi		= mb->avg_v1;

	m->F		= Force<Frame::Body>(
		-m->thrust*(m->is + m->a1),
		 m->thrust*(m->ib + m->b1),
		-m->thrust
	);

	m->M		= Moment<Frame::Body>(
		m->F[1] * m->h + m->dl_db1 * m->b1,
		m->F[2] * m->d + m->dm_da1 * m->a1 - m->F[0] * m->h,
		m->torque * m->dir
	);

	// Tail Rotor Calculations

	tb->a		= t->a;
	tb->b		= t->b;
	tb->c		= t->c;
	tb->Cd0		= t->cd0;
	tb->collective	= c->tr_col
			- c->gyro_gain * cg->pqr[2];
	tb->e		= 0.7;			// oswalds efficency factor
	tb->omega	= c->tr_rev*C_TWOPI/60.0;
	tb->R		= t->r;
	tb->R0		= t->r0;
	tb->rho		= rho;
	tb->twst	= t->twst;
	tb->Vperp	= m->dir * (cg->uvw[1] - t->d*cg->pqr[2]);

	tb->step();

	t->thrust	= tb->T + tb->T*t->duct;
	t->power	= tb->P - tb->P*t->duct;

	t->F		= Force<Frame::Body>(
		0.0,
		t->thrust * m->dir,
		0.0
	);

	t->M		= Moment<Frame::Body>(
		 t->F[1] * t->h,
		0.0,
		-t->F[1] * t->d
	);


	/*
	 *  Compute the aerodynamic forces from each of the
	 * fins / fuselage / skids / etc.  This uses the induced
	 * velocity from the main rotor, computed above.
	 */
	FOR_ALL( std::vector<Fin>, fin, this->fins,
		fin->step( cg, m->vi );
	);


	// Main Rotor TPP Dynamics
	// Everything gets wrapped into args for
	// the Runge Kutta routine.
	// for RK4 routine (rotor dynamics)
	Vector<4>		Xdot;
	Vector<4>		X(
		m->a1,
		m->b1,
		fb->d,
		fb->c
	);

	static double old_ma1;