Initial commit of "Rocket Workbench".
Taken from sources in CVS at:
https://sourceforge.net/projects/rocketworkbench/
Sources extracted in two steps:
1. Pull entire project tree into a subdir "rwb" via "rsync":
rsync -a a.cvs.sourceforge.net::cvsroot/rocketworkbench/ rwb/.
2. Export sources:
export CVSROOT=$(pwd)/rwb
SUBDIRS="analyser cpropep cpropep-web CVSROOT data libcompat libcpropep libnum libsimulation libthermo prop rocketworkbench rockflight"
mkdir rwbx; cd rwbx
cvs export -D now ${SUBDIRS}
After this (and some backups for safety), the directory content was
added to a Git repo:
git init .
git add *
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,291 @@
|
||||
|
||||
\documentclass[10pt]{article}
|
||||
|
||||
\usepackage{vmargin}
|
||||
\usepackage{amsmath}
|
||||
|
||||
|
||||
\newcommand{\er}{\vec{e}_r}
|
||||
\newcommand{\el}{\vec{e}_\lambda}
|
||||
\newcommand{\eu}{\vec{e}_\mu}
|
||||
|
||||
|
||||
|
||||
\begin{document}
|
||||
|
||||
\section{Inertial acceleration of the vehicle mass center in body
|
||||
reference frame}
|
||||
|
||||
Generally, the absolute acceleration of a particle $A$ in a moving
|
||||
reference frame of center B is:
|
||||
|
||||
$$\vec{a}_A = \vec{a}_B + \vec{\alpha}\times\vec{r} +
|
||||
\vec{\omega}\times(\vec{\omega}\times\vec{r})
|
||||
+2\vec{\omega}\times\vec{v}_{rel} + \vec{a}_{rel}$$
|
||||
|
||||
Choosing an earth centered reference frame with constant angular
|
||||
velocity ($\vec{\omega}^E$) and no acceleration yield to the simpler form
|
||||
|
||||
$$\vec{a}_{A_E} = \vec{\omega}^E_E\times(\vec{\omega}^E_E\times\vec{r}^E_E)
|
||||
+2\vec{\omega}^E_E\times\vec{v}^{rel}_E + \vec{a}^{rel}_E$$
|
||||
|
||||
where the subscript $E$ indicate that the vector are expressed in the
|
||||
earth reference frame.
|
||||
|
||||
Transforming this vectorial notation to matrix notation and noting
|
||||
that the angular velocity of $F_B$ relative to $F_E$ is
|
||||
$\vec{\omega}^B - \vec{\omega}^E$ yield to:
|
||||
|
||||
$$\vec{a}_{A_E} = \tilde{\omega}^E_E\tilde{\omega}^E_E\vec{r}^E_E
|
||||
+2\tilde{\omega}^E_E\vec{v}^{rel}_E + \vec{a}^{rel}_E$$
|
||||
|
||||
We could now expressed this equation in term of component in the body
|
||||
fixed reference frame.
|
||||
|
||||
$$\vec{a}_{A_B} = L_{BE}\vec{a}_{A_E} =
|
||||
L_{BE}(\tilde{\omega}^E_E\tilde{\omega}^E_E\vec{r}^E_E
|
||||
+2\tilde{\omega}^E_E\vec{v}^{rel}_E + \vec{a}^{rel}_E)$$
|
||||
|
||||
$$ = L_{BE}\tilde{\omega}^E_E\tilde{\omega}^E_E\vec{r}^E_E +
|
||||
2L_{BE}\tilde{\omega}^E_E\vec{v}^{rel}_E + (\tilde{\omega}^B -
|
||||
\tilde{\omega}^E)_B\vec{v}^{rel}_B + \vec{a}^{rel}_B$$
|
||||
|
||||
|
||||
We could then transform
|
||||
|
||||
$$L_{BE}\tilde{\omega}^E_E\vec{v}^{rel}_E = (L_{BE}\tilde{\omega}^E_E
|
||||
L_{EB})(L_{BE}\vec{v}^{rel}_E) = \tilde{\omega}^E_B\vec{v}^{rel}_B$$
|
||||
|
||||
and
|
||||
|
||||
$$L_{BE}\tilde{\omega}^E_E\tilde{\omega}^E_E\vec{r}^E_E =
|
||||
(L_{BE}\tilde{\omega}^E_E L_{EB})(L_{BE}\tilde{\omega}^E_E
|
||||
L_{EB})(L_{BE}\vec{r}^E_E) =
|
||||
\tilde{\omega}^E_B\tilde{\omega}^E_B\vec{r}^E_B$$
|
||||
|
||||
We now have
|
||||
|
||||
$$\vec{a}_{A_B} = \vec{a}^{rel}_B + (\tilde{\omega}^B -
|
||||
\tilde{\omega}^E)_B\vec{v}^E_B +
|
||||
\tilde{\omega}^E_B\tilde{\omega}^E_B\vec{r}^E_B +
|
||||
2\tilde{\omega}^E_B\vec{v}^{rel}_B$$
|
||||
|
||||
which simplify to
|
||||
|
||||
$$\vec{a}_{A_B} = \vec{a}^{rel}_B + (\tilde{\omega}^B +
|
||||
\tilde{\omega}^E)_B\vec{v}^E_B +
|
||||
\tilde{\omega}^E_B\tilde{\omega}^E_B\vec{r}^E_B$$
|
||||
|
||||
|
||||
Defining vector component in $F_B$
|
||||
|
||||
$$\vec{v}^E_B = \begin{bmatrix}u\\v\\w\end{bmatrix}$$
|
||||
$$\vec{\omega}^B_B = \begin{bmatrix}p\\q\\r\end{bmatrix}$$
|
||||
$$\vec{\omega}^E_B = \begin{bmatrix}p^E_B\\q^E_B\\r^E_B\end{bmatrix} =
|
||||
L_{BV}\begin{pmatrix}\cos{\lambda}\\0\\-\sin{\lambda}\end{pmatrix}\omega^E$$
|
||||
|
||||
and matrix notation of angular velocity
|
||||
|
||||
$$\tilde{\omega}^B_B =
|
||||
\begin{bmatrix}0&-r&q\\r&0&-p\\q&p&0\end{bmatrix}$$
|
||||
|
||||
$$\tilde{\omega}^E_B =
|
||||
\begin{bmatrix}0&-r^E_B&q^E_B\\r^E_B&0&-p^E_B\\q^E_B&p^E_B&0\end{bmatrix}$$
|
||||
|
||||
|
||||
\section{Acceleration of vehicle mass center espressed in a spherical
|
||||
coordinate system relative to rotating earth}
|
||||
|
||||
The spherical coordinate system is compose of the three vector
|
||||
($\er$, $\el$, $\eu$), where $\er$ point in the direction of the mass
|
||||
center of the vehicle, $\el$ point in the north direction and is
|
||||
perpendicular to $\er$, $\eu$ point east and is perpendicular to both
|
||||
previous vectors. We could also define the direction of $\er$ with two
|
||||
angle, $\mu$ is the angle between the equatorial projection of $\er$
|
||||
and a reference meridian on the earth. $\lambda$ is the angle between
|
||||
$\er$ and the equator. We could then define the relative velocity and
|
||||
acceleration of the point C relative to earth by:
|
||||
|
||||
$$\vec{v}_{rel} = \dot{r}\er + r\dot{\mu}\cos{\lambda}\eu + r\dot{\lambda}\el$$
|
||||
|
||||
\begin{align*}
|
||||
\vec{a}_{rel} = & (\ddot{r} - r\dot{\lambda}^2 -
|
||||
r\dot{\mu}^2\cos^2{\lambda})\er +\\
|
||||
& ((2\dot{r}\dot{\mu} + r\ddot{\mu})\cos{\lambda} -
|
||||
2r\dot{\mu}\dot{\lambda}\sin{\lambda})\eu +\\
|
||||
& (2\dot{r}\dot{\lambda} + r\ddot{\lambda} +
|
||||
r\dot{\mu}^2\sin{\lambda}\cos{\lambda})\el
|
||||
\end{align*}
|
||||
|
||||
|
||||
We can now derive the inertial velocity and acceleration of C:
|
||||
|
||||
\begin{align*}
|
||||
\vec{v}_C & = \vec{v}_O + \vec{v}_{rel} + \vec{\omega}\times\vec{r}\\
|
||||
& = \dot{r}\er + r\dot{\mu}\cos{\lambda}\eu +
|
||||
r\dot{\lambda}\el + \omega\vec{k}\times r\er\\
|
||||
& = \dot{r}\er + r\cos{\lambda}(\dot{\mu} + \omega)\eu
|
||||
+ r\dot{\lambda}\el
|
||||
\end{align*}
|
||||
|
||||
$$\vec{a}_C = \vec{a}_O + \vec{\alpha}\times\vec{r} +
|
||||
\vec{\omega}\times(\vec{\omega}\times\vec{r})
|
||||
+2\vec{\omega}\times\vec{v}_{rel} + \vec{a}_{rel}$$
|
||||
|
||||
Each term will be evaluate separatly:
|
||||
|
||||
\begin{align*}
|
||||
\vec{\omega}\times(\vec{\omega}\times\vec{r}) & =
|
||||
\omega\vec{k}\times(\omega\vec{k}\times r\er)\\
|
||||
& = \omega\vec{k}\times(\omega r\cos{\lambda}\eu)\\
|
||||
& = \omega^2r\cos{\lambda}(-\cos{\lambda}\er+\sin{\lambda}\el)\\
|
||||
& = -\omega^2r\cos^2{\lambda}\er + \omega^2r\cos{\lambda}\sin{\lambda}\el
|
||||
\end{align*}
|
||||
|
||||
\begin{align*}
|
||||
\vec{\omega}\times\vec{v}_{rel} & = \omega\vec{k}\times(\dot{r}\er +
|
||||
r\dot{\mu}\cos{\lambda}\eu + r\dot{\lambda}\el)\\
|
||||
& = \omega\dot{r}\cos{\lambda}\eu + \omega
|
||||
r\dot{\mu}\cos{\lambda}(-\cos{\lambda}\er + \sin{\lambda}\el) +
|
||||
\omega r\dot{\lambda}(-\cos{\lambda})\eu\\
|
||||
& = \omega\cos{\lambda}(\dot{r}-r\dot{\lambda})\eu - \omega
|
||||
r\dot{\mu}\cos^2{\lambda}\er + \omega r\dot{\mu}\cos{\lambda}\sin{\lambda}\el
|
||||
\end{align*}
|
||||
|
||||
|
||||
The inertial acceleration of C is then:
|
||||
|
||||
\begin{align*}
|
||||
\vec{a}_C = & (\ddot{r} - r\dot{\lambda}^2 -
|
||||
r\dot{\mu}^2\cos^2{\lambda} - 2\omega
|
||||
r\dot{\mu}\cos^2{\lambda} - \omega^2r\cos^2{\lambda})\er\\
|
||||
& + ((2\dot{r}\dot{\mu} + r\ddot{\mu})\cos{\lambda} -
|
||||
2r\dot{\mu}\dot{\lambda}\sin{\lambda}
|
||||
+ 2\omega\cos{\lambda}(\dot{r}-r\dot{\lambda})\eu\\
|
||||
& + (2\dot{r}\dot{\lambda} + r\ddot{\lambda} +
|
||||
r\dot{\mu}^2\sin{\lambda}\cos{\lambda} +2\omega
|
||||
r\dot{\mu}\cos{\lambda}\sin{\lambda} +
|
||||
\omega^2r\cos{\lambda}\sin{\lambda})\el
|
||||
\end{align*}
|
||||
|
||||
which simplify to:
|
||||
\begin{align*}
|
||||
\vec{a}_C = & (\ddot{r} - r\dot{\lambda}^2 -
|
||||
r(\omega+\dot{\mu})^2\cos^2{\lambda})\er\\
|
||||
& + ((2\dot{r}\dot{\mu} + r\ddot{\mu})\cos{\lambda} -
|
||||
2r\dot{\mu}\dot{\lambda}\sin{\lambda}
|
||||
+ 2\omega\cos{\lambda}(\dot{r}-r\dot{\lambda})\eu\\
|
||||
& + (2\dot{r}\dot{\lambda} + r\ddot{\lambda} +
|
||||
r\sin{\lambda}\cos{\lambda}(\omega+\dot{\mu})^2)\el
|
||||
\end{align*}
|
||||
|
||||
|
||||
We must now project the inertial acceleration from this vehicled
|
||||
carried frame to the bidy fixed frame with the conversion matrix based
|
||||
on the euler angle which define the angular position of $F_B$ relative
|
||||
to $F_V$.
|
||||
|
||||
$$L_{BV} = \begin{bmatrix} \cos{\theta}\cos{\psi}&
|
||||
\cos{\theta}\sin{\psi}& -\sin{\theta}\\
|
||||
\sin{\phi}\sin{\theta}\cos{\psi}-\cos{\phi}\sin{\psi}&
|
||||
\sin{\phi}\sin{\theta}\sin{\psi}+\cos{\phi}\cos{\psi}&
|
||||
\sin{\phi}\cos{\theta}\\
|
||||
\cos{\phi}\sin{\theta}\cos{\psi}+\sin{\phi}\sin{\psi}&
|
||||
\cos{\phi}\sin{\theta}\sin{\psi}-\sin{\phi}\cos{\psi}&
|
||||
\cos{\phi}\cos{\theta}
|
||||
\end{bmatrix}
|
||||
$$
|
||||
|
||||
\begin{align*}
|
||||
\vec{a}_{C_B} & = L_{BV}\vec{a}_C\\
|
||||
& = L_{BV}(\vec{\omega}\times(\vec{\omega}\times\vec{r})
|
||||
+ 2\vec{\omega}\times\vec{v}_{rel} + \vec{a}_{rel})\\
|
||||
& = L_{BV}\begin{bmatrix}r\cos{\lambda}\sin{\lambda}\\ 0\\
|
||||
-r\cos^2{\lambda}\end{bmatrix}\omega^2 +
|
||||
2\vec{\omega}^E_B\times\vec{v}_{rel_B} +
|
||||
\vec{a}_{rel_B} + (\vec{\omega}^B -
|
||||
\vec{\omega}^E)_B\times\vec{v}^E_B \\
|
||||
& = L_{BV}\begin{bmatrix}r\cos{\lambda}\sin{\lambda}\\ 0\\
|
||||
-r\cos^2{\lambda}\end{bmatrix}\omega^2 +
|
||||
\vec{a}_{rel_B} + (\vec{\omega}^B +
|
||||
\vec{\omega}^E)_B\times\vec{v}^E_B\\
|
||||
& = \begin{bmatrix}\dot{u}\\\dot{v}\\\dot{w}\end{bmatrix} +
|
||||
\begin{bmatrix}c_x\\c_y\\c_z\end{bmatrix} +
|
||||
\begin{bmatrix}(q + q^E_B)w - (r + r^E_B)v\\
|
||||
(r + r^E_B)u - (p + p^E_B)w\\
|
||||
(p + p^E_B)v - (q + q^E_B)u\end{bmatrix}
|
||||
\end{align*}
|
||||
|
||||
|
||||
|
||||
We will also use the moment equation:
|
||||
|
||||
\begin{align*}
|
||||
L & = I_x\dot{p} - (I_y - I_z)qr\\
|
||||
M & = I_y\dot{q} - (I_z - I_x)rp\\
|
||||
N & = I_z\dot{r} - (I_x - I_y)pq
|
||||
\end{align*}
|
||||
|
||||
|
||||
\section{System of equation}
|
||||
|
||||
\subsection{Independant variable}
|
||||
|
||||
\begin{align*}
|
||||
(\phi, \theta, \psi)\quad & \text{Euler angle defining orientation of the
|
||||
vehicle relative to $F_V$}\\
|
||||
(u, v, w)\quad & \text{Velocity of the vehicle relative to earth}\\
|
||||
(p, q, r)\quad & \text{Absolute angle rate}\\
|
||||
(\lambda, \mu, R)\quad & \text{Position of the rocket relative to earth}\\
|
||||
(p^E_B, q^E_B, r^E_B)\quad & \text{Earth angular velocity in body axis}\\
|
||||
(P, Q, R) \quad & \text{Relative angular velocity of body relative ro $F_V$
|
||||
expressed in $F_B$}
|
||||
\end{align*}
|
||||
|
||||
\subsection{List of equations}
|
||||
|
||||
$$X - mg\sin{\theta} = m(\dot{u} + c_x + (q^E_B+q)w - (r^E_B+r)v)$$
|
||||
$$Y + mg\cos{\theta}\sin{\phi} = m(\dot{v} + c_y + (r^E_B+r)u - (p^E_B+p)w)$$
|
||||
$$Z + mg\cos{\theta}\cos{\phi} = m(\dot{w} + c_z + (p^E_B+pvw -
|
||||
(q^E_B+q)u)$$
|
||||
|
||||
\begin{align*}
|
||||
L & = I_x\dot{p} - (I_y - I_z)qr\\
|
||||
M & = I_y\dot{q} - (I_z - I_x)rp\\
|
||||
N & = I_z\dot{r} - (I_x - I_y)pq
|
||||
\end{align*}
|
||||
|
||||
$$\begin{bmatrix}\dot{\phi}\\\dot{\theta}\\\dot{\psi}\end{bmatrix} =
|
||||
\begin{bmatrix}1&\sin{\phi}\tan{\theta}&\cos{\phi}\tan{\theta}\\
|
||||
0&\cos{\phi}&-\sin{\phi}\\
|
||||
0&\sin{\phi}\sec{\theta}&\cos{\phi}\sec{\theta}\end{bmatrix}
|
||||
\begin{bmatrix}P\\Q\\R\end{bmatrix}$$
|
||||
|
||||
|
||||
$$\begin{bmatrix}P\\Q\\R\end{bmatrix} =
|
||||
\begin{bmatrix}p\\q\\r\end{bmatrix} -
|
||||
L_{BV}\begin{bmatrix}(\omega^E -
|
||||
\dot{\mu})\cos{\lambda}\\-\dot{\lambda}\\
|
||||
-(\omega^E + \dot{\mu})\sin{\lambda}\end{bmatrix}$$
|
||||
|
||||
$$\begin{bmatrix}r\dot{\lambda}\\r\dot{\mu}\cos{\lambda}\\\dot{r}
|
||||
\end{bmatrix} = L_{VB}\begin{bmatrix}u\\v\\w\end{bmatrix}$$
|
||||
|
||||
$$\vec{\omega}^E_B = \begin{bmatrix}p^E_B\\q^E_B\\r^E_B\end{bmatrix} =
|
||||
L_{BV}\begin{pmatrix}\cos{\lambda}\\0\\-\sin{\lambda}\end{pmatrix}\omega^E$$
|
||||
|
||||
|
||||
$$L_{VB} = \begin{bmatrix}\cos{\theta}\cos{\psi}&
|
||||
\sin{\phi}\sin{\theta}\cos{\psi} - \cos{\phi}\sin{\psi}&
|
||||
\cos{\phi}\sin{\theta}\cos{\psi} + \sin{\phi}\sin{\psi}\\
|
||||
\cos{\theta}\sin{\psi}&
|
||||
\sin{\phi}\sin{\theta}\sin{\psi} + \cos{\phi}\cos{\psi}&
|
||||
\cos{\phi}\sin{\theta}\sin{\psi} - \sin{\phi}\cos{\psi}\\
|
||||
-\sin{\theta}&
|
||||
\sin{\phi}\cos{\theta}&
|
||||
\cos{\phi}\cos{\theta}\end{bmatrix}$$
|
||||
|
||||
\end{document}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
|
||||
CC = gcc
|
||||
COPT = -g -Wall -O3 #-pg
|
||||
|
||||
ROOT = ../..
|
||||
|
||||
LIB = -lMagick -lrockflight -lnum -lm
|
||||
|
||||
LIBDIR = -L$(ROOT)/libnum/lib -L/usr/X11R6/lib/ -L../lib/
|
||||
|
||||
INCDIR = -I$(ROOT)/libnum/include -I../src/ \
|
||||
-I/usr/X11R6/include/magick -I/usr/X11R6/include/
|
||||
|
||||
DEF =
|
||||
|
||||
PROG = viewer
|
||||
|
||||
OBJS = viewer.o
|
||||
|
||||
all: $(PROG) $(PROG2)
|
||||
|
||||
.c.o:
|
||||
$(CC) $(DEF) $(INCDIR) $(COPT) -c $*.c -o $*.o
|
||||
|
||||
$(PROG): $(OBJS) $(LIBNAME)
|
||||
$(CC) $(COPT) $(OBJS) $(LIBDIR) $(LIB) -o $@
|
||||
|
||||
clean:
|
||||
rm -f *.o *~
|
||||
|
||||
deep-clean: clean
|
||||
rm -f $(PROG)
|
||||
@@ -0,0 +1,454 @@
|
||||
|
||||
#include <X11/Xlib.h> // Every Xlib program must include this
|
||||
#include <linux/mc146818rtc.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h> // So we got the profile for 10 seconds
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "rocflight.h"
|
||||
#include "num.h"
|
||||
#include "state.h"
|
||||
|
||||
|
||||
#define PIXEL_M 15
|
||||
#define PIXEL_M_S 15
|
||||
|
||||
#define H 40
|
||||
#define L 40
|
||||
#define SCALEY 1
|
||||
|
||||
#define WIDTH L * PIXEL_M
|
||||
#define HEIGHT (int)(H * PIXEL_M * SCALEY)
|
||||
|
||||
#define MAX_HISTORY 200
|
||||
#define FREQ 32
|
||||
#define STEP (1.0/FREQ)
|
||||
|
||||
#define POSX_TO_PIXEL(x) (WIDTH/2 + (int)((x) * PIXEL_M))
|
||||
#define POSY_TO_PIXEL(y) (HEIGHT - (int)((y) * PIXEL_M * SCALEY))
|
||||
|
||||
|
||||
struct rocketAnim_t
|
||||
{
|
||||
int width, height;
|
||||
|
||||
GC gc;
|
||||
GC tmpgc;
|
||||
Display *display;
|
||||
Window window;
|
||||
Pixmap pixmap;
|
||||
unsigned long red, blue, white, green;
|
||||
|
||||
XFontStruct *font_info;
|
||||
int font_height;
|
||||
|
||||
} rocket;
|
||||
|
||||
|
||||
|
||||
int eom(int neq, double time, double *y, double *dy, void *data);
|
||||
|
||||
|
||||
|
||||
unsigned long AllocColor(char *colName)
|
||||
{
|
||||
Colormap cmap;
|
||||
XColor c0, c1;
|
||||
cmap = DefaultColormap(rocket.display, 0);
|
||||
XAllocNamedColor(rocket.display, cmap, colName, &c1, &c0);
|
||||
return(c1.pixel);
|
||||
}
|
||||
|
||||
void ExposeScreen(void)
|
||||
{
|
||||
XCopyArea(rocket.display, rocket.pixmap,
|
||||
rocket.window, rocket.tmpgc,
|
||||
0, 0, rocket.width, rocket.height, 0, 0);
|
||||
}
|
||||
|
||||
void rocketInit(void)
|
||||
{
|
||||
Display *d;
|
||||
Window w;
|
||||
Pixmap p;
|
||||
XEvent e;
|
||||
|
||||
d = rocket.display = XOpenDisplay(NULL);
|
||||
|
||||
w = rocket.window = XCreateSimpleWindow(d, DefaultRootWindow(d),
|
||||
0, 0,
|
||||
rocket.width, rocket.height,
|
||||
2, BlackPixel(d, 0),
|
||||
WhitePixel(d, 0));
|
||||
|
||||
p = rocket.pixmap = XCreatePixmap(d, DefaultRootWindow(d),
|
||||
rocket.width, rocket.height,
|
||||
DefaultDepth(d, 0));
|
||||
|
||||
rocket.gc = XCreateGC(d, w, 0, 0);
|
||||
rocket.tmpgc = XCreateGC(d, w, 0, 0);
|
||||
rocket.red = AllocColor("red");
|
||||
rocket.blue = AllocColor("blue");
|
||||
rocket.white = AllocColor("white");
|
||||
rocket.green = AllocColor("green");
|
||||
|
||||
rocket.font_info = XQueryFont(d, XGContextFromGC(rocket.gc));
|
||||
rocket.font_height = rocket.font_info->ascent + rocket.font_info->descent;
|
||||
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.white);
|
||||
XFillRectangle(d, p, rocket.gc, 0, 0, rocket.width, rocket.height);
|
||||
XMapWindow(d, w);
|
||||
XSelectInput(d, w, StructureNotifyMask);
|
||||
|
||||
for(;;) {
|
||||
XNextEvent(d, &e);
|
||||
if (e.type == MapNotify)
|
||||
break;
|
||||
}
|
||||
XFlush(d);
|
||||
}
|
||||
|
||||
void DrawLine(int x1, int y1, int x2, int y2)
|
||||
{
|
||||
/* Draw the new line */
|
||||
XDrawLine(rocket.display, rocket.pixmap, rocket.gc, x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
void DrawPoint(int x, int y)
|
||||
{
|
||||
/* draw the point */
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.red);
|
||||
XDrawPoint(rocket.display, rocket.pixmap, rocket.gc, x, y);
|
||||
}
|
||||
|
||||
void DrawFillCircle(int x, int y, int r)
|
||||
{
|
||||
/* Draw the circle */
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.red);
|
||||
XFillArc(rocket.display, rocket.pixmap, rocket.gc,
|
||||
x - r, y - r, 2*r, 2*r, 0, 360*64);
|
||||
}
|
||||
|
||||
void DrawSystem(double *y)
|
||||
{
|
||||
int i, j;
|
||||
|
||||
double x1, y1;
|
||||
|
||||
double theta;
|
||||
double vitesse_tot;
|
||||
|
||||
char vitessex[32];
|
||||
char vitessey[32];
|
||||
char vitesset[32];
|
||||
|
||||
char alpha_str[32];
|
||||
|
||||
char theta_str[32];
|
||||
char velocity_str[32];
|
||||
|
||||
double velocity;
|
||||
|
||||
theta = y[10];
|
||||
|
||||
velocity = sqrt(y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
|
||||
|
||||
vitesse_tot =
|
||||
sqrt(pow(y[0], 2) + pow(y[2], 2));
|
||||
|
||||
sprintf(velocity_str, "Velocity : % .2f m/s", velocity);
|
||||
sprintf(alpha_str, "Angle of attack : % 6.2f deg", atan(y[2]/y[0])*180/M_PI);
|
||||
sprintf(theta_str, "Theta : % .2f deg", theta*180/M_PI);
|
||||
|
||||
sprintf(vitessex, "Vitesse axe x : % .2f", y[0]);
|
||||
sprintf(vitessey, "Vitesse axe y : % .2f", y[2]);
|
||||
sprintf(vitesset, "Vitesse absolue: % .2f", vitesse_tot);
|
||||
|
||||
/* Clear the screen */
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.white);
|
||||
XFillRectangle(rocket.display, rocket.pixmap,
|
||||
rocket.gc, 0, 0, rocket.width, rocket.height);
|
||||
|
||||
//DrawFillCircle(POSX_TO_PIXEL(x), POSY_TO_PIXEL(y), 10);
|
||||
//printf("%f\n", y[10]);
|
||||
|
||||
|
||||
/* Draw the rocket */
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.red);
|
||||
|
||||
x1 = 0 + 8*cos(theta);
|
||||
y1 = 20 + 8*sin(theta);
|
||||
|
||||
DrawLine(POSX_TO_PIXEL(0 - sin(theta)), POSY_TO_PIXEL(20 + cos(theta)),
|
||||
POSX_TO_PIXEL(x1 - sin(theta)), POSY_TO_PIXEL(y1 + cos(theta)));
|
||||
DrawLine(POSX_TO_PIXEL(0 + sin(theta)), POSY_TO_PIXEL(20 - cos(theta)),
|
||||
POSX_TO_PIXEL(x1 + sin(theta)), POSY_TO_PIXEL(y1 - cos(theta)));
|
||||
DrawLine(POSX_TO_PIXEL(x1 - sin(theta)), POSY_TO_PIXEL(y1 + cos(theta)),
|
||||
POSX_TO_PIXEL(x1 + 4*cos(theta)), POSY_TO_PIXEL(y1 + 4*sin(theta)));
|
||||
DrawLine(POSX_TO_PIXEL(x1 + sin(theta)), POSY_TO_PIXEL(y1 - cos(theta)),
|
||||
POSX_TO_PIXEL(x1 + 4*cos(theta)), POSY_TO_PIXEL(y1 + 4*sin(theta)));
|
||||
|
||||
|
||||
XSetForeground(rocket.display, rocket.gc, rocket.blue);
|
||||
|
||||
|
||||
x1 = (y[0]*cos(theta) - y[2]*sin(theta))/50;
|
||||
y1 = 20 + (y[0]*sin(theta) + y[2]*cos(theta))/50;
|
||||
|
||||
DrawLine(POSX_TO_PIXEL(0), POSY_TO_PIXEL(20),
|
||||
POSX_TO_PIXEL(x1), POSY_TO_PIXEL(y1));
|
||||
|
||||
//DrawLine(x, y,
|
||||
|
||||
/*
|
||||
if (modeleInfo.full)
|
||||
j = MAX_HISTORY;
|
||||
else
|
||||
j = modeleInfo.lasthistory;
|
||||
|
||||
for (i = 0; i < j; i++)
|
||||
DrawPoint(POSX_TO_PIXEL(modeleInfo.pointhistory[i][0]),
|
||||
POSY_TO_PIXEL(modeleInfo.pointhistory[i][1]));
|
||||
*/
|
||||
|
||||
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 10,
|
||||
HEIGHT - 3*rocket.font_height, velocity_str,
|
||||
strlen(velocity_str));
|
||||
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 10,
|
||||
HEIGHT - 2*rocket.font_height, alpha_str, strlen(alpha_str));
|
||||
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 10,
|
||||
HEIGHT - rocket.font_height, theta_str, strlen(theta_str));
|
||||
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 100,
|
||||
HEIGHT - 3*rocket.font_height, vitessex,
|
||||
strlen(vitessex));
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 100,
|
||||
HEIGHT - 2*rocket.font_height, vitessey,
|
||||
strlen(vitessey));
|
||||
XDrawString(rocket.display, rocket.pixmap, rocket.gc, 100,
|
||||
HEIGHT - 1*rocket.font_height, vitesset,
|
||||
strlen(vitesset));
|
||||
|
||||
|
||||
|
||||
/* XFillRectangle(rocket.display, rocket.pixmap, rocket.gc, 140,
|
||||
HEIGHT - 4*rocket.font_height + rocket.font_info->ascent/2,
|
||||
(int)(PIXEL_M_S * fabs(modeleInfo.state[2])),
|
||||
rocket.font_height/2);
|
||||
|
||||
XFillRectangle(rocket.display, rocket.pixmap, rocket.gc, 140,
|
||||
HEIGHT - 3*rocket.font_height + rocket.font_info->ascent/2,
|
||||
(int)(PIXEL_M_S * fabs(modeleInfo.state[3])),
|
||||
rocket.font_height/2);
|
||||
|
||||
XFillRectangle(rocket.display, rocket.pixmap, rocket.gc, 140,
|
||||
HEIGHT - 2*rocket.font_height + rocket.font_info->ascent/2,
|
||||
(int)(PIXEL_M_S * vitesse_tot),
|
||||
rocket.font_height/2);
|
||||
|
||||
*/
|
||||
|
||||
ExposeScreen();
|
||||
XFlush(rocket.display);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
int i, n;
|
||||
int neq = 12;
|
||||
|
||||
double time;
|
||||
double res[12];
|
||||
|
||||
double *ans;
|
||||
double *init_cond;
|
||||
|
||||
double u = 100; /* velocity component in x */
|
||||
double v = 0; /* velocity component in y */
|
||||
double w = 0; /* velocity component in z */
|
||||
|
||||
double P = 0; /* angular velocity */
|
||||
double Q = 0; /* */
|
||||
double R = 0; /* */
|
||||
|
||||
double lambda = 0*(M_PI/180); /* latitude (positive north) */
|
||||
double mu = 0*(M_PI/180); /* longitude (positive east) */
|
||||
double Re = EARTH_RAD + 5000; /* distance from earth center */
|
||||
double phi = 0*(M_PI/180); /* */
|
||||
double theta = 45*(M_PI/180); /* positive up */
|
||||
double psi = 0*(M_PI/180); /* positive cw*/
|
||||
|
||||
|
||||
// double dt = 0.05;
|
||||
double duration = 10;
|
||||
|
||||
state_t s;
|
||||
|
||||
|
||||
int fd, retval;
|
||||
unsigned long tmp, data;
|
||||
struct rtc_time rtc_tm;
|
||||
|
||||
/* Initialize the window */
|
||||
rocket.width = WIDTH;
|
||||
rocket.height = HEIGHT;
|
||||
rocketInit();
|
||||
|
||||
|
||||
/* open and configure the realtime clock */
|
||||
fd = open ("/dev/rtc", O_RDONLY);
|
||||
if (fd == -1)
|
||||
{
|
||||
perror("/dev/rtc");
|
||||
exit(errno);
|
||||
}
|
||||
|
||||
/* Set the periodic IRQ rate to FREQ Hz */
|
||||
retval = ioctl(fd, RTC_IRQP_SET, FREQ);
|
||||
if (retval == -1)
|
||||
{
|
||||
perror("ioctl");
|
||||
exit(errno);
|
||||
}
|
||||
|
||||
/* Enable periodic interrupts */
|
||||
retval = ioctl(fd, RTC_PIE_ON, 0);
|
||||
if (retval == -1)
|
||||
{
|
||||
perror("ioctl");
|
||||
exit(errno);
|
||||
}
|
||||
|
||||
//initModele();
|
||||
|
||||
|
||||
s.m = 10.0;
|
||||
|
||||
s.Ix = 25e-3;
|
||||
s.Iy = 3;
|
||||
s.Iz = 3;
|
||||
|
||||
s.Faero[0] = 0;
|
||||
s.Faero[1] = 0;
|
||||
s.Faero[2] = 0;
|
||||
|
||||
s.Maero[0] = 0;
|
||||
s.Maero[1] = 0;
|
||||
s.Maero[2] = 0;
|
||||
|
||||
init_cond = (double *) malloc(sizeof(double) * neq);
|
||||
|
||||
s.L_BV[0][0] = cos(theta)*cos(psi);
|
||||
s.L_BV[0][1] = cos(theta)*sin(psi);
|
||||
s.L_BV[0][2] =-sin(theta);
|
||||
s.L_BV[1][0] = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
|
||||
s.L_BV[1][1] = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
|
||||
s.L_BV[1][2] = sin(phi)*cos(theta);
|
||||
s.L_BV[2][0] = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
|
||||
s.L_BV[2][1] = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
|
||||
s.L_BV[2][2] = cos(phi)*cos(theta);
|
||||
|
||||
init_cond[0] = u; /* velocity component in x */
|
||||
init_cond[1] = v; /* velocity component in y */
|
||||
init_cond[2] = w; /* velocity component in z */
|
||||
init_cond[3] = P+ (s.L_BV[0][0]*WE*cos(lambda)-s.L_BV[0][2]*WE*sin(lambda));
|
||||
init_cond[4] = Q+ (s.L_BV[1][0]*WE*cos(lambda)-s.L_BV[1][2]*WE*sin(lambda));
|
||||
init_cond[5] = R+ (s.L_BV[2][0]*WE*cos(lambda)-s.L_BV[2][2]*WE*sin(lambda));
|
||||
init_cond[6] = lambda; /* latitude */
|
||||
init_cond[7] = mu; /* longitude */
|
||||
init_cond[8] = Re; /* radius */
|
||||
init_cond[9] = phi; /* phi */
|
||||
init_cond[10] = theta; /* theta */
|
||||
init_cond[11] = psi; /* psi */
|
||||
|
||||
time = 0;
|
||||
|
||||
// eom(neq, time, init_cond, res, &s);
|
||||
/*
|
||||
fprintf(stderr, "% e (u dot)\n", res[0]);
|
||||
fprintf(stderr, "% e (v dot)\n", res[1]);
|
||||
fprintf(stderr, "% e (w dot)\n", res[2]);
|
||||
fprintf(stderr, "% e (p dot)\n", res[3]);
|
||||
fprintf(stderr, "% e (q dot)\n", res[4]);
|
||||
fprintf(stderr, "% e (r dot)\n", res[5]);
|
||||
fprintf(stderr, "% e (lambda dot)\n", res[6]);
|
||||
fprintf(stderr, "% e (mu dot)\n", res[7]);
|
||||
fprintf(stderr, "% e (R dot)\n", res[8]);
|
||||
fprintf(stderr, "% e (phi dot)\n", res[9]);
|
||||
fprintf(stderr, "% e (theta dot)\n", res[10]);
|
||||
fprintf(stderr, "% e (psi dot)\n", res[11]);
|
||||
*/
|
||||
|
||||
n = NUM_rk4 (eom, neq, STEP, duration, init_cond, &ans, &s);
|
||||
|
||||
|
||||
|
||||
/* set the initial conditions */
|
||||
//modeleInfo.state[0] = 7.0;
|
||||
//modeleInfo.state[1] = 25.0;//.3944;
|
||||
//modeleInfo.state[2] = 0.0;
|
||||
//modeleInfo.state[3] = 0.0;
|
||||
|
||||
|
||||
// for( i = 0; i < n; i++)
|
||||
// {
|
||||
// printf("% .12e ", ans[0 + neq*i]); /* u */
|
||||
// printf("% .12e ", ans[1 + neq*i]); /* v */
|
||||
// printf("% .12e ", ans[2 + neq*i]); /* w */
|
||||
// printf("% .12e ", ans[3 + neq*i]); /* p */
|
||||
// printf("% .12e ", ans[4 + neq*i]); /* q */
|
||||
// printf("% .12e ", ans[5 + neq*i]); /* r */
|
||||
// printf("% .12e ", ans[6 + neq*i]); /* lambda */
|
||||
// printf("% .12e ", ans[7 + neq*i]); /* mu */
|
||||
// printf("% .12e ", ans[8 + neq*i]); /* R */
|
||||
// printf("% .12e ", ans[9 + neq*i]); /* phi */
|
||||
// printf("% .12e ", ans[10 + neq*i]); /* theta */
|
||||
// printf("% .12e \n", ans[11 + neq*i]); /* psi */
|
||||
// }
|
||||
|
||||
|
||||
// DrawSystem(y);
|
||||
|
||||
for (i = 0; i < n; i++)
|
||||
{
|
||||
/* This blocks */
|
||||
retval = read(fd, &data, sizeof(unsigned long));
|
||||
if (retval == -1)
|
||||
{
|
||||
perror("read");
|
||||
exit(errno);
|
||||
}
|
||||
|
||||
DrawSystem(ans + neq*i);
|
||||
}
|
||||
|
||||
/* Disable periodic interrupts */
|
||||
retval = ioctl(fd, RTC_PIE_OFF, 0);
|
||||
if (retval == -1)
|
||||
{
|
||||
perror("ioctl");
|
||||
exit(errno);
|
||||
}
|
||||
|
||||
|
||||
free(init_cond);
|
||||
//free(ans);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
|
||||
CC = gcc
|
||||
COPT = -g -Wall -O3 #-pg
|
||||
|
||||
ROOT = ../..
|
||||
LIB = -lrockflight -lnum -lm
|
||||
LIBDIR = -L$(ROOT)/libnum/lib \
|
||||
-L../lib/
|
||||
INCDIR = -I$(ROOT)/libnum/include \
|
||||
-I../include/
|
||||
LIBNAME = librockflight.a
|
||||
|
||||
DEF =
|
||||
PROG = rocflight
|
||||
OBJS = rocflight.o
|
||||
|
||||
LIBOBJS = eom.o atmos.o aero.o engine.o
|
||||
|
||||
all: $(LIBNAME) $(PROG)
|
||||
|
||||
.c.o:
|
||||
$(CC) $(DEF) $(INCDIR) $(COPT) -c $*.c -o $*.o
|
||||
|
||||
$(LIBNAME): $(LIBOBJS)
|
||||
ar -r $@ $(LIBOBJS)
|
||||
ranlib $@
|
||||
mv $(LIBNAME) ../lib
|
||||
|
||||
|
||||
$(PROG): $(OBJS) $(LIBNAME)
|
||||
$(CC) $(COPT) $(OBJS) $(LIBDIR) $(LIB) -o $@
|
||||
|
||||
clean:
|
||||
rm -f *.o *~
|
||||
|
||||
deep-clean: clean
|
||||
rm -f ../lib/$(LIBNAME) $(PROG)
|
||||
@@ -0,0 +1,104 @@
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "aero.h"
|
||||
|
||||
#include "atmos.h"
|
||||
#include "state.h"
|
||||
|
||||
#define EARTH_RAD 6.3567668e6
|
||||
|
||||
/* Compute aerodynamic forces and moments */
|
||||
int aero(double *y, state_t *s)
|
||||
{
|
||||
|
||||
double u = y[0]; /* velocity component in x */
|
||||
double v = y[1]; /* velocity component in y */
|
||||
double w = y[2]; /* velocity component in z */
|
||||
// double p = y[3]; /* angular velocity */
|
||||
// double q = y[4]; /* */
|
||||
// double r = y[5]; /* */
|
||||
// double lambda = y[6]; /* latitude */
|
||||
// double mu = y[7]; /* longitude */
|
||||
double Re = y[8]; /* distance from earth center */
|
||||
// double phi = y[9]; /* euler angles */
|
||||
// double theta = y[10]; /* */
|
||||
// double psi = y[11]; /* */
|
||||
|
||||
double X = 0;
|
||||
double Y = 0;
|
||||
double Z = 0;
|
||||
|
||||
double FD = 0; /* Drag */
|
||||
double FL = 0; /* Lift */
|
||||
double FB = 0; /* sideslip force */
|
||||
|
||||
// double SL = 0; /* Cross spin */
|
||||
// double SB = 0; /* Cross spin */
|
||||
|
||||
/* restoring moments */
|
||||
double ML = 0;
|
||||
double MM = 0;
|
||||
double MN = 0;
|
||||
|
||||
/* damping moments */
|
||||
double MG = 0;
|
||||
double MH = 0;
|
||||
double MI = 0;
|
||||
|
||||
AIR air;
|
||||
|
||||
double V; /* airspeed */
|
||||
double alpha; /* angle of attack */
|
||||
double beta; /* angle of sideslip */
|
||||
|
||||
/* compute atmospheric properties */
|
||||
atmosphere(&air, Re - EARTH_RAD);
|
||||
//printf("T = %f, P = %f, rho = %f, c = %f\n", air.T, air.P, air.rho, air.c);
|
||||
|
||||
V = sqrt(u*u + v*v + w*w);
|
||||
|
||||
if (V == 0.0)
|
||||
return 0;
|
||||
|
||||
alpha = atan(w/u);
|
||||
beta = asin(v/V);
|
||||
|
||||
FD = - s->Cdrag * air.rho * s->D * s->D * V * V;
|
||||
FL = s->Clift * air.rho * s->D * s->D * V * V * sin(alpha);
|
||||
FB = 0;// Cbeta * qbar * A * sin(beta);
|
||||
|
||||
//printf("FD = %f, FL = %f\n", FD, FL);
|
||||
|
||||
ML = 0;
|
||||
MG = 0;//- Cdamping * air.rho * V * s->P * A * C / 2;
|
||||
|
||||
MM = - s->Cmoment * air.rho * pow(s->D, 3) * V * V * sin(alpha);
|
||||
MH = - s->Cdamping * air.rho * pow(s->D, 4) * V * s->Q;
|
||||
|
||||
//printf("MM = %f, MH = %f\n", MM, MH);
|
||||
|
||||
MN = 0;//- Cmoment * qbar * A * C * sin(beta);
|
||||
MI = 0;//- Cdamping * air.rho * V * s->R * A * C / 2;
|
||||
|
||||
|
||||
/* transform wind axis forces to body axis forces */
|
||||
X = FD*cos(alpha)*cos(beta) - FB*cos(alpha)*sin(beta) - FL*sin(alpha);
|
||||
Y = FD*sin(beta) + FB*cos(beta);
|
||||
Z = FD*sin(alpha)*cos(beta) - FB*sin(alpha)*sin(beta) + FL*cos(alpha);
|
||||
|
||||
|
||||
s->Faero[0] = X;
|
||||
s->Faero[1] = Y;
|
||||
s->Faero[2] = Z;
|
||||
|
||||
s->Maero[0] = ML + MG;
|
||||
s->Maero[1] = MM + MH;
|
||||
s->Maero[2] = MN + MI;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
#ifndef __aero_h__
|
||||
#define __aero_h__
|
||||
|
||||
#include "state.h"
|
||||
|
||||
int aero(double *y, state_t *s);
|
||||
|
||||
#endif __aero_h__
|
||||
@@ -0,0 +1,103 @@
|
||||
/************************************************************************
|
||||
File: atmos.c
|
||||
Author: David E. Hall
|
||||
Date: 24 April 1996
|
||||
Description: A quick and dirty atmosphere calculator based upon the
|
||||
module found in the ExtREME (Extendible Round Earth Missile Emulation)
|
||||
simulation (authored by your's truly as well).
|
||||
*************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "atmos.h"
|
||||
|
||||
/*--- physical constants */
|
||||
#define EARTH_RAD 6.3567668e6 /* earth radius (m) */
|
||||
#define GRAVITY -9.806655352 /* gravitation accel (m/s2) */
|
||||
#define R 287.05376255 /* gas constant for air */
|
||||
|
||||
int atmosphere (AIR *a, double altitude)
|
||||
{
|
||||
|
||||
/*
|
||||
This function determines the atmospheric conditions at any given
|
||||
altitude. Note: the routine is a translation of an old FORTRAN
|
||||
routine of unknown origin and thus I have no idea as to what is
|
||||
being referenced in the original notes listed below....
|
||||
This atmosphere model is after the ISO Standard atmosphere as
|
||||
documented in ref. no. ISO 2533-1975(E) which has a first edition
|
||||
date of 1975-05-15 and corrected and reprinted date of 1978-12-15.
|
||||
This atmosphere is identical with the ICAO and WMO Standard
|
||||
atmospheres from -2000 to 32000 meters.
|
||||
*/
|
||||
|
||||
static double Abreak[9] = {-2.0, 11.0, 20.0, 32.0, 47.0,
|
||||
52.0, 61.0, 79.0, 88.74 };
|
||||
static double Tnot[9] = {301.15, 216.65, 216.65, 228.65, 270.65,
|
||||
270.65, 252.65, 180.65, 180.65 };
|
||||
static double Tslope[9] = {-6.5, 0.0, 1.0, 2.8, 0.0,
|
||||
-2.0, -4.0, 0.0, 0.0 };
|
||||
static double Pnot[9] = {127774.0, 22632.0, 5475.87, 868.014,
|
||||
110.906, 59.002, 18.208, 1.03757, 0.00 };
|
||||
int zone = -1;
|
||||
double alt;
|
||||
double delta_alt;
|
||||
double x, y;
|
||||
|
||||
//fprintf(stderr, "%f\n", altitude);
|
||||
/* Convert to geopotential altitude */
|
||||
alt = EARTH_RAD*altitude/(EARTH_RAD+altitude)/1000.0;
|
||||
|
||||
while ((zone < 8) && (alt >= Abreak[zone + 1]))
|
||||
zone++;
|
||||
|
||||
if (zone < 0)
|
||||
{ /* Below atmosphere data! */
|
||||
fprintf(stderr, "*** ERROR: Below atmospheric model (%f)\n", alt);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* The meat of the function */
|
||||
delta_alt = alt - Abreak[zone];
|
||||
a->T = Tnot[zone] + Tslope[zone] * delta_alt;
|
||||
|
||||
switch (zone)
|
||||
{
|
||||
case 1:
|
||||
case 4:
|
||||
case 7:
|
||||
case 8:
|
||||
a->P = Pnot[zone] * exp(GRAVITY*delta_alt*1000.0/R
|
||||
/a->T);
|
||||
break;
|
||||
|
||||
default:
|
||||
x = 1.0 + Tslope[zone] * delta_alt / Tnot[zone];
|
||||
y = GRAVITY * 1000.0 / Tslope[zone] / R;
|
||||
a->P = Pnot[zone] * pow(x,y);
|
||||
break;
|
||||
}
|
||||
|
||||
a->rho = a->P / R / a->T;
|
||||
a->c = 20.046796 * sqrt(a->T); /* c = sqrt(kRT) k=1.4 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
int main(int argc, char *argw[])
|
||||
{
|
||||
AIR air;
|
||||
double alt;
|
||||
|
||||
for (alt = 0; alt < 100000; alt += 1000)
|
||||
{
|
||||
atmosphere(&air, alt);
|
||||
printf("%f %f %f %f\n", air.temperature, air.pressure,
|
||||
air.density, air.mach1);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef __atmos_h__
|
||||
#define __atmos_h__
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double T;
|
||||
double P;
|
||||
double rho;
|
||||
double c;
|
||||
} AIR;
|
||||
|
||||
int atmosphere (AIR *a, double altitude);
|
||||
|
||||
#endif __atmos_h__
|
||||
@@ -0,0 +1,23 @@
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "state.h"
|
||||
#include "engine.h"
|
||||
|
||||
|
||||
/* Add engine component of force and moment */
|
||||
int engine(double *y, state_t *s)
|
||||
{
|
||||
|
||||
s->Feng[0] = 1000.0;
|
||||
s->Feng[1] = 0.0;
|
||||
s->Feng[2] = 0.0;
|
||||
|
||||
s->Meng[0] = 0.0;
|
||||
s->Meng[1] = 0.0;
|
||||
s->Meng[2] = 0.0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
|
||||
#ifndef __engine_h__
|
||||
#define __engine_h__
|
||||
|
||||
int engine(double *y, state_t *s);
|
||||
|
||||
#endif __engine_h__
|
||||
@@ -0,0 +1,168 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "aero.h"
|
||||
|
||||
#include "rocflight.h"
|
||||
#include "state.h"
|
||||
#include "atmos.h"
|
||||
#include "engine.h"
|
||||
|
||||
#define SEC(THETA) (1./cos(THETA))
|
||||
|
||||
int eom(int neq, double time, double *y, double *dy, void *data)
|
||||
{
|
||||
state_t *s = (state_t *) data;
|
||||
|
||||
double u = y[0]; /* velocity component in x */
|
||||
double v = y[1]; /* velocity component in y */
|
||||
double w = y[2]; /* velocity component in z */
|
||||
double p = y[3]; /* angular velocity */
|
||||
double q = y[4]; /* */
|
||||
double r = y[5]; /* */
|
||||
double lambda = y[6]; /* latitude */
|
||||
// double mu = y[7]; /* longitude */
|
||||
double Re = y[8]; /* distance from earth center */
|
||||
double phi = y[9]; /* euler angles */
|
||||
double theta = y[10]; /* */
|
||||
double psi = y[11]; /* */
|
||||
|
||||
|
||||
/* aerodynamic forces */
|
||||
double X = 0.0;
|
||||
double Y = 0.0;
|
||||
double Z = 0.0;
|
||||
|
||||
/* moments */
|
||||
double L = 0.0;
|
||||
double M = 0.0;
|
||||
double N = 0.0;
|
||||
|
||||
/* transformation matrix (vector in vehicle carried reference to
|
||||
body reference */
|
||||
s->L_BV[0][0] = cos(theta)*cos(psi);
|
||||
s->L_BV[0][1] = cos(theta)*sin(psi);
|
||||
s->L_BV[0][2] = -sin(theta);
|
||||
s->L_BV[1][0] = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
|
||||
s->L_BV[1][1] = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
|
||||
s->L_BV[1][2] = sin(phi)*cos(theta);
|
||||
s->L_BV[2][0] = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
|
||||
s->L_BV[2][1] = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
|
||||
s->L_BV[2][2] = cos(phi)*cos(theta);
|
||||
|
||||
/* Inverse of the previous matrix */
|
||||
s->L_VB[0][0] = s->L_BV[0][0];
|
||||
s->L_VB[0][1] = s->L_BV[1][0];
|
||||
s->L_VB[0][2] = s->L_BV[2][0];
|
||||
s->L_VB[1][0] = s->L_BV[0][1];
|
||||
s->L_VB[1][1] = s->L_BV[1][1];
|
||||
s->L_VB[1][2] = s->L_BV[2][1];
|
||||
s->L_VB[2][0] = s->L_BV[0][2];
|
||||
s->L_VB[2][1] = s->L_BV[1][2];
|
||||
s->L_VB[2][2] = s->L_BV[2][2];
|
||||
|
||||
/* Earth angular velocity vector expressed in body reference */
|
||||
s->pE = (s->L_BV[0][0]*cos(lambda) - s->L_BV[0][2]*sin(lambda)) * WE;
|
||||
s->qE = (s->L_BV[1][0]*cos(lambda) - s->L_BV[1][2]*sin(lambda)) * WE;
|
||||
s->rE = (s->L_BV[2][0]*cos(lambda) - s->L_BV[2][2]*sin(lambda)) * WE;
|
||||
|
||||
/* Centripetal acceleration */
|
||||
s->Cx = (s->L_BV[0][0]*Re*cos(lambda)*sin(lambda) +
|
||||
s->L_BV[0][2]*Re*cos(lambda)*cos(lambda)) * WE2;
|
||||
s->Cy = (s->L_BV[1][0]*Re*cos(lambda)*sin(lambda) +
|
||||
s->L_BV[1][2]*Re*cos(lambda)*cos(lambda)) * WE2;
|
||||
s->Cz = (s->L_BV[2][0]*Re*cos(lambda)*sin(lambda) +
|
||||
s->L_BV[2][2]*Re*cos(lambda)*cos(lambda)) * WE2;
|
||||
|
||||
/* gravitational model */
|
||||
s->g = UGC*ME/(Re*Re);
|
||||
|
||||
/* compute aerodynamic forces and moments */
|
||||
if (aero(y, s))
|
||||
return -1;
|
||||
|
||||
X += s->Faero[0]; //printf("X = %f\n", s->Faero[0]);
|
||||
Y += s->Faero[1]; //printf("Y = %f\n", s->Faero[1]);
|
||||
Z += s->Faero[2]; //printf("Z = %f\n", s->Faero[2]);
|
||||
|
||||
L += s->Maero[0]; //printf("L = %f\n", s->Maero[0]);
|
||||
M += s->Maero[1]; //printf("M = %f\n", s->Maero[1]);
|
||||
N += s->Maero[2]; //printf("N = %f\n", s->Maero[2]);
|
||||
|
||||
if (engine(y, s))
|
||||
return -1;
|
||||
|
||||
/* add the thrust of the engine */
|
||||
X += s->Feng[0];
|
||||
Y += s->Feng[1];
|
||||
Z += s->Feng[2];
|
||||
|
||||
L += s->Meng[0];
|
||||
M += s->Meng[1];
|
||||
N += s->Meng[2];
|
||||
|
||||
/* acceleration of the rocket in the body axis */
|
||||
dy[0] = X/s->m - s->g*sin(theta) - s->Cx - (s->qE + q)*w +
|
||||
(s->rE + r)*v;
|
||||
dy[1] = Y/s->m + s->g*cos(theta)*sin(phi) - s->Cy - (s->rE + r)*u +
|
||||
(s->pE + p)*w;
|
||||
dy[2] = Z/s->m + s->g*cos(theta)*cos(phi) - s->Cz - (s->pE + p)*v +
|
||||
(s->qE + q)*u;
|
||||
|
||||
/* angular acceleration equation */
|
||||
dy[3] = (L + (s->Iy - s->Iz)*q*r)/s->Ix;
|
||||
dy[4] = (M + (s->Iz - s->Ix)*r*p)/s->Iy;
|
||||
dy[5] = (N + (s->Ix - s->Iy)*p*q)/s->Iz;
|
||||
|
||||
/* position relative to earth */
|
||||
dy[6] = (s->L_VB[0][0]*u + s->L_VB[0][1]*v+s->L_VB[0][2]*w)/Re;
|
||||
dy[7] = (s->L_VB[1][0]*u + s->L_VB[1][1]*v+s->L_VB[1][2]*w)/(Re*cos(lambda));
|
||||
dy[8] =-(s->L_VB[2][0]*u + s->L_VB[2][1]*v+s->L_VB[2][2]*w);
|
||||
|
||||
/* relative angular velocity of vehicle relative to atmosphere */
|
||||
s->P = p - (s->L_BV[0][0]*(WE + dy[7])*cos(lambda) -
|
||||
s->L_BV[0][1]*dy[6] - s->L_BV[0][2]*(WE + dy[7])*sin(lambda));
|
||||
s->Q = q - (s->L_BV[1][0]*(WE + dy[7])*cos(lambda) -
|
||||
s->L_BV[1][1]*dy[6] - s->L_BV[1][2]*(WE + dy[7])*sin(lambda));
|
||||
s->R = r - (s->L_BV[2][0]*(WE + dy[7])*cos(lambda) -
|
||||
s->L_BV[2][1]*dy[6] - s->L_BV[2][2]*(WE + dy[7])*sin(lambda));
|
||||
|
||||
|
||||
/* euler angle equation */
|
||||
/* we have to check for division by zero */
|
||||
if (abs(cos(theta)) > 0.001)
|
||||
{
|
||||
dy[9] = s->P + sin(phi)*tan(theta)*s->Q + cos(phi)*tan(theta)*s->R;
|
||||
dy[10] = cos(phi) *s->Q - sin(phi) *s->R;
|
||||
dy[11] = sin(phi)*SEC(theta)*s->Q + cos(phi)*SEC(theta)*s->R;
|
||||
}
|
||||
else
|
||||
{
|
||||
dy[9] = s->P;
|
||||
dy[10] = cos(phi)*s->Q - sin(phi)*s->R;
|
||||
dy[11] = 0;
|
||||
}
|
||||
|
||||
// printf("%f %f %f %f %f %f %f %f %f %f %f %f\n",
|
||||
// dy[0], dy[1], dy[2], dy[3], dy[4], dy[5], dy[6], dy[7], dy[8],
|
||||
// dy[9], dy[10], dy[11]);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,168 @@
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "rocflight.h"
|
||||
|
||||
#include "num.h"
|
||||
#include "state.h"
|
||||
|
||||
int eom(int neq, double time, double *y, double *dy, void *data);
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
double epsilon = 1e-4;
|
||||
|
||||
int i, n;
|
||||
int neq = 12;
|
||||
|
||||
double time;
|
||||
//double res[12];
|
||||
|
||||
double *ans;
|
||||
double *init_cond;
|
||||
|
||||
double u = 100; /* velocity component in x */
|
||||
double v = 0; /* velocity component in y */
|
||||
double w = 10; /* velocity component in z */
|
||||
|
||||
double P = 0; /* angular velocity */
|
||||
double Q = 0; /* */
|
||||
double R = 0; /* */
|
||||
|
||||
double lambda = 0*(M_PI/180); /* latitude (positive north) */
|
||||
double mu = 0*(M_PI/180); /* longitude (positive east) */
|
||||
double Re = EARTH_RAD + 5000; /* distance from earth center */
|
||||
double phi = 0*(M_PI/180); /* */
|
||||
double theta = 80*(M_PI/180); /* positive up */
|
||||
double psi = 0*(M_PI/180); /* positive cw*/
|
||||
|
||||
|
||||
double dt = 0.01;
|
||||
double duration = 5;
|
||||
|
||||
state_t s;
|
||||
s.m = 16.0; /* in kg */
|
||||
|
||||
s.Ix = 0.0025;
|
||||
s.Iy = 0.84;
|
||||
s.Iz = 0.84;
|
||||
|
||||
/* aerodynamic coefficients */
|
||||
s.Cdrag = 0.2;
|
||||
s.Clift = 2.3;
|
||||
s.Cbeta = 2.3;
|
||||
s.Cspin = 20.0;
|
||||
s.Cmoment = 6.32;
|
||||
s.Cdamping = 31.6;
|
||||
|
||||
s.D = 0.1143;
|
||||
// s.A = M_PI*s.D*s.D/4; /* reference area (m^2) */
|
||||
// s.C = 1; /* reference distance */
|
||||
|
||||
s.Faero[0] = 0;
|
||||
s.Faero[1] = 0;
|
||||
s.Faero[2] = 0;
|
||||
|
||||
s.Maero[0] = 0;
|
||||
s.Maero[1] = 0;
|
||||
s.Maero[2] = 0;
|
||||
|
||||
init_cond = (double *) malloc(sizeof(double) * neq);
|
||||
|
||||
s.L_BV[0][0] = cos(theta)*cos(psi);
|
||||
s.L_BV[0][1] = cos(theta)*sin(psi);
|
||||
s.L_BV[0][2] =-sin(theta);
|
||||
s.L_BV[1][0] = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
|
||||
s.L_BV[1][1] = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
|
||||
s.L_BV[1][2] = sin(phi)*cos(theta);
|
||||
s.L_BV[2][0] = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
|
||||
s.L_BV[2][1] = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
|
||||
s.L_BV[2][2] = cos(phi)*cos(theta);
|
||||
|
||||
init_cond[0] = u; /* velocity component in x */
|
||||
init_cond[1] = v; /* velocity component in y */
|
||||
init_cond[2] = w; /* velocity component in z */
|
||||
init_cond[3] = P+ (s.L_BV[0][0]*WE*cos(lambda)-s.L_BV[0][2]*WE*sin(lambda));
|
||||
init_cond[4] = Q+ (s.L_BV[1][0]*WE*cos(lambda)-s.L_BV[1][2]*WE*sin(lambda));
|
||||
init_cond[5] = R+ (s.L_BV[2][0]*WE*cos(lambda)-s.L_BV[2][2]*WE*sin(lambda));
|
||||
init_cond[6] = lambda; /* latitude */
|
||||
init_cond[7] = mu; /* longitude */
|
||||
init_cond[8] = Re; /* radius */
|
||||
init_cond[9] = phi; /* phi */
|
||||
init_cond[10] = theta; /* theta */
|
||||
init_cond[11] = psi; /* psi */
|
||||
|
||||
time = 0;
|
||||
|
||||
// eom(neq, time, init_cond, res, &s);
|
||||
/*
|
||||
fprintf(stderr, "% e (u dot)\n", res[0]);
|
||||
fprintf(stderr, "% e (v dot)\n", res[1]);
|
||||
fprintf(stderr, "% e (w dot)\n", res[2]);
|
||||
fprintf(stderr, "% e (p dot)\n", res[3]);
|
||||
fprintf(stderr, "% e (q dot)\n", res[4]);
|
||||
fprintf(stderr, "% e (r dot)\n", res[5]);
|
||||
fprintf(stderr, "% e (lambda dot)\n", res[6]);
|
||||
fprintf(stderr, "% e (mu dot)\n", res[7]);
|
||||
fprintf(stderr, "% e (R dot)\n", res[8]);
|
||||
fprintf(stderr, "% e (phi dot)\n", res[9]);
|
||||
fprintf(stderr, "% e (theta dot)\n", res[10]);
|
||||
fprintf(stderr, "% e (psi dot)\n", res[11]);
|
||||
*/
|
||||
|
||||
//n = NUM_rk4 (eom, neq, dt, duration, init_cond, &ans, &s);
|
||||
|
||||
n = NUM_rkf (eom, neq, dt, duration, init_cond, &ans, epsilon, &s);
|
||||
|
||||
fprintf(stderr, "Memory used: %d kB\n", n*(neq+1)*sizeof(double)/1024);
|
||||
|
||||
printf("# Created by rocflight\n");
|
||||
printf("# name: M\n");
|
||||
printf("# type: matrix\n");
|
||||
printf("# rows: %d\n", n);//(int)(duration/dt) + 1);
|
||||
printf("# columns: %d\n", neq + 1);
|
||||
|
||||
|
||||
//for( i = 0; i < n; i++)
|
||||
//{
|
||||
// printf("% .12e ", ans[0 + neq*i]); /* u */
|
||||
// printf("% .12e ", ans[1 + neq*i]); /* v */
|
||||
// printf("% .12e ", ans[2 + neq*i]); /* w */
|
||||
// printf("% .12e ", ans[3 + neq*i]); /* p */
|
||||
// printf("% .12e ", ans[4 + neq*i]); /* q */
|
||||
// printf("% .12e ", ans[5 + neq*i]); /* r */
|
||||
// printf("% .12e ", ans[6 + neq*i]); /* lambda */
|
||||
// printf("% .12e ", ans[7 + neq*i]); /* mu */
|
||||
// printf("% .12e ", ans[8 + neq*i]); /* R */
|
||||
// printf("% .12e ", ans[9 + neq*i]); /* phi */
|
||||
// printf("% .12e ", ans[10 + neq*i]); /* theta */
|
||||
// printf("% .12e \n", ans[11 + neq*i]); /* psi */
|
||||
//}
|
||||
|
||||
|
||||
for( i = 0; i < n; i++)
|
||||
{
|
||||
printf("% .12e ", ans[12 + (neq+1)*i]); /* t */
|
||||
printf("% .12e ", ans[0 + (neq+1)*i]); /* u */
|
||||
printf("% .12e ", ans[1 + (neq+1)*i]); /* v */
|
||||
printf("% .12e ", ans[2 + (neq+1)*i]); /* w */
|
||||
printf("% .12e ", ans[3 + (neq+1)*i]); /* p */
|
||||
printf("% .12e ", ans[4 + (neq+1)*i]); /* q */
|
||||
printf("% .12e ", ans[5 + (neq+1)*i]); /* r */
|
||||
printf("% .12e ", ans[6 + (neq+1)*i]); /* lambda */
|
||||
printf("% .12e ", ans[7 + (neq+1)*i]); /* mu */
|
||||
printf("% .12e ", ans[8 + (neq+1)*i]); /* R */
|
||||
printf("% .12e ", ans[9 + (neq+1)*i]); /* phi */
|
||||
printf("% .12e ", ans[10 + (neq+1)*i]); /* theta */
|
||||
printf("% .12e ", ans[11 + (neq+1)*i]); /* psi */
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
free(init_cond);
|
||||
//free(ans);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef __rocflight_h__
|
||||
#define __rocflight_h__
|
||||
|
||||
|
||||
/* universal gravitational constant */
|
||||
#define UGC 6.672e-11
|
||||
/* earth angular velocity */
|
||||
#define WE 7.27e-5
|
||||
/* square of earth angular velocity */
|
||||
#define WE2 5.28529e-9
|
||||
/* earth mass */
|
||||
#define ME 5.98e24
|
||||
|
||||
/* earth radius */
|
||||
#define EARTH_RAD 6.3567668e6
|
||||
|
||||
#endif __rocflight_h__
|
||||
@@ -0,0 +1,68 @@
|
||||
|
||||
#ifndef __state_h__
|
||||
#define __state_h__
|
||||
|
||||
typedef double real;
|
||||
|
||||
typedef real scalar_t;
|
||||
typedef real vector_t[3];
|
||||
typedef real matrix_t[3][3];
|
||||
|
||||
|
||||
typedef struct state
|
||||
{
|
||||
scalar_t m;
|
||||
scalar_t g;
|
||||
|
||||
scalar_t Ix;
|
||||
scalar_t Iy;
|
||||
scalar_t Iz;
|
||||
|
||||
/* aerodynamic coefficients */
|
||||
scalar_t Clift;
|
||||
scalar_t Cbeta;
|
||||
scalar_t Cdrag;
|
||||
scalar_t Cspin;
|
||||
scalar_t Cmoment;
|
||||
scalar_t Cdamping;
|
||||
|
||||
scalar_t D; /* Diameter of the rocket */
|
||||
scalar_t A; /* reference area (m^2) */
|
||||
scalar_t C; /* reference distance */
|
||||
|
||||
/* aerodynamic forces */
|
||||
vector_t Faero; /* (X, Y, Z) */
|
||||
/* moments */
|
||||
vector_t Maero; /* (L, M, N) */
|
||||
|
||||
/* engine forces and moments */
|
||||
vector_t Feng;
|
||||
vector_t Meng;
|
||||
|
||||
/* component of centripetal acceleration in F_B */
|
||||
scalar_t Cx;
|
||||
scalar_t Cy;
|
||||
scalar_t Cz;
|
||||
|
||||
/* earth angular velocity in F_B */
|
||||
scalar_t pE;
|
||||
scalar_t qE;
|
||||
scalar_t rE;
|
||||
|
||||
/* relative angular velocity of F_B to F_V */
|
||||
scalar_t P;
|
||||
scalar_t Q;
|
||||
scalar_t R;
|
||||
|
||||
matrix_t L_BV;
|
||||
matrix_t L_VB;
|
||||
|
||||
} state_t;
|
||||
|
||||
#endif /* __state_h__ */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
function rho = density(y)
|
||||
|
||||
P0 = 1.013e5; # standard stmospheric pressure
|
||||
Ma = 0.0289644; # atmosphere molar mass
|
||||
Rg = 8.3145; # perfect gas constant
|
||||
T = 273.15; # standard temperature
|
||||
|
||||
G = 6.67259e-11; # gravitational constant
|
||||
Mt = 5.98e24; # earth mass
|
||||
Rt = 6370000; # earth radius
|
||||
|
||||
rho = (P0*Ma/(Rg*T))*exp( ((Ma*G*Mt)/(Rg*T))*((1/(Rt+y))-(1/Rt)) );
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,27 @@
|
||||
function dy = flight(y, t)
|
||||
|
||||
G = 6.672e-11;
|
||||
M = 5.98e24;
|
||||
W = 7.27e-5;
|
||||
|
||||
m = 100; # kg
|
||||
Cd = 0.75;
|
||||
A = 0.1;
|
||||
rho = density(y(1)-6370000);
|
||||
Cr = 0.5*Cd*rho*A;
|
||||
|
||||
v = sqrt( y(4)^2 + (y(1)*y(6)*cos(y(2)))^2 + (y(1)*y(5))^2);
|
||||
|
||||
dy(1) = y(4);
|
||||
dy(2) = y(5);
|
||||
dy(3) = y(6);
|
||||
|
||||
dy(4) = -(G*M)/(y(1)^2) - Cr*v*y(4)/m + y(1)*(y(5)^2) + ((W + y(6))^2)*y(1)*(cos(y(2))^2);
|
||||
|
||||
dy(5) = (-1/y(1))*( Cr*v*y(1)*y(5)/m + y(1)*cos(y(2))*sin(y(2))*(W+y(6))^2 +
|
||||
2*y(4)*y(5));
|
||||
dy(6) = (-1/(y(1)*cos(y(2)))) * (Cr*v*y(1)*y(6)*cos(y(2))/m +
|
||||
2*(W*(y(4)-y(1)*y(5))+y(4)*y(6))*cos(y(2)) -
|
||||
2*y(1)*y(6)*y(5)*sin(y(2)));
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,16 @@
|
||||
function dy = flight_vacuum(y, t)
|
||||
|
||||
G = 6.672e-11;
|
||||
M = 5.98e24;
|
||||
W = 7.27e-5;
|
||||
|
||||
dy(1) = y(4);
|
||||
dy(2) = y(5);
|
||||
dy(3) = y(6);
|
||||
|
||||
dy(4) = -(G*M)/(y(1)^2) + y(1)*(y(5)^2) + ((W + y(6))^2)*y(1)*(cos(y(2))^2);
|
||||
dy(5) = (-1/y(1))*( y(1)*cos(y(2))*sin(y(2))*(W+y(6))^2 + 2*y(4)*y(5));
|
||||
dy(6) = (-1/(y(1)*cos(y(2))))*(2*(W*(y(4)-y(1)*y(5))+y(4)*y(6))*cos(y(2)) -
|
||||
2*y(1)*y(6)*y(5)*sin(y(2)));
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,31 @@
|
||||
|
||||
altitude = 0; # altitude in meter
|
||||
latitude = 15; # latitude in decimal degree
|
||||
longitude = 0; # longitude in decimal degree
|
||||
|
||||
velocity = 1000; # magnitude of the initial velocity in meter/s
|
||||
angle = 0; # angle with the horizontal in degree
|
||||
direction = 90; # direction (0=east 90=north 180=west 270=south)
|
||||
|
||||
R = 6.37e6; # earth radius in meter
|
||||
|
||||
r = R + altitude;
|
||||
r_dot = velocity*sin(angle*pi/180);
|
||||
u_dot = velocity*cos(angle*pi/180)*cos(direction*pi/180)/(r*cos(latitude*pi/180));
|
||||
l_dot = velocity*cos(angle*pi/180)*sin(direction*pi/180)/r;
|
||||
|
||||
init = [ r; latitude*pi/180; longitude*pi/180; r_dot; l_dot; u_dot; ];
|
||||
|
||||
t = 0:0.1:200;
|
||||
|
||||
y = lsode('flight_vacuum', init, t);
|
||||
|
||||
hold off
|
||||
plot(y(:,1).*cos(y(:,2)), y(:,1).*sin(y(:,2)));
|
||||
hold on;
|
||||
plot(R*cos(y(:,2)), R*sin(y(:,2)));
|
||||
|
||||
#plot(y(:,3)*180/pi, y(:,2)*180/pi);
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user