diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 6368cb59f5..3cdb298422 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -97,5 +97,5 @@ CC = gcc CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -I../airborne -I../../var/BOOZ LDFLAGS=`pkg-config glib-2.0 --libs` -lm -lmeschach -booz_sim: main_booz_sim.c booz_flight_model.c +booz_sim: main_booz_sim.c booz_flight_model.c booz_flight_model_utils.c $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c new file mode 100644 index 0000000000..3ae0a3b330 --- /dev/null +++ b/sw/simulator/booz_flight_model.c @@ -0,0 +1,69 @@ +#include "booz_flight_model.h" + +#include "booz_flight_model_params.h" +#include "booz_flight_model_utils.h" +#include "airframe.h" + +#include "6dof.h" + + +struct BoozState bs; + +static void motor_model_run( double dt ); +static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot); + +void booz_flight_model_init( void ) { + bs.position = v_get(AXIS_NB); + bs.speed = v_get(AXIS_NB); + bs.eulers = v_get(AXIS_NB); + bs.eulers_dot = v_get(AXIS_NB); + bs.body_rates = v_get(AXIS_NB); + bs.bat_voltage = BAT_VOLTAGE; + bs.mot_voltage = v_get(SERVOS_NB); + bs.mot_omega = v_get(SERVOS_NB); +} + +void booz_flight_model_run( double dt, double* commands ) { + int i; + for (i=0; ive[i] = bs.bat_voltage * commands[i]; + motor_model_run(dt); + +} + + +static void motor_model_run( double dt ) { + rk4(motor_model_derivative, bs.mot_omega, bs.mot_voltage, dt); +} + +static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) { + static VEC *temp1 = VNULL; + static VEC *temp2 = VNULL; + temp1 = v_resize(temp1,SERVOS_NB); + temp2 = v_resize(temp2,SERVOS_NB); + + // omega_dot = -1/THAU*omega - Kq*omega*omega + Kv/THAU * v; + temp1 = sv_mlt(-1./THAU, x, temp1); /* temp1 = -1/THAU * x */ + temp2 = v_star(x, x, temp2); /* temp2 = x^2 */ + xdot = v_mltadd(temp1, temp2, -Kq, xdot); /* xdot = temp1 - Kq*temp2 */ + xdot = v_mltadd(xdot, u, Kv/THAU, xdot); /* xdot = xdot + Kv/THAU * u */ + +} + + + + +#if 0 + VEC* foo1 = v_get(3); + foo1->ve[0] = 1; + foo1->ve[1] = 2; + foo1->ve[2] = 3; + VEC* foo2 = v_get(3); + foo2->ve[0] = 1; + foo2->ve[1] = 2; + foo2->ve[2] = 3; + VEC* foo3 = v_get(3); + v_star(foo1, foo2, foo3); + + printf("%f %f %f\n", foo3->ve[0], foo3->ve[1], foo3->ve[2] ); +#endif diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h new file mode 100644 index 0000000000..ef70300248 --- /dev/null +++ b/sw/simulator/booz_flight_model.h @@ -0,0 +1,31 @@ +#ifndef BOOZ_FLIGHT_MODEL_H +#define BOOZ_FLIGHT_MODEL_H + +#include + +struct BoozState { + /* position in earth frame */ + VEC* position; + /* speed in earth frame */ + VEC* speed; + /* euler angles in earth frame */ + VEC* eulers; + /* euler angles derivatives in earth frame */ + VEC* eulers_dot; + /* body rates */ + VEC* body_rates; + /* battery volatge in V */ + double bat_voltage; + /* motors supply voltage in V */ + VEC* mot_voltage; + /* motors rotational speed in rad/s */ + VEC* mot_omega; +}; + +extern struct BoozState bs; + +extern void booz_flight_model_init( void ); +extern void booz_flight_model_run( double t, double* commands ); + + +#endif /* BOOZ_FLIGHT_MODEL_H */ diff --git a/sw/simulator/booz_flight_model_params.h b/sw/simulator/booz_flight_model_params.h new file mode 100644 index 0000000000..52aced8909 --- /dev/null +++ b/sw/simulator/booz_flight_model_params.h @@ -0,0 +1,49 @@ +#ifndef BOOZ_FLIGHT_MODEL_PARAMS_H +#define BOOZ_FLIGHT_MODEL_PARAMS_H + +/* drag coefficient of the body */ +#define C_d_body .2 +/* thrust aerodynamic coefficient */ +#define C_t 1. +/* moment aerodynamic coefficient */ +#define C_q 0.1 +/* propeller radius in m */ +#define PROP_RADIUS 0.125 +/* propeller area in m2 */ +#define PROP_AREA (M_PI * PROP_RADIUS * PROP_RADIUS) +/* air density in kg/m3 */ +#define RHO 1.225 +/* gravity in m/s2 */ +#define G 9.81 +/* mass in kg */ +#define MASS 0.5 +/* inertia on x axis in m2 */ +#define Ix .5 +/* inertia on y axis in m2 */ +#define Iy .5 +/* inertia on z axis in m2 */ +#define Iz 1. +/* lenght between centers of vehicle and prop in m */ +#define L 0.25 +/* height between cg and prop plane in m */ +#define H 0.04 + + +/* motors parameters + + from http://cherokee.stanford.edu/~starmac/docs/DynamicsSummary + + omega_dot = -1/thau * omega - Kq * omega^2 + Kv/thau * Va + + with Q = Kq * omega * omega + +*/ +#define BAT_VOLTAGE 11. + +#define THAU 100. +#define Kq .01 +#define Kv 10000. + + + +#endif /* BOOZ_FLIGHT_MODEL_PARAMS_H */ diff --git a/sw/simulator/booz_flight_model_utils.c b/sw/simulator/booz_flight_model_utils.c new file mode 100644 index 0000000000..5a09850089 --- /dev/null +++ b/sw/simulator/booz_flight_model_utils.c @@ -0,0 +1,37 @@ +#include "booz_flight_model_utils.h" + +void rk4(ode_fun f, VEC* x, VEC* u, double dt) { + + static VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL; + static VEC *temp=VNULL; + + v1 = v_resize(v1,x->dim); + v2 = v_resize(v2,x->dim); + v3 = v_resize(v3,x->dim); + v4 = v_resize(v4,x->dim); + temp = v_resize(temp,x->dim); + + // MEM_STAT_REG(v1,TYPE_VEC); + // MEM_STAT_REG(v2,TYPE_VEC); + // MEM_STAT_REG(v3,TYPE_VEC); + // MEM_STAT_REG(v4,TYPE_VEC); + // MEM_STAT_REG(temp,TYPE_VEC); + + f(x, u, v1); + v_mltadd(x,v1,0.5*dt,temp); /* temp = x + .5*dt*v1 */ + f(temp, u, v2); + v_mltadd(x,v2,0.5*dt,temp); /* temp = x + .5*dt*v2 */ + f(temp,u, v3); + v_mltadd(x,v3,dt,temp); /* temp = x + dt*v3 */ + f(temp, u, v4); + + /* now add: v1+2*v2+2*v3+v4 */ + v_copy(v1,temp); /* temp = v1 */ + v_mltadd(temp,v2,2.0,temp); /* temp = v1+2*v2 */ + v_mltadd(temp,v3,2.0,temp); /* temp = v1+2*v2+2*v3 */ + v_add(temp,v4,temp); /* temp = v1+2*v2+2*v3+v4 */ + + /* adjust x */ + v_mltadd(x,temp,dt/6.0,x); /* x = x+(h/6)*temp */ + +} diff --git a/sw/simulator/booz_flight_model_utils.h b/sw/simulator/booz_flight_model_utils.h new file mode 100644 index 0000000000..f676362657 --- /dev/null +++ b/sw/simulator/booz_flight_model_utils.h @@ -0,0 +1,10 @@ +#ifndef BOOZ_FM_UTILS_H +#define BOOZ_FM_UTILS_H + +#include + +typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot); + +extern void rk4(ode_fun f, VEC* x, VEC* u, double dt); + +#endif /* BOOZ_FM_UTILS_H */ diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c new file mode 100644 index 0000000000..8cfa23259a --- /dev/null +++ b/sw/simulator/main_booz_sim.c @@ -0,0 +1,36 @@ +#include + +#include "booz_flight_model.h" + +/* 250Hz <-> 4ms */ +#define TIMEOUT_PERIOD 4 +#define DT (TIMEOUT_PERIOD*1e-3) +double sim_time; + +double commands[] = {0.7, 0.7, 0.7, 0.7}; + +gboolean timeout_callback(gpointer data) { + + booz_flight_model_run(DT, commands); + sim_time += DT; + + printf("%f %f %f %f %f\n", sim_time, bs.mot_omega->ve[0], bs.mot_omega->ve[1], + bs.mot_omega->ve[2], bs.mot_omega->ve[3]); + + return TRUE; +} + +int main ( int argc, char** argv) { + + sim_time = 0.; + booz_flight_model_init(); + + + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + + g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL); + + g_main_loop_run(ml); + + return 0; +}