mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -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)
|
||||
|
||||
@@ -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; i<SERVOS_NB; i++)
|
||||
bs.mot_voltage->ve[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
|
||||
@@ -0,0 +1,31 @@
|
||||
#ifndef BOOZ_FLIGHT_MODEL_H
|
||||
#define BOOZ_FLIGHT_MODEL_H
|
||||
|
||||
#include <matrix.h>
|
||||
|
||||
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 */
|
||||
@@ -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 */
|
||||
@@ -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 */
|
||||
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
#ifndef BOOZ_FM_UTILS_H
|
||||
#define BOOZ_FM_UTILS_H
|
||||
|
||||
#include <matrix.h>
|
||||
|
||||
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 */
|
||||
@@ -0,0 +1,36 @@
|
||||
#include <glib.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user