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:
R. J. Dev. Kwan
2021-01-20 15:50:36 -08:00
commit eb05416991
411 changed files with 362300 additions and 0 deletions
File diff suppressed because it is too large Load Diff
+291
View File
@@ -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}
+32
View File
@@ -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)
+454
View File
@@ -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;
}
+37
View File
@@ -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)
+104
View File
@@ -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;
}
+8
View File
@@ -0,0 +1,8 @@
#ifndef __aero_h__
#define __aero_h__
#include "state.h"
int aero(double *y, state_t *s);
#endif __aero_h__
+103
View File
@@ -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;
}
*/
+14
View File
@@ -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__
+23
View File
@@ -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;
}
+7
View File
@@ -0,0 +1,7 @@
#ifndef __engine_h__
#define __engine_h__
int engine(double *y, state_t *s);
#endif __engine_h__
+168
View File
@@ -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;
}
+168
View File
@@ -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;
}
+17
View File
@@ -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__
+68
View File
@@ -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__ */
+14
View File
@@ -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
+27
View File
@@ -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
+16
View File
@@ -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
+31
View File
@@ -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);